Merge branch 'melodic' into patch-2

Author: ooeygui

.github/workflows/linux_ROS1.yaml

@@ -10,7 +10,7 @@ on:
 
 jobs:
   build:
-    runs-on: [ubuntu-latest]
+    runs-on: [ubuntu-18.04]
     strategy:
       fail-fast: false
       matrix:
@@ -25,6 +25,7 @@ jobs:
       run: |
         sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
         sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+        curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
         sudo apt update
         sudo apt install ros-melodic-desktop-full
       env:

.github/workflows/main.yml

@@ -10,7 +10,7 @@ on:
 
 jobs:
   build:
-    runs-on: [windows-latest]
+    runs-on: [windows-2019]
     strategy:
       fail-fast: false
       matrix:

CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   nodelet
   cv_bridge
+  camera_info_manager
 )
 
 ###################################

cmake/Findk4abt.cmake

@@ -47,25 +47,25 @@ elseif(CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows")
     if(EXISTS ${_sdk_dir})
         set(_k4abt_lib_path "${_sdk_dir}/${RELATIVE_WIN_K4ABT_LIB_PATH}")
         if(NOT EXISTS "${_k4abt_lib_path}")
-            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find k4a.lib at ${_k4abt_lib_path}")
+            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find k4abt.lib at ${_k4abt_lib_path}")
             return()
         endif()
 
         set(_k4abt_bin_path "${_sdk_dir}/${RELATIVE_WIN_K4ABT_DLL_PATH}")
         if(NOT EXISTS "${_k4abt_bin_path}")
-            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find k4arecord.lib at ${_k4abt_bin_path}")
+            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find k4abt.dll at ${_k4abt_bin_path}")
             return()
         endif()
 
         set(_dnn_model_path "${_sdk_dir}/${RELATIVE_WIN_DNN_MODEL_PATH}")
         if(NOT EXISTS "${_dnn_model_path}")
-            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find k4arecord.lib at ${_dnn_model_path}")
+            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find dnn_model.onnx at ${_dnn_model_path}")
             return()
         endif()
 
         set(_onnx_runtime_bin_path "${_sdk_dir}/${RELATIVE_WIN_ONNX_RUNTIME_DLL_PATH}")
         if(NOT EXISTS "${_onnx_runtime_bin_path}")
-            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find k4a.dll at ${_onnx_runtime_bin_path}")
+            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find onnxruntime.dll at ${_onnx_runtime_bin_path}")
             return()
         endif()
 

docs/building.md

@@ -17,7 +17,7 @@ Follow the [installation instructions](https://github.com/microsoft/Azure-Kinect
 
 The Azure Kinect ROS Driver includes CMake files which will try to locate the Azure Kinect Sensor SDK. Installing the SDK in a non-default location will result in compile failures when CMake is unable to locate the SDK.
 
-The Azure Kinect ROS Driver requires version of v1.1.0 of the Azure Kinect Sensor SDK to compile.
+The Azure Kinect ROS Driver requires version of v1.3.0 of the Azure Kinect Sensor SDK to compile.
 
 #### Alternate SDK Installation
 

docs/usage.md

@@ -2,6 +2,12 @@
 
 The Azure Kinect ROS Driver node exposes Azure Kinect DK sensor streams to ROS.
 
+## Starting the Driver
+
+The ROS package contains two main launch files to start the driver:
+- `driver.launch` starts the driver as a separate node which publishes the raw images and the factory calibration. **The published images are not rectified.**
+- `kinect_rgbd.launch` starts the driver as a nodelet in a nodelet manager and starts the RGB-D processing from `rgbd_launch` which will load additional nodelets for image rectification and registration. This will publish raw (unrectified) as well as rectified images.
+
 ## Setup and Building
 
 Please see the [building guide](building.md).
@@ -27,7 +33,7 @@ The node accepts the following parameters:
 - `body_tracking_smoothing_factor` (float) : Defaults to `0.0`. Controls the temporal smoothing across frames. Set between `0` for no smoothing and `1` for full smoothing. Less smoothing will increase the responsiveness of the detected skeletons but will cause more positional and oriantational jitters.
 - `rescale_ir_to_mono8` (bool) : Defaults to `false`. Whether to rescale the IR image to an 8-bit monochrome image for visualization and further processing. A scaling factor (`ir_mono8_scaling_factor`) is applied.
 - `ir_mono8_scaling_factor` (float) : Defaults to `1.0`. Scaling factor to apply when converting IR to mono8 (see rescale_ir_to_mono8). If using illumination, use the value 0.5-1. If using passive IR, use 10.
-- `imu_rate_target` (int) : Defaults to `0`. Controls the desired IMU message rate, which is rounded to the closest allowable value.  IMU samples from the device are integrated and a mean sample is published at this rate. A value of `0` is interpreted to mean a request for the maximum rate from the sensor (approx. 1.6 kHz).  
+- `imu_rate_target` (int) : Defaults to `0`. Controls the desired IMU message rate, which is rounded to the closest allowable value.  IMU samples from the device are integrated and a mean sample is published at this rate. A value of `0` is interpreted to mean a request for the maximum rate from the sensor (approx. 1.6 kHz).
 - `wired_sync_mode` (int) : Defaults to `0`. Sets the external wired synchronization mode. The modes are: `0: OFF (STANDALONE)`, `1: MASTER`, `2: SUBORDINATE`.
 - `subordinate_delay_off_master_usec` (int) : Defaults to `0`. Delay subordinate camera off master camera by specified amount in usec. Recommended minimum value is 160.
 #### Parameter Restrictions
@@ -66,3 +72,5 @@ Unlike previous ROS Kinect drivers, the Azure Kinect ROS Driver provides calibra
 
 - tf2 : The relative offsets of the cameras and IMU are loaded from the Azure Kinect DK extrinsics calibration information. The various co-ordinate frames published by the node to TF2 are based on the calibration data. Please note that the calibration information only provides the relative positions of the IMU, color and depth cameras. It does not provide the offset between the sensors and the mechanical housing of the camera. The transform between the `camera_base` frame and `depth_camera_link` frame are based on the mechanical specifications of the Azure Kinect DK, and are not corrected by calibration.
 - sensor_msgs::CameraInfo : Intrinsics calibration data for the cameras is converted into a ROS-compatible format. Fully populated CameraInfo messages are published for both the depth and color cameras, allowing ROS to undistort the images using image_proc and project them into point clouds using depth_image_proc. Using the point cloud functions in this node will provide GPU-accelerated results, but the same quality point clouds can be produced using standard ROS tools.
+
+The driver integrates the `camera_info_manager` and provides the `set_camera_info` service to override the factory RGB and IR intrinsics in the ROS node. This is useful for a [manual calibration](http://wiki.ros.org/camera_calibration). Note that this will not override the factory calibration stored on the device.

include/azure_kinect_ros_driver/k4a_ros_device.h

@@ -23,6 +23,7 @@
 #include <sensor_msgs/Temperature.h>
 #include <k4a/k4a.hpp>
 #include <k4arecord/playback.hpp>
+#include <camera_info_manager/camera_info_manager.h>
 
 #if defined(K4A_BODY_TRACKING)
 #include <visualization_msgs/MarkerArray.h>
@@ -47,11 +48,6 @@ class K4AROSDevice
   void stopCameras();
   void stopImu();
 
-  // Get camera calibration information for the depth camera
-  void getDepthCameraInfo(sensor_msgs::CameraInfo& camera_info);
-
-  void getRgbCameraInfo(sensor_msgs::CameraInfo& camera_info);
-
   k4a_result_t getDepthFrame(const k4a::capture& capture, sensor_msgs::ImagePtr& depth_frame, bool rectified);
 
   k4a_result_t getPointCloud(const k4a::capture& capture, sensor_msgs::PointCloud2Ptr& point_cloud);
@@ -86,6 +82,9 @@ class K4AROSDevice
                                    sensor_msgs::PointCloud2Ptr& point_cloud);
 
   void framePublisherThread();
+#if defined(K4A_BODY_TRACKING)
+  void bodyPublisherThread();
+#endif
   void imuPublisherThread();
 
   // Gets a timestap from one of the captures images
@@ -112,6 +111,8 @@ class K4AROSDevice
   // ROS Node variables
   ros::NodeHandle node_;
   ros::NodeHandle private_node_;
+  ros::NodeHandle node_rgb_;
+  ros::NodeHandle node_ir_;
 
   image_transport::ImageTransport image_transport_;
 
@@ -135,6 +136,8 @@ class K4AROSDevice
 
   ros::Publisher pointcloud_publisher_;
 
+  std::shared_ptr<camera_info_manager::CameraInfoManager> ci_mngr_rgb_, ci_mngr_ir_;
+
 #if defined(K4A_BODY_TRACKING)
   ros::Publisher body_marker_publisher_;
 
@@ -144,6 +147,8 @@ class K4AROSDevice
   // Parameters
   K4AROSDeviceParams params_;
 
+  std::string serial_number_;
+
   // K4A device
   k4a::device k4a_device_;
   K4ACalibrationTransformData calibration_data_;
@@ -155,6 +160,8 @@ class K4AROSDevice
 #if defined(K4A_BODY_TRACKING)
   // Body tracker
   k4abt::tracker k4abt_tracker_;
+  std::atomic_int16_t k4abt_tracker_queue_size_;
+  std::thread body_publisher_thread_;
 #endif
 
   std::chrono::nanoseconds device_to_realtime_offset_{0};

include/azure_kinect_ros_driver/k4a_ros_device_params.h

@@ -33,6 +33,10 @@
 #define ROS_PARAM_LIST                                                                                                 \
   LIST_ENTRY(sensor_sn, "The serial number of the sensor this node should connect to.", std::string, std::string(""))  \
   LIST_ENTRY(depth_enabled, "True if depth camera should be enabled", bool, true)                                      \
+  LIST_ENTRY(depth_unit, "Depth distance units. Options are: "+                                                        \
+                         sensor_msgs::image_encodings::TYPE_32FC1+" (32 bit float metre) or "+                         \
+                         sensor_msgs::image_encodings::TYPE_16UC1+" (16 bit integer millimetre)",                      \
+                         std::string, sensor_msgs::image_encodings::TYPE_16UC1)                                        \
   LIST_ENTRY(depth_mode,                                                                                               \
              "The mode of the depth camera. Options are: NFOV_2X2BINNED, NFOV_UNBINNED, WFOV_2X2BINNED, "              \
              "WFOV_UNBINNED, PASSIVE_IR",                                                                              \
@@ -53,6 +57,8 @@
              "Whether the RGB pointcloud is rendered in the depth frame (true) or RGB frame (false). Will either "     \
              "match the resolution of the depth camera (true) or the RGB camera (false)",                              \
              bool, true)                                                                                               \
+  LIST_ENTRY(calibration_url, 'URL to folder with calibration files (default: "file://$HOME/.ros/camera_info/").',    \
+             std::string, {})                                                                                          \
   LIST_ENTRY(tf_prefix, "The prefix prepended to tf frame ID's", std::string, std::string())                           \
   LIST_ENTRY(recording_file, "Path to a recording file to open instead of opening a device", std::string,              \
              std::string(""))                                                                                          \

launch/driver.launch

@@ -9,14 +9,14 @@ Licensed under the MIT License.
 
   <group if="$(arg overwrite_robot_description)">
     <param name="robot_description"
-      command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro tf_prefix:=$(arg tf_prefix)/" />
+      command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro tf_prefix:=$(arg tf_prefix)" />
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
   </group>
 
   <group unless="$(arg overwrite_robot_description)">
     <param name="azure_description"
-      command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro tf_prefix:=$(arg tf_prefix)/" />
+      command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro tf_prefix:=$(arg tf_prefix)" />
     <node name="joint_state_publisher_azure" pkg="joint_state_publisher" type="joint_state_publisher">
       <remap from="robot_description" to="azure_description" />
     </node>  
@@ -27,6 +27,7 @@ Licensed under the MIT License.
 
   <arg name="depth_enabled"           default="true" />           <!-- Enable or disable the depth camera -->
   <arg name="depth_mode"              default="WFOV_UNBINNED" />  <!-- Set the depth camera mode, which affects FOV, depth range, and camera resolution. See Azure Kinect documentation for full details. Valid options: NFOV_UNBINNED, NFOV_2X2BINNED, WFOV_UNBINNED, WFOV_2X2BINNED, and PASSIVE_IR -->
+  <arg name="depth_unit"              default="16UC1" />          <!-- Depth distance units. Options are: "32FC1" (32 bit float metre) or "16UC1" (16 bit integer millimetre) -->
   <arg name="color_enabled"           default="true" />           <!-- Enable or disable the color camera -->
   <arg name="color_format"            default="bgra" />           <!-- The format of RGB camera. Valid options: bgra, jpeg -->
   <arg name="color_resolution"        default="1536P" />          <!-- Resolution at which to run the color camera. Valid options: 720P, 1080P, 1440P, 1536P, 2160P, 3072P -->
@@ -49,6 +50,7 @@ Licensed under the MIT License.
   <node pkg="azure_kinect_ros_driver" type="node" name="azure_kinect_ros_driver" output="screen" required="$(arg required)">
     <param name="depth_enabled"     type="bool"   value="$(arg depth_enabled)" />
     <param name="depth_mode"        type="string" value="$(arg depth_mode)" />
+    <param name="depth_unit"        type="string" value="$(arg depth_unit)" />
     <param name="color_enabled"     type="bool"   value="$(arg color_enabled)" />
     <param name="color_format"      type="string" value="$(arg color_format)" />
     <param name="color_resolution"  type="string" value="$(arg color_resolution)" />