diff --git a/src/rm_bringup/config/node_params/camera_driver_params.yaml b/src/rm_bringup/config/node_params/camera_driver_params.yaml index 1ca3ff1..2c2bf37 100644 --- a/src/rm_bringup/config/node_params/camera_driver_params.yaml +++ b/src/rm_bringup/config/node_params/camera_driver_params.yaml @@ -9,3 +9,4 @@ offsetY: 0 camera_name: "hik" recording: false #是否录制 + open_delay_ms: 2000 # Camera open delay after previous instance closed (ms) diff --git a/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp b/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp index 84b8c09..d7c92ff 100644 --- a/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp +++ b/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp @@ -98,8 +98,10 @@ public: } // Watch dog timer (also handles initial open) + // Add initial delay to allow previous camera instance to fully release resources + int open_delay_ms = this->declare_parameter("open_delay_ms", 1000); timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000), std::bind(&HikCameraNode::timerCallback, this)); + std::chrono::milliseconds(open_delay_ms), std::bind(&HikCameraNode::timerCallback, this)); // Start capture thread capture_thread_ = std::thread([this]() { this->captureLoop(); }); @@ -208,20 +210,29 @@ private: void timerCallback() { - // Open camera - while (!is_open_ && rclcpp::ok()) { + // Open camera with retry delay + if (!is_open_) { + static int retry_count = 0; if (!open()) { - FYT_ERROR("camera_driver", "open() failed"); + retry_count++; + if (retry_count >= 3) { + FYT_ERROR("camera_driver", "open() failed {} times, will keep retrying", retry_count); + } close(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); return; } + retry_count = 0; + is_open_ = true; } // Watchdog: no frames for too long => reconnect double dt = (this->now() - latest_frame_stamp_).seconds(); if (dt > 5.0) { - FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds", dt); + FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds, reconnecting...", dt); close(); + is_open_ = false; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); } }