MotionPlanning RViz plugin has problems displaying trajectory execution

[bit-pirate]

Short version:

When hitting execute after planning finished, the executed robot motion is not displayed properly in RViz. At first, the robot is not moving at all, then suddenly it jumps and one can see the last bits of the executed trajectory.

One can verify that the robot motion can be shown properly by adding another RobotModel instance to RViz.

Apparently, the Plan & Execute option does not show the described problem.

More details can be found in this email conversation in the MoveIt Google group: https://groups.google.com/d/msg/moveit-users/_aUxccGuSHU/d-RzXq9cM70J

[acornacorn]

I believe the difference between "Execute" vs "Plan and execute" is that with "Plan and Execute" the move_group process sends a plan to rviz which gets visualized (just as it does when the "plan" button is pressed). So the reason it "works" when you hit "Plan and Execute" is that you are seeing the visualization of the new plan.

When I run MoveIt! with no robot (e.g. roslaunch pr2_moveit_config demo.launch) I notice that the "/move_group/fake_controller_joint_states" only publishes a single message (presumably the final state of the trajectory). Perhaps the "fake controller" assumes the robot follows the trajectory with infinite speed and acceleration? So if you are using the "fake controller" this is probably the problem (and I think this is a bug we need to fix).

If you are using a real robot then the "Scene Robot" displayed in Rviz by the MotionPlanningDisplay should follow the trajectory of the actual robot if the topics are set up correctly. The move_group node should see the joint states on the /joint_states topic and should publish the robot state to rviz on the /move_group/monitored_planning_scene topic. You can confirm this by running

• rostopic list /joint_states
• rostopic list /move_group/monitored_planning_scene in 2 different terminal windows. If your robot moves very quickly then you may not see this in Rviz because the /move_group/monitored_planning_scene gets published at about 4Hz. So you should see the motion in Rviz if the robot takes several seconds to move, but if the robots motion is complete within about a second you may only see the Rviz robot "jump" to its final position.

Another theory: perhaps when MoveIt is executing a trajectory it is not sending updates on /move_group/monitored_planning_scene? (just guessing here - need to confirm)

Are you using a real, simulated, or "fake" robot? If you are using a real robot, can you check whether messages are being published on /move_group/monitored_planning_scene while the robot is moving?