Skip to content

Commit 6e42681

Browse files
committed
Fix tutorial
1 parent 378045d commit 6e42681

File tree

5 files changed

+16
-10
lines changed

5 files changed

+16
-10
lines changed

doc/controller_configuration/src/moveit_controller_manager_example.cpp

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,13 @@ class ExampleControllerHandle : public moveit_controller_manager::MoveItControll
4949
{
5050
}
5151

52-
bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/, const ExecutionCompleteCallback& /*cb*/) override
52+
bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/, const ExecutionCompleteCallback& cb) override
5353
{
5454
// do whatever is needed to actually execute this trajectory
55+
56+
// then if there is a callback, return the status of the execution. For example, signal success
57+
if (cb)
58+
cb(moveit_controller_manager::ExecutionStatus::SUCCEEDED);
5559
return true;
5660
}
5761

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
add_executable(simultaneous_trajectory_execution_tutorial src/simultaneous_trajectory_execution_tutorial.cpp)
2-
target_link_libraries(simultaneous_trajectory_execution_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
2+
target_link_libraries(simultaneous_trajectory_execution_tutorial ${catkin_LIBRARIES})
33
install(TARGETS simultaneous_trajectory_execution_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
44

55
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -3,24 +3,25 @@ Simultaneous Trajectory Execution
33

44
Introduction
55
------------
6-
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.
6+
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.
77

88

9-
.. only:: html
9+
The following GIF shows a simple example of simultaneous execution of trajectories through the **Rviz Motion Planning** plugin.
1010

11-
.. figure:: simultaneous-execution-rviz.gif
11+
.. only:: html
1212

13-
Simultaneous execution of several trajectories through Rviz plugin.
13+
.. figure:: images/simultaneous-execution-rviz.gif
1414

15+
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>`_.
1516

1617
Getting Started
1718
---------------
1819
If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
1920

20-
Setup
21+
(Optional) Setup
2122
---------------
22-
The simultaneous trajectory execution feature can be enabled or disabled through the dynamic reconfigure parameter `/move_group/trajectory_execution/enable_simultaneous_execution`.
23-
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`.
23+
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**.
24+
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**.
2425

2526
Running the code
2627
----------------
@@ -34,7 +35,7 @@ In the second shell, run the launch file for this demo: ::
3435

3536
Expected Output
3637
---------------
37-
Though, two independent trajectories for two different joint groups have been planned, both can be simultaneously executed.
38+
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.
3839

3940
The entire code
4041
---------------

index.rst

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,7 @@ Miscellaneous
9797
doc/benchmarking/benchmarking_tutorial
9898
doc/tests/tests_tutorial
9999
doc/test_debugging/test_debugging_tutorial
100+
doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial
100101

101102
Attribution
102103
-----------

0 commit comments

Comments
 (0)