Skip to content

Fix: Enable colored pointcloud when frames arrive asynchronously #3487

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JINGERGER wants to merge 1 commit into
base: r/4.57.6
Choose a base branch
from
Open

Fix: Enable colored pointcloud when frames arrive asynchronously #3487

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions realsense2_camera/include/base_realsense_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -405,6 +405,7 @@ namespace realsense2_camera
std::shared_ptr<PointcloudFilter> _pc_filter;
std::vector<std::shared_ptr<NamedFilter>> _filters;
std::vector<rs2::sensor> _dev_sensors;
rs2::frame _cached_color_frame; // Cache most recent color frame for pointcloud texturing
std::vector<std::unique_ptr<RosSensor>> _available_ros_sensors;

std::map<rs2_stream, std::shared_ptr<rs2::align>> _align;
Expand Down
3 changes: 2 additions & 1 deletion realsense2_camera/include/pointcloud_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@ namespace realsense2_camera
PointcloudFilter(std::shared_ptr<rs2::filter> filter, RosNodeBase& node, std::shared_ptr<Parameters> 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();
Expand Down
11 changes: 10 additions & 1 deletion realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>(_filters.size()));
for (auto filter_it : _filters)
{
Expand Down Expand Up @@ -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)
Expand Down
62 changes: 49 additions & 13 deletions realsense2_camera/src/pointcloud_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<rs2::pointcloud>(_filter);

@aikido-pr-checks aikido-pr-checks Bot Feb 1, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::static_pointer_cast to rs2::pointcloud followed by pc_filter->map_to(...) can dereference an object of the wrong runtime type. Verify the filter is actually a pointcloud (e.g., dynamic type check or store as correct type) before calling map_to.

Details

✨ AI Reasoning
​A newly added MapTexture method casts the stored _filter (shared_ptrrs2::filter) to a rs2::pointcloud with std::static_pointer_cast and then calls map_to on the result. If _filter does not actually point to a rs2::pointcloud instance at runtime, the static cast yields an invalid object and calling map_to may dereference invalid memory and cause a segmentation fault. The code checks only that _filter is non-null and that color_frame is valid; there is no runtime type verification before calling map_to. This change introduced the unsafe cast and direct call, which did not exist before. The problematic code is at the line performing the static_pointer_cast and subsequent method call.

🔧 How do I fix it?
Add null checks before dereferencing pointers, validate array bounds before access, avoid using pointers after free/delete, don't write to string literals, and prefer smart pointers in modern C++.

Reply @AikidoSec feedback: [FEEDBACK] to get better review comments in the future.
Reply @AikidoSec ignore: [REASON] to ignore this issue.
More info

@Nir-Az Nir-Az Feb 1, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@JINGERGER can you handle this comment please?

pc_filter->map_to(color_frame);
}
}

void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n)
{
size_t i;
Expand All @@ -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<std::mutex> lock_guard(_mutex_publisher);
if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count())))
if (!_pointcloud_publisher || !(_pointcloud_publisher->get_subscription_count()))

Copilot AI Feb 1, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Unnecessary style change removing parentheses around the first condition check. This change reduces consistency with the second condition which still uses parentheses, and doesn't provide any functional improvement. Consider keeping the original style for consistency.

Suggested change
if (!_pointcloud_publisher || !(_pointcloud_publisher->get_subscription_count()))
if (!(_pointcloud_publisher) || !(_pointcloud_publisher->get_subscription_count()))

Copilot uses AI. Check for mistakes.
return;
}

Expand All @@ -101,29 +111,38 @@ 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<float>(texture_source_id));

if (use_texture)
{
Comment on lines +114 to 117

Copilot AI Feb 1, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line has been moved outside the conditional blocks where it's used (lines 135 and 146-148), but texture_source_id may not be initialized at line 114 if use_texture is false. This could lead to using an uninitialized variable. The variable declaration should remain inside the conditional blocks where texture_source_id is guaranteed to be set.

Suggested change
std::string texture_source_name = _filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast<float>(texture_source_id));
if (use_texture)
{
if (use_texture)
{
std::string texture_source_name = _filter->get_option_value_description(
rs2_option::RS2_OPTION_STREAM_FILTER,
static_cast<float>(texture_source_id));

Copilot uses AI. Check for mistakes.
std::set<rs2_format> available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8, rs2_format::RS2_FORMAT_Z16 };

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<float>(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;
Comment on lines +127 to +130

Copilot AI Feb 1, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When a cached color frame is available, warn_count is reset but the code doesn't verify that the cached frame format is compatible with available_formats. The cached frame should be validated against available_formats before being accepted as a valid texture source, similar to how frames from the frameset are checked at line 121-122.

Suggested change
// Try using cached color frame if texture source is Color
if (texture_source_id == RS2_STREAM_COLOR && cached_color_frame)
{
warn_count = 0;
// Try using cached color frame if texture source is Color and format is compatible
if (texture_source_id == RS2_STREAM_COLOR && cached_color_frame)
{
auto cached_format = cached_color_frame.get_profile().format();
if (available_formats.find(cached_format) != available_formats.end())
{
warn_count = 0;
}
else
{
warn_count++;
ROS_WARN_STREAM_COND(warn_count == DISPLAY_WARN_NUMBER,
"No compatible format for pointcloud chosen texture " << texture_source_name);
return;
}

Copilot uses AI. Check for mistakes.
}
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<float>(texture_source_id)
);
ROS_WARN_STREAM_COND(
warn_count == DISPLAY_WARN_NUMBER,
"No matching stream for texture '" << texture_source_name
Expand Down Expand Up @@ -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<rs2::video_frame>();
// 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<rs2::video_frame>())

Copilot AI Feb 1, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The check for cached_color_frame.is<rs2::video_frame>() is redundant with the check at line 188. Consider removing the type check here since it's validated again before use, or consolidate the logic to avoid duplicate validation.

Suggested change
else if (cached_color_frame && cached_color_frame.is<rs2::video_frame>())
else if (cached_color_frame)

Copilot uses AI. Check for mistakes.
{
texture_frame_holder = cached_color_frame;
}

if (!texture_frame_holder || !texture_frame_holder.is<rs2::video_frame>())
{
// No texture frame available at all
return;
}

rs2::video_frame texture_frame = texture_frame_holder.as<rs2::video_frame>();
texture_width = texture_frame.get_width();
texture_height = texture_frame.get_height();
num_colors = texture_frame.get_bytes_per_pixel();
Expand Down