Skip to content

use zero-copy for images #8

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jplapp wants to merge 6 commits into
base: iron
Choose a base branch
from
Open

use zero-copy for images #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3f0443b
use zero-copy for images
Nov 4, 2024
bc058b8
test
jplapp Jan 8, 2026
5046760
declare
jplapp Jan 8, 2026
e2713e7
threads
jplapp Jan 8, 2026
4496c67
Merge pull request #11 from logivations/autoreset
jplapp Jan 9, 2026
3eb0171
downsample
jplapp Apr 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 17 additions & 8 deletions include/azure_kinect_ros_driver/k4a_ros_device.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<sensor_msgs::msg::Image>& 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);

Expand All @@ -66,7 +66,7 @@ class K4AROSDevice : public rclcpp::Node

k4a_result_t getImuFrame(const k4a_imu_sample_t& capture, std::shared_ptr<sensor_msgs::msg::Imu>& imu_frame);

k4a_result_t getRbgFrame(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::Image>& 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<sensor_msgs::msg::CompressedImage>& jpeg_image);

k4a_result_t getIrFrame(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::Image>& ir_image);
Expand All @@ -82,8 +82,8 @@ class K4AROSDevice : public rclcpp::Node
#endif

private:
k4a_result_t renderBGRA32ToROS(std::shared_ptr<sensor_msgs::msg::Image>& rgb_frame, k4a::image& k4a_bgra_frame);
k4a_result_t renderDepthToROS(std::shared_ptr<sensor_msgs::msg::Image>& 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<sensor_msgs::msg::Image>& ir_image, k4a::image& k4a_ir_frame);

k4a_result_t fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud);
Expand All @@ -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);

Expand All @@ -121,17 +124,17 @@ class K4AROSDevice : public rclcpp::Node
void printTimestampDebugMessage(const std::string& name, const rclcpp::Time& timestamp);


image_transport::Publisher rgb_raw_publisher_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb_raw_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr rgb_jpeg_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgb_raw_camerainfo_publisher_;

image_transport::Publisher depth_raw_publisher_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth_raw_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depth_raw_camerainfo_publisher_;

image_transport::Publisher depth_rect_publisher_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth_rect_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depth_rect_camerainfo_publisher_;

image_transport::Publisher rgb_rect_publisher_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb_rect_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgb_rect_camerainfo_publisher_;

image_transport::Publisher ir_raw_publisher_;
Expand Down Expand Up @@ -171,8 +174,14 @@ 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
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_;

Expand Down
13 changes: 12 additions & 1 deletion include/azure_kinect_ros_driver/k4a_ros_device_params.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,18 @@
int, 0) \
LIST_ENTRY(subordinate_delay_off_master_usec, \
"Delay subordinate camera off master camera by specified amount in usec.", \
int, 0)
int, 0) \
LIST_ENTRY(reset_device_interval, \
"Interval in seconds to periodically reset and recreate the device. Set to 0 to disable.", \
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
{
Expand Down
10 changes: 10 additions & 0 deletions launch/driver.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down Expand Up @@ -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:
Expand Down
Loading