Merge branch 'melodic' into patch-2

CMakeLists.txt

@@ -6,6 +6,8 @@ project(azure_kinect_ros_driver LANGUAGES C CXX)
 
 list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_CURRENT_LIST_DIR}/cmake)
 
+option(CUDA_SUPPORT "use CUDA support onnxruntime library" ON)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -14,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   sensor_msgs
   image_transport
+  image_geometry
   nodelet
   tf2
   tf2_ros
@@ -24,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS
   camera_info_manager
 )
 
+find_package(OpenCV REQUIRED)
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -77,6 +81,7 @@ set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX ""
 ############################
 
 message("Finding K4A SDK binaries")
+message("--------------------------------------------${CMAKE_MODULE_PATH}-----------------------------------------")
 
 # Disable cached locations for K4A SDK binaries.
 # Do this to force the search logic to happen correctly.
@@ -92,20 +97,121 @@ find_package(k4a 1.3.0 QUIET MODULE REQUIRED)
 set(K4A_LIBS k4a::k4a;k4a::k4arecord)
 
 # Try to find and enable the body tracking SDK
-find_package(k4abt 1.0.0 QUIET MODULE)
+find_package(k4abt 1.0.1 QUIET MODULE)
 if (k4abt_FOUND)
   list(APPEND K4A_LIBS k4abt::k4abt)
   message(STATUS "Body Tracking SDK found: compiling support for Body Tracking")
   target_compile_definitions(${PROJECT_NAME}_node PUBLIC K4A_BODY_TRACKING)
   target_compile_definitions(${PROJECT_NAME}_nodelet PUBLIC K4A_BODY_TRACKING)
+  message("!!! Body Tracking SDK found: body tracking features will be available !!!")
 else()
   message("!!! Body Tracking SDK not found: body tracking features will not be available !!!")
 endif()
+if(MSVC AND NOT k4abt_FOUND)
+  if(CUDA_SUPPORT)
+
+      set(KBT "Microsoft.Azure.Kinect.BodyTracking.1.0.1")
+      set(KBT_URL "https://www.nuget.org/api/v2/package/Microsoft.Azure.Kinect.BodyTracking/1.0.1")
+      set(KBT_SHA512 "546ebf82551ca809213c7540c4a7c3ccdd14d1ae0ac5d8475b3a98439c4a68cf3d5b4e3fa70676d524ac820815f77611bf2d8ffba96933ad5b317db392d34f20")
+      file(DOWNLOAD
+      ${KBT_URL}
+      ${CMAKE_CURRENT_BINARY_DIR}/kbt.nuget
+      EXPECTED_HASH SHA512=${KBT_SHA512}
+      SHOW_PROGRESS
+      )
+
+      file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${KBT}")
+        execute_process(COMMAND tar xvzf "${CMAKE_CURRENT_BINARY_DIR}/kbt.nuget" 
+                    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${KBT}"
+      )
+
+      file(GLOB KBT_ONNX_DLL
+        "${CMAKE_CURRENT_BINARY_DIR}/${KBT}/lib/native/amd64/release/onnxruntime.dll"
+      )
+
+      file(GLOB KBT_DLL
+      "${CMAKE_CURRENT_BINARY_DIR}/${KBT}/lib/native/amd64/release/k4abt.dll"
+      )
+
+      file(GLOB KBT_LIB
+      "${CMAKE_CURRENT_BINARY_DIR}/${KBT}/lib/native/amd64/release/k4abt.lib"
+      )
+
+      file(GLOB KBT_DNN_MODEL
+      "${CMAKE_CURRENT_BINARY_DIR}/${KBT}/content/dnn_model_2_0.onnx"
+      )
+
+      add_library(k4abt::k4abt SHARED IMPORTED GLOBAL)
+
+      target_include_directories(
+          k4abt::k4abt
+          INTERFACE
+              ${CMAKE_CURRENT_BINARY_DIR}/${KBT}/build/native/include
+      )
+
+      set_property(TARGET k4abt::k4abt PROPERTY IMPORTED_CONFIGURATIONS "")
+      set_property(TARGET k4abt::k4abt PROPERTY IMPORTED_LOCATION "${KBT_DLL}")
+      set_property(TARGET k4abt::k4abt PROPERTY IMPORTED_IMPLIB "${KBT_LIB}")
+      set_property(TARGET k4abt::k4abt PROPERTY IMPORTED_LINK_DEPENDENT_LIBRARIES "${KBT_DNN_MODEL};${KBT_ONNX_DLL}")
+      set(k4abt_FOUND TRUE)
+      set(k4abt_VERSION "1.0.1")
+      list(APPEND K4A_LIBS k4abt::k4abt)
+      target_compile_definitions(${PROJECT_NAME}_node PUBLIC K4A_BODY_TRACKING)
+      target_compile_definitions(${PROJECT_NAME}_nodelet PUBLIC K4A_BODY_TRACKING)
+
+      set(KBT_DEPENDENCIES "Microsoft.Azure.Kinect.BodyTracking.Dependencies.0.9.1")
+      set(KBTD_URL "https://www.nuget.org/api/v2/package/Microsoft.Azure.Kinect.BodyTracking.Dependencies/0.9.1")
+      set(KBTD_SHA512 "5df5ceb2f7905a3e208a085a29ef02feb1820ffe819563af77c272ad7e068cf3a158c0ce610e421829b5f7ebbb628c45f56617344d6d1ef0a31d719253cf881f")
+      file(DOWNLOAD
+      ${KBTD_URL}
+      ${CMAKE_CURRENT_BINARY_DIR}/kbtd.nuget
+      EXPECTED_HASH SHA512=${KBTD_SHA512}
+      SHOW_PROGRESS
+      )
+
+      file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${KBT_DEPENDENCIES}")
+        execute_process(COMMAND tar xvzf "${CMAKE_CURRENT_BINARY_DIR}/kbtd.nuget" 
+                    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${KBT_DEPENDENCIES}"
+      )
+
+      file(GLOB KBTD_DLLS
+        "${CMAKE_CURRENT_BINARY_DIR}/${KBT_DEPENDENCIES}/lib/native/amd64/release/*.dll"
+      )
+
+      file(COPY ${KBTD_DLLS} DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION})
+      file(COPY ${KBTD_DLLS} DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION})
+
+      set(CUDNN_DEPENDENCIES "Microsoft.Azure.Kinect.BodyTracking.Dependencies.cuDNN.0.9.1")
+      set(CUDNN_URL "https://www.nuget.org/api/v2/package/Microsoft.Azure.Kinect.BodyTracking.Dependencies.cuDNN/0.9.1")
+      set(CUDNN_SHA512 "a9986d5d7f06e0445cb65dfe1363c3e827df2c87b80ecfa05894c062714bf2092b3f87defd136cd2a89b9e588e773080ecf7079e8ce7e428426c4a00259c5085")
+      file(DOWNLOAD
+      ${CUDNN_URL}
+      ${CMAKE_CURRENT_BINARY_DIR}/cudnn.nuget
+      EXPECTED_HASH SHA512=${CUDNN_SHA512}
+      SHOW_PROGRESS
+      )
+
+      file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${CUDNN_DEPENDENCIES}")
+
+      execute_process(COMMAND tar xvzf "${CMAKE_CURRENT_BINARY_DIR}/cudnn.nuget" 
+                  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${CUDNN_DEPENDENCIES}"
+                  )
+
+      file(GLOB CUDNN_DLLS
+      "${CMAKE_CURRENT_BINARY_DIR}/${CUDNN_DEPENDENCIES}/lib/native/amd64/release/*.dll"
+      )
+      file(COPY ${CUDNN_DLLS} DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION})
+      file(COPY ${CUDNN_DLLS} DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION})
+
+
+  else()
+    message(FATAL_ERROR "Body Tracking SDK needs a NVIDIA GEFORCE GTX 1070 or better")
+  endif()
+endif()
 
 # This reads the K4A_LIBS and K4A_INSTALL_REQUIRED variables and decides how to install
 # the various shared objects / DLLs
 include(Installk4a)
-
 ##################################
 ###### END AZURE KINECT SDK ######
 ##################################
@@ -118,11 +224,13 @@ include_directories(
 target_link_libraries(${PROJECT_NAME}_node
   ${K4A_LIBS}
   ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
 )
 
 target_link_libraries(${PROJECT_NAME}_nodelet
   ${K4A_LIBS}
   ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
 )
 
 #############

cmake/Findk4abt.cmake

@@ -6,7 +6,7 @@ set(RELATIVE_WIN_BIN_DIR "sdk/windows-desktop/amd64/release/bin")
 set(RELATIVE_WIN_K4ABT_LIB_PATH "${RELATIVE_WIN_LIB_DIR}/k4abt.lib")
 
 set(RELATIVE_WIN_K4ABT_DLL_PATH "${RELATIVE_WIN_BIN_DIR}/k4abt.dll")
-set(RELATIVE_WIN_DNN_MODEL_PATH "${RELATIVE_WIN_BIN_DIR}/dnn_model.onnx")
+set(RELATIVE_WIN_DNN_MODEL_PATH "${RELATIVE_WIN_BIN_DIR}/dnn_model_2_0.onnx")
 set(RELATIVE_WIN_ONNX_RUNTIME_DLL_PATH "${RELATIVE_WIN_BIN_DIR}/onnxruntime.dll")
 
 # K4A BT versions have exactly 3 components: major.minor.rev
@@ -59,7 +59,7 @@ elseif(CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows")
 
         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 dnn_model.onnx at ${_dnn_model_path}")
+            quiet_message(WARNING "Rejecting SDK located at ${_sdk_dir}: Could not find dnn_model_2_0.onnx at ${_dnn_model_path}")
             return()
         endif()
 

include/azure_kinect_ros_driver/k4a_ros_device.h

@@ -28,6 +28,13 @@
 #if defined(K4A_BODY_TRACKING)
 #include <visualization_msgs/MarkerArray.h>
 #include <k4abt.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Pose.h>
+#include <opencv2/opencv.hpp>
+#include <image_geometry/pinhole_camera_model.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #endif
 
 // Project headers
@@ -63,13 +70,15 @@ class K4AROSDevice
   k4a_result_t getIrFrame(const k4a::capture& capture, sensor_msgs::ImagePtr& ir_image);
 
 #if defined(K4A_BODY_TRACKING)
-  k4a_result_t getBodyMarker(const k4abt_body_t& body, visualization_msgs::MarkerPtr marker_msg, int jointType,
+k4a_result_t getBodyMarker(const k4abt_body_t& body, visualization_msgs::MarkerPtr marker_msg, geometry_msgs::TransformStamped& transform_msg, int bodyNum, int jointType,
                              ros::Time capture_time);
 
   k4a_result_t getBodyIndexMap(const k4abt::frame& body_frame, sensor_msgs::ImagePtr body_index_map_image);
 
   k4a_result_t renderBodyIndexMapToROS(sensor_msgs::ImagePtr body_index_map_image, k4a::image& k4a_body_index_map,
                                        const k4abt::frame& body_frame);
+
+  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg);
 #endif
 
  private:
@@ -140,8 +149,14 @@ class K4AROSDevice
 
 #if defined(K4A_BODY_TRACKING)
   ros::Publisher body_marker_publisher_;
+  tf2_ros::TransformBroadcaster br;
 
   image_transport::Publisher body_index_map_publisher_;
+  image_transport::Publisher image_tf_publisher_;
+  image_transport::CameraSubscriber image_subscriber_;
+  image_geometry::PinholeCameraModel cam_model_;
+  tf2_ros::Buffer tfBuffer;
+  tf2_ros::TransformListener* tfListener;
 #endif
 
   // Parameters
@@ -162,6 +177,9 @@ class K4AROSDevice
   k4abt::tracker k4abt_tracker_;
   std::atomic_int16_t k4abt_tracker_queue_size_;
   std::thread body_publisher_thread_;
+
+  std::vector<std::string> joint_names_{"Pelvis", "Spine_Naval", "Spine_Chest", "Neck", "Clavicle_left", "Shoulder_left", "Elbow_left", "Wrist_left", "Hand_left", "Handtip_left", "thumb_left", "Clavicle_right", "Shoulder_right", "Elbow_right", "Wrist_right", "Hand_right", "Handtip_right", "Thumb_right", "Hip_left", "Knee_left", "Ankle_left", "Foot_left", "Hip_right", "Knee_right", "Ankle_right", "Foot_right", "Head", "Nose", "Eye_Left", "Ear_Left", "Eye_Right", "Ear_Right"};
+  size_t num_bodies;
 #endif
 
   std::chrono::nanoseconds device_to_realtime_offset_{0};

launch/driver_with_bodytracking.launch

@@ -0,0 +1,10 @@
+<!--
+Copyright (c) Microsoft Corporation. All rights reserved.
+Licensed under the MIT License.
+-->
+
+<launch>
+    <include file="$(find azure_kinect_ros_driver)/launch/driver.launch">
+        <arg name="body_tracking_enabled" value="true" />
+    </include>
+</launch>

package.xml

@@ -18,6 +18,7 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>image_transport</depend>
+  <depend>image_geometry</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
@@ -30,6 +31,7 @@
   <depend>K4A</depend>
 
   <exec_depend>rgbd_launch</exec_depend>
+  <depend>OpenCV</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />