Fix for our usage

sbgisen · Oct 9, 2024 · b842bb7 · b842bb7
1 parent 2c74da4
commit b842bb7
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 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
@@ -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
@@ -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
@@ -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<CompressedImage>("rgb/image_raw/compressed", 1);
+    rgb_jpeg_publisher_ = this->create_publisher<CompressedImage>(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<CameraInfo>("rgb/camera_info", 1);
+  rgb_raw_camerainfo_publisher_ = this->create_publisher<CameraInfo>(params_.rgb_namespace + "/camera_info", 1);
 
   depth_raw_publisher_ = image_transport_->advertise("depth/image_raw", 1, true);
   depth_raw_camerainfo_publisher_ = this->create_publisher<CameraInfo>("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);