From 851d1944750f14731a0e20d2ffc6c4af92834ba9 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Thu, 6 Feb 2025 14:16:37 +0200 Subject: [PATCH 1/2] align_depth_to_infra2 enabled, pointcloud and align_depth filters to own files --- realsense2_camera/CMakeLists.txt | 4 + .../pointcloud/rs_pointcloud_infra_launch.py | 58 +++++ .../include/align_depth_filter.h | 33 +++ .../include/base_realsense_node.h | 2 + realsense2_camera/include/named_filter.h | 28 --- realsense2_camera/include/pointcloud_filter.h | 47 ++++ realsense2_camera/src/align_depth_filter.cpp | 29 +++ realsense2_camera/src/base_realsense_node.cpp | 8 + realsense2_camera/src/named_filter.cpp | 227 ----------------- realsense2_camera/src/pointcloud_filter.cpp | 236 ++++++++++++++++++ realsense2_camera/src/rs_node_setup.cpp | 4 +- 11 files changed, 420 insertions(+), 256 deletions(-) create mode 100644 realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py create mode 100644 realsense2_camera/include/align_depth_filter.h create mode 100644 realsense2_camera/include/pointcloud_filter.h create mode 100644 realsense2_camera/src/align_depth_filter.cpp create mode 100644 realsense2_camera/src/pointcloud_filter.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3453a665de..4aefc67f6f 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -144,6 +144,8 @@ set(SOURCES src/dynamic_params.cpp src/sensor_params.cpp src/named_filter.cpp + src/pointcloud_filter.cpp + src/align_depth_filter.cpp src/profile_manager.cpp src/image_publisher.cpp src/tfs.cpp @@ -205,6 +207,8 @@ set(INCLUDES include/dynamic_params.h include/sensor_params.h include/named_filter.h + include/pointcloud_filter.h + include/align_depth_filter.h include/ros_param_backend.h include/profile_manager.h include/image_publisher.h) diff --git a/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py b/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py new file mode 100644 index 0000000000..d54669ff6d --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py @@ -0,0 +1,58 @@ +# Copyright 2025 Intel Corporation. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# DESCRIPTION # +# ----------- # +# Use this launch file to launch a device and align depth to infrared 1. +# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters +# command line example: +# ros2 launch realsense2_camera rs_align_depth_to infra_launch.py + +"""Launch realsense2_camera node.""" +from launch import LaunchDescription +import launch_ros.actions +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.absolute())) +import os +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch')) +import rs_launch + +local_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'}, + {'name': 'camera_namespace', 'default': 'camera', 'description': 'camera namespace'}, + {'name': 'enable_color', 'default': 'false', 'description': 'enable color stream'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_infra1', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_sync', 'default': 'true', 'description': 'enable sync mode'}, + {'name': 'pointcloud.enable', 'default': 'true', 'description': 'enable pointcloud'}, + {'name': 'pointcloud.stream_filter', 'default': '3', 'description': 'pointcloud with infrared'}, + ] + +def set_configurable_parameters(local_params): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params]) + + +def generate_launch_description(): + params = rs_launch.configurable_parameters + return LaunchDescription( + rs_launch.declare_configurable_parameters(local_parameters) + + rs_launch.declare_configurable_parameters(params) + + [ + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params)} + ) + ]) diff --git a/realsense2_camera/include/align_depth_filter.h b/realsense2_camera/include/align_depth_filter.h new file mode 100644 index 0000000000..9b9473f80e --- /dev/null +++ b/realsense2_camera/include/align_depth_filter.h @@ -0,0 +1,33 @@ +// Copyright 2025 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include "named_filter.h" + +namespace realsense2_camera +{ + class AlignDepthFilter : public NamedFilter + { + public: + AlignDepthFilter(std::shared_ptr filter, std::function update_align_depth_func, + std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled = false); + }; +} diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index e6b4287a81..760db69f84 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -112,6 +112,8 @@ namespace realsense2_camera bool _is_enabled; }; + class AlignDepthFilter; + class PointcloudFilter; class BaseRealSenseNode { public: diff --git a/realsense2_camera/include/named_filter.h b/realsense2_camera/include/named_filter.h index 7fef1564c0..8e9b53d6ad 100644 --- a/realsense2_camera/include/named_filter.h +++ b/realsense2_camera/include/named_filter.h @@ -47,32 +47,4 @@ namespace realsense2_camera rclcpp::Logger _logger; }; - - class PointcloudFilter : public NamedFilter - { - public: - PointcloudFilter(std::shared_ptr filter, rclcpp::Node& 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); - - private: - void setParameters(); - - private: - bool _is_enabled_pc; - rclcpp::Node& _node; - bool _allow_no_texture_points; - bool _ordered_pc; - std::mutex _mutex_publisher; - rclcpp::Publisher::SharedPtr _pointcloud_publisher; - std::string _pointcloud_qos; - }; - - class AlignDepthFilter : public NamedFilter - { - public: - AlignDepthFilter(std::shared_ptr filter, std::function update_align_depth_func, - std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled = false); - }; } diff --git a/realsense2_camera/include/pointcloud_filter.h b/realsense2_camera/include/pointcloud_filter.h new file mode 100644 index 0000000000..d01ea86c75 --- /dev/null +++ b/realsense2_camera/include/pointcloud_filter.h @@ -0,0 +1,47 @@ +// Copyright 2025 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include "named_filter.h" + +namespace realsense2_camera +{ + class PointcloudFilter : public NamedFilter + { + public: + PointcloudFilter(std::shared_ptr filter, rclcpp::Node& 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); + + private: + void setParameters(); + + private: + bool _is_enabled_pc; + rclcpp::Node& _node; + bool _allow_no_texture_points; + bool _ordered_pc; + std::mutex _mutex_publisher; + rclcpp::Publisher::SharedPtr _pointcloud_publisher; + std::string _pointcloud_qos; + }; +} diff --git a/realsense2_camera/src/align_depth_filter.cpp b/realsense2_camera/src/align_depth_filter.cpp new file mode 100644 index 0000000000..1031da94d8 --- /dev/null +++ b/realsense2_camera/src/align_depth_filter.cpp @@ -0,0 +1,29 @@ +// Copyright 2025 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + + +using namespace realsense2_camera; + + +AlignDepthFilter::AlignDepthFilter(std::shared_ptr filter, + std::function update_align_depth_func, + std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): + NamedFilter(filter, parameters, logger, is_enabled, false) +{ + _params.registerDynamicOptions(*(_filter.get()), "align_depth"); + _params.getParameters()->setParamT("align_depth.enable", _is_enabled, update_align_depth_func); + _parameters_names.push_back("align_depth.enable"); +} diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index cb1ee3e72a..568eca1a37 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -23,6 +23,8 @@ // Header files for disabling intra-process comms for static broadcaster. #include #include +#include "pointcloud_filter.h" +#include "align_depth_filter.h" using namespace realsense2_camera; @@ -635,6 +637,12 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) publishFrame(f, t, COLOR, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false); continue; } + rs2::video_frame original_infra2_frame = frameset.get_infrared_frame(2); + if (original_infra2_frame && _align_depth_filter->is_enabled()) + { + publishFrame(f, t, INFRA2, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false); + continue; + } } publishFrame(f, t, sip, _images, _info_publishers, _image_publishers); } diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 31fd80a8cb..bb487b5e7a 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/realsense2_camera/src/named_filter.cpp @@ -71,230 +71,3 @@ rs2::frame NamedFilter::Process(rs2::frame frame) return frame; } } - - -PointcloudFilter::PointcloudFilter(std::shared_ptr filter, rclcpp::Node& node, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): - NamedFilter(filter, parameters, logger, is_enabled, false), - _node(node), - _allow_no_texture_points(ALLOW_NO_TEXTURE_POINTS), - _ordered_pc(ORDERED_PC) - { - setParameters(); - } - -void PointcloudFilter::setParameters() -{ - std::string module_name = create_graph_resource_name(rs2_to_ros(_filter->get_info(RS2_CAMERA_INFO_NAME))); - std::string param_name(module_name + "." + "allow_no_texture_points"); - _params.getParameters()->setParamT(param_name, _allow_no_texture_points); - _parameters_names.push_back(param_name); - - param_name = module_name + "." + std::string("ordered_pc"); - _params.getParameters()->setParamT(param_name, _ordered_pc); - _parameters_names.push_back(param_name); - - param_name = module_name + "." + std::string("pointcloud_qos"); - rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; - crnt_descriptor.description = "Available options are:\n" + list_available_qos_strings(); - _pointcloud_qos = _params.getParameters()->setParam(param_name, DEFAULT_QOS, [this](const rclcpp::Parameter& parameter) - { - try - { - qos_string_to_qos(parameter.get_value()); - _pointcloud_qos = parameter.get_value(); - ROS_WARN_STREAM("re-enable the stream for the change to take effect."); - } - catch(const std::exception& e) - { - ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is unknown. Set ROS param back to: " << _pointcloud_qos); - _params.getParameters()->queueSetRosValue(parameter.get_name(), _pointcloud_qos); - } - }, crnt_descriptor); - _parameters_names.push_back(param_name); - NamedFilter::setParameters([this](const rclcpp::Parameter& ) - { - setPublisher(); - }); -} - -void PointcloudFilter::setPublisher() -{ - std::lock_guard lock_guard(_mutex_publisher); - if ((_is_enabled) && (!_pointcloud_publisher)) - { - _pointcloud_publisher = _node.create_publisher("~/depth/color/points", - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)), - qos_string_to_qos(_pointcloud_qos))); - } - else if ((!_is_enabled) && (_pointcloud_publisher)) - { - _pointcloud_publisher.reset(); - } -} - -void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) -{ - size_t i; - - for (i=0; i < n; ++i) - dst[n-1-i] = src[i]; - -} - -void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id) -{ - { - std::lock_guard lock_guard(_mutex_publisher); - if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count()))) - return; - } - rs2_stream texture_source_id = static_cast(_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER)); - bool use_texture = texture_source_id != RS2_STREAM_ANY; - static int warn_count(0); - static const int DISPLAY_WARN_NUMBER(5); - rs2::frameset::iterator texture_frame_itr = frameset.end(); - if (use_texture) - { - std::set available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 }; - - 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_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; - } - warn_count = 0; - } - - int texture_width(0), texture_height(0); - int num_colors(0); - - const rs2::vertex* vertex = pc.get_vertices(); - const rs2::texture_coordinate* color_point = pc.get_texture_coordinates(); - - rs2_intrinsics depth_intrin = pc.get_profile().as().get_intrinsics(); - - sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique(); - - sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(pc.size()); - if (_ordered_pc) - { - msg_pointcloud->width = depth_intrin.width; - msg_pointcloud->height = depth_intrin.height; - msg_pointcloud->is_dense = false; - } - - vertex = pc.get_vertices(); - size_t valid_count(0); - if (use_texture) - { - rs2::video_frame texture_frame = (*texture_frame_itr).as(); - texture_width = texture_frame.get_width(); - texture_height = texture_frame.get_height(); - num_colors = texture_frame.get_bytes_per_pixel(); - uint8_t* color_data = (uint8_t*)texture_frame.get_data(); - std::string format_str; - switch(texture_frame.get_profile().format()) - { - case RS2_FORMAT_RGB8: - format_str = "rgb"; - break; - case RS2_FORMAT_Y8: - format_str = "intensity"; - break; - default: - throw std::runtime_error("Unhandled texture format passed in pointcloud " + std::to_string(texture_frame.get_profile().format())); - } - msg_pointcloud->point_step = addPointField(*msg_pointcloud, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32, msg_pointcloud->point_step); - msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; - msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); - - sensor_msgs::PointCloud2Iteratoriter_x(*msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(*msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(*msg_pointcloud, "z"); - sensor_msgs::PointCloud2Iteratoriter_color(*msg_pointcloud, format_str); - color_point = pc.get_texture_coordinates(); - - float color_pixel[2]; - for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, color_point++) - { - float i(color_point->u); - float j(color_point->v); - bool valid_color_pixel(i >= 0.f && i <=1.f && j >= 0.f && j <=1.f); - bool valid_pixel(vertex->z > 0 && (valid_color_pixel || _allow_no_texture_points)); - if (valid_pixel || _ordered_pc) - { - *iter_x = vertex->x; - *iter_y = vertex->y; - *iter_z = vertex->z; - - if (valid_color_pixel) - { - color_pixel[0] = i * texture_width; - color_pixel[1] = j * texture_height; - int pixx = static_cast(color_pixel[0]); - int pixy = static_cast(color_pixel[1]); - int offset = (pixy * texture_width + pixx) * num_colors; - reverse_memcpy(&(*iter_color), color_data+offset, num_colors); // PointCloud2 order of rgb is bgr. - } - ++iter_x; ++iter_y; ++iter_z; - ++iter_color; - ++valid_count; - } - } - } - else - { - msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; - msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); - - sensor_msgs::PointCloud2Iteratoriter_x(*msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(*msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(*msg_pointcloud, "z"); - - for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++) - { - bool valid_pixel(vertex->z > 0); - if (valid_pixel || _ordered_pc) - { - *iter_x = vertex->x; - *iter_y = vertex->y; - *iter_z = vertex->z; - - ++iter_x; ++iter_y; ++iter_z; - ++valid_count; - } - } - } - msg_pointcloud->header.stamp = t; - msg_pointcloud->header.frame_id = frame_id; - if (!_ordered_pc) - { - msg_pointcloud->width = valid_count; - msg_pointcloud->height = 1; - msg_pointcloud->is_dense = true; - modifier.resize(valid_count); - } - { - std::lock_guard lock_guard(_mutex_publisher); - if (_pointcloud_publisher) - _pointcloud_publisher->publish(std::move(msg_pointcloud)); - } -} - - -AlignDepthFilter::AlignDepthFilter(std::shared_ptr filter, - std::function update_align_depth_func, - std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): - NamedFilter(filter, parameters, logger, is_enabled, false) -{ - _params.registerDynamicOptions(*(_filter.get()), "align_depth"); - _params.getParameters()->setParamT("align_depth.enable", _is_enabled, update_align_depth_func); - _parameters_names.push_back("align_depth.enable"); -} diff --git a/realsense2_camera/src/pointcloud_filter.cpp b/realsense2_camera/src/pointcloud_filter.cpp new file mode 100644 index 0000000000..2408b7a202 --- /dev/null +++ b/realsense2_camera/src/pointcloud_filter.cpp @@ -0,0 +1,236 @@ +// Copyright 2025 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + + +using namespace realsense2_camera; + + +PointcloudFilter::PointcloudFilter(std::shared_ptr filter, rclcpp::Node& node, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): + NamedFilter(filter, parameters, logger, is_enabled, false), + _node(node), + _allow_no_texture_points(ALLOW_NO_TEXTURE_POINTS), + _ordered_pc(ORDERED_PC) + { + setParameters(); + } + +void PointcloudFilter::setParameters() +{ + std::string module_name = create_graph_resource_name(rs2_to_ros(_filter->get_info(RS2_CAMERA_INFO_NAME))); + std::string param_name(module_name + "." + "allow_no_texture_points"); + _params.getParameters()->setParamT(param_name, _allow_no_texture_points); + _parameters_names.push_back(param_name); + + param_name = module_name + "." + std::string("ordered_pc"); + _params.getParameters()->setParamT(param_name, _ordered_pc); + _parameters_names.push_back(param_name); + + param_name = module_name + "." + std::string("pointcloud_qos"); + rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + crnt_descriptor.description = "Available options are:\n" + list_available_qos_strings(); + _pointcloud_qos = _params.getParameters()->setParam(param_name, DEFAULT_QOS, [this](const rclcpp::Parameter& parameter) + { + try + { + qos_string_to_qos(parameter.get_value()); + _pointcloud_qos = parameter.get_value(); + ROS_WARN_STREAM("re-enable the stream for the change to take effect."); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is unknown. Set ROS param back to: " << _pointcloud_qos); + _params.getParameters()->queueSetRosValue(parameter.get_name(), _pointcloud_qos); + } + }, crnt_descriptor); + _parameters_names.push_back(param_name); + NamedFilter::setParameters([this](const rclcpp::Parameter& ) + { + setPublisher(); + }); +} + +void PointcloudFilter::setPublisher() +{ + std::lock_guard lock_guard(_mutex_publisher); + if ((_is_enabled) && (!_pointcloud_publisher)) + { + _pointcloud_publisher = _node.create_publisher("~/depth/color/points", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)), + qos_string_to_qos(_pointcloud_qos))); + } + else if ((!_is_enabled) && (_pointcloud_publisher)) + { + _pointcloud_publisher.reset(); + } +} + +void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) +{ + size_t i; + + for (i=0; i < n; ++i) + dst[n-1-i] = src[i]; + +} + +void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id) +{ + { + std::lock_guard lock_guard(_mutex_publisher); + if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count()))) + return; + } + rs2_stream texture_source_id = static_cast(_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER)); + bool use_texture = texture_source_id != RS2_STREAM_ANY; + static int warn_count(0); + static const int DISPLAY_WARN_NUMBER(5); + rs2::frameset::iterator texture_frame_itr = frameset.end(); + if (use_texture) + { + std::set available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 }; + + 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_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; + } + warn_count = 0; + } + + int texture_width(0), texture_height(0); + int num_colors(0); + + const rs2::vertex* vertex = pc.get_vertices(); + const rs2::texture_coordinate* color_point = pc.get_texture_coordinates(); + + rs2_intrinsics depth_intrin = pc.get_profile().as().get_intrinsics(); + + sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique(); + + sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(pc.size()); + if (_ordered_pc) + { + msg_pointcloud->width = depth_intrin.width; + msg_pointcloud->height = depth_intrin.height; + msg_pointcloud->is_dense = false; + } + + vertex = pc.get_vertices(); + size_t valid_count(0); + if (use_texture) + { + rs2::video_frame texture_frame = (*texture_frame_itr).as(); + texture_width = texture_frame.get_width(); + texture_height = texture_frame.get_height(); + num_colors = texture_frame.get_bytes_per_pixel(); + uint8_t* color_data = (uint8_t*)texture_frame.get_data(); + std::string format_str; + switch(texture_frame.get_profile().format()) + { + case RS2_FORMAT_RGB8: + format_str = "rgb"; + break; + case RS2_FORMAT_Y8: + format_str = "intensity"; + break; + default: + throw std::runtime_error("Unhandled texture format passed in pointcloud " + std::to_string(texture_frame.get_profile().format())); + } + msg_pointcloud->point_step = addPointField(*msg_pointcloud, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32, msg_pointcloud->point_step); + msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; + msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); + + sensor_msgs::PointCloud2Iteratoriter_x(*msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(*msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(*msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_color(*msg_pointcloud, format_str); + color_point = pc.get_texture_coordinates(); + + float color_pixel[2]; + for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, color_point++) + { + float i(color_point->u); + float j(color_point->v); + bool valid_color_pixel(i >= 0.f && i <=1.f && j >= 0.f && j <=1.f); + bool valid_pixel(vertex->z > 0 && (valid_color_pixel || _allow_no_texture_points)); + if (valid_pixel || _ordered_pc) + { + *iter_x = vertex->x; + *iter_y = vertex->y; + *iter_z = vertex->z; + + if (valid_color_pixel) + { + color_pixel[0] = i * texture_width; + color_pixel[1] = j * texture_height; + int pixx = static_cast(color_pixel[0]); + int pixy = static_cast(color_pixel[1]); + int offset = (pixy * texture_width + pixx) * num_colors; + reverse_memcpy(&(*iter_color), color_data+offset, num_colors); // PointCloud2 order of rgb is bgr. + } + ++iter_x; ++iter_y; ++iter_z; + ++iter_color; + ++valid_count; + } + } + } + else + { + msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; + msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); + + sensor_msgs::PointCloud2Iteratoriter_x(*msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(*msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(*msg_pointcloud, "z"); + + for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++) + { + bool valid_pixel(vertex->z > 0); + if (valid_pixel || _ordered_pc) + { + *iter_x = vertex->x; + *iter_y = vertex->y; + *iter_z = vertex->z; + + ++iter_x; ++iter_y; ++iter_z; + ++valid_count; + } + } + } + msg_pointcloud->header.stamp = t; + msg_pointcloud->header.frame_id = frame_id; + if (!_ordered_pc) + { + msg_pointcloud->width = valid_count; + msg_pointcloud->height = 1; + msg_pointcloud->is_dense = true; + modifier.resize(valid_count); + } + { + std::lock_guard lock_guard(_mutex_publisher); + if (_pointcloud_publisher) + _pointcloud_publisher->publish(std::move(msg_pointcloud)); + } +} diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index df0c15d8c7..66629003eb 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -16,6 +16,8 @@ #include #include #include +#include "pointcloud_filter.h" +#include "align_depth_filter.h" using namespace realsense2_camera; using namespace rs2; @@ -258,7 +260,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi _info_publishers[sip] = _node.create_publisher(camera_info.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); - if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2) + if (_align_depth_filter->is_enabled() && (sip != DEPTH))// && sip.second < 2) { std::stringstream aligned_image_raw, aligned_camera_info; aligned_image_raw << "~/" << "aligned_depth_to_" << stream_name << "/image_raw"; From 1e78110c7f151f5900f725bf49e53bcb83c7c508 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Sun, 9 Feb 2025 11:59:35 +0200 Subject: [PATCH 2/2] cr adjustments --- .../examples/pointcloud/rs_pointcloud_infra_launch.py | 4 ++-- realsense2_camera/src/base_realsense_node.cpp | 2 +- realsense2_camera/src/rs_node_setup.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py b/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py index d54669ff6d..20ef07f760 100644 --- a/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py +++ b/realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py @@ -14,7 +14,7 @@ # DESCRIPTION # # ----------- # -# Use this launch file to launch a device and align depth to infrared 1. +# Use this launch file to launch a device and align depth to infrared 2. # The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters # command line example: # ros2 launch realsense2_camera rs_align_depth_to infra_launch.py @@ -36,7 +36,7 @@ {'name': 'camera_namespace', 'default': 'camera', 'description': 'camera namespace'}, {'name': 'enable_color', 'default': 'false', 'description': 'enable color stream'}, {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, - {'name': 'enable_infra1', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_infra2', 'default': 'true', 'description': 'enable depth stream'}, {'name': 'enable_sync', 'default': 'true', 'description': 'enable sync mode'}, {'name': 'pointcloud.enable', 'default': 'true', 'description': 'enable pointcloud'}, {'name': 'pointcloud.stream_filter', 'default': '3', 'description': 'pointcloud with infrared'}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 568eca1a37..9a6a3fda16 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -600,6 +600,7 @@ 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); ROS_DEBUG("num_filters: %d", static_cast(_filters.size())); for (auto filter_it : _filters) @@ -637,7 +638,6 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) publishFrame(f, t, COLOR, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false); continue; } - rs2::video_frame original_infra2_frame = frameset.get_infrared_frame(2); if (original_infra2_frame && _align_depth_filter->is_enabled()) { publishFrame(f, t, INFRA2, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 66629003eb..ef0a2f1dfc 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -260,7 +260,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi _info_publishers[sip] = _node.create_publisher(camera_info.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); - if (_align_depth_filter->is_enabled() && (sip != DEPTH))// && sip.second < 2) + if (_align_depth_filter->is_enabled() && (sip != DEPTH)) { std::stringstream aligned_image_raw, aligned_camera_info; aligned_image_raw << "~/" << "aligned_depth_to_" << stream_name << "/image_raw";