From 53e4ee41b94c4f22c1b72f60809b12190fc696ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=98=A4=ED=98=84=EC=88=98?= <72138421+ssapsu@users.noreply.github.com> Date: Sun, 25 Aug 2024 13:27:30 +0100 Subject: [PATCH] Bug: Fixed Pointcloud Color issues & remapping ros2 args --- CMakeLists.txt | 8 +- .../depth_conversions.hpp | 23 +-- launch/depthimage_to_pointcloud2.launch.py | 41 +++-- package.xml | 3 + src/depthimage_to_pointcloud2_node.cpp | 165 +++++++----------- 5 files changed, 107 insertions(+), 133 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c562534..74722ad 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/include/depthimage_to_pointcloud2/depth_conversions.hpp b/include/depthimage_to_pointcloud2/depth_conversions.hpp index 6848454..509752c 100644 --- a/include/depthimage_to_pointcloud2/depth_conversions.hpp +++ b/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(v,u); - } else if(cv_ptr->image.type()==CV_8UC3) { - //RGB - rgb = (int)cv_ptr->image.at(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(v, u)[2]) << 16; - rgb |= ((int)cv_ptr->image.at(v, u)[1]) << 8; - rgb |= ((int)cv_ptr->image.at(v, u)[0]); - } + if (cv_ptr->image.type() == CV_8UC1) { + // grayscale + rgb = cv_ptr->image.at(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(v, u); + rgb = (intensity[0] << 16) | (intensity[1] << 8) | intensity[2]; // Convert BGR to RGB correctly } } *iter_rgb = *reinterpret_cast(&rgb); + } } } diff --git a/launch/depthimage_to_pointcloud2.launch.py b/launch/depthimage_to_pointcloud2.launch.py index 766ca0b..c0b6bef 100644 --- a/launch/depthimage_to_pointcloud2.launch.py +++ b/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') ] ) - ]) \ No newline at end of file + ]) diff --git a/package.xml b/package.xml index 5ebfad5..e3b82cb 100644 --- a/package.xml +++ b/package.xml @@ -5,11 +5,14 @@ A simple node to convert a depth image and camera info into a PointCloud2. Apache License 2.0 + Hyeonsu Oh + ament_cmake image_geometry rclcpp sensor_msgs + message_filters ament_lint_auto ament_lint_common diff --git a/src/depthimage_to_pointcloud2_node.cpp b/src/depthimage_to_pointcloud2_node.cpp index 5172e54..ef69de0 100644 --- a/src/depthimage_to_pointcloud2_node.cpp +++ b/src/depthimage_to_pointcloud2_node.cpp @@ -24,125 +24,94 @@ #include #include #include - -// #include -// #include -// #include -// #include -// #include - -/* 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 +#include +#include 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("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( - "image", 10, std::bind(&Depthimage2Pointcloud2::imageCb, this, _1)); - } + g_pub_point_cloud = this->create_publisher("pointcloud2", 10); - depthimage_sub = this->create_subscription( - "depth", 10, std::bind(&Depthimage2Pointcloud2::depthCb, this, _1)); - cam_info_sub = this->create_subscription( - "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("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(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(); - 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(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(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(); + 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(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(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::SharedPtr g_pub_point_cloud; - rclcpp::Subscription::SharedPtr depthimage_sub; - rclcpp::Subscription::SharedPtr image_sub; - rclcpp::Subscription::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::SharedPtr g_pub_point_cloud; + message_filters::Subscriber depth_sub; + message_filters::Subscriber rgb_sub; + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + std::shared_ptr> sync; + rclcpp::Subscription::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(rclcpp::NodeOptions())); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}