From 8a78dec9518d026a1b83c63180b1c49660c8fcf5 Mon Sep 17 00:00:00 2001 From: Bryson Tanner Date: Wed, 30 Jul 2025 13:08:57 -0500 Subject: [PATCH 1/5] chore: Update subframes_tutorial to compile with ROS2 Replaces namespace ros:: with rclcpp:: Uses msg_pkg::msg:: namespaces for message types Use new RCLCPP logging macros --- CMakeLists.txt | 2 +- doc/examples/subframes/CMakeLists.txt | 14 +- .../subframes/src/subframes_tutorial.cpp | 123 +++++++++--------- 3 files changed, 76 insertions(+), 63 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b191a24c6..fcac779e24 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/doc/examples/subframes/CMakeLists.txt b/doc/examples/subframes/CMakeLists.txt index 50d29ac6ad..bb296fc29d 100644 --- a/doc/examples/subframes/CMakeLists.txt +++ b/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} +) diff --git a/doc/examples/subframes/src/subframes_tutorial.cpp b/doc/examples/subframes/src/subframes_tutorial.cpp index 2e7967250b..27c203acdd 100644 --- a/doc/examples/subframes/src/subframes_tutorial.cpp +++ b/doc/examples/subframes/src/subframes_tutorial.cpp @@ -35,7 +35,7 @@ /* Author: Felix von Drigalski */ // ROS -#include +#include // MoveIt #include @@ -43,15 +43,17 @@ #include // TF2 -#include -#include +#include +#include + +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 `_ // 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,13 +230,14 @@ 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 @@ -243,23 +246,25 @@ int main(int argc, char** argv) // In the main function, we first spawn the objects in the planning scene, then attach the cylinder to the robot. // 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."); + 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("robot_description"); + auto planning_scene_monitor = + std::make_shared(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_marker_array", 10); - auto showFrames = [&](geometry_msgs::PoseStamped target, const std::string& eef) { - visualization_msgs::MarkerArray markers; + auto marker_publisher = + node->create_publisher("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 +278,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,20 +292,20 @@ 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) { @@ -308,7 +313,7 @@ int main(int argc, char** argv) } 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 +333,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 +344,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,13 +379,13 @@ 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"); } @@ -388,8 +393,8 @@ int main(int argc, char** argv) { 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 +403,40 @@ 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."); + 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; } From 3284144617c63f221922290116283cceca7dbb4c Mon Sep 17 00:00:00 2001 From: Bryson Tanner Date: Wed, 30 Jul 2025 14:11:11 -0500 Subject: [PATCH 2/5] chore: Update launch file to a python launch file --- .../subframes/launch/subframes_tutorial.launch | 6 ------ .../subframes/launch/subframes_tutorial.launch.py | 14 ++++++++++++++ 2 files changed, 14 insertions(+), 6 deletions(-) delete mode 100644 doc/examples/subframes/launch/subframes_tutorial.launch create mode 100644 doc/examples/subframes/launch/subframes_tutorial.launch.py diff --git a/doc/examples/subframes/launch/subframes_tutorial.launch b/doc/examples/subframes/launch/subframes_tutorial.launch deleted file mode 100644 index 9438c1789b..0000000000 --- a/doc/examples/subframes/launch/subframes_tutorial.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/doc/examples/subframes/launch/subframes_tutorial.launch.py b/doc/examples/subframes/launch/subframes_tutorial.launch.py new file mode 100644 index 0000000000..1e7a807300 --- /dev/null +++ b/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]) From 21347f8029a4144961eae7dd5ceba8c243f3a757 Mon Sep 17 00:00:00 2001 From: Bryson Tanner Date: Wed, 30 Jul 2025 14:51:36 -0500 Subject: [PATCH 3/5] fix: Allow the collision object to touch the panda hand and fingers --- doc/examples/subframes/src/subframes_tutorial.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/examples/subframes/src/subframes_tutorial.cpp b/doc/examples/subframes/src/subframes_tutorial.cpp index 27c203acdd..79323baabe 100644 --- a/doc/examples/subframes/src/subframes_tutorial.cpp +++ b/doc/examples/subframes/src/subframes_tutorial.cpp @@ -250,6 +250,7 @@ int main(int argc, char** argv) att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; + att_coll_object.touch_links = std::vector{ "panda_hand", "panda_leftfinger", "panda_rightfinger" }; RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); // END_SUB_TUTORIAL @@ -426,6 +427,7 @@ int main(int argc, char** argv) att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; + att_coll_object.touch_links = std::vector{ "panda_hand", "panda_leftfinger", "panda_rightfinger" }; RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); } From 17a3ff15a56802f1d492b35ee1b4cfaff8573dfc Mon Sep 17 00:00:00 2001 From: Bryson Tanner Date: Wed, 30 Jul 2025 14:58:37 -0500 Subject: [PATCH 4/5] docs: Update documentation for subframes tutorial to have the correct commands --- doc/examples/subframes/subframes_tutorial.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/examples/subframes/subframes_tutorial.rst b/doc/examples/subframes/subframes_tutorial.rst index b62a5ee259..59252e1ef5 100644 --- a/doc/examples/subframes/subframes_tutorial.rst +++ b/doc/examples/subframes/subframes_tutorial.rst @@ -28,18 +28,18 @@ Running The Demo ---------------- After having completed the steps in :doc:`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 ` in the moveit_tutorials GitHub project and is explained in detail below. +The code for this example can be seen :codedir:`here ` 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 From 7fd5a8bf21faec6fbc06d2955b47980ef3f899b7 Mon Sep 17 00:00:00 2001 From: Bryson Tanner Date: Wed, 30 Jul 2025 15:04:37 -0500 Subject: [PATCH 5/5] chore: Add explanatory comment for touch_links part --- doc/examples/subframes/src/subframes_tutorial.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/doc/examples/subframes/src/subframes_tutorial.cpp b/doc/examples/subframes/src/subframes_tutorial.cpp index 79323baabe..900276c047 100644 --- a/doc/examples/subframes/src/subframes_tutorial.cpp +++ b/doc/examples/subframes/src/subframes_tutorial.cpp @@ -244,13 +244,15 @@ int main(int argc, char** argv) // 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::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; - att_coll_object.touch_links = std::vector{ "panda_hand", "panda_leftfinger", "panda_rightfinger" }; + att_coll_object.touch_links = std::vector{ "panda_leftfinger", "panda_rightfinger" }; RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); // END_SUB_TUTORIAL @@ -427,7 +429,7 @@ int main(int argc, char** argv) att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; - att_coll_object.touch_links = std::vector{ "panda_hand", "panda_leftfinger", "panda_rightfinger" }; + att_coll_object.touch_links = std::vector{ "panda_leftfinger", "panda_rightfinger" }; RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); }