APG-1890 Adds Guards around updating firmware (#13)

SushiRaj99 · web-flow · commit b264276f1e85 · 2025-10-23T15:42:42.000-04:00
diff --git a/docs/launch_parameters.md b/docs/launch_parameters.md
@@ -14,8 +14,10 @@ The following are the launch parameters available:
   * The USB port of the camera. This is required when multiple cameras are used
 * **device_num**
   * The number of devices. This must be filled in if multiple cameras are required
-* **upgrade_firmware**
+* **firmware_upgrade_filepath**
   * The input parameter is the firmware path
+* **expected_firmware_version**
+  * The expected firmware version
 * **preset_firmware_path**
   * The input parameter is the perset firmware path. If multiple paths are input, each path needs to be separated by `,`and a maximum of 3 firmware paths can be input
 * **uvc_backend**
diff --git a/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py b/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py
@@ -57,7 +57,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('preset_firmware_path', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
diff --git a/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py b/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py
@@ -57,7 +57,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('preset_firmware_path', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py
@@ -57,7 +57,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('preset_firmware_path', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
@@ -117,7 +117,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   std::string extension_path_;
   static backward::SignalHandling sh;  // for stack trace
-  std::string upgrade_firmware_;
+  std::string firmware_upgrade_filepath_;
+  std::string expected_firmware_version_;
   bool enable_streams_on_startup_ {true};
 };
 }  // namespace orbbec_camera
diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py
@@ -56,7 +56,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
         DeclareLaunchArgument('uvc_backend', default_value='libuvc'),#libuvc or v4l2
diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py
@@ -56,7 +56,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
         DeclareLaunchArgument('uvc_backend', default_value='libuvc'),#libuvc or v4l2
diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py
@@ -56,7 +56,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('preset_firmware_path', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py
@@ -58,7 +58,8 @@ def generate_launch_description():
         DeclareLaunchArgument('serial_number', default_value=''),
         DeclareLaunchArgument('usb_port', default_value=''),
         DeclareLaunchArgument('device_num', default_value='1'),
-        DeclareLaunchArgument('upgrade_firmware', default_value=''),
+        DeclareLaunchArgument('firmware_upgrade_filepath', default_value=''),
+        DeclareLaunchArgument('expected_firmware_version', default_value=''),
         DeclareLaunchArgument('preset_firmware_path', default_value=''),
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp
@@ -125,7 +125,8 @@ void OBCameraNodeDriver::init() {
   auto log_level = obLogSeverityFromString(log_level_str);
   connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
   enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
-  upgrade_firmware_ = declare_parameter<std::string>("upgrade_firmware", "");
+  firmware_upgrade_filepath_ = declare_parameter<std::string>("firmware_upgrade_filepath", "");
+  expected_firmware_version_ = declare_parameter<std::string>("expected_firmware_version", "");
   g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
   g_time_domain = declare_parameter<std::string>("time_domain", g_time_domain);
   preset_firmware_path_ =
@@ -497,12 +498,19 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
       std::chrono::high_resolution_clock::now() - start_time_);
   RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
-  if (!upgrade_firmware_.empty()) {
-    device_->updateFirmware(
-        upgrade_firmware_.c_str(),
-        std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
-                  std::placeholders::_2, std::placeholders::_3),
-        false);
+  if (!firmware_upgrade_filepath_.empty() && !expected_firmware_version_.empty()) {
+    if (device_info_->getFirmwareVersion() != expected_firmware_version_) {
+      RCLCPP_INFO_STREAM(logger_,
+                      "Firmware does not match the expected version : " <<
+                      expected_firmware_version_ << ". Upgrading device.");
+      device_->updateFirmware(
+          firmware_upgrade_filepath_.c_str(),
+          std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
+                    std::placeholders::_2, std::placeholders::_3),
+          false);
+    } else {
+      RCLCPP_INFO_STREAM(logger_, "Firmware matches the expected version: " << expected_firmware_version_);
+    }
   }
   if (ob_camera_node_) {
     ob_camera_node_->startIMU();
@@ -750,7 +758,7 @@ void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const cha
     RCLCPP_INFO(logger_, "Reboot device");
     ob_camera_node_->rebootDevice();
     device_connected_ = false;
-    upgrade_firmware_ = "";
+    firmware_upgrade_filepath_ = "";
   }
 }
 }  // namespace orbbec_camera
