From 1219f25639e1b42e67aa4824edcdbeb8c25cf755 Mon Sep 17 00:00:00 2001 From: slz Date: Tue, 7 Jul 2026 15:15:17 +0800 Subject: [PATCH] feat: publish LRM obstacle distance topic --- .../include/orbbec_camera/ob_camera_node.h | 7 +++ .../launch/gemini_330_series.launch.py | 2 + orbbec_camera/src/ob_camera_node.cpp | 59 +++++++++++++++++++ 3 files changed, 68 insertions(+) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index cd906450..d7956bb1 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -64,6 +64,7 @@ #include "orbbec_camera/image_publisher.h" #include "orbbec_camera/frame_timestamp_csv_logger.h" #include "jpeg_decoder.h" +#include #include #if __has_include() @@ -185,6 +186,8 @@ class OBCameraNode { void onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& status); + void publishLrmObstacleDistance(); + void setupCameraCtrlServices(); void stopStreams(); @@ -437,6 +440,8 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_fan_work_mode_srv_; rclcpp::Service::SharedPtr toggle_sensors_srv_; rclcpp::Service::SharedPtr get_ldp_measure_distance_srv_; + rclcpp::TimerBase::SharedPtr lrm_obstacle_distance_timer_; + rclcpp::Publisher::SharedPtr lrm_obstacle_distance_pub_; bool enable_sync_output_accel_gyro_ = false; bool publish_tf_ = false; @@ -599,6 +604,8 @@ class OBCameraNode { // soft ware trigger rclcpp::TimerBase::SharedPtr software_trigger_timer_; std::chrono::milliseconds software_trigger_period_{33}; + bool enable_lrm_obstacle_distance_publish_ = false; + double lrm_obstacle_distance_publish_rate_ = 10.0; bool enable_heartbeat_ = false; std::string industry_mode_ = ""; bool enable_color_undistortion_ = false; diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 1e00062a..4ed727db 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -155,6 +155,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), DeclareLaunchArgument('enable_ldp', default_value='true'), + DeclareLaunchArgument('enable_lrm_obstacle_distance_publish', default_value='false'), + DeclareLaunchArgument('lrm_obstacle_distance_publish_rate', default_value='10.0'), DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 7f83acd5..f042e6e3 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -137,6 +137,14 @@ void OBCameraNode::clean() noexcept { frame_timestamp_csv_logger_->shutdown(); frame_timestamp_csv_logger_.reset(); } + if (software_trigger_timer_) { + software_trigger_timer_->cancel(); + software_trigger_timer_.reset(); + } + if (lrm_obstacle_distance_timer_) { + lrm_obstacle_distance_timer_->cancel(); + lrm_obstacle_distance_timer_.reset(); + } RCLCPP_WARN_STREAM(logger_, "Stop tf thread"); if (tf_thread_ && tf_thread_->joinable()) { tf_thread_->join(); @@ -1226,6 +1234,19 @@ void OBCameraNode::getParameters() { depth_registration_ = false; } setAndGetNodeParameter(enable_ldp_, "enable_ldp", true); + setAndGetNodeParameter(enable_lrm_obstacle_distance_publish_, + "enable_lrm_obstacle_distance_publish", false); + setAndGetNodeParameter(lrm_obstacle_distance_publish_rate_, + "lrm_obstacle_distance_publish_rate", 10.0); + if (enable_lrm_obstacle_distance_publish_ && !enable_ldp_) { + RCLCPP_INFO_STREAM(logger_, "enable_lrm_obstacle_distance_publish is true, enabling LDP"); + enable_ldp_ = true; + } + if (lrm_obstacle_distance_publish_rate_ <= 0.0) { + RCLCPP_WARN_STREAM(logger_, "Invalid lrm_obstacle_distance_publish_rate " + << lrm_obstacle_distance_publish_rate_ << ", reset to 10.0"); + lrm_obstacle_distance_publish_rate_ = 10.0; + } setAndGetNodeParameter(soft_filter_max_diff_, "soft_filter_max_diff", -1); setAndGetNodeParameter(soft_filter_speckle_size_, "soft_filter_speckle_size", -1); setAndGetNodeParameter(liner_accel_cov_, "linear_accel_cov", 0.0003); @@ -1566,6 +1587,44 @@ void OBCameraNode::setupPublishers() { std_msgs::msg::String msg; msg.data = filter_status_.dump(2); filter_status_pub_->publish(msg); + + if (enable_lrm_obstacle_distance_publish_) { + lrm_obstacle_distance_pub_ = + node_->create_publisher("lrm/obstacle_distance", rclcpp::QoS(10)); + RCLCPP_INFO_STREAM(logger_, "Publishing LRM obstacle distance on lrm/obstacle_distance at " + << lrm_obstacle_distance_publish_rate_ << " Hz"); + auto publish_period = std::chrono::duration_cast( + std::chrono::duration(1.0 / lrm_obstacle_distance_publish_rate_)); + if (publish_period < std::chrono::milliseconds(1)) { + publish_period = std::chrono::milliseconds(1); + } + lrm_obstacle_distance_timer_ = + node_->create_wall_timer(publish_period, [this]() { publishLrmObstacleDistance(); }); + } +} + +void OBCameraNode::publishLrmObstacleDistance() { + if (!lrm_obstacle_distance_pub_) { + return; + } + if (lrm_obstacle_distance_pub_->get_subscription_count() == 0) { + return; + } + try { + std_msgs::msg::Int32 msg; + msg.data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT); + lrm_obstacle_distance_pub_->publish(msg); + } catch (const ob::Error &e) { + auto message = e.getMessage(); + RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000, + "Failed to publish LRM obstacle distance: %s", message.c_str()); + } catch (const std::exception &e) { + RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000, + "Failed to publish LRM obstacle distance: %s", e.what()); + } catch (...) { + RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000, + "Failed to publish LRM obstacle distance: unknown error"); + } } void OBCameraNode::publishPointCloud(const std::shared_ptr &frame_set) {