do not wait for robot, when trajectory_execution_manager stopped with PREEMPTED mode

[k-okada]

trajectory_execution_manager wait for next robot state, this will stops ~1 sec when we override current executing trajectory.

[ INFO] [1533023405.479727020]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1533023405.484524626]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1533023405.486173640]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533023405.510122152]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533023405.510232552]: Solution found in 0.024775 seconds
[ INFO] [1533023405.523313210]: SimpleSetup: Path simplification took 0.012476 seconds and changed from 3 to 2 states
[ INFO] [1533023405.526738463]: Execution request received
[ INFO] [1533023405.578794073]: Fake execution of trajectory
[ INFO] [1533023407.579548479]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1533023407.582952498]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1533023407.583825762]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533023407.606891583]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533023407.607002163]: Solution found in 0.023672 seconds
[ INFO] [1533023407.620108272]: SimpleSetup: Path simplification took 0.013015 seconds and changed from 3 to 2 states
[ INFO] [1533023407.679038789]: Fake trajectory execution cancelled   <=========== 407.6 [sec] the current goal is preempted
[ INFO] [1533023407.679100462]: Stopped trajectory execution.
[ INFO] [1533023408.679470014]: Didn't received robot state (joint angles) with recent timestamp within 1 seconds. <======= wait 1 sec for next robot state
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1533023408.679625169]: Failed to receive current joint state
[ INFO] [1533023408.679772038]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1533023408.680340150]: Execution completed: PREEMPTED
[ INFO] [1533023408.680648688]: Execution request received            <=========== 408.6 [sec] new goal was receved
[ INFO] [1533023408.779808745]: Fake execution of trajectory
[ INFO] [1533023411.079939023]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1533023411.080211127]: Execution completed: SUCCEEDED

After this PR,

[ INFO] [1533023561.259610183]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1533023561.264388860]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1533023561.265995864]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533023561.291128655]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1533023561.291196843]: Solution found in 0.025912 seconds
[ INFO] [1533023561.309167360]: SimpleSetup: Path simplification took 0.017804 seconds and changed from 4 to 2 states
[ INFO] [1533023561.312112481]: Execution request received
[ INFO] [1533023561.359115064]: Fake execution of trajectory
[ INFO] [1533023563.360082949]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1533023563.363034718]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1533023563.363927580]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533023563.376378378]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533023563.376489688]: Solution found in 0.013074 seconds
[ INFO] [1533023563.394300361]: SimpleSetup: Path simplification took 0.017697 seconds and changed from 3 to 2 states
[ INFO] [1533023563.459474719]: Fake trajectory execution cancelled   <======== 563.4[sec] new goal arrived, and current goal was preempted
[ INFO] [1533023563.459962443]: Stopped trajectory execution.
[ INFO] [1533023563.460168626]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1533023563.460436114]: Execution completed: PREEMPTED
[ INFO] [1533023563.460744467]: Execution request received
[ INFO] [1533023563.560184835]: Fake execution of trajectory          <======== 563.5[sec] new goal was executed
[ INFO] [1533023565.860131666]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1533023565.860379810]: Execution completed: SUCCEEDED

this is sample program we used.

import rospy
import moveit_commander
import time

rospy.init_node('test')
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander('right_arm')
arm.set_max_velocity_scaling_factor(0.1)

pose = arm.get_current_pose().pose
pose.position.z += 0.2
pose.position.y -= 0.3

print("send 1st position ", pose.position)
arm.set_pose_target(pose)
p = arm.plan()
arm.execute(p, wait=False)
time.sleep(2)

pose.position.z -= 0.2
pose.position.y += 0.3

print("send 2nd position ", pose.position)
arm.set_pose_target(pose)
p = arm.plan()
arm.execute(p, wait=True)

without this PR

with this PR

[v4hn]

Plain and simple. I approve.

Thanks @k-okada for your many contributions lately!

[rhaschke]

@v4hn @k-okada I suggest to call waitForRobotToStop() only if status will be SUCCEEDED. Do you agree?

[v4hn]

Yes, that makes sense.
Upon failure we might even end up with no joint state updates anymore, so your modification is a bit more graceful.

I approve.