Port planning scene ros api tutorial

CMakeLists.txt

@@ -61,7 +61,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 # add_subdirectory(doc/pick_place)
 # add_subdirectory(doc/planning)
 add_subdirectory(doc/planning_scene)
-# add_subdirectory(doc/planning_scene_ros_api)
+add_subdirectory(doc/planning_scene_ros_api)
 add_subdirectory(doc/quickstart_in_rviz)
 # add_subdirectory(doc/robot_model_and_robot_state)
 # add_subdirectory(doc/state_display)

doc/planning_scene_ros_api/CMakeLists.txt

@@ -1,8 +1,13 @@
 add_executable(planning_scene_ros_api_tutorial
     src/planning_scene_ros_api_tutorial.cpp)
-target_link_libraries(planning_scene_ros_api_tutorial
-    ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-install(TARGETS planning_scene_ros_api_tutorial DESTINATION
-    ${CATKIN_PACKAGE_BIN_DESTINATION})
+target_include_directories(planning_scene_ros_api_tutorial
+    PUBLIC include)
+ament_target_dependencies(planning_scene_ros_api_tutorial
+    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(TARGETS planning_scene_ros_api_tutorial
+    DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+)

doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch.py

@@ -0,0 +1,19 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+
+    # Planning Scene ROS API Tutorial executable
+    planning_scene_ros_api_tutorial = Node(
+        name="planning_scene_ros_api_tutorial",
+        package="moveit2_tutorials",
+        executable="planning_scene_ros_api_tutorial",
+        prefix="xterm -e",
+        output="screen",
+    )
+
+    return LaunchDescription([planning_scene_ros_api_tutorial])

doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst

@@ -1,8 +1,3 @@
-:moveit1:
-
-..
-   Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag)
-
 Planning Scene ROS API
 ==================================
 
@@ -16,19 +11,24 @@ Getting Started
 ---------------
 If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
 
+**Note:** Because **MoveitVisualTools** has not been ported to ROS2 this tutorial has made use of xterm and a simple prompter to help the user progress through each demo step.
+To install xterm please run the following command: ::
+
+  sudo apt-get install -y xterm
+
 Running the code
 ----------------
 Open two shells. In the first shell start RViz and wait for everything to finish loading: ::
 
-  roslaunch panda_moveit_config demo.launch
+  ros2 launch moveit2_tutorials demo.launch.py
 
 In the second shell, run the launch file for this demo: ::
 
-  roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch
+  ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py
 
 **Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_.
 
-After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
+After a short moment, the RViz window should appear and look similar to `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
 
 Expected Output
 ---------------
@@ -38,22 +38,11 @@ In RViz, you should be able to see the following:
  * Object gets detached from the robot.
  * Object is removed from the planning scene.
 
-.. role:: red
-
-**Note:** You may see an error message reading :red:`Found empty JointState message`. This is a known bug and will be fixed soon.
-
 The entire code
 ---------------
 The entire code can be seen :codedir:`here in the MoveIt GitHub project<planning_scene_ros_api>`.
 
 .. tutorial-formatter:: ./src/planning_scene_ros_api_tutorial.cpp
 
-The launch file
----------------
-The entire launch file is :codedir:`here <planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch>` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package.
-
-Debugging the Planning Scene Monitor
-------------------------------------
-To aid in debugging detached and attached collision objects, a command-line tool will help introspect your system: ::
-
-  rosrun moveit_ros_planning moveit_print_planning_scene_info
+..
+  TODO(JafarAbdi): Add the launch file section back (see https://github.com/ros-planning/moveit_tutorials/blob/master/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst#the-launch-file)

doc/planning_scene_ros_api/src/planning_scene_ros_api_tutorial.cpp

@@ -34,36 +34,51 @@
 
 /* Author: Sachin Chitta, Michael Lautman */
 
-#include <ros/ros.h>
-#include <geometry_msgs/Pose.h>
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 
 // MoveIt
-#include <moveit_msgs/PlanningScene.h>
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/GetStateValidity.h>
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit_msgs/msg/planning_scene.hpp>
+#include <moveit_msgs/msg/attached_collision_object.hpp>
+#include <moveit_msgs/srv/get_state_validity.hpp>
+#include <moveit_msgs/msg/display_robot_state.hpp>
+#include <moveit_msgs/srv/apply_planning_scene.hpp>
 
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/robot_state/conversions.h>
 
-#include <moveit_visual_tools/moveit_visual_tools.h>
+/* #include <moveit_visual_tools/moveit_visual_tools.h>  This has not been ported to ros2 yet */
+#include <rviz_visual_tools/rviz_visual_tools.hpp>
+/* this is a standin for moveit_visual_tools prompt */
+#include <moveit/macros/console_colors.h>
 
-int main(int argc, char** argv)
+void prompt(const std::string& message)
 {
-  ros::init(argc, argv, "planning_scene_ros_api_tutorial");
-  ros::AsyncSpinner spinner(1);
-  spinner.start();
+  printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str());
+  fflush(stdout);
+  while (std::cin.get() != '\n' && rclcpp::ok())
+    ;
+}
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_ros_api_tutorial");
 
-  ros::NodeHandle node_handle;
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  auto node = rclcpp::Node::make_shared("planning_scene_ros_api_tutorial", node_options);
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  std::thread([&executor]() { executor.spin(); }).detach();
   // BEGIN_TUTORIAL
   //
   // Visualization
   // ^^^^^^^^^^^^^
   // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
   // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
-  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+  rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "planning_scene_ros_api_tutorial", node);
   visual_tools.deleteAllMarkers();
 
   // ROS API
@@ -77,33 +92,34 @@ int main(int argc, char** argv)
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
   // We create a publisher and wait for subscribers.
   // Note that this topic may need to be remapped in the launch file.
-  ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
-  ros::WallDuration sleep_t(0.5);
-  while (planning_scene_diff_publisher.getNumSubscribers() < 1)
+  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher =
+      node->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1);
+  while (planning_scene_diff_publisher->get_subscription_count() < 1)
   {
-    sleep_t.sleep();
+    rclcpp::sleep_for(std::chrono::milliseconds(500));
   }
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
+  prompt("Press 'Enter' to continue the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
 
   // Define the attached object message
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
   // We will use this message to add or
   // subtract the object from the world
   // and to attach the object to the robot.
-  moveit_msgs::AttachedCollisionObject attached_object;
+  moveit_msgs::msg::AttachedCollisionObject attached_object;
   attached_object.link_name = "panda_hand";
   /* The header must contain a valid TF frame*/
   attached_object.object.header.frame_id = "panda_hand";
   /* The id of the object */
   attached_object.object.id = "box";
 
   /* A default pose */
-  geometry_msgs::Pose pose;
+  geometry_msgs::msg::Pose pose;
   pose.position.z = 0.11;
   pose.orientation.w = 1.0;
 
   /* Define a box to be attached */
-  shape_msgs::SolidPrimitive primitive;
+  shape_msgs::msg::SolidPrimitive primitive;
   primitive.type = primitive.BOX;
   primitive.dimensions.resize(3);
   primitive.dimensions[0] = 0.075;
@@ -127,12 +143,13 @@ int main(int argc, char** argv)
   // the set of collision objects in the "world" part of the
   // planning scene. Note that we are using only the "object"
   // field of the attached_object message here.
-  ROS_INFO("Adding the object into the world at the location of the hand.");
-  moveit_msgs::PlanningScene planning_scene;
+  RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the hand.");
+  moveit_msgs::msg::PlanningScene planning_scene;
   planning_scene.world.collision_objects.push_back(attached_object.object);
   planning_scene.is_diff = true;
-  planning_scene_diff_publisher.publish(planning_scene);
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+  planning_scene_diff_publisher->publish(planning_scene);
+  prompt("Press 'Enter' to continue the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
 
   // Interlude: Synchronous vs Asynchronous updates
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -146,15 +163,15 @@ int main(int argc, char** argv)
   //
   // While most of this tutorial uses the latter mechanism (given the long sleeps
   // inserted for visualization purposes asynchronous updates do not pose a problem),
-  // it would is perfectly justified to replace the planning_scene_diff_publisher
+  // it would be perfectly justified to replace the planning_scene_diff_publisher
   // by the following service client:
-  ros::ServiceClient planning_scene_diff_client =
-      node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
-  planning_scene_diff_client.waitForExistence();
+  rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr planning_scene_diff_client =
+      node->create_client<moveit_msgs::srv::ApplyPlanningScene>("apply_planning_scene");
+  planning_scene_diff_client->wait_for_service();
   // and send the diffs to the planning scene via a service call:
-  moveit_msgs::ApplyPlanningScene srv;
-  srv.request.scene = planning_scene;
-  planning_scene_diff_client.call(srv);
+  auto request = std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>();
+  request->scene = planning_scene;
+  planning_scene_diff_client->async_send_request(request);
   // Note that this does not continue until we are sure the diff has been applied.
   //
   // Attach an object to the robot
@@ -169,7 +186,7 @@ int main(int argc, char** argv)
   //  * Attaching the object to the robot
 
   /* First, define the REMOVE object message*/
-  moveit_msgs::CollisionObject remove_object;
+  moveit_msgs::msg::CollisionObject remove_object;
   remove_object.id = "box";
   remove_object.header.frame_id = "panda_hand";
   remove_object.operation = remove_object.REMOVE;
@@ -178,14 +195,15 @@ int main(int argc, char** argv)
   // attached objects or collisions objects by clearing those fields
   // first.
   /* Carry out the REMOVE + ATTACH operation */
-  ROS_INFO("Attaching the object to the hand and removing it from the world.");
+  RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world.");
   planning_scene.world.collision_objects.clear();
   planning_scene.world.collision_objects.push_back(remove_object);
   planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
   planning_scene.robot_state.is_diff = true;
-  planning_scene_diff_publisher.publish(planning_scene);
+  planning_scene_diff_publisher->publish(planning_scene);
 
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+  prompt("Press 'Enter' to continue the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
 
   // Detach an object from the robot
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -194,7 +212,7 @@ int main(int argc, char** argv)
   //  * Re-introducing the object into the environment
 
   /* First, define the DETACH object message*/
-  moveit_msgs::AttachedCollisionObject detach_object;
+  moveit_msgs::msg::AttachedCollisionObject detach_object;
   detach_object.object.id = "box";
   detach_object.link_name = "panda_hand";
   detach_object.object.operation = attached_object.object.REMOVE;
@@ -203,16 +221,17 @@ int main(int argc, char** argv)
   // attached objects or collisions objects by clearing those fields
   // first.
   /* Carry out the DETACH + ADD operation */
-  ROS_INFO("Detaching the object from the robot and returning it to the world.");
+  RCLCPP_INFO(LOGGER, "Detaching the object from the robot and returning it to the world.");
   planning_scene.robot_state.attached_collision_objects.clear();
   planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
   planning_scene.robot_state.is_diff = true;
   planning_scene.world.collision_objects.clear();
   planning_scene.world.collision_objects.push_back(attached_object.object);
   planning_scene.is_diff = true;
-  planning_scene_diff_publisher.publish(planning_scene);
+  planning_scene_diff_publisher->publish(planning_scene);
 
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+  prompt("Press 'Enter' to continue the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
 
   // Remove the object from the collision world
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -221,15 +240,16 @@ int main(int argc, char** argv)
   // Note, also how we make sure that the diff message contains no other
   // attached objects or collisions objects by clearing those fields
   // first.
-  ROS_INFO("Removing the object from the world.");
+  RCLCPP_INFO(LOGGER, "Removing the object from the world.");
   planning_scene.robot_state.attached_collision_objects.clear();
   planning_scene.world.collision_objects.clear();
   planning_scene.world.collision_objects.push_back(remove_object);
-  planning_scene_diff_publisher.publish(planning_scene);
+  planning_scene_diff_publisher->publish(planning_scene);
   // END_TUTORIAL
 
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo");
+  prompt("Press 'Enter' to end the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo"); */
 
-  ros::shutdown();
+  rclcpp::shutdown();
   return 0;
 }