From e90f8d9027ae4b62d9c06a25d6132c6dbc25f687 Mon Sep 17 00:00:00 2001 From: JINGERGER Date: Sun, 1 Feb 2026 11:45:00 +0800 Subject: [PATCH] Fix: Enable colored pointcloud when frames arrive asynchronously - Cache most recent color frame for pointcloud texturing - Call map_to() on pointcloud filter when color frame arrives - Use cached color frame when not present in depth frameset - Fixes issue where colored pointcloud was not published without enable_sync This allows colored pointcloud generation without requiring enable_sync=true, maintaining full framerate while providing proper RGB texture mapping. Tested on RealSense D555 with librealsense v2.57.6 --- .../include/base_realsense_node.h | 1 + realsense2_camera/include/pointcloud_filter.h | 3 +- realsense2_camera/src/base_realsense_node.cpp | 11 +++- realsense2_camera/src/pointcloud_filter.cpp | 62 +++++++++++++++---- 4 files changed, 62 insertions(+), 15 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 09f82aef97..76c281a49f 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -405,6 +405,7 @@ namespace realsense2_camera std::shared_ptr _pc_filter; std::vector> _filters; std::vector _dev_sensors; + rs2::frame _cached_color_frame; // Cache most recent color frame for pointcloud texturing std::vector> _available_ros_sensors; std::map> _align; diff --git a/realsense2_camera/include/pointcloud_filter.h b/realsense2_camera/include/pointcloud_filter.h index b2d210957e..00d7bb4617 100644 --- a/realsense2_camera/include/pointcloud_filter.h +++ b/realsense2_camera/include/pointcloud_filter.h @@ -30,7 +30,8 @@ namespace realsense2_camera PointcloudFilter(std::shared_ptr filter, RosNodeBase& node, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled=false); void setPublisher(); - void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id); + void MapTexture(const rs2::frame& color_frame); + void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id, const rs2::frame& cached_color_frame = rs2::frame()); private: void setParameters(); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 5c851962fd..3352b29cf2 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -605,6 +605,15 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) rs2::video_frame original_color_frame = frameset.get_color_frame(); rs2::video_frame original_infra2_frame = frameset.get_infrared_frame(2); + // Cache color frame for pointcloud texturing when frames arrive in separate framesets + if (original_color_frame) { + _cached_color_frame = original_color_frame; + // Map the color frame to the pointcloud filter for proper texture mapping + if (_pc_filter) { + _pc_filter->MapTexture(original_color_frame); + } + } + ROS_DEBUG("num_filters: %d", static_cast(_filters.size())); for (auto filter_it : _filters) { @@ -924,7 +933,7 @@ void BaseRealSenseNode::SetBaseStream() void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset) { std::string frame_id = OPTICAL_FRAME_ID(DEPTH); - _pc_filter->Publish(pc, t, frameset, frame_id); + _pc_filter->Publish(pc, t, frameset, frame_id, _cached_color_frame); } bool BaseRealSenseNode::shouldPublishCameraInfo(const stream_index_pair& sip) diff --git a/realsense2_camera/src/pointcloud_filter.cpp b/realsense2_camera/src/pointcloud_filter.cpp index f2bc7cc678..dd08e72713 100644 --- a/realsense2_camera/src/pointcloud_filter.cpp +++ b/realsense2_camera/src/pointcloud_filter.cpp @@ -79,6 +79,16 @@ void PointcloudFilter::setPublisher() } } +void PointcloudFilter::MapTexture(const rs2::frame& color_frame) +{ + if (_filter && color_frame) + { + // Cast to rs2::pointcloud and call map_to + auto pc_filter = std::static_pointer_cast(_filter); + pc_filter->map_to(color_frame); + } +} + void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) { size_t i; @@ -88,11 +98,11 @@ void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) } -void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id) +void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id, const rs2::frame& cached_color_frame) { { std::lock_guard lock_guard(_mutex_publisher); - if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count()))) + if (!_pointcloud_publisher || !(_pointcloud_publisher->get_subscription_count())) return; } @@ -101,7 +111,8 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: static int warn_count(0); static const int DISPLAY_WARN_NUMBER(15); rs2::frameset::iterator texture_frame_itr = frameset.end(); - + std::string texture_source_name = _filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast(texture_source_id)); + if (use_texture) { std::set available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8, rs2_format::RS2_FORMAT_Z16 }; @@ -109,21 +120,29 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f) {return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) && (available_formats.find(f.get_profile().format()) != available_formats.end()); }); + + // If texture frame not found in frameset, try to use cached color frame if (texture_frame_itr == frameset.end()) { - warn_count++; - std::string texture_source_name = _filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast(texture_source_id)); - ROS_WARN_STREAM_COND(warn_count == DISPLAY_WARN_NUMBER, "No stream match for pointcloud chosen texture " << texture_source_name); - return; + // Try using cached color frame if texture source is Color + if (texture_source_id == RS2_STREAM_COLOR && cached_color_frame) + { + warn_count = 0; + } + else + { + warn_count++; + ROS_WARN_STREAM_COND(warn_count == DISPLAY_WARN_NUMBER, "No stream match for pointcloud chosen texture " << texture_source_name); + return; + } + } + else + { + warn_count = 0; } - warn_count = 0; } else { warn_count++; - std::string texture_source_name = _filter->get_option_value_description( - rs2_option::RS2_OPTION_STREAM_FILTER, - static_cast(texture_source_id) - ); ROS_WARN_STREAM_COND( warn_count == DISPLAY_WARN_NUMBER, "No matching stream for texture '" << texture_source_name @@ -155,7 +174,24 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: size_t valid_count(0); if (use_texture) { - rs2::video_frame texture_frame = (*texture_frame_itr).as(); + // Use cached color frame if not found in frameset, otherwise use frame from frameset + rs2::frame texture_frame_holder; + if (texture_frame_itr != frameset.end()) + { + texture_frame_holder = (*texture_frame_itr); + } + else if (cached_color_frame && cached_color_frame.is()) + { + texture_frame_holder = cached_color_frame; + } + + if (!texture_frame_holder || !texture_frame_holder.is()) + { + // No texture frame available at all + return; + } + + rs2::video_frame texture_frame = texture_frame_holder.as(); texture_width = texture_frame.get_width(); texture_height = texture_frame.get_height(); num_colors = texture_frame.get_bytes_per_pixel();