Support humble (#257)

* fix declare_parameter

* add angles to Cmakelist

* Warning fixes

* remove obsolete parameters

* remove set parameter

* add joint_state_publisher

Co-authored-by: John Doe <[email protected]>
Co-authored-by: amr17 <[email protected]>

Author: 3 people

CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
+find_package(angles REQUIRED)
 
 ###########
 ## Build ##
@@ -53,7 +54,8 @@ ament_target_dependencies(${PROJECT_NAME}_node rclcpp
   tf2
   tf2_ros
   tf2_geometry_msgs
-  cv_bridge)
+  cv_bridge
+  angles)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the

include/azure_kinect_ros_driver/k4a_ros_device.h

@@ -165,7 +165,7 @@ class K4AROSDevice : public rclcpp::Node
   volatile bool running_;
 
   // Last capture timestamp for synchronizing playback capture and imu thread
-  std::atomic_int64_t last_capture_time_usec_;
+  std::atomic_uint64_t last_capture_time_usec_;
 
   // Last imu timestamp for synchronizing playback capture and imu thread
   std::atomic_uint64_t last_imu_time_usec_;

launch/rectify_test.launch

@@ -91,7 +91,6 @@ Licensed under the MIT License.
       <param name="fps"               value="5" />
       <param name="point_cloud"       value="true" />
       <param name="rgb_point_cloud"   value="true" />
-      <param name="required"          value="true" />
     </node>
 
   </group>

launch/slam_rtabmap.launch

@@ -45,7 +45,6 @@ Licensed under the MIT License.
       <param name="fps"               value="30" />
       <param name="point_cloud"       value="false" />
       <param name="rgb_point_cloud"   value="false" />
-      <param name="required"          value="true" />
       <param name="imu_rate_target"   value="100" />
 
     </node>

package.xml

@@ -18,6 +18,7 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>visualization_msgs</depend>
+  <depend>joint_state_publisher</depend>
   <depend>image_transport</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

src/k4a_calibration_transform_data.cpp

@@ -15,7 +15,7 @@
 #include <sensor_msgs/distortion_models.hpp>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/convert.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 // Project headers
 //

src/k4a_ros_device.cpp

@@ -58,30 +58,25 @@ K4AROSDevice::K4AROSDevice()
   static const std::string compressed_png_level = "/compressed/png_level";
 
   // Declare node parameters
-  this->declare_parameter("depth_enabled");
-  this->declare_parameter("depth_mode");
-  this->declare_parameter("color_enabled");
-  this->declare_parameter("color_format");
-  this->declare_parameter("color_resolution");
-  this->declare_parameter("fps");
-  this->declare_parameter("point_cloud");
-  this->declare_parameter("rgb_point_cloud");
-  this->declare_parameter("point_cloud_in_depth_frame");
-  this->declare_parameter("required");
-  this->declare_parameter("sensor_sn");
-  this->declare_parameter("recording_file");
-  this->declare_parameter("recording_loop_enabled");
-  this->declare_parameter("body_tracking_enabled");
-  this->declare_parameter("body_tracking_smoothing_factor");
-  this->declare_parameter("rescale_ir_to_mono8");
-  this->declare_parameter("ir_mono8_scaling_factor");
-  this->declare_parameter("imu_rate_target");
-  this->declare_parameter("wired_sync_mode");
-  this->declare_parameter("subordinate_delay_off_master_usec");
-  this->declare_parameter({depth_raw_topic + compressed_format});
-  this->declare_parameter({depth_raw_topic + compressed_png_level});
-  this->declare_parameter({depth_rect_topic + compressed_format});
-  this->declare_parameter({depth_rect_topic + compressed_png_level});
+  this->declare_parameter("depth_enabled", rclcpp::ParameterValue(true));
+  this->declare_parameter("depth_mode", rclcpp::ParameterValue("NFOV_UNBINNED"));
+  this->declare_parameter("color_enabled", rclcpp::ParameterValue(false));
+  this->declare_parameter("color_format", rclcpp::ParameterValue("bgra"));
+  this->declare_parameter("color_resolution", rclcpp::ParameterValue("720P"));
+  this->declare_parameter("fps", rclcpp::ParameterValue(5));
+  this->declare_parameter("point_cloud", rclcpp::ParameterValue(true));
+  this->declare_parameter("rgb_point_cloud", rclcpp::ParameterValue(false));
+  this->declare_parameter("point_cloud_in_depth_frame", rclcpp::ParameterValue(true));
+  this->declare_parameter("sensor_sn", rclcpp::ParameterValue(""));
+  this->declare_parameter("recording_file", rclcpp::ParameterValue(""));
+  this->declare_parameter("recording_loop_enabled", rclcpp::ParameterValue(false));
+  this->declare_parameter("body_tracking_enabled", rclcpp::ParameterValue(false));
+  this->declare_parameter("body_tracking_smoothing_factor", rclcpp::ParameterValue(0.0f));
+  this->declare_parameter("rescale_ir_to_mono8", rclcpp::ParameterValue(false));
+  this->declare_parameter("ir_mono8_scaling_factor", rclcpp::ParameterValue(1.0f));
+  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));
 
   // 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) \
@@ -194,7 +189,7 @@ K4AROSDevice::K4AROSDevice()
       {
         device = k4a::device::open(i);
       }
-      catch (exception)
+      catch (exception const&)
       {
         RCLCPP_ERROR_STREAM(this->get_logger(),"Failed to open K4A device at index " << i);
         continue;
@@ -259,14 +254,6 @@ K4AROSDevice::K4AROSDevice()
   depth_raw_publisher_ = image_transport_->advertise("depth/image_raw", 1, true);
   depth_raw_camerainfo_publisher_ = this->create_publisher<CameraInfo>("depth/camera_info", 1);
 
-  if (params_.depth_unit == sensor_msgs::image_encodings::TYPE_16UC1) {
-    // set lowest PNG compression for maximum FPS
-    this->set_parameter({depth_raw_topic + compressed_format, "png"});
-    this->set_parameter({depth_raw_topic + compressed_png_level, 1});
-    this->set_parameter({depth_rect_topic + compressed_format, "png"});
-    this->set_parameter({depth_rect_topic + compressed_png_level, 1});
-  }
-
   depth_raw_publisher_ = image_transport_->advertise(depth_raw_topic, 1, true);
   depth_raw_camerainfo_publisher_ = this->create_publisher<CameraInfo>("depth/camera_info", 1);
 
@@ -863,7 +850,6 @@ void K4AROSDevice::framePublisherThread()
 {
   rclcpp::Rate loop_rate(params_.fps);
 
-  k4a_wait_result_t wait_result;
   k4a_result_t result;
 
   CameraInfo rgb_raw_camera_info;