From 18847bc73cb114217da88263ab9e95784ddf2999 Mon Sep 17 00:00:00 2001
From: Joe Dong <jian.dong@pobox.com>
Date: Sun, 29 Sep 2024 16:41:04 +0800
Subject: [PATCH] add enable hardware reset param

---
 README.MD                                       |  2 ++
 .../orbbec_camera/ob_camera_node_driver.h       |  2 ++
 .../launch/gemini_330_series.launch.py          |  1 +
 orbbec_camera/src/ob_camera_node_driver.cpp     | 17 +++++++++++++++--
 4 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/README.MD b/README.MD
index 7be0d6a3..b61e2226 100644
--- a/README.MD
+++ b/README.MD
@@ -326,6 +326,8 @@ the firmware, and if hardware logging is desired, it should also be set to `true
 - `ir_brightness`: IR brightness.
 - `color_ae_max_exposure`: Color auto exposure maximum exposure.
 - `ir_ae_max_exposure`: IR auto exposure maximum exposure.
+- `enable_hardware_reset`: This option enables the hardware reset function. By default, it is set to `false`. 
+If set to `true`, the camera will reboot upon its first connection.
 
 **IMPORTANT**: *Please carefully read the instructions regarding software filtering settings
 at [this link](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/). If you are uncertain, do not modify
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
index 47ff41e8..89a89058 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
@@ -107,5 +107,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   static backward::SignalHandling sh;  // for stack trace
+  bool enable_hardware_reset_ = false;
+  bool hardware_reset_done_ = false;
 };
 }  // namespace orbbec_camera
diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py
index aa6e9c77..82648f3b 100644
--- a/orbbec_camera/launch/gemini_330_series.launch.py
+++ b/orbbec_camera/launch/gemini_330_series.launch.py
@@ -180,6 +180,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_hardware_reset', default_value='false'),
     ]
 
     def get_params(context, args):
diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp
index f9f7c582..36c9bbbc 100644
--- a/orbbec_camera/src/ob_camera_node_driver.cpp
+++ b/orbbec_camera/src/ob_camera_node_driver.cpp
@@ -114,6 +114,7 @@ void OBCameraNodeDriver::init() {
   connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
   enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
   g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
+  enable_hardware_reset_ = declare_parameter<bool>("enable_hardware_reset", false);
   ob::Context::setLoggerToConsole(log_level);
   orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666);
   if (orb_device_lock_shm_fd_ < 0) {
@@ -365,6 +366,18 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
 }
 
 void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
+  if (device_) {
+    RCLCPP_INFO_STREAM(logger_, "Device is not nullptr, reset device");
+    device_.reset();
+  }
+  if (enable_hardware_reset_ && !hardware_reset_done_) {
+    RCLCPP_INFO_STREAM(logger_, "Enable hardware reset, reboot device");
+    device->reboot();
+    RCLCPP_INFO_STREAM(logger_, "Reboot device done");
+    hardware_reset_done_ = true;
+    device_connected_ = false;
+    return;
+  }
   device_ = device;
   CHECK_NOTNULL(device_);
   CHECK_NOTNULL(device_.get());
@@ -435,8 +448,8 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int
     RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
     return;
   }
-  RCLCPP_INFO_STREAM(logger_, "Connecting to net device ip: " << net_device_ip
-                             << " port: " << net_device_port);
+  RCLCPP_INFO_STREAM(
+      logger_, "Connecting to net device ip: " << net_device_ip << " port: " << net_device_port);
   std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
   auto device = ctx_->createNetDevice(net_device_ip.c_str(), net_device_port);
   if (device == nullptr) {