From b41b35b060c8af90de700618795744b37f55197d Mon Sep 17 00:00:00 2001 From: armangb1 Date: Sat, 14 Jun 2025 17:35:16 +0330 Subject: [PATCH] Add dynamic frame ID parameters for RGB and depth images - Introduced parameters for customizable frame IDs in the Kinect component. - Updated the constructor to declare and retrieve these parameters. - Modified the timer callback to use the dynamic frame IDs for publishing. --- include/kinect_ros2/kinect_ros2_component.hpp | 6 ++ src/kinect_ros2_component.cpp | 60 ++++++++++++++++--- 2 files changed, 57 insertions(+), 9 deletions(-) diff --git a/include/kinect_ros2/kinect_ros2_component.hpp b/include/kinect_ros2/kinect_ros2_component.hpp index 3a9f234..ad36cda 100644 --- a/include/kinect_ros2/kinect_ros2_component.hpp +++ b/include/kinect_ros2/kinect_ros2_component.hpp @@ -34,6 +34,12 @@ class KinectRosComponent : public rclcpp::Node static void rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t timestamp); void timer_callback(); + + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + void on_parameter_event(const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult parameters_callback( + const std::vector & parameters); }; } diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp index 5ce32de..dbc3f0d 100644 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -21,6 +21,16 @@ KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) std::string pkg_share = ament_index_cpp::get_package_share_directory("kinect_ros2"); + this->declare_parameter("rgb_frame_id", "kinect_rgb"); + this->declare_parameter("depth_frame_id", "kinect_depth"); + + std::string rgb_frame_id = this->get_parameter("rgb_frame_id").as_string(); + std::string depth_frame_id = this->get_parameter("depth_frame_id").as_string(); + + // In constructor, after declaring parameters: + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&KinectRosComponent::parameters_callback, this, std::placeholders::_1)); + //todo: use parameters depth_info_manager_ = std::make_shared( this, "kinect", @@ -31,9 +41,9 @@ KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) "file://" + pkg_share + "/cfg/calibration_rgb.yaml"); rgb_info_ = rgb_info_manager_->getCameraInfo(); - rgb_info_.header.frame_id = "kinect_rgb"; + rgb_info_.header.frame_id = rgb_frame_id; depth_info_ = depth_info_manager_->getCameraInfo(); - depth_info_.header.frame_id = "kinect_depth"; + depth_info_.header.frame_id = depth_frame_id; depth_pub_ = image_transport::create_camera_publisher(this, "depth/image_raw"); rgb_pub_ = image_transport::create_camera_publisher(this, "image_raw"); @@ -102,6 +112,32 @@ KinectRosComponent::~KinectRosComponent() } + + +void KinectRosComponent::on_parameter_event(const std::vector & parameters) +{ + for (const auto & param : parameters) { + if (param.get_name() == "rgb_frame_id") { + rgb_info_.header.frame_id = param.as_string(); + } else if (param.get_name() == "depth_frame_id") { + depth_info_.header.frame_id = param.as_string(); + } + } +} + +rcl_interfaces::msg::SetParametersResult KinectRosComponent::parameters_callback( + const std::vector & parameters) +{ + on_parameter_event(parameters); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + + + + /* The freenect lib stores the depth data in a static region of the memory, so the best way to create a cv::Mat is passing the pointer of that region, avoiding the need of copying the data to a new cv::Mat. This way, the callback only used to set a flag that indicates that a new image @@ -137,19 +173,20 @@ void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t void KinectRosComponent::timer_callback() { freenect_process_events(fn_ctx_); - auto header = std_msgs::msg::Header(); - header.frame_id = "kinect_depth"; - auto stamp = now(); - header.stamp = stamp; - depth_info_.header.stamp = stamp; + auto stamp = this->now(); if (_depth_flag) { + auto depth_header = std_msgs::msg::Header(); + depth_header.frame_id = depth_info_.header.frame_id; + depth_header.stamp = stamp; + depth_info_.header.stamp = stamp; + //convert 16bit to 8bit mono // cv::Mat depth_8UC1(_depth_image, CV_16UC1); // depth_8UC1.convertTo(depth_8UC1, CV_8UC1); - auto msg = cv_bridge::CvImage(header, "16UC1", _depth_image).toImageMsg(); + auto msg = cv_bridge::CvImage(depth_header, "16UC1", _depth_image).toImageMsg(); depth_pub_.publish(*msg, depth_info_); // cv::imshow("Depth", _depth_image); @@ -158,7 +195,12 @@ void KinectRosComponent::timer_callback() } if (_rgb_flag) { - auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "rgb8", _rgb_image).toImageMsg(); + auto rgb_header = std_msgs::msg::Header(); + rgb_header.frame_id = rgb_info_.header.frame_id; + rgb_header.stamp = stamp; + rgb_info_.header.stamp = stamp; + + auto msg = cv_bridge::CvImage(rgb_header, "rgb8", _rgb_image).toImageMsg(); rgb_pub_.publish(*msg, rgb_info_); // cv::imshow("RGB", _rgb_image);