From b842bb7d128edae2d883b3d0275823916d0c180d Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Fri, 20 Sep 2024 14:59:09 +0900 Subject: [PATCH] Fix for our usage --- .../k4a_calibration_transform_data.h | 6 +++--- .../k4a_ros_device_params.h | 5 ++++- src/k4a_ros_device.cpp | 14 +++++++++----- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/include/azure_kinect_ros_driver/k4a_calibration_transform_data.h b/include/azure_kinect_ros_driver/k4a_calibration_transform_data.h index ecf8b282..d37e43c4 100644 --- a/include/azure_kinect_ros_driver/k4a_calibration_transform_data.h +++ b/include/azure_kinect_ros_driver/k4a_calibration_transform_data.h @@ -49,9 +49,9 @@ class K4ACalibrationTransformData : public rclcpp::Node k4a::image transformed_depth_image_; std::string tf_prefix_ = ""; - std::string camera_base_frame_ = "camera_base"; - std::string rgb_camera_frame_ = "rgb_camera_link"; - std::string depth_camera_frame_ = "depth_camera_link"; + std::string camera_base_frame_ = "base"; + std::string rgb_camera_frame_ = "rgb_optical_frame"; + std::string depth_camera_frame_ = "depth_optical_frame"; std::string imu_frame_ = "imu_link"; private: diff --git a/include/azure_kinect_ros_driver/k4a_ros_device_params.h b/include/azure_kinect_ros_driver/k4a_ros_device_params.h index a074be9a..2adda04a 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device_params.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device_params.h @@ -80,7 +80,10 @@ int, 0) \ LIST_ENTRY(subordinate_delay_off_master_usec, \ "Delay subordinate camera off master camera by specified amount in usec.", \ - int, 0) + int, 0) \ + LIST_ENTRY(rgb_namespace, \ + "The namespace of RGB topics", \ + std::string, std::string("rgb")) class K4AROSDeviceParams : public rclcpp::Node { diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 43be8305..41b4f870 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -77,6 +77,8 @@ K4AROSDevice::K4AROSDevice() this->declare_parameter("imu_rate_target", rclcpp::ParameterValue(0)); this->declare_parameter("wired_sync_mode", rclcpp::ParameterValue(0)); this->declare_parameter("subordinate_delay_off_master_usec", rclcpp::ParameterValue(0)); + this->declare_parameter("tf_prefix", rclcpp::ParameterValue("")); + this->declare_parameter("rgb_namespace", rclcpp::ParameterValue("rgb")); // Collect ROS parameters from the param server or from the command line #define LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) \ @@ -243,13 +245,13 @@ K4AROSDevice::K4AROSDevice() // others can subscribe to 'rgb/image_raw' with compressed_image_transport. // This technique is described in: // http://wiki.ros.org/compressed_image_transport#Publishing_compressed_images_directly - rgb_jpeg_publisher_ = this->create_publisher("rgb/image_raw/compressed", 1); + rgb_jpeg_publisher_ = this->create_publisher(params_.rgb_namespace + "/image_raw/compressed", 1); } else if (params_.color_format == "bgra") { - rgb_raw_publisher_ = image_transport_->advertise("rgb/image_raw", 1, true); + rgb_raw_publisher_ = image_transport_->advertise(params_.rgb_namespace + "/image_raw", 1, true); } - rgb_raw_camerainfo_publisher_ = this->create_publisher("rgb/camera_info", 1); + rgb_raw_camerainfo_publisher_ = this->create_publisher(params_.rgb_namespace + "/camera_info", 1); depth_raw_publisher_ = image_transport_->advertise("depth/image_raw", 1, true); depth_raw_camerainfo_publisher_ = this->create_publisher("depth/camera_info", 1); @@ -1053,7 +1055,8 @@ void K4AROSDevice::framePublisherThread() // Recordings may not have synchronized captures. For unsynchronized captures without color image skip rgb frame. if (params_.color_format == "jpeg") { - if ((this->count_subscribers("rgb/image_raw/compressed") > 0 || this->count_subscribers("rgb/camera_info") > 0) && + if ((this->count_subscribers(params_.rgb_namespace + "/image_raw/compressed") > 0 || + this->count_subscribers(params_.rgb_namespace + "/camera_info") > 0) && (k4a_device_ || capture.get_color_image() != nullptr)) { result = getJpegRgbFrame(capture, rgb_jpeg_frame); @@ -1078,7 +1081,8 @@ void K4AROSDevice::framePublisherThread() } else if (params_.color_format == "bgra") { - if ((this->count_subscribers("rgb/image_raw") > 0 || this->count_subscribers("rgb/camera_info") > 0) && + if ((this->count_subscribers(params_.rgb_namespace + "/image_raw") > 0 || + this->count_subscribers(params_.rgb_namespace + "/camera_info") > 0) && (k4a_device_ || capture.get_color_image() != nullptr)) { result = getRbgFrame(capture, rgb_raw_frame);