Example of 3D Perception has been updated to ROS2

CMakeLists.txt

@@ -20,7 +20,7 @@ find_package(moveit_visual_tools REQUIRED)
 find_package(geometric_shapes REQUIRED)
 #find_package(pcl_ros REQUIRED)
 #find_package(pcl_conversions REQUIRED)
-#find_package(rosbag REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -35,6 +35,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   ament_cmake
   rclcpp
+  rosbag2_cpp
   rclcpp_action
   tf2_geometry_msgs
   tf2_ros
@@ -62,7 +63,7 @@ add_subdirectory(doc/examples/motion_planning_api)
 # add_subdirectory(doc/examples/motion_planning_pipeline)
 add_subdirectory(doc/examples/move_group_interface)
 # add_subdirectory(doc/examples/move_group_python_interface)
-# add_subdirectory(doc/examples/perception_pipeline)
+add_subdirectory(doc/examples/perception_pipeline)
 # add_subdirectory(doc/examples/pick_place)
 # add_subdirectory(doc/examples/planning)
 add_subdirectory(doc/examples/planning_scene)

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})
+#add_executable(cylinder_segment src/cylinder_segment.cpp)
+#target_link_libraries(cylinder_segment ${ament_LIBRARIES})
 
 add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp)
-target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_include_directories(bag_publisher_maintain_time
+    PUBLIC include)
+ament_target_dependencies(bag_publisher_maintain_time
+    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
-install(
-TARGETS
-    bag_publisher_maintain_time
-    cylinder_segment
-DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(TARGETS bag_publisher_maintain_time
+DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(DIRECTORY bags
+  DESTINATION share/${PROJECT_NAME}
+)
+

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

@@ -0,0 +1,19 @@
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  duration:
+    nanoseconds: 0
+  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: 4

doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.py

@@ -0,0 +1,50 @@
+from os.path import join as pathjoin
+
+from launch import LaunchDescription, launch_description_sources
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+from ament_index_python.packages import get_package_share_directory as pkgpath
+from launch.actions import IncludeLaunchDescription
+from moveit_configs_utils import MoveItConfigsBuilder
+
+ROS_NAMESPACE = ''
+
+def generate_launch_description():
+
+    ld = LaunchDescription()
+    package_panda_moveit_config = "moveit_resources_panda_moveit_config"
+    package_dir_panda_moveit_config = pkgpath(package_panda_moveit_config)
+
+    ld.add_action(IncludeLaunchDescription(
+                    launch_description_sources.PythonLaunchDescriptionSource(
+                        package_dir_panda_moveit_config + '/launch/demo.launch.py')))
+
+    ld.add_action( Node(
+            namespace = ROS_NAMESPACE,
+            package = 'moveit2_tutorials',
+            executable = 'bag_publisher_maintain_time',
+            name = 'point_clouds',
+            output = 'screen',
+            emulate_tty = True,
+        ) )
+
+    ld.add_action(Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="to_panda",
+        output="log",
+        arguments=["0","0","0", "0","0","0","world","panda_link0"],
+    ))
+
+    ld.add_action(Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="to_camera",
+        output="log",
+        arguments=["0.115","0.427","0.570","0","0.2","1.92","camera_rgb_optical_frame","world"],
+    ))
+
+
+
+    return ld

doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp

@@ -33,50 +33,74 @@
  *********************************************************************/
 
 /* Author: Ridhwan Luthra */
-
-#include "ros/ros.h"
-#include <ros/package.h>
-#include <rosbag/bag.h>
-#include <rosbag/view.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <thread>
+#include <chrono>
+#include <rclcpp/rclcpp.hpp>
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "rosbag2_cpp/readers/sequential_reader.hpp"
+#include "rosbag2_cpp/typesupport_helpers.hpp"
+#include "rosbag2_cpp/storage_options.hpp"
+#include "rosbag2_cpp/converter_options.hpp"
+#include "rosbag2_cpp/converter_interfaces/serialization_format_converter.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "bag_publisher_maintain_time");
-  ros::NodeHandle nh;
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  auto node__ = rclcpp::Node::make_shared("bag_publisher_maintain_time", node_options);
 
-  ros::Publisher point_cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1);
-  ros::Rate loop_rate(0.1);
+  auto point_cloud_publisher = node__->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "/camera/depth_registered/points", rclcpp::SystemDefaultsQoS());
 
   // Variable holding the rosbag containing point cloud data.
-  rosbag::Bag bagfile;
-  std::string path = ros::package::getPath("moveit_tutorials");
-  path += "/doc/perception_pipeline/bags/perception_tutorial.bag";
-  bagfile.open(path, rosbag::bagmode::Read);
+  rosbag2_cpp::readers::SequentialReader reader;
+  rosbag2_storage::StorageOptions storage_options{};
+  std::string path = ament_index_cpp::get_package_share_directory("moveit2_tutorials");
+  path += "/bags/perception_tutorial";
+  storage_options.uri = path;
+  storage_options.storage_id = "sqlite3";
+
+  rosbag2_cpp ::ConverterOptions converter_options{};
+  converter_options.input_serialization_format = "cdr";
+  converter_options.output_serialization_format = "cdr";
+  reader.open(storage_options, converter_options);
 
   std::vector<std::string> topics;
   topics.push_back("/camera/depth_registered/points");
 
   // Iterator for topics in bag.
-  rosbag::View bagtopics_iter(bagfile, rosbag::TopicQuery(topics));
-
-  for (auto const msg : bagtopics_iter)
+  if (reader.has_next())
   {
-    sensor_msgs::PointCloud2::Ptr point_cloud_ptr = msg.instantiate<sensor_msgs::PointCloud2>();
-    if (point_cloud_ptr == nullptr)
-    {
-      std::cout << "error" << std::endl;
-      break;
-    }
+    // serialized data
+    auto serialized_message = reader.read_next();
+
+    // deserialization and conversion to ros message
+    sensor_msgs::msg::PointCloud2 msg;
+    auto ros_message = std::make_shared<rosbag2_cpp::rosbag2_introspection_message_t>();
+    ros_message->time_stamp = 0;
+    ros_message->message = nullptr;
+    ros_message->allocator = rcutils_get_default_allocator();
+    ros_message->message = &msg;
+    auto type_library = rosbag2_cpp::get_typesupport_library("sensor_msgs/PointCloud2", "rosidl_typesupport_cpp");
+    auto type_support =
+        rosbag2_cpp::get_typesupport_handle("sensor_msgs/PointCloud2", "rosidl_typesupport_cpp", type_library);
+
+    rosbag2_cpp::SerializationFormatConverterFactory factory;
+    std::unique_ptr<rosbag2_cpp::converter_interfaces::SerializationFormatDeserializer> cdr_deserializer_;
+    cdr_deserializer_ = factory.load_deserializer("cdr");
+
+    cdr_deserializer_->deserialize(serialized_message, type_support, ros_message);
 
-    while (ros::ok())
+    while (rclcpp::ok())
     {
-      point_cloud_ptr->header.stamp = ros::Time::now();
-      point_cloud_publisher.publish(*point_cloud_ptr);
-      ros::spinOnce();
-      loop_rate.sleep();
+      msg.header.stamp = node__->now();
+      point_cloud_publisher->publish(msg);
+      rclcpp::spin_some(node__);
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
   }
-  bagfile.close();
+  reader.close();
   return 0;
 }