Add tf_prefix parameter (#39)

ROS device parameter 'tf_prefix' is added to distinguish tf frame
ID's when multiple nodes are running.

include/azure_kinect_ros_driver/k4a_calibration_transform_data.h

@@ -43,6 +43,7 @@ public:
     k4a::image transformed_rgb_image_;
     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";

include/azure_kinect_ros_driver/k4a_ros_device_params.h

@@ -36,6 +36,7 @@
     LIST_ENTRY(fps, "The FPS of the RGB and Depth cameras. Options are: 5, 15, 30", int, 5) \
     LIST_ENTRY(point_cloud, "A PointCloud2 based on depth data. Requires depth_enabled=true, and cannot be used with depth_mode=PASSIVE_IR", bool, true) \
     LIST_ENTRY(rgb_point_cloud, "Add RGB camera data to the point cloud. Requires point_cloud=true and color_enabled=true", bool, false) \
+    LIST_ENTRY(tf_prefix, "The prefix prepended to tf frame ID's", std::string, std::string()) \
 
 class K4AROSDeviceParams
 {

src/k4a_calibration_transform_data.cpp

@@ -25,6 +25,7 @@ void K4ACalibrationTransformData::initialize(const k4a::device &device,
 {
     k4a_calibration_ = device.get_calibration(depth_mode, resolution);
     k4a_transformation_ = k4a::transformation(k4a_calibration_);
+    tf_prefix_ = params.tf_prefix;
 
     print();
 
@@ -178,8 +179,8 @@ void K4ACalibrationTransformData::publishRgbToBaseTf()
     geometry_msgs::TransformStamped static_transform;
 
     static_transform.header.stamp = ros::Time::now();
-    static_transform.header.frame_id = camera_base_frame_;
-    static_transform.child_frame_id = rgb_camera_frame_;
+    static_transform.header.frame_id = tf_prefix_ + camera_base_frame_;
+    static_transform.child_frame_id = tf_prefix_ + rgb_camera_frame_;
 
     tf2::Vector3 extrinsic_translation((target.xyz.z / -1000.0f), (target.xyz.x / 1000.0f), (target.xyz.y / 1000.0f));
     extrinsic_translation += getDepthToBaseTranslationCorrection();
@@ -229,8 +230,8 @@ void K4ACalibrationTransformData::publishImuToBaseTf()
     geometry_msgs::TransformStamped static_transform;
 
     static_transform.header.stamp = ros::Time::now();
-    static_transform.header.frame_id = camera_base_frame_;
-    static_transform.child_frame_id = imu_frame_;
+    static_transform.header.frame_id = tf_prefix_ + camera_base_frame_;
+    static_transform.child_frame_id = tf_prefix_ + imu_frame_;
 
     tf2::Vector3 extrinsic_translation((target.xyz.x / 1000.0f), (target.xyz.y / -1000.0f), (target.xyz.z / -1000.0f));
     extrinsic_translation += getDepthToBaseTranslationCorrection();
@@ -269,8 +270,8 @@ void K4ACalibrationTransformData::publishDepthToBaseTf()
     geometry_msgs::TransformStamped static_transform;
 
     static_transform.header.stamp = ros::Time::now();
-    static_transform.header.frame_id = camera_base_frame_;
-    static_transform.child_frame_id = depth_camera_frame_;
+    static_transform.header.frame_id = tf_prefix_ + camera_base_frame_;
+    static_transform.child_frame_id = tf_prefix_ + depth_camera_frame_;
 
     tf2::Vector3 depth_translation = getDepthToBaseTranslationCorrection();
     static_transform.transform.translation.x = depth_translation.x();
@@ -352,7 +353,7 @@ tf2::Quaternion K4ACalibrationTransformData::getColorToDepthRotationCorrection()
 
 void K4ACalibrationTransformData::getDepthCameraInfo(sensor_msgs::CameraInfo &camera_info)
 {
-    camera_info.header.frame_id = depth_camera_frame_;
+    camera_info.header.frame_id = tf_prefix_ + depth_camera_frame_;
     camera_info.width = getDepthWidth();
     camera_info.height = getDepthHeight();
     camera_info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
@@ -401,7 +402,7 @@ void K4ACalibrationTransformData::getDepthCameraInfo(sensor_msgs::CameraInfo &ca
 
 void K4ACalibrationTransformData::getRgbCameraInfo(sensor_msgs::CameraInfo &camera_info)
 {
-    camera_info.header.frame_id = rgb_camera_frame_;
+    camera_info.header.frame_id = tf_prefix_ + rgb_camera_frame_;
     camera_info.width = getColorWidth();
     camera_info.height = getColorHeight();
     camera_info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;