[MoveIt!] Cannot update robot's state
Hey everyone!
My problem is related to this one: http://answers.ros.org/question/25211... . Also to this one: http://answers.ros.org/question/25492... .
The thing is, much like the OP of these questions, Often I would receive the following warning message from MoveIt! when trying to execute trajectories (in Gazebo):
Failed to validate trajectory: couldn't receive full current joint state within 1s
After that, I am no longer able to execute more trajectories with MoveIt! until I restart the whole simulation.
Unlike what the comments and answers of the previously referenced questions suggest, my move_group node is subscribed to the correct joint_states topic, and the joint angles of the robots are being published here (@50Hz). In fact, while I have this problem in ROS Kinetic, my set-up works just fine in ROS Indigo.
Even when the previous warning does not appear, I would get this other one:
Failed to fetch current robot state
After this warning, I can still work with MoveIt!, although it is a strong indicative that the robot's state is not being monitored and/or updated by the move_group node.
I am working with a Barrett WAM simulated arm (actually, with a couple of them combined into a single robot). As I said, I am working with ROS Kinetic and with the version of MoveIt! that is being shipped by the Ubuntu's repository. Also, everything works fine in ROS Indigo (which, of course, comes with an older MoveIt!'s version). Below you can see a fragment of a log I got in a session with MoveIt! in Kinetic:
[ INFO] [1490477334.335206408, 42.968000000]: Cancelling execution for iri_wam_picker_controller
[ WARN] [1490477335.335728535, 43.168000000]: Failed to receive current joint state
[ INFO] [1490477335.336702721, 43.168000000]: Execution completed: TIMED_OUT
[ WARN] [1490477380.333069544, 52.725000000]: Failed to fetch current robot state.
[ INFO] [1490477380.333307431, 52.725000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1490477380.345604383, 52.725000000]: Planner configuration 'picker[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner i$
constructed.
[ INFO] [1490477380.357309208, 52.728000000]: picker[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1490477380.359983571, 52.729000000]: picker[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1490477380.364642171, 52.729000000]: picker[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1490477380.368296469, 52.730000000]: picker[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1490477380.529691461, 52.771000000]: picker[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1490477380.547800109, 52.775000000]: picker[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1490477380.549793251, 52.776000000]: picker[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1490477380.562846623, 52.777000000]: picker[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1490477380.564459663, 52.777000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.209642 seconds
[ INFO] [1490477380.564928778, 52.777000000]: picker[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1490477380.567619783, 52.778000000]: picker ...
Are you sure all joints of the robot are published via /joint_states? Could you compare the list of joints published between indigo/kinetic?
Thank you very much for your interest. Yes, I am pretty sure that all the 14 (7 per arm) joints are being published into the /joint_states topic, both in Indigo and in Kinetic. There is a relevant update, tho. I've managed to make it work in Kinetic changing something seemingly unrelated.
deactivating the validation is not unrelated at all :) But it does not fix the underlying problem that apparently your move_group does not receive the current state of the robot. Did you check the timestamps of the joint_state message? If there is too much delay to the "current" simulation time
then this could explain the failure to update the state too.
Sorry, "unrelated" was a poorly selected word D: Probably I was thinking of "unwanted" or "undesired". Right now I cannot test this on my laptop (I'm working on my lab's computer, the one that has Indigo installed), but I'll check as soon as I can. I'll let you know. Thanks!
I am not sure, but I believe that the error is related to the timestamps, as you suggest, and Rviz already provided a good hint ("Maybe failed to update robot state, time diff: 0,209s"). I think there is a significant difference between the last published joint_states messages and the current time.
Hi, have you managed to fix the problem, and if so, how? I'm running into the same issue. Thanks!
The root of this problem seemed to be the outdated joint state messages. If the delay exceeds one second then you have a serious problem with your setup.
Apart from this, the
waitForCurrentStatemethod expects a background spinner to process incoming joint state messages.