ros2-ify perception pipeline

CMakeLists.txt

@@ -58,6 +58,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)

conf.py

@@ -217,14 +217,16 @@
         "https://github.com/"
         + html_context["github_user"]
         + "/panda_moveit_config/blob/"
-        + "melodic-devel"
+        + "rolling-devel"
         + "/%s",
         "",
     ),
     "moveit_resources_codedir": (
         "https://github.com/"
-        + html_context["github_user"]
-        + "/moveit_resources/tree/ros2"
+#        + html_context["github_user"]
+        + "130s"
+#        + "/moveit_resources/tree/ros2"
+        + "/moveit_resources/tree/feature-panda-moveit-perception"
         + "/%s",
         "",
     ),  # TODO(dlu): use ros_distro when noetic-devel branch is available

doc/examples/perception_pipeline/CMakeLists.txt

@@ -1,13 +1,23 @@
-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
+  pcl_ros
+  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})
+set(APP_CYLINDER detect_and_add_cylinder_collision_object_demo)
+add_executable(${APP_CYLINDER} src/detect_and_add_cylinder_collision_object_demo.cpp)
+ament_target_dependencies(${APP_CYLINDER}
+  ${SUBTUTORIAL_DEPENDS}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS})
+install(TARGETS ${APP_CYLINDER} RUNTIME DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY bags launch
+    DESTINATION share/${PROJECT_NAME}
+)

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

@@ -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/launch/detect_and_add_cylinder_collision_object_demo.launch

@@ -1,7 +1,7 @@
 <launch>
-  <include file="$(find moveit_tutorials)/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch" />
+  <include file="$(find-pkg-share moveit2_tutorials)/launch/obstacle_avoidance_demo.launch.xml" />
 
   <!-- Run the detection and adding cylinder node -->
-  <node pkg="moveit_tutorials" type="cylinder_segment" name="point_cloud_preprocessor" />
+  <node pkg="moveit2_tutorials" exec="detect_and_add_cylinder_collision_object_demo" name="point_cloud_preprocessor" />
 
 </launch>

doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.xml

@@ -0,0 +1,23 @@
+<launch>
+  <arg name="bagfile" default="$(find-pkg-share moveit2_tutorials)/bags/perception_tutorial.bag" />
+  <arg name="launchfile_panda"
+       default="$(find-pkg-share moveit_resources_panda_moveit_config)/launch/demo.launch.py" />
+  <!--
+  In this tutorial we wait much longer for the robot transforms than we usually would,
+  because the user's machine might be quite slow.
+  Arg name is shortened although the final lengthy Param name is: 'robot_description_planning/shape_transform_cache_lookup_wait_tim'
+  -->
+  <arg name="shape_transform_cache_lookup_wait_time"
+       default="0.5" />
+
+  <!-- Play the rosbag that contains the pointcloud data -->
+  <executable cmd="ros2 bag play -l --clock 10.0 $(var bagfile)" output="screen" />
+
+  <include file="$(var launchfile_panda)">
+    <arg name="shape_transform_cache_lookup_wait_time" value="$(var shape_transform_cache_lookup_wait_time)" />
+    <arg name="rviz_tutorial" value="false" />
+  </include>
+
+  <!-- If needed, broadcast static tf for robot root -->  
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="to_camera" args="0.00 -.40 0.60 0.19 0.07 -1.91 world camera_rgb_optical_frame" />
+</launch>