Add tutorial for 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/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} ${Boost_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,47 @@
+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.
+
+
+.. only:: html
+
+   .. figure:: simultaneous-execution-rviz.gif
+
+   Simultaneous execution of several trajectories through Rviz plugin.
+
+
+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
+---------------
+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`.
+
+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
+---------------
+Though, two independent trajectories for two different joint groups have been planned, both can be 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;
+}