Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 2 commits into from Aug 23, 2018

Conversation

Projects
None yet
3 participants
@k-okada
Copy link
Contributor

commented Jul 31, 2018

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
screenshot from 2018-07-31 17-14-15

with this PR
screenshot from 2018-07-31 16-57-03

@v4hn

v4hn approved these changes Aug 20, 2018

Copy link
Member

left a comment

Plain and simple. I approve.

Thanks @k-okada for your many contributions lately!

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Aug 23, 2018

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

@v4hn

This comment has been minimized.

Copy link
Member

commented Aug 23, 2018

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.

@rhaschke rhaschke merged commit 9a78ad8 into ros-planning:kinetic-devel Aug 23, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

rhaschke added a commit that referenced this pull request Aug 23, 2018

only waitForRobotToStop() when SUCCEEDED (#1011)
do not wait for robot convergence, when trajectory_execution_manager finishes with status != SUCCEEDED
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.