diff --git a/CMakeLists.txt b/CMakeLists.txt
index df0b2a74..8b991ec3 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h
index 60451349..2c2de2f4 100644
--- a/include/azure_kinect_ros_driver/k4a_ros_device.h
+++ b/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_;
diff --git a/launch/rectify_test.launch b/launch/rectify_test.launch
index e4a9df5f..ce181024 100644
--- a/launch/rectify_test.launch
+++ b/launch/rectify_test.launch
@@ -91,7 +91,6 @@ Licensed under the MIT License.
-
diff --git a/launch/slam_rtabmap.launch b/launch/slam_rtabmap.launch
index b70e2ce3..8a80cea7 100644
--- a/launch/slam_rtabmap.launch
+++ b/launch/slam_rtabmap.launch
@@ -45,7 +45,6 @@ Licensed under the MIT License.
-
diff --git a/package.xml b/package.xml
index 8900ccaa..38ebac06 100644
--- a/package.xml
+++ b/package.xml
@@ -18,6 +18,7 @@
std_msgs
sensor_msgs
visualization_msgs
+ joint_state_publisher
image_transport
tf2
tf2_ros
diff --git a/src/k4a_calibration_transform_data.cpp b/src/k4a_calibration_transform_data.cpp
index 5b3bdb26..b1bed412 100644
--- a/src/k4a_calibration_transform_data.cpp
+++ b/src/k4a_calibration_transform_data.cpp
@@ -15,7 +15,7 @@
#include
#include
#include
-#include
+#include
// Project headers
//
diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp
index d20a816d..b17884fd 100644
--- a/src/k4a_ros_device.cpp
+++ b/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("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("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;