使用服务控制相机曝光

src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp

@@ -34,6 +34,7 @@
 #include <image_transport/image_transport.hpp>
 #include <rm_interfaces/msg/armors.hpp>
 #include <rm_interfaces/srv/set_mode.hpp>
+#include <std_msgs/msg/float32.hpp>
 
 #include <cv_bridge/cv_bridge.h>
 #include <tf2_ros/buffer.h>
@@ -95,6 +96,7 @@ private:
   rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
   rclcpp::Publisher<rm_interfaces::msg::Armors>::SharedPtr armors_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr auto_exposure_pub_;
   rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_mode_srv_;
 
   // Image transport for debug

src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp

@@ -118,6 +118,10 @@ ArmorYoloDetectorNode::ArmorYoloDetectorNode(const rclcpp::NodeOptions& options)
   marker_pub_ =
       this->create_publisher<visualization_msgs::msg::MarkerArray>("armor_detector/marker", 10);
 
+  // Auto exposure publisher
+  auto_exposure_pub_ =
+      this->create_publisher<std_msgs::msg::Float32>("auto_exposure/set_exposure", 10);
+
   // Debug
   debug_ = this->declare_parameter("debug", false);
   debug_log_interval_frames_ =
@@ -1000,14 +1004,11 @@ double ArmorYoloDetectorNode::computeBrightnessInRois(
 }
 
 void ArmorYoloDetectorNode::callSetExposureService(double exposure_time) {
-  // Set exposure time via parameter - camera driver should subscribe to this parameter
+  // Publish exposure time to camera driver via topic
-  // or this node should be in same container with camera driver to share parameter
+  std_msgs::msg::Float32 msg;
-  auto result = this->set_parameter(rclcpp::Parameter("exposure_time", static_cast<int>(exposure_time)));
+  msg.data = exposure_time;
-  if (result.successful) {
+  auto_exposure_pub_->publish(msg);
-    FYT_DEBUG("armor_yolo_detect", "Set exposure time to {:.1f}", exposure_time);
+  FYT_DEBUG("armor_yolo_detect", "Auto exposure set exposure_time: {:.1f}", exposure_time);
-  } else {
-    FYT_WARN("armor_yolo_detect", "Failed to set exposure time: {}", result.reason);
-  }
 }
 
 }  // namespace armor_yolo_detect

src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp

@@ -11,6 +11,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/float32.hpp>
 #include <string>
 #include <thread>
 
@@ -65,6 +66,20 @@ public:
     // Heartbeat
     heartbeat_ = fyt::HeartBeatPublisher::create(this);
 
+    // Auto exposure subscriber - receives target exposure time from armor detector
+    auto_exposure_sub_ = this->create_subscription<std_msgs::msg::Float32>(
+        "auto_exposure/set_exposure", rclcpp::SensorDataQoS(),
+        [this](const std_msgs::msg::Float32::SharedPtr msg) {
+          if (msg->data > 0) {
+            std::lock_guard<std::mutex> lock(camera_mutex_);
+            exposure_time_ = static_cast<int>(msg->data);
+            if (camera_handle_ != nullptr) {
+              (void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
+              FYT_DEBUG("camera_driver", "Auto exposure set exposure_time: {}", exposure_time_);
+            }
+          }
+        });
+
     // Param set callback
     params_callback_handle_ = this->add_on_set_parameters_callback(
       std::bind(&HikCameraNode::parametersCallback, this, std::placeholders::_1));
@@ -482,6 +497,9 @@ private:
   // Heartbeat
   fyt::HeartBeatPublisher::SharedPtr heartbeat_;
 
+  // Auto exposure subscriber
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr auto_exposure_sub_;
+
   // Params
   int frame_rate_{210};
   int exposure_time_{5000};