Skip to content

Commit

Permalink
Merge pull request #4 from sbgisen/feature/diagnostic
Browse files Browse the repository at this point in the history 
Publish diagnostic
  • Loading branch information
@gakutasu
gakutasu authored Jan 16, 2025
2 parents 2d4b27f + ca9443f commit 87a0bdd
Show file tree
Hide file tree
Showing 4 changed files with 79 additions and 26 deletions.
4 changes: 3 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ find_package(tf2_geometry_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(angles REQUIRED)
find_package(diagnostic_updater REQUIRED)

###########
## Build ##
Expand All @@ -55,7 +56,8 @@ ament_target_dependencies(${PROJECT_NAME}_node rclcpp
tf2_ros
tf2_geometry_msgs
cv_bridge
angles)
angles
diagnostic_updater)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
Expand Down
18 changes: 13 additions & 5 deletions include/azure_kinect_ros_driver/k4a_ros_device.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@
#include <sensor_msgs/msg/temperature.hpp>
#include <k4a/k4a.hpp>
#include <k4arecord/playback.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>

#if defined(K4A_BODY_TRACKING)
#include <visualization_msgs/msg/marker_array.hpp>
Expand Down Expand Up @@ -61,7 +63,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 getRgbFrame(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::Image>& 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 Down Expand Up @@ -131,10 +133,6 @@ class K4AROSDevice : public rclcpp::Node
image_transport::Publisher ir_raw_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr ir_raw_camerainfo_publisher_;

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_orientation_publisher_;

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;

#if defined(K4A_BODY_TRACKING)
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr body_marker_publisher_;

Expand Down Expand Up @@ -174,6 +172,16 @@ class K4AROSDevice : public rclcpp::Node
// Threads
std::thread frame_publisher_thread_;
std::thread imu_publisher_thread_;

double frequency_;
diagnostic_updater::Updater updater_;
std::shared_ptr<diagnostic_updater::TopicDiagnostic> rgb_diagnostic_;
std::shared_ptr<diagnostic_updater::TopicDiagnostic> depth_diagnostic_;
std::shared_ptr<diagnostic_updater::TopicDiagnostic> ir_diagnostic_;
double imu_frequency_;
double fps_frequency_;
std::shared_ptr<diagnostic_updater::DiagnosedPublisher<sensor_msgs::msg::Imu>> diagnosed_imu_publisher_;
std::shared_ptr<diagnostic_updater::DiagnosedPublisher<sensor_msgs::msg::PointCloud2>> diagnosed_pointcloud_publisher_;
};

#endif // K4A_ROS_DEVICE_H
1 change: 1 addition & 0 deletions package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
<depend>angles</depend>
<depend>cv_bridge</depend>
<depend>K4A</depend>
<depend>diagnostic_updater</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
82 changes: 62 additions & 20 deletions src/k4a_ros_device.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,8 @@ K4AROSDevice::K4AROSDevice()
// clang-format on
last_capture_time_usec_(0),
last_imu_time_usec_(0),
imu_stream_end_of_file_(false)
imu_stream_end_of_file_(false),
updater_(this)
{
// Declare an image transport
auto image_transport_ = new image_transport::ImageTransport(static_cast<rclcpp::Node::SharedPtr>(this));
Expand Down Expand Up @@ -79,6 +80,7 @@ K4AROSDevice::K4AROSDevice()
this->declare_parameter("subordinate_delay_off_master_usec", rclcpp::ParameterValue(0));
this->declare_parameter("tf_prefix", rclcpp::ParameterValue(""));
this->declare_parameter("rgb_namespace", rclcpp::ParameterValue("rgb"));
this->declare_parameter<double>("diagnostic_tolerance", 0.1);

// 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) \
Expand Down Expand Up @@ -256,9 +258,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);

depth_raw_publisher_ = image_transport_->advertise(depth_raw_topic, 1, true);
depth_raw_camerainfo_publisher_ = this->create_publisher<CameraInfo>("depth/camera_info", 1);

depth_rect_publisher_ = image_transport_->advertise(depth_rect_topic, 1, true);
depth_rect_camerainfo_publisher_ = this->create_publisher<CameraInfo>("depth_to_rgb/camera_info", 1);

Expand All @@ -268,11 +267,42 @@ K4AROSDevice::K4AROSDevice()
ir_raw_publisher_ = image_transport_->advertise("ir/image_raw", 1, true);
ir_raw_camerainfo_publisher_ = this->create_publisher<CameraInfo>("ir/camera_info", 1);

imu_orientation_publisher_ = this->create_publisher<Imu>("imu", 200);

auto tolerance = this->get_parameter("diagnostic_tolerance").as_double();
imu_frequency_ = params_.imu_rate_target == 0 ? IMU_MAX_RATE : params_.imu_rate_target;
fps_frequency_ = params_.fps;
auto imu_pub = this->create_publisher<Imu>("imu", 200);
diagnosed_imu_publisher_ = std::make_shared<diagnostic_updater::DiagnosedPublisher<Imu>>(
imu_pub, updater_, diagnostic_updater::FrequencyStatusParam(&imu_frequency_, &imu_frequency_, tolerance, 10),
diagnostic_updater::TimeStampStatusParam());
if (params_.point_cloud || params_.rgb_point_cloud) {
pointcloud_publisher_ = this->create_publisher<PointCloud2>("points2", 1);
auto pointcloud_pub = this->create_publisher<PointCloud2>("points2", 1);
diagnosed_pointcloud_publisher_ = std::make_shared<diagnostic_updater::DiagnosedPublisher<PointCloud2>>(
pointcloud_pub, updater_,
diagnostic_updater::FrequencyStatusParam(&fps_frequency_, &fps_frequency_, tolerance, 10),
diagnostic_updater::TimeStampStatusParam());
}
updater_.setHardwareID("k4a");
if (params_.color_enabled && params_.color_format == "bgra")
{
rgb_diagnostic_ = std::make_shared<diagnostic_updater::TopicDiagnostic>(
rgb_raw_publisher_.getTopic(), updater_,
diagnostic_updater::FrequencyStatusParam(&fps_frequency_, &fps_frequency_, tolerance, 10),
diagnostic_updater::TimeStampStatusParam());
}
if (params_.depth_enabled)
{
if (calibration_data_.k4a_calibration_.depth_mode != K4A_DEPTH_MODE_PASSIVE_IR){
depth_diagnostic_ = std::make_shared<diagnostic_updater::TopicDiagnostic>(
depth_raw_publisher_.getTopic(), updater_,
diagnostic_updater::FrequencyStatusParam(&fps_frequency_, &fps_frequency_, tolerance, 10),
diagnostic_updater::TimeStampStatusParam());
}
ir_diagnostic_ = std::make_shared<diagnostic_updater::TopicDiagnostic>(
ir_raw_publisher_.getTopic(), updater_,
diagnostic_updater::FrequencyStatusParam(&fps_frequency_, &fps_frequency_, tolerance, 10),
diagnostic_updater::TimeStampStatusParam());
}
updater_.force_update();

#if defined(K4A_BODY_TRACKING)
if (params_.body_tracking_enabled) {
Expand Down Expand Up @@ -510,7 +540,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<sensor_msgs::msg::Image>& rgb_image,
k4a_result_t K4AROSDevice::getRgbFrame(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::Image>& rgb_image,
bool rectified = false)
{
k4a::image k4a_bgra_frame = capture.get_color_image();
Expand Down Expand Up @@ -939,8 +969,11 @@ void K4AROSDevice::framePublisherThread()
// Only create ir frame when we are using a device or we have an ir image.
// Recordings may not have synchronized captures. For unsynchronized captures without ir image skip ir frame.

if ((this->count_subscribers("ir/image_raw") > 0 || this->count_subscribers("ir/camera_info") > 0) &&
(k4a_device_ || capture.get_ir_image() != nullptr))
if (this->count_subscribers("ir/image_raw") == 0 && this->count_subscribers("ir/camera_info") == 0)
{
ir_diagnostic_->tick(this->get_clock()->now());
}
else if (k4a_device_ || capture.get_ir_image() != nullptr)
{
// IR images are available in all depth modes
result = getIrFrame(capture, ir_raw_frame);
Expand All @@ -962,6 +995,7 @@ void K4AROSDevice::framePublisherThread()

ir_raw_publisher_.publish(ir_raw_frame);
ir_raw_camerainfo_publisher_->publish(ir_raw_camera_info);
ir_diagnostic_->tick(capture_time);
}
}

Expand All @@ -972,8 +1006,11 @@ void K4AROSDevice::framePublisherThread()
// Recordings may not have synchronized captures. For unsynchronized captures without depth image skip depth
// frame.

if ((this->count_subscribers("depth/image_raw") > 0 || this->count_subscribers("depth/camera_info") > 0) &&
(k4a_device_ || capture.get_depth_image() != nullptr))
if ((this->count_subscribers("depth/image_raw") == 0 && this->count_subscribers("depth/camera_info") == 0))
{
depth_diagnostic_->tick(this->get_clock()->now());
}
else if (k4a_device_ || capture.get_depth_image() != nullptr)
{
result = getDepthFrame(capture, depth_raw_frame);

Expand All @@ -994,6 +1031,7 @@ void K4AROSDevice::framePublisherThread()

depth_raw_publisher_.publish(depth_raw_frame);
depth_raw_camerainfo_publisher_->publish(depth_raw_camera_info);
depth_diagnostic_->tick(capture_time);
}
}

Expand Down Expand Up @@ -1081,11 +1119,14 @@ void K4AROSDevice::framePublisherThread()
}
else if (params_.color_format == "bgra")
{
if ((this->count_subscribers(params_.rgb_namespace + "/image_raw") > 0 ||
this->count_subscribers(params_.rgb_namespace + "/camera_info") > 0) &&
(k4a_device_ || capture.get_color_image() != nullptr))
if (this->count_subscribers(params_.rgb_namespace + "/image_raw") == 0 &&
this->count_subscribers(params_.rgb_namespace + "/camera_info") == 0)
{
rgb_diagnostic_->tick(this->get_clock()->now());
}
else if (k4a_device_ || capture.get_color_image() != nullptr)
{
result = getRbgFrame(capture, rgb_raw_frame);
result = getRgbFrame(capture, rgb_raw_frame);

if (result != K4A_RESULT_SUCCEEDED)
{
Expand All @@ -1103,6 +1144,7 @@ void K4AROSDevice::framePublisherThread()
// Re-synchronize the header timestamps since we cache the camera calibration message
rgb_raw_camera_info.header.stamp = capture_time;
rgb_raw_camerainfo_publisher_->publish(rgb_raw_camera_info);
rgb_diagnostic_->tick(capture_time);
}

// We can only rectify the color into the depth co-ordinates if the depth camera is enabled and processing depth
Expand All @@ -1113,7 +1155,7 @@ void K4AROSDevice::framePublisherThread()
(this->count_subscribers("rgb_to_depth/image_raw") > 0 || this->count_subscribers("rgb_to_depth/camera_info") > 0) &&
(k4a_device_ || (capture.get_color_image() != nullptr && capture.get_depth_image() != nullptr)))
{
result = getRbgFrame(capture, rgb_rect_frame, true /* rectified */);
result = getRgbFrame(capture, rgb_rect_frame, true /* rectified */);

if (result != K4A_RESULT_SUCCEEDED)
{
Expand Down Expand Up @@ -1173,7 +1215,7 @@ void K4AROSDevice::framePublisherThread()

if (params_.point_cloud || params_.rgb_point_cloud)
{
pointcloud_publisher_->publish(*point_cloud);
diagnosed_pointcloud_publisher_->publish(*point_cloud);
}
}

Expand Down Expand Up @@ -1328,7 +1370,7 @@ void K4AROSDevice::imuPublisherThread()
if (std::abs(imu_msg->angular_velocity.x) > DBL_EPSILON ||
std::abs(imu_msg->angular_velocity.y) > DBL_EPSILON ||
std::abs(imu_msg->angular_velocity.z) > DBL_EPSILON){
imu_orientation_publisher_->publish(*imu_msg);
diagnosed_imu_publisher_->publish(*imu_msg);
}
}
}
Expand Down Expand Up @@ -1375,7 +1417,7 @@ void K4AROSDevice::imuPublisherThread()
if (std::abs(imu_msg->angular_velocity.x) > DBL_EPSILON ||
std::abs(imu_msg->angular_velocity.y) > DBL_EPSILON ||
std::abs(imu_msg->angular_velocity.z) > DBL_EPSILON){
imu_orientation_publisher_->publish(*imu_msg);
diagnosed_imu_publisher_->publish(*imu_msg);
}

last_imu_time_usec_ = sample.acc_timestamp_usec;
Expand Down

0 comments on commit 87a0bdd

Please sign in to comment.