From 3f0443bfebf2aab7bef36414ae6c5403edf1e0dd Mon Sep 17 00:00:00 2001 From: Testing Date: Mon, 4 Nov 2024 21:27:38 +0100 Subject: [PATCH 1/5] use zero-copy for images --- .../azure_kinect_ros_driver/k4a_ros_device.h | 16 +++--- src/k4a_ros_device.cpp | 57 +++++++++---------- 2 files changed, 36 insertions(+), 37 deletions(-) diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h index 44f0cfcd..51eb302e 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device.h @@ -57,7 +57,7 @@ class K4AROSDevice : public rclcpp::Node void getRgbCameraInfo(sensor_msgs::msg::CameraInfo& camera_info); - k4a_result_t getDepthFrame(const k4a::capture& capture, std::shared_ptr& depth_frame, bool rectified); + k4a_result_t getDepthFrame(const k4a::capture& capture, sensor_msgs::msg::Image::UniquePtr& depth_frame, bool rectified); k4a_result_t getPointCloud(const k4a::capture& capture, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); @@ -66,7 +66,7 @@ class K4AROSDevice : public rclcpp::Node k4a_result_t getImuFrame(const k4a_imu_sample_t& capture, std::shared_ptr& imu_frame); - k4a_result_t getRbgFrame(const k4a::capture& capture, std::shared_ptr& rgb_frame, bool rectified); + k4a_result_t getRbgFrame(const k4a::capture& capture, sensor_msgs::msg::Image::UniquePtr& rgb_frame, bool rectified); k4a_result_t getJpegRgbFrame(const k4a::capture& capture, std::shared_ptr& jpeg_image); k4a_result_t getIrFrame(const k4a::capture& capture, std::shared_ptr& ir_image); @@ -82,8 +82,8 @@ class K4AROSDevice : public rclcpp::Node #endif private: - k4a_result_t renderBGRA32ToROS(std::shared_ptr& rgb_frame, k4a::image& k4a_bgra_frame); - k4a_result_t renderDepthToROS(std::shared_ptr& depth_image, k4a::image& k4a_depth_frame); + k4a_result_t renderBGRA32ToROS(sensor_msgs::msg::Image::UniquePtr& rgb_image, k4a::image& k4a_bgra_frame); + k4a_result_t renderDepthToROS(sensor_msgs::msg::Image::UniquePtr& depth_image, k4a::image& k4a_depth_frame); k4a_result_t renderIrToROS(std::shared_ptr& ir_image, k4a::image& k4a_ir_frame); k4a_result_t fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); @@ -121,17 +121,17 @@ class K4AROSDevice : public rclcpp::Node void printTimestampDebugMessage(const std::string& name, const rclcpp::Time& timestamp); - image_transport::Publisher rgb_raw_publisher_; + rclcpp::Publisher::SharedPtr rgb_raw_publisher_; rclcpp::Publisher::SharedPtr rgb_jpeg_publisher_; rclcpp::Publisher::SharedPtr rgb_raw_camerainfo_publisher_; - image_transport::Publisher depth_raw_publisher_; + rclcpp::Publisher::SharedPtr depth_raw_publisher_; rclcpp::Publisher::SharedPtr depth_raw_camerainfo_publisher_; - image_transport::Publisher depth_rect_publisher_; + rclcpp::Publisher::SharedPtr depth_rect_publisher_; rclcpp::Publisher::SharedPtr depth_rect_camerainfo_publisher_; - image_transport::Publisher rgb_rect_publisher_; + rclcpp::Publisher::SharedPtr rgb_rect_publisher_; rclcpp::Publisher::SharedPtr rgb_rect_camerainfo_publisher_; image_transport::Publisher ir_raw_publisher_; diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 54236c1a..7c796724 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -238,6 +238,10 @@ K4AROSDevice::K4AROSDevice(const rclcpp::NodeOptions & options) } // Register our topics + rclcpp::QoS custom_qos(KeepLast(1), rmw_qos_profile_sensor_data); + rclcpp::PublisherOptions sub_options; + sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + if (params_.color_format == "jpeg") { // JPEG images are directly published on 'rgb/image_raw/compressed' so that @@ -248,20 +252,17 @@ K4AROSDevice::K4AROSDevice(const rclcpp::NodeOptions & options) } else if (params_.color_format == "bgra") { - rgb_raw_publisher_ = image_transport_->advertise("rgb/image_raw", 1, true); + rgb_raw_publisher_ = this->create_publisher("rgb/image_raw", custom_qos, sub_options); } rgb_raw_camerainfo_publisher_ = this->create_publisher("rgb/camera_info", 1); - depth_raw_publisher_ = image_transport_->advertise("depth/image_raw", 1, true); - depth_raw_camerainfo_publisher_ = this->create_publisher("depth/camera_info", 1); - - depth_raw_publisher_ = image_transport_->advertise(depth_raw_topic, 1, true); + depth_raw_publisher_ = this->create_publisher(depth_raw_topic, custom_qos, sub_options); depth_raw_camerainfo_publisher_ = this->create_publisher("depth/camera_info", 1); - depth_rect_publisher_ = image_transport_->advertise(depth_rect_topic, 1, true); + depth_rect_publisher_ = this->create_publisher(depth_rect_topic, custom_qos, sub_options); depth_rect_camerainfo_publisher_ = this->create_publisher("depth_to_rgb/camera_info", 1); - rgb_rect_publisher_ = image_transport_->advertise("rgb_to_depth/image_raw", 1, true); + rgb_rect_publisher_ = this->create_publisher("rgb_to_depth/image_raw", custom_qos, sub_options); rgb_rect_camerainfo_publisher_ = this->create_publisher("rgb_to_depth/camera_info", 1); ir_raw_publisher_ = image_transport_->advertise("ir/image_raw", 1, true); @@ -270,10 +271,7 @@ K4AROSDevice::K4AROSDevice(const rclcpp::NodeOptions & options) imu_orientation_publisher_ = this->create_publisher("imu", 200); if (params_.point_cloud || params_.rgb_point_cloud) { - rclcpp::QoS custom_qos(KeepLast(1), rmw_qos_profile_sensor_data); - rclcpp::PublisherOptions options; - options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - pointcloud_publisher_ = this->create_publisher("points2", custom_qos, options); + pointcloud_publisher_ = this->create_publisher("points2", custom_qos, sub_options); } #if defined(K4A_BODY_TRACKING) @@ -475,7 +473,7 @@ void K4AROSDevice::stopImu() } } -k4a_result_t K4AROSDevice::getDepthFrame(const k4a::capture& capture, std::shared_ptr& depth_image, +k4a_result_t K4AROSDevice::getDepthFrame(const k4a::capture& capture, sensor_msgs::msg::Image::UniquePtr& depth_image, bool rectified = false) { k4a::image k4a_depth_frame = capture.get_depth_image(); @@ -497,7 +495,7 @@ k4a_result_t K4AROSDevice::getDepthFrame(const k4a::capture& capture, std::share return renderDepthToROS(depth_image, k4a_depth_frame); } -k4a_result_t K4AROSDevice::renderDepthToROS(std::shared_ptr& depth_image, k4a::image& k4a_depth_frame) +k4a_result_t K4AROSDevice::renderDepthToROS(sensor_msgs::msg::Image::UniquePtr& depth_image, k4a::image& k4a_depth_frame) { cv::Mat depth_frame_buffer_mat(k4a_depth_frame.get_height_pixels(), k4a_depth_frame.get_width_pixels(), CV_16UC1, k4a_depth_frame.get_buffer()); @@ -517,9 +515,9 @@ k4a_result_t K4AROSDevice::renderDepthToROS(std::shared_ptr( + *cv_bridge::CvImage(std_msgs::msg::Header(), encoding, depth_frame_buffer_mat).toImageMsg() + ); return K4A_RESULT_SUCCEEDED; } @@ -574,7 +572,7 @@ k4a_result_t K4AROSDevice::getJpegRgbFrame(const k4a::capture& capture, std::sha return K4A_RESULT_SUCCEEDED; } -k4a_result_t K4AROSDevice::getRbgFrame(const k4a::capture& capture, std::shared_ptr& rgb_image, +k4a_result_t K4AROSDevice::getRbgFrame(const k4a::capture& capture, sensor_msgs::msg::Image::UniquePtr& rgb_image, bool rectified = false) { k4a::image k4a_bgra_frame = capture.get_color_image(); @@ -607,18 +605,19 @@ k4a_result_t K4AROSDevice::getRbgFrame(const k4a::capture& capture, std::shared_ return renderBGRA32ToROS(rgb_image, k4a_bgra_frame); } -// Helper function that renders any BGRA K4A frame to a ROS ImagePtr. Useful for rendering intermediary frames -// during debugging of image processing functions -k4a_result_t K4AROSDevice::renderBGRA32ToROS(std::shared_ptr& rgb_image, k4a::image& k4a_bgra_frame) +k4a_result_t K4AROSDevice::renderBGRA32ToROS(sensor_msgs::msg::Image::UniquePtr& rgb_image, k4a::image& k4a_bgra_frame) { cv::Mat rgb_buffer_mat(k4a_bgra_frame.get_height_pixels(), k4a_bgra_frame.get_width_pixels(), CV_8UC4, k4a_bgra_frame.get_buffer()); - rgb_image = cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::BGRA8, rgb_buffer_mat).toImageMsg(); + rgb_image = std::make_unique( + *cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::BGRA8, rgb_buffer_mat).toImageMsg() + ); return K4A_RESULT_SUCCEEDED; } + k4a_result_t K4AROSDevice::getRgbPointCloudInDepthFrame(const k4a::capture& capture, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { @@ -994,10 +993,10 @@ void K4AROSDevice::framePublisherThread() } CompressedImage::SharedPtr rgb_jpeg_frame(new CompressedImage); - Image::SharedPtr rgb_raw_frame(new Image); - Image::SharedPtr rgb_rect_frame(new Image); - Image::SharedPtr depth_raw_frame(new Image); - Image::SharedPtr depth_rect_frame(new Image); + Image::UniquePtr rgb_raw_frame = std::make_unique(); + Image::UniquePtr rgb_rect_frame = std::make_unique(); + Image::UniquePtr depth_raw_frame = std::make_unique(); + Image::UniquePtr depth_rect_frame = std::make_unique(); Image::SharedPtr ir_raw_frame(new Image); PointCloud2::UniquePtr point_cloud = std::make_unique(); @@ -1060,7 +1059,7 @@ void K4AROSDevice::framePublisherThread() depth_raw_frame->header.stamp = capture_time; depth_raw_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_; - depth_raw_publisher_.publish(depth_raw_frame); + depth_raw_publisher_->publish(std::move(depth_raw_frame)); depth_raw_camerainfo_publisher_->publish(depth_raw_camera_info); } } @@ -1089,7 +1088,7 @@ void K4AROSDevice::framePublisherThread() depth_rect_frame->header.stamp = capture_time; depth_rect_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.rgb_camera_frame_; - depth_rect_publisher_.publish(depth_rect_frame); + depth_rect_publisher_->publish(std::move(depth_rect_frame)); // Re-synchronize the header timestamps since we cache the camera calibration message depth_rect_camera_info.header.stamp = capture_time; @@ -1164,7 +1163,7 @@ void K4AROSDevice::framePublisherThread() rgb_raw_frame->header.stamp = capture_time; rgb_raw_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.rgb_camera_frame_; - rgb_raw_publisher_.publish(rgb_raw_frame); + rgb_raw_publisher_->publish(std::move(rgb_raw_frame)); // Re-synchronize the header timestamps since we cache the camera calibration message rgb_raw_camera_info.header.stamp = capture_time; @@ -1192,7 +1191,7 @@ void K4AROSDevice::framePublisherThread() rgb_rect_frame->header.stamp = capture_time; rgb_rect_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_; - rgb_rect_publisher_.publish(rgb_rect_frame); + rgb_rect_publisher_->publish(std::move(rgb_rect_frame)); // Re-synchronize the header timestamps since we cache the camera calibration message rgb_rect_camera_info.header.stamp = capture_time; From bc058b8983375f8278a3025649b2c3f5c7043463 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Thu, 8 Jan 2026 16:20:31 +0100 Subject: [PATCH 2/5] test --- .../azure_kinect_ros_driver/k4a_ros_device.h | 6 + .../k4a_ros_device_params.h | 3 + src/k4a_ros_device.cpp | 169 +++++++++++++++++- 3 files changed, 175 insertions(+), 3 deletions(-) diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h index 51eb302e..65ab6e6c 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device.h @@ -97,6 +97,9 @@ class K4AROSDevice : public rclcpp::Node #endif void imuPublisherThread(); + // Recreate the K4A device after poll failures + bool recreateDevice(); + // Gets a timestap from one of the captures images std::chrono::microseconds getCaptureTimestamp(const k4a::capture& capture); @@ -173,6 +176,9 @@ class K4AROSDevice : public rclcpp::Node int count_not_get_capture_{0}; + // Timestamp of last device reset for periodic reset feature + std::chrono::steady_clock::time_point last_device_reset_time_; + // Last capture timestamp for synchronizing playback capture and imu thread std::atomic_uint64_t last_capture_time_usec_; diff --git a/include/azure_kinect_ros_driver/k4a_ros_device_params.h b/include/azure_kinect_ros_driver/k4a_ros_device_params.h index a074be9a..044d51be 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device_params.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device_params.h @@ -80,6 +80,9 @@ int, 0) \ LIST_ENTRY(subordinate_delay_off_master_usec, \ "Delay subordinate camera off master camera by specified amount in usec.", \ + int, 0) \ + LIST_ENTRY(reset_device_interval, \ + "Interval in seconds to periodically reset and recreate the device. Set to 0 to disable.", \ int, 0) class K4AROSDeviceParams : public rclcpp::Node diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 7c796724..0abb65bb 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -10,6 +10,7 @@ #include #include #include +#include // Library headers // @@ -428,6 +429,9 @@ k4a_result_t K4AROSDevice::startCameras() // Prevent the worker thread from exiting immediately running_ = true; + // Initialize the last device reset time for periodic reset feature + last_device_reset_time_ = std::chrono::steady_clock::now(); + // Start the thread that will update diagnostics update_diagnostics_thread_ = thread(&K4AROSDevice::startDiagnosticsUpdaterThread, this); @@ -473,6 +477,128 @@ void K4AROSDevice::stopImu() } } +bool K4AROSDevice::recreateDevice() +{ + RCLCPP_WARN(this->get_logger(), "Attempting to recreate K4A device..."); + + // Stop and close the current device + try + { + if (k4a_device_) + { + RCLCPP_INFO(this->get_logger(), "Stopping cameras on current device..."); + k4a_device_.stop_cameras(); + k4a_device_.stop_imu(); + k4a_device_.close(); + k4a_device_ = nullptr; + } + } + catch (const std::exception& e) + { + RCLCPP_WARN(this->get_logger(), "Exception while closing device: %s", e.what()); + } + + // Reset the device using AzureKinectFirmwareTool + RCLCPP_INFO(this->get_logger(), "Resetting device using AzureKinectFirmwareTool..."); + int reset_result = std::system("AzureKinectFirmwareTool -Reset"); + if (reset_result == 0) + { + RCLCPP_INFO(this->get_logger(), "Device reset successful"); + } + else + { + RCLCPP_WARN(this->get_logger(), "Device reset returned %d, continuing with device recreation...", reset_result); + } + + // Wait for the device to re-enumerate after reset + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Try to reopen the device + uint32_t k4a_device_count = k4a::device::get_installed_count(); + if (k4a_device_count == 0) + { + RCLCPP_ERROR(this->get_logger(), "No K4A devices found during recreation"); + return false; + } + + for (uint32_t i = 0; i < k4a_device_count; i++) + { + try + { + k4a::device device = k4a::device::open(i); + + // Match by serial number if specified + if (params_.sensor_sn != "") + { + if (device.get_serialnum() == params_.sensor_sn) + { + k4a_device_ = std::move(device); + break; + } + } + else if (i == 0) + { + k4a_device_ = std::move(device); + break; + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR(this->get_logger(), "Failed to open K4A device at index %d: %s", i, e.what()); + continue; + } + } + + if (!k4a_device_) + { + RCLCPP_ERROR(this->get_logger(), "Failed to reopen K4A device"); + return false; + } + + RCLCPP_INFO(this->get_logger(), "K4A device reopened: %s", k4a_device_.get_serialnum().c_str()); + + // Restart the cameras with the same configuration + k4a_device_configuration_t k4a_configuration = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL; + k4a_result_t result = params_.GetDeviceConfig(&k4a_configuration); + if (result != K4A_RESULT_SUCCEEDED) + { + RCLCPP_ERROR(this->get_logger(), "Failed to generate device configuration during recreation"); + return false; + } + + // Reinitialize calibration data + calibration_data_.initialize(k4a_device_, k4a_configuration.depth_mode, k4a_configuration.color_resolution, params_); + + // Start cameras + try + { + k4a_device_.start_cameras(&k4a_configuration); + RCLCPP_INFO(this->get_logger(), "Cameras restarted successfully"); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(this->get_logger(), "Failed to restart cameras: %s", e.what()); + return false; + } + + // Restart IMU if it was enabled + if (params_.imu_rate_target > 0) + { + try + { + k4a_device_.start_imu(); + RCLCPP_INFO(this->get_logger(), "IMU restarted successfully"); + } + catch (const std::exception& e) + { + RCLCPP_WARN(this->get_logger(), "Failed to restart IMU: %s", e.what()); + } + } + + RCLCPP_INFO(this->get_logger(), "K4A device recreation completed successfully"); + return true; +} + k4a_result_t K4AROSDevice::getDepthFrame(const k4a::capture& capture, sensor_msgs::msg::Image::UniquePtr& depth_image, bool rectified = false) { @@ -941,13 +1067,50 @@ void K4AROSDevice::framePublisherThread() { if (k4a_device_) { + // Check for periodic device reset + if (params_.reset_device_interval > 0) + { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - last_device_reset_time_).count(); + if (elapsed >= params_.reset_device_interval) + { + RCLCPP_INFO(this->get_logger(), "Periodic device reset triggered after %ld seconds", elapsed); + if (recreateDevice()) + { + RCLCPP_INFO(this->get_logger(), "Periodic device reset completed successfully"); + last_device_reset_time_ = std::chrono::steady_clock::now(); + count_not_get_capture_ = 0; + waitTime = firstFrameWaitTime; + continue; // Continue to next iteration with new device + } + else + { + RCLCPP_FATAL(this->get_logger(), "Periodic device reset failed: aborting..."); + rclcpp::shutdown(); + return; + } + } + } + while (!k4a_device_.get_capture(&capture, waitTime)) { if (count_not_get_capture_ > 10) { - RCLCPP_FATAL(this->get_logger(),"Failed to poll cameras: aborting..."); - rclcpp::shutdown(); - return; + RCLCPP_WARN(this->get_logger(), "Failed to poll cameras after %d attempts, attempting to recreate device...", count_not_get_capture_); + if (recreateDevice()) + { + RCLCPP_INFO(this->get_logger(), "Device recreated successfully, resuming capture..."); + last_device_reset_time_ = std::chrono::steady_clock::now(); + count_not_get_capture_ = 0; + waitTime = firstFrameWaitTime; + break; // Break inner while to retry capture with new device + } + else + { + RCLCPP_FATAL(this->get_logger(), "Failed to recreate device: aborting..."); + rclcpp::shutdown(); + return; + } } RCLCPP_ERROR(this->get_logger(),"Failed to poll cameras: trying again..."); count_not_get_capture_++; From 504676026f50edee0b52134e3e59fa99d09c34c6 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Thu, 8 Jan 2026 16:32:38 +0100 Subject: [PATCH 3/5] declare --- src/k4a_ros_device.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 0abb65bb..47cb8541 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -79,6 +79,7 @@ K4AROSDevice::K4AROSDevice(const rclcpp::NodeOptions & options) 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)); + this->declare_parameter("reset_device_interval", 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) \ From e2713e733705ebb0ec04f939bc3a2b7cb6fec93c Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Thu, 8 Jan 2026 16:37:43 +0100 Subject: [PATCH 4/5] threads --- .../azure_kinect_ros_driver/k4a_ros_device.h | 3 ++ src/k4a_ros_device.cpp | 40 ++++++++++++++----- 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h index 65ab6e6c..c0e48827 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device.h @@ -174,6 +174,9 @@ class K4AROSDevice : public rclcpp::Node // Thread control volatile bool running_; + // Flag to signal device recreation is in progress (other threads should pause) + std::atomic_bool device_recreating_{false}; + int count_not_get_capture_{0}; // Timestamp of last device reset for periodic reset feature diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 47cb8541..89213216 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -482,6 +482,12 @@ bool K4AROSDevice::recreateDevice() { RCLCPP_WARN(this->get_logger(), "Attempting to recreate K4A device..."); + // Signal other threads to pause device access + device_recreating_ = true; + + // Give other threads time to see the flag and stop accessing the device + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Stop and close the current device try { @@ -519,6 +525,7 @@ bool K4AROSDevice::recreateDevice() if (k4a_device_count == 0) { RCLCPP_ERROR(this->get_logger(), "No K4A devices found during recreation"); + device_recreating_ = false; return false; } @@ -553,6 +560,7 @@ bool K4AROSDevice::recreateDevice() if (!k4a_device_) { RCLCPP_ERROR(this->get_logger(), "Failed to reopen K4A device"); + device_recreating_ = false; return false; } @@ -564,6 +572,7 @@ bool K4AROSDevice::recreateDevice() if (result != K4A_RESULT_SUCCEEDED) { RCLCPP_ERROR(this->get_logger(), "Failed to generate device configuration during recreation"); + device_recreating_ = false; return false; } @@ -579,24 +588,26 @@ bool K4AROSDevice::recreateDevice() catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "Failed to restart cameras: %s", e.what()); + device_recreating_ = false; return false; } - // Restart IMU if it was enabled - if (params_.imu_rate_target > 0) + // Restart IMU + try { - try - { - k4a_device_.start_imu(); - RCLCPP_INFO(this->get_logger(), "IMU restarted successfully"); - } - catch (const std::exception& e) - { - RCLCPP_WARN(this->get_logger(), "Failed to restart IMU: %s", e.what()); - } + k4a_device_.start_imu(); + RCLCPP_INFO(this->get_logger(), "IMU restarted successfully"); + } + catch (const std::exception& e) + { + RCLCPP_WARN(this->get_logger(), "Failed to restart IMU: %s", e.what()); } RCLCPP_INFO(this->get_logger(), "K4A device recreation completed successfully"); + + // Signal other threads that device is ready + device_recreating_ = false; + return true; } @@ -1518,6 +1529,13 @@ void K4AROSDevice::imuPublisherThread() while (running_ && rclcpp::ok()) { + // Skip device access during recreation + if (device_recreating_) + { + loop_rate.sleep(); + continue; + } + if (k4a_device_) { // IMU messages are delivered in batches at 300 Hz. Drain the queue of IMU messages by From 3eb0171180498be7808b8fc6c721cb81720ebe4e Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Tue, 14 Apr 2026 22:45:32 +0200 Subject: [PATCH 5/5] downsample --- .../k4a_ros_device_params.h | 10 +- launch/driver.launch.py | 10 ++ src/k4a_ros_device.cpp | 116 +++++++++++------- 3 files changed, 93 insertions(+), 43 deletions(-) diff --git a/include/azure_kinect_ros_driver/k4a_ros_device_params.h b/include/azure_kinect_ros_driver/k4a_ros_device_params.h index 044d51be..0abf8e36 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device_params.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device_params.h @@ -83,7 +83,15 @@ int, 0) \ LIST_ENTRY(reset_device_interval, \ "Interval in seconds to periodically reset and recreate the device. Set to 0 to disable.", \ - int, 0) + int, 0) \ + LIST_ENTRY(point_cloud_downsample_factor, \ + "Downsample factor for the point cloud. 1 = full resolution, 2 = half in each dimension (1/4 points), " \ + "etc.", \ + int, 1) \ + LIST_ENTRY(point_cloud_max_range, \ + "Maximum range in meters for the point cloud. Points beyond this distance are dropped. " \ + "Set to 0.0 to disable.", \ + float, 0.0f) class K4AROSDeviceParams : public rclcpp::Node { diff --git a/launch/driver.launch.py b/launch/driver.launch.py index 711b4cf3..1c867fa8 100644 --- a/launch/driver.launch.py +++ b/launch/driver.launch.py @@ -129,6 +129,14 @@ def generate_launch_description(): 'imu_rate_target', default_value="0", description="Desired output rate of IMU messages. Set to 0 (default) for full rate (1.6 kHz)."), + DeclareLaunchArgument( + 'point_cloud_downsample_factor', + default_value="1", + description="Downsample factor for the point cloud. 1 = full resolution, 2 = half in each dimension (1/4 points), etc."), + DeclareLaunchArgument( + 'point_cloud_max_range', + default_value="0.0", + description="Maximum range in meters for the point cloud. Points beyond this distance are dropped. Set to 0.0 to disable."), DeclareLaunchArgument( 'wired_sync_mode', default_value="0", @@ -160,6 +168,8 @@ def generate_launch_description(): {'rescale_ir_to_mono8': launch.substitutions.LaunchConfiguration('rescale_ir_to_mono8')}, {'ir_mono8_scaling_factor': launch.substitutions.LaunchConfiguration('ir_mono8_scaling_factor')}, {'imu_rate_target': launch.substitutions.LaunchConfiguration('imu_rate_target')}, + {'point_cloud_downsample_factor': launch.substitutions.LaunchConfiguration('point_cloud_downsample_factor')}, + {'point_cloud_max_range': launch.substitutions.LaunchConfiguration('point_cloud_max_range')}, {'wired_sync_mode': launch.substitutions.LaunchConfiguration('wired_sync_mode')}, {'subordinate_delay_off_master_usec': launch.substitutions.LaunchConfiguration('subordinate_delay_off_master_usec')}]), # If flag overwrite_robot_description is set: diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 89213216..d91b9746 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -80,6 +80,8 @@ K4AROSDevice::K4AROSDevice(const rclcpp::NodeOptions & options) this->declare_parameter("wired_sync_mode", rclcpp::ParameterValue(0)); this->declare_parameter("subordinate_delay_off_master_usec", rclcpp::ParameterValue(0)); this->declare_parameter("reset_device_interval", rclcpp::ParameterValue(0)); + this->declare_parameter("point_cloud_downsample_factor", rclcpp::ParameterValue(1)); + this->declare_parameter("point_cloud_max_range", rclcpp::ParameterValue(0.0f)); // 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) \ @@ -842,12 +844,10 @@ k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, sensor_msg k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_image, const k4a::image& color_image, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { - point_cloud->height = pointcloud_image.get_height_pixels(); - point_cloud->width = pointcloud_image.get_width_pixels(); - point_cloud->is_dense = false; - point_cloud->is_bigendian = false; + const int src_width = pointcloud_image.get_width_pixels(); + const int src_height = pointcloud_image.get_height_pixels(); - const size_t point_count = pointcloud_image.get_height_pixels() * pointcloud_image.get_width_pixels(); + const size_t point_count = static_cast(src_width) * src_height; const size_t pixel_count = color_image.get_size() / sizeof(BgraPixel); if (point_count != pixel_count) { @@ -855,6 +855,20 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag return K4A_RESULT_FAILED; } + const int ds = std::max(1, params_.point_cloud_downsample_factor); + const int new_width = src_width / ds; + const int new_height = src_height / ds; + + // Max range: compare in millimeters against the raw int16 buffer to avoid unnecessary float conversion + const int16_t max_range_mm = (params_.point_cloud_max_range > 0.0f) + ? static_cast(std::min(params_.point_cloud_max_range * 1000.0f, static_cast(INT16_MAX))) + : 0; + + point_cloud->height = new_height; + point_cloud->width = new_width; + point_cloud->is_dense = false; + point_cloud->is_bigendian = false; + sensor_msgs::PointCloud2Modifier pcd_modifier(*point_cloud); pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); @@ -866,32 +880,36 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag sensor_msgs::PointCloud2Iterator iter_g(*point_cloud, "g"); sensor_msgs::PointCloud2Iterator iter_b(*point_cloud, "b"); - pcd_modifier.resize(point_count); + pcd_modifier.resize(static_cast(new_width) * new_height); const int16_t* point_cloud_buffer = reinterpret_cast(pointcloud_image.get_buffer()); const uint8_t* color_buffer = color_image.get_buffer(); - for (size_t i = 0; i < point_count; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) + for (int row = 0; row < new_height; row++) { - // Z in image frame: - float z = static_cast(point_cloud_buffer[3 * i + 2]); - // Alpha value: - uint8_t a = color_buffer[4 * i + 3]; - if (z <= 0.0f || a == 0) + for (int col = 0; col < new_width; col++, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) { - *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); - *iter_r = *iter_g = *iter_b = 0; - } - else - { - constexpr float kMillimeterToMeter = 1.0 / 1000.0f; - *iter_x = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 0]); - *iter_y = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 1]); - *iter_z = kMillimeterToMeter * z; - - *iter_r = color_buffer[4 * i + 2]; - *iter_g = color_buffer[4 * i + 1]; - *iter_b = color_buffer[4 * i + 0]; + const size_t i = static_cast(row * ds) * src_width + col * ds; + // Z in image frame: + int16_t z_raw = point_cloud_buffer[3 * i + 2]; + // Alpha value: + uint8_t a = color_buffer[4 * i + 3]; + if (z_raw <= 0 || a == 0 || (max_range_mm > 0 && z_raw > max_range_mm)) + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); + *iter_r = *iter_g = *iter_b = 0; + } + else + { + constexpr float kMillimeterToMeter = 1.0 / 1000.0f; + *iter_x = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 0]); + *iter_y = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 1]); + *iter_z = kMillimeterToMeter * static_cast(z_raw); + + *iter_r = color_buffer[4 * i + 2]; + *iter_g = color_buffer[4 * i + 1]; + *iter_b = color_buffer[4 * i + 0]; + } } } @@ -900,13 +918,23 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { - point_cloud->height = pointcloud_image.get_height_pixels(); - point_cloud->width = pointcloud_image.get_width_pixels(); + const int src_width = pointcloud_image.get_width_pixels(); + const int src_height = pointcloud_image.get_height_pixels(); + + const int ds = std::max(1, params_.point_cloud_downsample_factor); + const int new_width = src_width / ds; + const int new_height = src_height / ds; + + // Max range: compare in millimeters against the raw int16 buffer to avoid unnecessary float conversion + const int16_t max_range_mm = (params_.point_cloud_max_range > 0.0f) + ? static_cast(std::min(params_.point_cloud_max_range * 1000.0f, static_cast(INT16_MAX))) + : 0; + + point_cloud->height = new_height; + point_cloud->width = new_width; point_cloud->is_dense = false; point_cloud->is_bigendian = false; - const size_t point_count = pointcloud_image.get_height_pixels() * pointcloud_image.get_width_pixels(); - sensor_msgs::PointCloud2Modifier pcd_modifier(*point_cloud); pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); @@ -914,24 +942,28 @@ k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, se sensor_msgs::PointCloud2Iterator iter_y(*point_cloud, "y"); sensor_msgs::PointCloud2Iterator iter_z(*point_cloud, "z"); - pcd_modifier.resize(point_count); + pcd_modifier.resize(static_cast(new_width) * new_height); const int16_t* point_cloud_buffer = reinterpret_cast(pointcloud_image.get_buffer()); - for (size_t i = 0; i < point_count; i++, ++iter_x, ++iter_y, ++iter_z) + for (int row = 0; row < new_height; row++) { - float z = static_cast(point_cloud_buffer[3 * i + 2]); - - if (z <= 0.0f) - { - *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); - } - else + for (int col = 0; col < new_width; col++, ++iter_x, ++iter_y, ++iter_z) { - constexpr float kMillimeterToMeter = 1.0 / 1000.0f; - *iter_x = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 0]); - *iter_y = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 1]); - *iter_z = kMillimeterToMeter * z; + const size_t i = static_cast(row * ds) * src_width + col * ds; + int16_t z_raw = point_cloud_buffer[3 * i + 2]; + + if (z_raw <= 0 || (max_range_mm > 0 && z_raw > max_range_mm)) + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); + } + else + { + constexpr float kMillimeterToMeter = 1.0 / 1000.0f; + *iter_x = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 0]); + *iter_y = kMillimeterToMeter * static_cast(point_cloud_buffer[3 * i + 1]); + *iter_z = kMillimeterToMeter * z_raw; + } } }