Merge branch 'feature/lrm-measure-distance-topic' into merge/ros_1.5.22

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 <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/string.hpp>
 
 #if __has_include(<cv_bridge/cv_bridge.hpp>)
@@ -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<SetInt32>::SharedPtr set_fan_work_mode_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_ldp_measure_distance_srv_;
+  rclcpp::TimerBase::SharedPtr lrm_obstacle_distance_timer_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::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;

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'),

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<bool>(enable_ldp_, "enable_ldp", true);
+  setAndGetNodeParameter<bool>(enable_lrm_obstacle_distance_publish_,
+                               "enable_lrm_obstacle_distance_publish", false);
+  setAndGetNodeParameter<double>(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<int>(soft_filter_max_diff_, "soft_filter_max_diff", -1);
   setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
   setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
@@ -1566,6 +1587,46 @@ 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<std_msgs::msg::Int32>("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::milliseconds>(
+        std::chrono::duration<double>(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;
+    {
+      std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+      msg.data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT);
+    }
+    lrm_obstacle_distance_pub_->publish(msg);
+  } catch (const ob::Error &e) {
+    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
+                         "Failed to publish LRM obstacle distance: %s", e.getMessage());
+  } 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<ob::FrameSet> &frame_set) {