Bug: Fixed Pointcloud Color issues & remapping ros2 args

CMakeLists.txt

@@ -11,11 +11,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
-
 find_package(image_geometry REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(cv_bridge)
+find_package(cv_bridge REQUIRED)
+find_package(message_filters REQUIRED)
 
 include_directories(include)
 
@@ -27,7 +27,8 @@ ament_target_dependencies(depthimage_to_pointcloud2_node
   "image_geometry"
   "rclcpp"
   "sensor_msgs"
-  "cv_bridge"
+  "cv_bridge"  # message_filters 대신 cv_bridge를 사용합니다.
+  "message_filters"
 )
 
 install(TARGETS depthimage_to_pointcloud2_node
@@ -41,7 +42,6 @@ install(DIRECTORY
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-
   ament_lint_auto_find_test_dependencies()
 endif()
 

include/depthimage_to_pointcloud2/depth_conversions.hpp

@@ -109,23 +109,18 @@ void convert(
       // and RGB
       int rgb = 0x000000;
       if (cv_ptr != nullptr) {
-        if (cv_ptr->image.type()==CV_8UC1) {
-          //grayscale
-          rgb &= cv_ptr->image.at<uchar>(v,u);
-        } else if(cv_ptr->image.type()==CV_8UC3) {
-          //RGB
-          rgb = (int)cv_ptr->image.at<cv::Vec3b>(0, 0)[0];
-
-        } else if(cv_ptr->image.type()==CV_8UC3 || cv_ptr->image.type()==CV_8UC4) {
-          //RGB or RGBA
-          if (cv_ptr->image.rows > v && cv_ptr->image.cols > u){
-            rgb |= ((int)cv_ptr->image.at<cv::Vec4b>(v, u)[2]) << 16;
-            rgb |= ((int)cv_ptr->image.at<cv::Vec4b>(v, u)[1]) << 8;
-            rgb |= ((int)cv_ptr->image.at<cv::Vec4b>(v, u)[0]);
-          }
+        if (cv_ptr->image.type() == CV_8UC1) {
+          // grayscale
+          rgb = cv_ptr->image.at<uchar>(v, u);
+          rgb = (rgb << 16) | (rgb << 8) | rgb; // Convert grayscale to RGB
+        } else if (cv_ptr->image.type() == CV_8UC3) {
+          // RGB
+          cv::Vec3b intensity = cv_ptr->image.at<cv::Vec3b>(v, u);
+          rgb = (intensity[0] << 16) | (intensity[1] << 8) | intensity[2]; // Convert BGR to RGB correctly
         }
       }
       *iter_rgb = *reinterpret_cast<float*>(&rgb);
+
     }
   }
 }

launch/depthimage_to_pointcloud2.launch.py

@@ -3,37 +3,44 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from launch.actions import DeclareLaunchArgument
+
 def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument(
             'full_sensor_topic',
-            default_value=['/my_depth_sensor'],
+            default_value=['/camera'],
             description='Base for topic (and node) names'),
         DeclareLaunchArgument(
             'range_max',
-            default_value='0.0',
+            default_value='19.0',
             description='Max range of depth sensor'),
         DeclareLaunchArgument(
             'use_quiet_nan',
-            default_value='true',
+            default_value='false',
             description='Use quiet NaN instead of range_max'),
+        DeclareLaunchArgument(
+            'depth_image_topic',
+            default_value='/camera/depth',
+            description='Depth image topic'),
         DeclareLaunchArgument(
             'rgb_image_topic',
-            default_value='',
+            default_value='/camera/rgb',
             description='Colorize the point cloud from external RGB image topic'),
         Node(
-        package="depthimage_to_pointcloud2",
-        executable="depthimage_to_pointcloud2_node",
-        output='screen',
-        name=[PythonExpression(["'", LaunchConfiguration('full_sensor_topic'), "'.split('/')[-1]"]), '_depth2pc2'],
-        parameters=[{'range_max': LaunchConfiguration('range_max'),
-                     'use_quiet_nan': LaunchConfiguration('use_quiet_nan'),
-                     'colorful': PythonExpression(["'true' if '", LaunchConfiguration('rgb_image_topic'), "' else 'false'"])}],
-        remappings=[
-            ("image", PythonExpression(["'", LaunchConfiguration('rgb_image_topic'), "/image' if '", LaunchConfiguration('rgb_image_topic'), "' else 'false'"])),
-            ("depth", [LaunchConfiguration('full_sensor_topic'), "/image"]),
-            ("depth_camera_info", [LaunchConfiguration('full_sensor_topic'), "/camera_info"]),
-            ("pointcloud2", [PythonExpression(["'", LaunchConfiguration('full_sensor_topic'), "'.split('/')[-1]"]), "_pointcloud2"])
+            package="depthimage_to_pointcloud2",
+            executable="depthimage_to_pointcloud2_node",
+            output='screen',
+            name='rgbd_to_pointcloud',
+            parameters=[
+                {'range_max': LaunchConfiguration('range_max'),
+                 'use_quiet_nan': LaunchConfiguration('use_quiet_nan'),
+                 'colorful': PythonExpression(["'true' if '", LaunchConfiguration('rgb_image_topic'), "' else 'false'"])}
+            ],
+            remappings=[
+                ("image", LaunchConfiguration('rgb_image_topic')),
+                ("depth", [LaunchConfiguration('depth_image_topic')]),
+                ("depth_camera_info", [LaunchConfiguration('full_sensor_topic'), "/camera_info"]),
+                ("pointcloud2", '/camera/pointcloud2')
             ]
         )
-    ])
+    ])

package.xml

@@ -5,11 +5,14 @@
   <description>A simple node to convert a depth image and camera info into a PointCloud2.</description>
   <license>Apache License 2.0</license>
 
+  <maintainer email="[email protected]">Hyeonsu Oh</maintainer>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>image_geometry</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>message_filters</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/depthimage_to_pointcloud2_node.cpp

@@ -24,125 +24,94 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
-
-// #include <limits>
-// #include <functional>
-// #include <memory>
-// #include <string>
-// #include <vector>
-
-/* Usage example remapping:
-$ ros2 run depthimage_to_pointcloud2 depthimage_to_pointcloud2_node \
---ros-args -r depth:=/my_depth_sensor/image -r depth_camera_info:=/my_depth_sensor/camera_info -r pointcloud2:=/my_output_topic
-*/
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
 
 using std::placeholders::_1;
-
-/* This example creates a subclass of Node and uses std::bind() to register a
-* member function as a callback from the timer. */
+using namespace message_filters;
 
 class Depthimage2Pointcloud2 : public rclcpp::Node
 {
-  public:
-    Depthimage2Pointcloud2(const rclcpp::NodeOptions & options)
-    : Node("depthimage_to_pointcloud2_node", options)
+public:
+    Depthimage2Pointcloud2() : Node("depthimage_to_pointcloud2_node")
     {
-      range_max = this->declare_parameter("range_max", 0.0);
-      use_quiet_nan = this->declare_parameter("use_quiet_nan", true);
-      colorful = this->declare_parameter("colorful", false);
-
-      g_pub_point_cloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud2", 10);
+        range_max = this->declare_parameter("range_max", 0.0);
+        use_quiet_nan = this->declare_parameter("use_quiet_nan", true);
+        colorful = this->declare_parameter("colorful", true);
 
-      if (colorful){
-        image_sub = this->create_subscription<sensor_msgs::msg::Image>(
-          "image", 10, std::bind(&Depthimage2Pointcloud2::imageCb, this, _1));
-      }
+        g_pub_point_cloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud2", 10);
 
-      depthimage_sub = this->create_subscription<sensor_msgs::msg::Image>(
-        "depth", 10, std::bind(&Depthimage2Pointcloud2::depthCb, this, _1));
-      cam_info_sub = this->create_subscription<sensor_msgs::msg::CameraInfo>(
-        "depth_camera_info", 10, std::bind(&Depthimage2Pointcloud2::infoCb, this, _1));
-    }
-
-  private:
-    void imageCb(const sensor_msgs::msg::Image::SharedPtr msg)
-    {
+        depth_sub.subscribe(this, "depth");
+        rgb_sub.subscribe(this, "image");
+        cam_info_sub = this->create_subscription<sensor_msgs::msg::CameraInfo>("depth_camera_info", 10, std::bind(&Depthimage2Pointcloud2::infoCb, this, _1));
 
-      try
-      {
-          cv_ptr = cv_bridge::toCvShare(msg, msg->encoding);
-      }
-      catch (cv_bridge::Exception& e)
-      {
-          RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
-          return;
-      }
+        sync.reset(new Synchronizer<MySyncPolicy>(MySyncPolicy(10), depth_sub, rgb_sub));
+        sync->registerCallback(std::bind(&Depthimage2Pointcloud2::callback, this, std::placeholders::_1, std::placeholders::_2));
     }
 
-  private:
-    void depthCb(const sensor_msgs::msg::Image::SharedPtr image)
+private:
+    void callback(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg)
     {
-      // The meat of this function is a port of the code from:
-      // https://github.com/ros-perception/image_pipeline/blob/92d7f6b/depth_image_proc/src/nodelets/point_cloud_xyz.cpp
-
-      if (nullptr == g_cam_info) {
-        // we haven't gotten the camera info yet, so just drop until we do
-        RCUTILS_LOG_WARN("No camera info, skipping point cloud conversion");
-        return;
-      }
-
-      sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg =
-        std::make_shared<sensor_msgs::msg::PointCloud2>();
-      cloud_msg->header = image->header;
-      cloud_msg->height = image->height;
-      cloud_msg->width = image->width;
-      cloud_msg->is_dense = false;
-      cloud_msg->is_bigendian = false;
-      cloud_msg->fields.clear();
-      cloud_msg->fields.reserve(2);
-
-      sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
-      pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
-
-      // g_cam_info here is a sensor_msg::msg::CameraInfo::SharedPtr,
-      // which we get from the depth_camera_info topic.
-      image_geometry::PinholeCameraModel model;
-      model.fromCameraInfo(g_cam_info);
-
-      if (image->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
-        depthimage_to_pointcloud2::convert<uint16_t>(image, cloud_msg, model, range_max, use_quiet_nan, cv_ptr);
-      } else if (image->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
-        depthimage_to_pointcloud2::convert<float>(image, cloud_msg, model, range_max, use_quiet_nan, cv_ptr);
-      } else {
-        RCUTILS_LOG_WARN_THROTTLE(RCUTILS_STEADY_TIME, 5000,
-          "Depth image has unsupported encoding [%s]", image->encoding.c_str());
-        return;
-      }
-
-      g_pub_point_cloud->publish(*cloud_msg);
+        if (nullptr == g_cam_info) {
+            RCLCPP_WARN(this->get_logger(), "No camera info, skipping point cloud conversion");
+            return;
+        }
+
+        cv_bridge::CvImageConstPtr cv_ptr;
+        try {
+            cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding);
+        } catch (cv_bridge::Exception& e) {
+            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
+            return;
+        }
+
+        sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+        cloud_msg->header = depth_msg->header;
+        cloud_msg->height = depth_msg->height;
+        cloud_msg->width = depth_msg->width;
+        cloud_msg->is_dense = false;
+        cloud_msg->is_bigendian = false;
+        sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
+        pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+
+        image_geometry::PinholeCameraModel model;
+        model.fromCameraInfo(g_cam_info);
+
+        if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
+            depthimage_to_pointcloud2::convert<uint16_t>(depth_msg, cloud_msg, model, range_max, use_quiet_nan, cv_ptr);
+        } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
+            depthimage_to_pointcloud2::convert<float>(depth_msg, cloud_msg, model, range_max, use_quiet_nan, cv_ptr);
+        } else {
+            RCLCPP_WARN(this->get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+            return;
+        }
+
+        g_pub_point_cloud->publish(*cloud_msg);
     }
 
     void infoCb(sensor_msgs::msg::CameraInfo::SharedPtr info)
     {
-      g_cam_info = info;
+        g_cam_info = info;
     }
 
-    sensor_msgs::msg::CameraInfo::SharedPtr g_cam_info;
-    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr g_pub_point_cloud;
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depthimage_sub;
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub;
-    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub;
-
-    cv_bridge::CvImageConstPtr cv_ptr;
     double range_max;
     bool use_quiet_nan;
     bool colorful;
+
+    sensor_msgs::msg::CameraInfo::SharedPtr g_cam_info;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr g_pub_point_cloud;
+    message_filters::Subscriber<sensor_msgs::msg::Image> depth_sub;
+    message_filters::Subscriber<sensor_msgs::msg::Image> rgb_sub;
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> MySyncPolicy;
+    std::shared_ptr<Synchronizer<MySyncPolicy>> sync;
+    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub;
 };
 
-int main(int argc, char * argv[])
+int main(int argc, char* argv[])
 {
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<Depthimage2Pointcloud2>(rclcpp::NodeOptions()));
-  rclcpp::shutdown();
-  return 0;
-}
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<Depthimage2Pointcloud2>());
+    rclcpp::shutdown();
+    return 0;
+}