diff --git a/doc/controller_configuration/src/moveit_controller_manager_example.cpp b/doc/controller_configuration/src/moveit_controller_manager_example.cpp index 0941368a9..da76836a9 100644 --- a/doc/controller_configuration/src/moveit_controller_manager_example.cpp +++ b/doc/controller_configuration/src/moveit_controller_manager_example.cpp @@ -49,9 +49,13 @@ class ExampleControllerHandle : public moveit_controller_manager::MoveItControll { } - bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/, const ExecutionCompleteCallback& /*cb*/) override + bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/, const ExecutionCompleteCallback& cb) override { // do whatever is needed to actually execute this trajectory + + // then if there is a callback, return the status of the execution. For example, signal success + if (cb) + cb(moveit_controller_manager::ExecutionStatus::SUCCEEDED); return true; } diff --git a/doc/simultaneous_trajectory_execution/CMakeLists.txt b/doc/simultaneous_trajectory_execution/CMakeLists.txt index a64a8e7b7..7c923f824 100644 --- a/doc/simultaneous_trajectory_execution/CMakeLists.txt +++ b/doc/simultaneous_trajectory_execution/CMakeLists.txt @@ -1,5 +1,5 @@ add_executable(simultaneous_trajectory_execution_tutorial src/simultaneous_trajectory_execution_tutorial.cpp) -target_link_libraries(simultaneous_trajectory_execution_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(simultaneous_trajectory_execution_tutorial ${catkin_LIBRARIES}) install(TARGETS simultaneous_trajectory_execution_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/doc/simultaneous_trajectory_execution/simultaneous-execution-rviz.gif b/doc/simultaneous_trajectory_execution/images/simultaneous-execution-rviz.gif similarity index 100% rename from doc/simultaneous_trajectory_execution/simultaneous-execution-rviz.gif rename to doc/simultaneous_trajectory_execution/images/simultaneous-execution-rviz.gif diff --git a/doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst b/doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst index 479e1642c..e1e314404 100644 --- a/doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst +++ b/doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst @@ -3,24 +3,25 @@ Simultaneous Trajectory Execution Introduction ------------ -MoveIt now allows simultaneous execution of trajectories, as long as, each trajectory uses a different set of controllers. For example, in a dual arm environment, each arm can execute a different set of trajectories without needing to wait for the other arm to finish moving or manually synchronizing the motion of both arm into a single trajectory. Optionally, a collision check is performed right before execution of new trajectories to prevent collisions with active trajectories. +MoveIt allows simultaneous execution of trajectories, as long as each trajectory uses a different set of controllers. For example, in a dual arm environment, each arm can execute a different set of trajectories without needing to wait for the other arm to finish moving or manually synchronizing the motion of both arm into a single trajectory. Optionally, a collision check is performed right before execution of new trajectories to prevent collisions with active trajectories. -.. only:: html +The following GIF shows a simple example of simultaneous execution of trajectories through the **Rviz Motion Planning** plugin. - .. figure:: simultaneous-execution-rviz.gif +.. only:: html - Simultaneous execution of several trajectories through Rviz plugin. + .. figure:: images/simultaneous-execution-rviz.gif +This tutorial presents how to use the Simultaneous Trajectory Execution feature through the `Move Group C++ Interface <../move_group_interface/move_group_interface_tutorial.html>`_ but it can be similarly used through the `Move Group Python Interface <../move_group_python_interface/move_group_python_interface_tutorial.html>`_ or `MoveIt Cpp <../moveit_cpp/moveitcpp_tutorial.html>`_. Getting Started --------------- If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_. -Setup +(Optional) Setup --------------- -The simultaneous trajectory execution feature can be enabled or disabled through the dynamic reconfigure parameter `/move_group/trajectory_execution/enable_simultaneous_execution`. -Optionally, an extra layer of collision checking, done right before execution of trajectories, can be enabled through the dynamic reconfigure parameter `/move_group/trajectory_execution/enable_collision_checking`. +The simultaneous execution feature is active by default. However, through the following the dynamic reconfigure parameter, it can be disabled, **/move_group/trajectory_execution/enable_simultaneous_execution**. +Similarly, an extra layer of collision checking, performed right before execution of trajectories has been added to the `TrajectoryExecutionManeger`, which can also be disabled through the dynamic reconfigure parameter **/move_group/trajectory_execution/enable_collision_checking**. Running the code ---------------- @@ -34,7 +35,7 @@ In the second shell, run the launch file for this demo: :: Expected Output --------------- -Though, two independent trajectories for two different joint groups have been planned, both can be simultaneously executed. +In a robotic environment with two Franka Panda robot arms, two different trajectories are planned, one for each robot arm. Then both trajectory are simultaneously executed. The entire code --------------- diff --git a/index.rst b/index.rst index f10a6cbc1..be7244485 100644 --- a/index.rst +++ b/index.rst @@ -97,6 +97,7 @@ Miscellaneous doc/benchmarking/benchmarking_tutorial doc/tests/tests_tutorial doc/test_debugging/test_debugging_tutorial + doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial Attribution -----------