Update subframes tutorial

CMakeLists.txt

@@ -73,7 +73,7 @@ add_subdirectory(doc/examples/planning_scene)
 add_subdirectory(doc/examples/planning_scene_ros_api)
 add_subdirectory(doc/examples/robot_model_and_robot_state)
 # add_subdirectory(doc/examples/state_display)
-# add_subdirectory(doc/examples/subframes)
+add_subdirectory(doc/examples/subframes)
 # add_subdirectory(doc/examples/tests)
 # add_subdirectory(doc/examples/trajopt_planner)
 # add_subdirectory(doc/examples/creating_moveit_plugins/lerp_motion_planner)

doc/examples/subframes/CMakeLists.txt

@@ -1,3 +1,11 @@
-add_executable(subframes_tutorial src/subframes_tutorial.cpp)
-target_link_libraries(subframes_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-install(TARGETS subframes_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+add_executable(subframes_tutorial
+    src/subframes_tutorial.cpp)
+ament_target_dependencies(subframes_tutorial
+    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+
+install(TARGETS subframes_tutorial
+    DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+)

doc/examples/subframes/launch/subframes_tutorial.launch.py

@@ -0,0 +1,14 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    # Subframes Tutorial executable
+    subframes_tutorial = Node(
+        name="subframes_tutorial",
+        package="moveit2_tutorials",
+        executable="subframes_tutorial",
+        output="screen",
+    )
+
+    return LaunchDescription([subframes_tutorial])

doc/examples/subframes/src/subframes_tutorial.cpp

@@ -35,23 +35,25 @@
 /* Author: Felix von Drigalski */
 
 // ROS
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 // MoveIt
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/move_group_interface/move_group_interface.h>
 
 // TF2
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("subframes_tutorial");
 
 // BEGIN_SUB_TUTORIAL plan1
 //
 // Creating the planning request
 // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 // In this tutorial, we use a small helper function to create our planning requests and move the robot.
-bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group,
+bool moveToCartPose(const geometry_msgs::msg::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group,
                     const std::string& end_effector_link)
 {
   // To use subframes of objects that are attached to the robot in planning, you need to set the end effector of your
@@ -70,13 +72,13 @@ bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_int
 
   // The rest of the planning is done as usual. Naturally, you can also use the ``go()`` command instead of
   // ``plan()`` and ``execute()``.
-  ROS_INFO_STREAM("Planning motion to pose:");
-  ROS_INFO_STREAM(pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z);
+  RCLCPP_INFO_STREAM(LOGGER, "Planning motion to pose:");
+  RCLCPP_INFO_STREAM(LOGGER, pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z);
   moveit::planning_interface::MoveGroupInterface::Plan myplan;
   if (group.plan(myplan) && group.execute(myplan))
     return true;
 
-  ROS_WARN("Failed to perform motion.");
+  RCLCPP_WARN(LOGGER, "Failed to perform motion.");
   return false;
 }
 // END_SUB_TUTORIAL
@@ -95,7 +97,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p
 
   // First, we start defining the `CollisionObject <http://docs.ros.org/api/moveit_msgs/html/msg/CollisionObject.html>`_
   // as usual.
-  moveit_msgs::CollisionObject box;
+  moveit_msgs::msg::CollisionObject box;
   box.id = "box";
   box.header.frame_id = "panda_hand";
   box.primitives.resize(1);
@@ -153,7 +155,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p
   box.subframe_poses[4].orientation = tf2::toMsg(orientation);
 
   // Next, define the cylinder
-  moveit_msgs::CollisionObject cylinder;
+  moveit_msgs::msg::CollisionObject cylinder;
   cylinder.id = "cylinder";
   cylinder.header.frame_id = "panda_hand";
   cylinder.primitives.resize(1);
@@ -179,17 +181,17 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p
 
   // BEGIN_SUB_TUTORIAL object2
   // Lastly, the objects are published to the PlanningScene. In this tutorial, we publish a box and a cylinder.
-  box.operation = moveit_msgs::CollisionObject::ADD;
-  cylinder.operation = moveit_msgs::CollisionObject::ADD;
+  box.operation = moveit_msgs::msg::CollisionObject::ADD;
+  cylinder.operation = moveit_msgs::msg::CollisionObject::ADD;
   planning_scene_interface.applyCollisionObjects({ box, cylinder });
 }
 // END_SUB_TUTORIAL
 
-void createArrowMarker(visualization_msgs::Marker& marker, const geometry_msgs::Pose& pose, const Eigen::Vector3d& dir,
-                       int id, double scale = 0.1)
+void createArrowMarker(visualization_msgs::msg::Marker& marker, const geometry_msgs::msg::Pose& pose,
+                       const Eigen::Vector3d& dir, int id, double scale = 0.1)
 {
-  marker.action = visualization_msgs::Marker::ADD;
-  marker.type = visualization_msgs::Marker::CYLINDER;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.type = visualization_msgs::msg::Marker::CYLINDER;
   marker.id = id;
   marker.scale.x = 0.1 * scale;
   marker.scale.y = 0.1 * scale;
@@ -206,11 +208,11 @@ void createArrowMarker(visualization_msgs::Marker& marker, const geometry_msgs::
   marker.color.a = 1.0;
 }
 
-void createFrameMarkers(visualization_msgs::MarkerArray& markers, const geometry_msgs::PoseStamped& target,
+void createFrameMarkers(visualization_msgs::msg::MarkerArray& markers, const geometry_msgs::msg::PoseStamped& target,
                         const std::string& ns, bool locked = false)
 {
   int id = markers.markers.size();
-  visualization_msgs::Marker m;
+  visualization_msgs::msg::Marker m;
   m.header.frame_id = target.header.frame_id;
   m.ns = ns;
   m.frame_locked = locked;
@@ -228,38 +230,44 @@ void createFrameMarkers(visualization_msgs::MarkerArray& markers, const geometry
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "panda_arm_subframes");
-  ros::NodeHandle nh;
-  ros::AsyncSpinner spinner(1);
-  spinner.start();
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_arm_subframes");
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  std::thread([&executor]() { executor.spin(); }).detach();
 
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-  moveit::planning_interface::MoveGroupInterface group("panda_arm");
+  moveit::planning_interface::MoveGroupInterface group(node, "panda_arm");
   group.setPlanningTime(10.0);
 
   // BEGIN_SUB_TUTORIAL sceneprep
   // Preparing the scene
   // ^^^^^^^^^^^^^^^^^^^
   // In the main function, we first spawn the objects in the planning scene, then attach the cylinder to the robot.
+  // We expect the cylinder to be touching the panda fingers, so we set the touch links accordingly so collision
+  // checking knows these collisions are expected.
   // Attaching the cylinder turns it purple in Rviz.
   spawnCollisionObjects(planning_scene_interface);
-  moveit_msgs::AttachedCollisionObject att_coll_object;
+  moveit_msgs::msg::AttachedCollisionObject att_coll_object;
   att_coll_object.object.id = "cylinder";
   att_coll_object.link_name = "panda_hand";
   att_coll_object.object.operation = att_coll_object.object.ADD;
-  ROS_INFO_STREAM("Attaching cylinder to robot.");
+  att_coll_object.touch_links = std::vector<std::string>{ "panda_leftfinger", "panda_rightfinger" };
+  RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot.");
   planning_scene_interface.applyAttachedCollisionObject(att_coll_object);
   // END_SUB_TUTORIAL
 
   // Fetch the current planning scene state once
-  auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
+  auto planning_scene_monitor =
+      std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(node, "robot_description");
   planning_scene_monitor->requestPlanningSceneState();
   planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor);
 
   // Visualize frames as rviz markers
-  ros::Publisher marker_publisher = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);
-  auto showFrames = [&](geometry_msgs::PoseStamped target, const std::string& eef) {
-    visualization_msgs::MarkerArray markers;
+  auto marker_publisher =
+      node->create_publisher<visualization_msgs::msg::MarkerArray>("visualization_marker_array", 10);
+  auto showFrames = [&](geometry_msgs::msg::PoseStamped target, const std::string& eef) {
+    visualization_msgs::msg::MarkerArray markers;
     // convert target pose into planning frame
     Eigen::Isometry3d tf;
     tf2::fromMsg(target.pose, tf);
@@ -273,12 +281,12 @@ int main(int argc, char** argv)
                              planning_scene->getFrameTransform(eef));
     createFrameMarkers(markers, target, "eef", true);
 
-    marker_publisher.publish(markers);
+    marker_publisher->publish(markers);
   };
 
   // Define a pose in the robot base.
   tf2::Quaternion target_orientation;
-  geometry_msgs::PoseStamped fixed_pose, target_pose;
+  geometry_msgs::msg::PoseStamped fixed_pose, target_pose;
   fixed_pose.header.frame_id = "panda_link0";
   fixed_pose.pose.position.y = -.4;
   fixed_pose.pose.position.z = .3;
@@ -287,28 +295,28 @@ int main(int argc, char** argv)
 
   // Set up a small command line interface to make the tutorial interactive.
   int character_input;
-  while (ros::ok())
+  while (rclcpp::ok())
   {
-    ROS_INFO("==========================\n"
-             "Press a key and hit Enter to execute an action. \n0 to exit"
-             "\n1 to move cylinder tip to box bottom \n2 to move cylinder tip to box top"
-             "\n3 to move cylinder tip to box corner 1 \n4 to move cylinder tip to box corner 2"
-             "\n5 to move cylinder tip to side of box"
-             "\n6 to return the robot to the start pose"
-             "\n7 to move the robot's wrist to a cartesian pose"
-             "\n8 to move cylinder/tip to the same cartesian pose"
-             "\n----------"
-             "\n10 to remove box and cylinder from the scene"
-             "\n11 to spawn box and cylinder"
-             "\n12 to attach the cylinder to the gripper\n");
+    RCLCPP_INFO(LOGGER, "==========================\n"
+                        "Press a key and hit Enter to execute an action. \n0 to exit"
+                        "\n1 to move cylinder tip to box bottom \n2 to move cylinder tip to box top"
+                        "\n3 to move cylinder tip to box corner 1 \n4 to move cylinder tip to box corner 2"
+                        "\n5 to move cylinder tip to side of box"
+                        "\n6 to return the robot to the start pose"
+                        "\n7 to move the robot's wrist to a cartesian pose"
+                        "\n8 to move cylinder/tip to the same cartesian pose"
+                        "\n----------"
+                        "\n10 to remove box and cylinder from the scene"
+                        "\n11 to spawn box and cylinder"
+                        "\n12 to attach the cylinder to the gripper\n");
     std::cin >> character_input;
     if (character_input == 0)
     {
       return 0;
     }
     else if (character_input == 1)
     {
-      ROS_INFO_STREAM("Moving to bottom of box with cylinder tip");
+      RCLCPP_INFO_STREAM(LOGGER, "Moving to bottom of box with cylinder tip");
 
       // BEGIN_SUB_TUTORIAL orientation
       // Setting the orientation
@@ -328,7 +336,7 @@ int main(int argc, char** argv)
     // The command "2" moves the cylinder tip to the top of the box (the right side in the top animation).
     else if (character_input == 2)
     {
-      ROS_INFO_STREAM("Moving to top of box with cylinder tip");
+      RCLCPP_INFO_STREAM(LOGGER, "Moving to top of box with cylinder tip");
       target_pose.header.frame_id = "box/top";
       target_orientation.setRPY(180.0 / 180.0 * M_PI, 0, 90.0 / 180.0 * M_PI);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
@@ -339,7 +347,7 @@ int main(int argc, char** argv)
     // END_SUB_TUTORIAL
     else if (character_input == 3)
     {
-      ROS_INFO_STREAM("Moving to corner1 of box with cylinder tip");
+      RCLCPP_INFO_STREAM(LOGGER, "Moving to corner1 of box with cylinder tip");
       target_pose.header.frame_id = "box/corner_1";
       target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
@@ -374,22 +382,22 @@ int main(int argc, char** argv)
     }
     else if (character_input == 7)
     {
-      ROS_INFO_STREAM("Moving to a pose with robot wrist");
+      RCLCPP_INFO_STREAM(LOGGER, "Moving to a pose with robot wrist");
       showFrames(fixed_pose, "panda_hand");
       moveToCartPose(fixed_pose, group, "panda_hand");
     }
     else if (character_input == 8)
     {
-      ROS_INFO_STREAM("Moving to a pose with cylinder tip");
+      RCLCPP_INFO_STREAM(LOGGER, "Moving to a pose with cylinder tip");
       showFrames(fixed_pose, "cylinder/tip");
       moveToCartPose(fixed_pose, group, "cylinder/tip");
     }
     else if (character_input == 10)
     {
       try
       {
-        ROS_INFO_STREAM("Removing box and cylinder.");
-        moveit_msgs::AttachedCollisionObject att_coll_object;
+        RCLCPP_INFO_STREAM(LOGGER, "Removing box and cylinder.");
+        moveit_msgs::msg::AttachedCollisionObject att_coll_object;
         att_coll_object.object.id = "box";
         att_coll_object.object.operation = att_coll_object.object.REMOVE;
         planning_scene_interface.applyAttachedCollisionObject(att_coll_object);
@@ -398,40 +406,41 @@ int main(int argc, char** argv)
         att_coll_object.object.operation = att_coll_object.object.REMOVE;
         planning_scene_interface.applyAttachedCollisionObject(att_coll_object);
 
-        moveit_msgs::CollisionObject co1, co2;
+        moveit_msgs::msg::CollisionObject co1, co2;
         co1.id = "box";
-        co1.operation = moveit_msgs::CollisionObject::REMOVE;
+        co1.operation = moveit_msgs::msg::CollisionObject::REMOVE;
         co2.id = "cylinder";
-        co2.operation = moveit_msgs::CollisionObject::REMOVE;
+        co2.operation = moveit_msgs::msg::CollisionObject::REMOVE;
         planning_scene_interface.applyCollisionObjects({ co1, co2 });
       }
       catch (const std::exception& exc)
       {
-        ROS_WARN_STREAM(exc.what());
+        RCLCPP_WARN_STREAM(LOGGER, exc.what());
       }
     }
     else if (character_input == 11)
     {
-      ROS_INFO_STREAM("Respawning test box and cylinder.");
+      RCLCPP_INFO_STREAM(LOGGER, "Respawning test box and cylinder.");
       spawnCollisionObjects(planning_scene_interface);
     }
     else if (character_input == 12)
     {
-      moveit_msgs::AttachedCollisionObject att_coll_object;
+      moveit_msgs::msg::AttachedCollisionObject att_coll_object;
       att_coll_object.object.id = "cylinder";
       att_coll_object.link_name = "panda_hand";
       att_coll_object.object.operation = att_coll_object.object.ADD;
-      ROS_INFO_STREAM("Attaching cylinder to robot.");
+      att_coll_object.touch_links = std::vector<std::string>{ "panda_leftfinger", "panda_rightfinger" };
+      RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot.");
       planning_scene_interface.applyAttachedCollisionObject(att_coll_object);
     }
     else
     {
-      ROS_INFO("Could not read input. Quitting.");
+      RCLCPP_INFO(LOGGER, "Could not read input. Quitting.");
       break;
     }
   }
 
-  ros::waitForShutdown();
+  rclcpp::shutdown();
   return 0;
 }
 

doc/examples/subframes/subframes_tutorial.rst

@@ -28,18 +28,18 @@ Running The Demo
 ----------------
 After having completed the steps in :doc:`Getting Started </doc/tutorials/getting_started/getting_started>`, open two terminals. In the first terminal, execute this command to load up a panda, and wait for everything to finish loading: ::
 
-    roslaunch panda_moveit_config demo.launch
+    ros2 launch moveit2_tutorials demo.launch.py
 
 In the second terminal run the tutorial: ::
 
-    rosrun moveit_tutorials subframes_tutorial
+    ros2 run moveit2_tutorials subframes_tutorial
 
 In this terminal you should be able to enter numbers from 1-12 to send commands, and to see how the robot and the scene react.
 
 
 The Code
 ---------------
-The code for this example can be seen :codedir:`here <examples/subframes>` in the moveit_tutorials GitHub project and is explained in detail below.
+The code for this example can be seen :codedir:`here <examples/subframes>` in the moveit2_tutorials GitHub project and is explained in detail below.
 
 The code spawns a box and a cylinder in the planning scene, attaches the cylinder to the
 robot, and then lets you send motion commands via the command line. It also defines two