Skip to content
This repository was archived by the owner on Jul 26, 2024. It is now read-only.

Commit d6e69db

Browse files
exo-coreexo-core
committed
new parameter to switch between CAD data and factory calibration
1 parent d28abdc commit d6e69db

File tree

7 files changed

+60
-48
lines changed

7 files changed

+60
-48
lines changed

‎include/azure_kinect_ros_driver/k4a_calibration_transform_data.h‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -46,10 +46,10 @@ class K4ACalibrationTransformData
4646
k4a::image transformed_depth_image_;
4747

4848
std::string tf_prefix_ = "";
49-
std::string camera_base_frame_ = "camera_base";
50-
std::string rgb_camera_frame_ = "rgb_camera_link";
51-
std::string depth_camera_frame_ = "depth_camera_link";
52-
std::string imu_frame_ = "imu_link";
49+
std::string camera_base_frame_ = "azure_camera_base";
50+
std::string rgb_camera_frame_ = "azure_rgb";
51+
std::string depth_camera_frame_ = "azure_depth";
52+
std::string imu_frame_ = "azure_imu";
5353

5454
private:
5555
void initialize(K4AROSDeviceParams params);

‎include/azure_kinect_ros_driver/k4a_ros_device_params.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,9 +57,10 @@
5757
"Whether the RGB pointcloud is rendered in the depth frame (true) or RGB frame (false). Will either " \
5858
"match the resolution of the depth camera (true) or the RGB camera (false)", \
5959
bool, true) \
60-
LIST_ENTRY(calibration_url, 'URL to folder with calibration files (default: "file://$HOME/.ros/camera_info/").', \
60+
LIST_ENTRY(calibration_url, 'URL to folder with calibration files (default: "file://$HOME/.ros/camera_info/").', \
6161
std::string, {}) \
6262
LIST_ENTRY(tf_prefix, "The prefix prepended to tf frame ID's", std::string, std::string()) \
63+
LIST_ENTRY(publish_calibrated_poses, "Publish calibrated sensors poses at runtime", bool, true) \
6364
LIST_ENTRY(recording_file, "Path to a recording file to open instead of opening a device", std::string, \
6465
std::string("")) \
6566
LIST_ENTRY(recording_loop_enabled, "True if the recording should be rewound at EOF", bool, false) \

‎launch/driver.launch‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,12 @@ Licensed under the MIT License.
55

66
<launch>
77
<arg name="tf_prefix" default="" /> <!-- Prefix added to tf frame IDs. It typically contains a trailing '_' unless empty. -->
8+
<arg name="publish_calibrated_poses" default="true" /> <!-- Flag to publish the relative poses between IMU, RGB and depth camera from factory calibration parameters during runtime. -->
89
<arg name="overwrite_robot_description" default="true" /> <!-- Flag to publish a standalone azure_description instead of the default robot_descrition parameter-->
910

1011
<group if="$(arg overwrite_robot_description)">
1112
<param name="robot_description"
12-
command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro tf_prefix:=$(arg tf_prefix)" />
13+
command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro tf_prefix:=$(arg tf_prefix) publish_calibrated_poses:=$(arg publish_calibrated_poses)" />
1314
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
1415
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
1516
</group>
@@ -60,6 +61,7 @@ Licensed under the MIT License.
6061
<param name="point_cloud_in_depth_frame" type="bool" value="$(arg point_cloud_in_depth_frame)" />
6162
<param name="sensor_sn" type="string" value="$(arg sensor_sn)" />
6263
<param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
64+
<param name="publish_calibrated_poses" type="bool" value="$(arg publish_calibrated_poses)" />
6365
<param name="recording_file" type="string" value="$(arg recording_file)" />
6466
<param name="recording_loop_enabled" type="bool" value="$(arg recording_loop_enabled)" />
6567
<param name="body_tracking_enabled" type="bool" value="$(arg body_tracking_enabled)" />

‎launch/kinect_rgbd.launch‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,12 @@
55

66
<!-- publish Azure Kinect coordinate frames -->
77
<arg name="tf_prefix" default="" /> <!-- Prefix added to tf frame IDs. It typically contains a trailing '_' unless empty. -->
8+
<arg name="publish_calibrated_poses" default="true" /> <!-- Flag to publish the relative poses between IMU, RGB and depth camera from factory calibration parameters during runtime. -->
89
<arg name="overwrite_robot_description" default="true" /> <!-- Flag to publish a standalone azure_description instead of the default robot_description parameter-->
910

1011
<group if="$(arg overwrite_robot_description)">
1112
<param name="robot_description"
12-
command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect_standalone.urdf.xacro tf_prefix:=$(arg tf_prefix)" />
13+
command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect_standalone.urdf.xacro tf_prefix:=$(arg tf_prefix) publish_calibrated_poses:=$(arg publish_calibrated_poses)" />
1314
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
1415
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
1516
</group>
@@ -95,6 +96,7 @@
9596
<param name="point_cloud_in_depth_frame" value="$(arg point_cloud_in_depth_frame)" />
9697
<param name="sensor_sn" value="$(arg sensor_sn)" />
9798
<param name="tf_prefix" value="$(arg tf_prefix)" />
99+
<param name="publish_calibrated_poses" value="$(arg publish_calibrated_poses)" />
98100
<param name="recording_file" value="$(arg recording_file)" />
99101
<param name="recording_loop_enabled" value="$(arg recording_loop_enabled)" />
100102
<param name="calibration_url" value="$(arg calibration_url)" />

‎src/k4a_calibration_transform_data.cpp‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -67,13 +67,15 @@ void K4ACalibrationTransformData::initialize(const K4AROSDeviceParams params)
6767
getColorWidth() * (int) sizeof(DepthPixel));
6868
}
6969

70-
// Publish various transforms needed by ROS.
71-
// We publish all TFs all the time, even if the respective sensor data
72-
// output is off. This allows us to just use the SDK calibrations, and one
73-
// cosmetic TF output between the depth and the base.
74-
publishDepthToBaseTf();
75-
publishImuToDepthTf();
76-
publishRgbToDepthTf();
70+
if (params.publish_calibrated_poses) {
71+
// Publish various transforms needed by ROS.
72+
// We publish all TFs all the time, even if the respective sensor data
73+
// output is off. This allows us to just use the SDK calibrations, and one
74+
// cosmetic TF output between the depth and the base.
75+
publishDepthToBaseTf();
76+
publishImuToDepthTf();
77+
publishRgbToDepthTf();
78+
}
7779
}
7880

7981
int K4ACalibrationTransformData::getDepthWidth() { return k4a_calibration_.depth_camera_calibration.resolution_width; }

‎urdf/azure_kinect_macro.urdf.xacro‎

Lines changed: 37 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
-->
2020

2121
<robot name="azure_kinect" xmlns:xacro="http://www.ros.org/wiki/xacro">
22-
<xacro:macro name="azure_kinect" params="prefix:='' parent:='' gazebo:=false *origin">
22+
<xacro:macro name="azure_kinect" params="prefix:='' parent:='' publish_calibrated_poses=true gazebo:=false *origin">
2323

2424
<xacro:if value="${parent != ''}">
2525
<joint name="${prefix}azure_in_joint" type="fixed">
@@ -44,41 +44,45 @@
4444
</collision>
4545
</link>
4646

47-
<link name="${prefix}camera_base" /> <!-- for compatability with old model -->
48-
<link name="${prefix}azure_rgb" />
49-
<link name="${prefix}azure_depth" />
50-
<link name="${prefix}azure_gyro" />
51-
<link name="${prefix}azure_microphone_0" />
52-
<link name="${prefix}azure_microphone_1" />
53-
<link name="${prefix}azure_microphone_2" />
54-
<link name="${prefix}azure_microphone_3" />
55-
<link name="${prefix}azure_microphone_4" />
56-
<link name="${prefix}azure_microphone_5" />
57-
<link name="${prefix}azure_microphone_6" />
47+
<link name="${prefix}azure_camera_base" /> <!-- for compatability with calibration parameters -->
5848

5949
<joint name="${prefix}azure_base_to_camera_base" type="fixed">
6050
<parent link="${prefix}azure_base" />
61-
<child link="${prefix}camera_base" />
51+
<child link="${prefix}azure_camera_base" />
6252
<origin xyz="0.062 0.0 0.0065" rpy="0 0 0" />
6353
</joint>
6454

65-
<joint name="${prefix}azure_base_to_rgb" type="fixed">
66-
<parent link="${prefix}azure_base" />
67-
<child link="${prefix}azure_rgb" />
68-
<origin xyz="0.0618 -0.032 0.0065" rpy="-${pi/2} 0 -${pi/2}" />
69-
</joint>
55+
<xacro:if value="${not publish_calibrated_poses or gazebo}">
56+
<link name="${prefix}azure_rgb" />
57+
<link name="${prefix}azure_depth" />
58+
<link name="${prefix}azure_imu" />
7059

71-
<joint name="${prefix}azure_base_to_depth" type="fixed">
72-
<parent link="${prefix}azure_base" />
73-
<child link="${prefix}azure_depth" />
74-
<origin xyz="0.0618 -0.0005 0.0065" rpy="-${96/180*pi} 0 -${pi/2}" />
75-
</joint>
60+
<joint name="${prefix}azure_base_to_rgb" type="fixed">
61+
<parent link="${prefix}azure_base" />
62+
<child link="${prefix}azure_rgb" />
63+
<origin xyz="0.0618 -0.032 0.0065" rpy="-${pi/2} 0 -${pi/2}" />
64+
</joint>
7665

77-
<joint name="${prefix}azure_base_to_gyro" type="fixed">
78-
<parent link="${prefix}azure_base" />
79-
<child link="${prefix}azure_gyro" />
80-
<origin xyz="0.0618 -0.0005 0.0065" rpy="0 ${pi} 0" />
81-
</joint>
66+
<joint name="${prefix}azure_base_to_depth" type="fixed">
67+
<parent link="${prefix}azure_base" />
68+
<child link="${prefix}azure_depth" />
69+
<origin xyz="0.0618 -0.0005 0.0065" rpy="-${96/180*pi} 0 -${pi/2}" />
70+
</joint>
71+
72+
<joint name="${prefix}azure_base_to_imu" type="fixed">
73+
<parent link="${prefix}azure_base" />
74+
<child link="${prefix}azure_imu" />
75+
<origin xyz="0.0618 -0.0005 0.0065" rpy="0 ${pi} 0" />
76+
</joint>
77+
</xacro:if>
78+
79+
<link name="${prefix}azure_microphone_0" />
80+
<link name="${prefix}azure_microphone_1" />
81+
<link name="${prefix}azure_microphone_2" />
82+
<link name="${prefix}azure_microphone_3" />
83+
<link name="${prefix}azure_microphone_4" />
84+
<link name="${prefix}azure_microphone_5" />
85+
<link name="${prefix}azure_microphone_6" />
8286

8387
<joint name="${prefix}azure_base_to_microphone_0" type="fixed">
8488
<parent link="${prefix}azure_base" />
@@ -141,7 +145,7 @@
141145
WFOV 2x2 binned: 0, 5, 15, 30 FPS
142146
WFOV unbinned: 0, 5, 15 FPS
143147
144-
Note: Frame rate also needs to be changed in gazebo plugin settings (line 130)
148+
Note: Frame rate also needs to be changed in gazebo plugin settings (line 183)
145149
-->
146150
<update_rate>5</update_rate>
147151
<camera>
@@ -168,17 +172,17 @@
168172
WFOV 2x2 binned: 0.25 - 2.88 m
169173
WFOV unbinned: 0.25 - 2.21 m
170174
171-
Note: Range also needs to be changed in gazebo plugin settings (lines 131 and 132)
175+
Note: Range also needs to be changed in gazebo plugin settings (lines 184 and 185)
172176
-->
173177
<near>0.25</near>
174178
<far>2.88</far>
175179
</clip>
176180
</camera>
177181
<plugin filename="libgazebo_ros_openni_kinect.so" name="${prefix}azure_camera_controller">
178182
<alwaysOn>true</alwaysOn>
179-
<updateRate>5</updateRate>
180-
<pointCloudCutoff>0.25</pointCloudCutoff>
181-
<pointcloudcutoffmax>2.88</pointcloudcutoffmax>
183+
<updateRate>5</updateRate> <!-- Frame rate -->
184+
<pointCloudCutoff>0.25</pointCloudCutoff> <!-- Near clip distance -->
185+
<pointcloudcutoffmax>2.88</pointcloudcutoffmax> <!-- Far clip distance -->
182186
<imageTopicName>rgb/image_raw</imageTopicName>
183187
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
184188
<depthImageTopicName>depth/image_raw</depthImageTopicName>

‎urdf/azure_kinect_standalone.urdf.xacro‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88

99
<robot name="azure_kinect" xmlns:xacro="http://www.ros.org/wiki/xacro">
1010
<xacro:arg name="tf_prefix" default=""/>
11+
<xacro:arg name="publish_calibrated_poses" default="true"/>
1112

1213
<link name="$(arg tf_prefix)azure_stand">
1314
<visual>
@@ -26,7 +27,7 @@
2627

2728
<xacro:include filename="$(find azure_kinect_ros_driver)/urdf/azure_kinect_macro.urdf.xacro" />
2829

29-
<xacro:azure_kinect prefix="$(arg tf_prefix)" parent="$(arg tf_prefix)azure_stand" gazebo="false">
30+
<xacro:azure_kinect prefix="$(arg tf_prefix)" parent="$(arg tf_prefix)azure_stand" gazebo="false" publish_calibrated_poses="$(arg publish_calibrated_poses)">
3031
<origin xyz="0 0 0.0235" rpy="0 -0.06 0" />
3132
</xacro:azure_kinect>
3233
</robot>

0 commit comments

Comments
 (0)