WIP Improve: ros2-ified perception tutorial .cpp files.

Author: 130s

CMakeLists.txt

@@ -59,6 +59,7 @@ add_subdirectory(doc/examples/motion_planning_pipeline)
 add_subdirectory(doc/examples/motion_planning_python_api)
 add_subdirectory(doc/examples/move_group_interface)
 add_subdirectory(doc/examples/moveit_cpp)
+add_subdirectory(doc/examples/perception_pipeline)
 add_subdirectory(doc/examples/planning_scene)
 add_subdirectory(doc/examples/planning_scene_ros_api)
 add_subdirectory(doc/examples/realtime_servo)

doc/examples/perception_pipeline/CMakeLists.txt

@@ -1,13 +1,20 @@
-add_executable(cylinder_segment src/cylinder_segment.cpp)
-target_link_libraries(cylinder_segment ${catkin_LIBRARIES})
+find_package(PCL REQUIRED)
+set(SUBTUTORIAL_DEPENDS
+  geometry_msgs
+  pcl_conversions
+  sensor_msgs
+)
+foreach(dep IN ITEMS ${SUBTUTORIAL_DEPENDS})
+  find_package(${dep} REQUIRED)
+endforeach()
 
-add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp)
-target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+include_directories(include ${PCL_INCLUDE_DIRS})
 
-install(
-TARGETS
-    bag_publisher_maintain_time
-    cylinder_segment
-DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+add_executable(cylinder_segment src/cylinder_segment.cpp)
+ament_target_dependencies(cylinder_segment
+  ${SUBTUTORIAL_DEPENDS}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY bags launch
+    DESTINATION share/${PROJECT_NAME}
+)

doc/examples/perception_pipeline/bags/perception_tutorial.bag

@@ -0,0 +1,27 @@
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: {}
+  duration:
+    nanoseconds: 0
+  files:
+  - duration:
+      nanoseconds: 0
+    message_count: 1
+    path: perception_tutorial.db3
+    starting_time:
+      nanoseconds_since_epoch: 1530016169214859695
+  message_count: 1
+  relative_file_paths:
+  - perception_tutorial.db3
+  starting_time:
+    nanoseconds_since_epoch: 1530016169214859695
+  storage_identifier: sqlite3
+  topics_with_message_count:
+  - message_count: 1
+    topic_metadata:
+      name: /camera/depth_registered/points
+      offered_qos_profiles: ''
+      serialization_format: cdr
+      type: sensor_msgs/msg/PointCloud2
+  version: 6

doc/examples/perception_pipeline/bags/perception_tutorial.bag/metadata.yaml

@@ -2,7 +2,8 @@
   <include file="$(find panda_moveit_config)/launch/demo.launch" />
 
   <!-- Play the rosbag that contains the pointcloud data -->
-  <node pkg="moveit_tutorials" type="bag_publisher_maintain_time" name="point_clouds" />
+  <arg namve="bagfile" value="$(find moveit2_tutorials)/share/moveit2_tutorials/bags/perception_tutorial.bag" />
+  <executable cmd="ros2 bag play -l ${arg bagfile)" output="screen" />  
 
   <!-- If needed, broadcast static tf for robot root -->
   <node pkg="tf2_ros" type="static_transform_publisher" name="to_panda" args="0 0 0 0 0 0  world panda_link0" />

doc/examples/perception_pipeline/bags/perception_tutorial.bag/perception_tutorial.db3

@@ -11,6 +11,8 @@ Getting Started
 ---------------
 If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started </doc/tutorials/getting_started/getting_started>`.
 
+Particularly, use ``rolling`` for this tutorial as some dependencies may not be available in ``humble`` or earlier (explained in `moveit2_tutorials!700 <https://github.com/ros-planning/moveit2_tutorials/pull/700#issuecomment-1581411304>`_).
+
 Configuration
 -------------
 
@@ -134,6 +136,16 @@ If you set the initial and the final location of the robot in a way that there i
 .. image:: obstacle_avoidance.gif
    :width: 700px
 
+Before running the software
++++++++++++++++++++++++++++
+This tutorial uses `moveit2_tutorials` that depends on `moveit_task_constructor`, whose installer has not yet been available in ros2 yet (progress tracked in `moveit_task_constructor#400 <https://github.com/ros-planning/moveit_task_constructor/issues/400>`_) so you need to get it via source code. Move into your colcon workspace and pull the MoveIt Task Constructor source: ::
+
+    cd ~/ws_moveit/src
+    git clone git@github.com:ros-planning/moveit_task_constructor.git -b ros2
+    cd ~/ws_moveit
+    colcon build --mixin release
+    source ~/ws_moveit/install/setup.bash
+
 Running the Interface
 +++++++++++++++++++++
 Roslaunch the launch file to run the code directly from moveit_tutorials: ::
@@ -174,9 +186,9 @@ You can follow its status in the `issue tracker <https://github.com/ros-planning
 
 Relevant Code
 +++++++++++++
-The entire code can be seen :codedir:`here<examples/perception_pipeline>` in the moveit_tutorials GitHub project.
+The entire code can be seen :codedir:`here <examples/perception_pipeline>` in the moveit_tutorials GitHub project.
 
-The details regarding the implementation of each of the perception pipeline function have been omitted in this tutorial as they are well documented `here <http://wiki.ros.org/pcl/Tutorials>`_.
+The details regarding the implementation of each of the perception pipeline function have been omitted in this tutorial as they are well documented on `ros1 wiki <http://wiki.ros.org/pcl/Tutorials>`_.
 
 .. |br| raw:: html
 

doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch

@@ -34,27 +34,27 @@
 
 /* Author: Ridhwan Luthra */
 
-#include <ros/ros.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit_msgs/msg/collision_object.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/filters/passthrough.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/segmentation/sac_segmentation.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include "std_msgs/msg/string.hpp"
 
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-
-class CylinderSegment
+class CylinderSegment : public rclcpp::Node
 {
 public:
-  CylinderSegment()
+  explicit CylinderSegment(const rclcpp::NodeOptions& options)
+  : rclcpp::Node("cylinder_segment")
   {
-    ros::NodeHandle nh;
     // Initialize subscriber to the raw point cloud
-    ros::Subscriber sub = nh.subscribe("/camera/depth_registered/points", 1, &CylinderSegment::cloudCB, this);
-    // Spin
-    ros::spin();
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "/camera/depth_registered/points", 10, std::bind(&CylinderSegment::cloudCB, this, std::placeholders::_1));
   }
 
   /** \brief Given the parameters of the cylinder add the cylinder to the planning scene.
@@ -67,12 +67,12 @@ class CylinderSegment
     // Adding Cylinder to Planning Scene
     // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
     // Define a collision object ROS message.
-    moveit_msgs::CollisionObject collision_object;
+    moveit_msgs::msg::CollisionObject collision_object;
     collision_object.header.frame_id = "camera_rgb_optical_frame";
     collision_object.id = "cylinder";
 
     // Define a cylinder which will be added to the world.
-    shape_msgs::SolidPrimitive primitive;
+    shape_msgs::msg::SolidPrimitive primitive;
     primitive.type = primitive.CYLINDER;
     primitive.dimensions.resize(2);
     /* Setting height of cylinder. */
@@ -81,7 +81,7 @@ class CylinderSegment
     primitive.dimensions[1] = cylinder_params->radius;
 
     // Define a pose for the cylinder (specified relative to frame_id).
-    geometry_msgs::Pose cylinder_pose;
+    geometry_msgs::msg::Pose cylinder_pose;
     /* Computing and setting quaternion from axis angle representation. */
     Eigen::Vector3d cylinder_z_direction(cylinder_params->direction_vec[0], cylinder_params->direction_vec[1],
                                          cylinder_params->direction_vec[2]);
@@ -165,7 +165,10 @@ class CylinderSegment
   }
 
   /** \brief Given a pointcloud extract the ROI defined by the user.
-      @param cloud - Pointcloud whose ROI needs to be extracted. */
+   *     <- @TODO 20230607 @130s doesn't see user input anywhere in this .cpp file?
+   *
+   *   @param cloud - Pointcloud whose ROI needs to be extracted.
+   **/
   void passThroughFilter(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
   {
     pcl::PassThrough<pcl::PointXYZRGB> pass;
@@ -269,7 +272,7 @@ class CylinderSegment
     extract.filter(*cloud);
   }
 
-  void cloudCB(const sensor_msgs::PointCloud2ConstPtr& input)
+  void cloudCB(const sensor_msgs::msg::PointCloud2::SharedPtr input)
   {
     // BEGIN_SUB_TUTORIAL callback
     //
@@ -304,7 +307,7 @@ class CylinderSegment
     // END_SUB_TUTORIAL
     if (cloud->points.empty())
     {
-      ROS_ERROR_STREAM_NAMED("cylinder_segment", "Can't find the cylindrical component.");
+      RCLCPP_ERROR(this->get_logger(), "cylinder_segment, can't find the cylindrical component.");
       return;
     }
     if (points_not_found)
@@ -341,6 +344,8 @@ class CylinderSegment
   }
 
 private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+
   // BEGIN_SUB_TUTORIAL param_struct
   // There are 4 fields and a total of 7 parameters used to define this.
   struct AddCylinderParams
@@ -364,7 +369,6 @@ class CylinderSegment
 int main(int argc, char** argv)
 {
   // Initialize ROS
-  ros::init(argc, argv, "cylinder_segment");
-  // Start the segmentor
-  CylinderSegment();
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<CylinderSegment>());
 }

doc/examples/perception_pipeline/perception_pipeline_tutorial.rst

@@ -19,6 +19,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>control_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>interactive_markers</depend>
   <depend>moveit_core</depend>
   <depend>moveit_ros_perception</depend>
@@ -27,11 +28,12 @@
   <depend>moveit_hybrid_planning</depend>
   <depend>moveit_visual_tools</depend>
   <depend>moveit_msgs</depend>
-  <!-- <depend>pcl_conversions</depend> -->
-  <!-- <depend>pcl_ros</depend> -->
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>pluginlib</depend>
   <!-- <depend>rosbag</depend> -->
   <depend>rviz_visual_tools</depend>
+  <depend>sensor_msgs</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
@@ -55,6 +57,7 @@
   <exec_depend>moveit_resources_panda_moveit_config</exec_depend>
   <exec_depend>moveit_configs_utils</exec_depend>
   <exec_depend>moveit_py</exec_depend>
+  <exec_depend>panda_moveit_config</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros2_control</exec_depend>
   <exec_depend>rviz2</exec_depend>