Wait for current robot state takes longer than trajectory generation

[jschleicher]

Commit

binary package ii ros-melodic-pilz-industrial-motion 0.4.3-0bionic.20190601.035050 amd64

Steps to reproduce

1. roslaunch prbt_moveit_config moveit_planning_execution.launch sim:=True pipeline:=pilz_command_planner
2. increase logger_level of move_group to debu
3. start a plan (e.g. using RViz)

Expected behavior

Planning starts immediately with latest robot state and takes less than 10 milliseconds to complete.

Observed behavior

https://github.com/PilzDE/pilz_industrial_motion/blob/4dadef42fc5051869fadbeacc8d168e04e9b2661/pilz_trajectory_generation/src/move_group_sequence_action.cpp#L92 Waiting for a recent robot state takes about 20 milliseconds (see log below), while the actual trajectory generation takes < 1 millisecond.

Logfiles

[DEBUG] [1562163383.528478620]: A new goal has been recieved by the single goal action server
[DEBUG] [1562163383.528668995]: Accepting a new goal
...
[DEBUG] [1562163383.529092701]: sync robot state to: 3.529s
[ INFO] [1562163383.551285418]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
...
[ INFO] [1562163383.552316534]: PTP Trajectory with 1 Points generated. Took 0.178501 ms.

Reference to test

Performance tests are not implemented / no automatic test suite could detect this.

[jschleicher]

I see several options and are open for discussion:

1. From a single-point-of-control, the recorded robot state from the last plan-and-execute-request should still be the current robot state of the robot (we have a position-controlled robot so there is no motion, if we don't control it). What do you think of simply removing this wait-line in our SequenceCapability?

2. In a previous discussion the argumentation was, if the start state is given inside the request, there is no need to wait (https://github.com/ros-planning/moveit/issues/868#issuecomment-393102371). That could be brought upstream as PR. Changes inside our python-API would query the current state after the previous command (maybe only moves the delay into another node and doesn't eliminate it).

3. Make the time configurable (upstream). Let the user choose an age inside his configuration, to wait for a current robot state at the time of request minus some offset. waitForCurrentRobotState(ros::Time::now() - ros::Duration(0,100e6);

What do you think?

[dbakovic]

From a single-point-of-control, the recorded robot state from the last plan-and-execute-request should still be the current robot state of the robot (we have a position-controlled robot so there is no motion, if we don't control it). What do you think of simply removing this wait-line in our SequenceCapability?

Will this still work when we enable the speed controller?

[jschleicher]

We drop the wait for now and issue a warning, if the state is too old. I'm preparing a PR.

[agutenkunst]

@jschleicher ping

[jschleicher]

@agutenkunst There are more places which waitForCurrentState, e.g. trajectory_execution_manager:1539, trajectory_execution_manager.cpp:940 and plan_execution.cpp:412 . I'd want to time and measure them altogether...