Simultaneous trajectory execution

CMakeLists.txt

@@ -74,3 +74,4 @@ add_subdirectory(doc/collision_environments)
 add_subdirectory(doc/visualizing_collisions)
 add_subdirectory(doc/bullet_collision_checker)
 add_subdirectory(doc/mesh_filter)
+add_subdirectory(doc/simultaneous_trajectory_execution)

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*/) 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;
   }
 

doc/simultaneous_trajectory_execution/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(simultaneous_trajectory_execution_tutorial src/simultaneous_trajectory_execution_tutorial.cpp)
+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})

doc/simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <node name="simultaneous_trajectory_execution_tutorial"
+        pkg="moveit_tutorials"
+        type="simultaneous_trajectory_execution_tutorial"
+        respawn="false"
+        output="screen">
+  </node>
+
+</launch>

doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst

@@ -0,0 +1,46 @@
+Simultaneous Trajectory Execution
+==================================
+
+Introduction
+------------
+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.
+
+
+The following GIF shows a simple example of simultaneous execution of trajectories through the **Rviz Motion Planning** 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>`_.
+
+(Optional) Setup
+---------------
+The simultaneous execution feature is active by default. However, through the following 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 `TrajectoryExecutionManager`, which can also be disabled through the dynamic reconfigure parameter **/move_group/trajectory_execution/enable_collision_checking**.
+
+Running the code
+----------------
+Open two shells. In the first shell start RViz and wait for everything to finish loading: ::
+
+  roslaunch moveit_resources_dual_panda_moveit_config demo.launch
+
+In the second shell, run the launch file for this demo: ::
+
+  roslaunch moveit_tutorials simultaneous_trajectory_execution_tutorial.launch
+
+Expected Output
+---------------
+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
+---------------
+The entire code can be seen :codedir:`here in the MoveIt GitHub project<simultaneous_trajectory_execution>`.
+
+.. tutorial-formatter:: ./src/simultaneous_trajectory_execution_tutorial.cpp
+
+The launch file
+---------------
+The entire launch file is :codedir:`here <simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch>` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package.

doc/simultaneous_trajectory_execution/src/simultaneous_trajectory_execution_tutorial.cpp

@@ -0,0 +1,87 @@
+/* Author: Cristian C. Beltran-Hernandez */
+
+#include <ros/ros.h>
+
+#include <memory>
+
+#include <geometry_msgs/PointStamped.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <stdlib.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "simultaneous_trajectory_execution_move_group");
+
+  // ROS spinning must be running for the MoveGroupInterface to get information
+  // about the robot's state. One way to do this is to start an AsyncSpinner
+  // beforehand.
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  // BEGIN_TUTORIAL
+  //
+  // Setup
+  // ^^^^^
+  // Let's start by creating planning groups for each robot arm.
+  // The panda dual arm environment has two planning groups defined as `panda_1` and `panda_2`
+  moveit::planning_interface::MoveGroupInterface panda_1_group("panda_1");
+  moveit::planning_interface::MoveGroupInterface panda_2_group("panda_2");
+
+  // Now, let's define a target pose for `panda_1`
+  geometry_msgs::PoseStamped panda_1_target_pose;
+  panda_1_target_pose.header.frame_id = "base";
+  panda_1_target_pose.pose.position.x = 0.450;
+  panda_1_target_pose.pose.position.y = -0.50;
+  panda_1_target_pose.pose.position.z = 1.600;
+  panda_1_target_pose.pose.orientation.x = 0.993436;
+  panda_1_target_pose.pose.orientation.y = 3.5161e-05;
+  panda_1_target_pose.pose.orientation.z = 0.114386;
+  panda_1_target_pose.pose.orientation.w = 2.77577e-05;
+
+  // And one for `panda_2`
+  geometry_msgs::PoseStamped panda_2_target_pose;
+  panda_2_target_pose.header.frame_id = "base";
+  panda_2_target_pose.pose.position.x = 0.450;
+  panda_2_target_pose.pose.position.y = 0.40;
+  panda_2_target_pose.pose.position.z = 1.600;
+  panda_2_target_pose.pose.orientation.x = 0.993434;
+  panda_2_target_pose.pose.orientation.y = -7.54803e-06;
+  panda_2_target_pose.pose.orientation.z = 0.114403;
+  panda_2_target_pose.pose.orientation.w = 3.67256e-05;
+
+  // Planning
+  // ^^^^^^^^
+  // Let's plan a trajectory for `panda_1` using the previously defined target pose.
+  panda_1_group.clearPoseTargets();
+  panda_1_group.setStartStateToCurrentState();
+  panda_1_group.setPoseTarget(panda_1_target_pose);
+
+  moveit::planning_interface::MoveGroupInterface::Plan panda_1_plan;
+  bool success1 = (panda_1_group.plan(panda_1_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  if (!success1)
+  {
+    ROS_INFO("Plan with Panda 1 did not succeeded");
+  }
+
+  // Same for `panda_2`.
+  panda_2_group.clearPoseTargets();
+  panda_2_group.setStartStateToCurrentState();
+  panda_2_group.setPoseTarget(panda_2_target_pose);
+
+  moveit::planning_interface::MoveGroupInterface::Plan panda_2_plan;
+  bool success2 = (panda_2_group.plan(panda_2_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  if (!success2)
+  {
+    ROS_INFO("Plan with Panda 2 did not succeeded");
+  }
+
+  // Simultaneous Execution
+  // ^^^^^^^^^^^^^^^^^^^^^^
+  // Finally, let's execute both plans asynchronously to have them run simultaneously.
+  panda_1_group.asyncExecute(panda_1_plan);
+  panda_2_group.asyncExecute(panda_2_plan);
+  // END_TUTORIAL
+
+  ros::shutdown();
+  return 0;
+}

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
 -----------