feat: add service to switch depth-color alignment mode

orbbec_camera/src/ros_service.cpp

@@ -15,6 +15,8 @@
  *******************************************************************************/
 
 #include "orbbec_camera/ob_camera_node.h"
+#include <algorithm>
+#include <cctype>
 #include <rclcpp/rclcpp.hpp>
 #include <nlohmann/json.hpp>
 #include <thread>
@@ -351,6 +353,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                    std::shared_ptr<SetBool::Response> response) {
         setStreamsEnableCallback(request, response);
       });
+  set_image_registration_mode_srv_ = node_->create_service<SetString>(
+      "set_image_registration_mode", [this](const std::shared_ptr<SetString::Request> request,
+                                            std::shared_ptr<SetString::Response> response) {
+        setImageRegistrationModeCallback(request, response);
+      });
   get_streams_enable_srv_ = node_->create_service<GetBool>(
       "get_streams_enable", [this](const std::shared_ptr<GetBool::Request> request,
                                    std::shared_ptr<GetBool::Response> response) {
@@ -632,6 +639,116 @@ void OBCameraNode::setStreamsEnableCallback(
   }
 }
 
+void OBCameraNode::setImageRegistrationModeCallback(
+    const std::shared_ptr<SetString::Request> request,
+    std::shared_ptr<SetString::Response> response) {
+  auto mode = request->data;
+  std::transform(mode.begin(), mode.end(), mode.begin(),
+                 [](unsigned char ch) { return static_cast<char>(std::toupper(ch)); });
+  if (mode != "OFF" && mode != "HW_D2C" && mode != "SW_D2C" && mode != "SW_C2D") {
+    response->success = false;
+    response->message = "Invalid image registration mode '" + request->data +
+                        "'. Valid values: OFF, HW_D2C, SW_D2C, SW_C2D";
+    return;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+
+  if (mode != "OFF" && (!enable_stream_[COLOR] || !enable_stream_[DEPTH])) {
+    response->success = false;
+    response->message =
+        "Image registration mode " + mode + " requires both color and depth streams to be enabled";
+    return;
+  }
+
+  const bool old_depth_registration = depth_registration_;
+  const std::string old_align_mode = align_mode_;
+  const OBStreamType old_align_target_stream = align_target_stream_;
+  const bool was_running = pipeline_started_.load();
+
+  auto mode_from_state = [](bool depth_registration, const std::string& align_mode,
+                            OBStreamType align_target_stream) {
+    if (!depth_registration) {
+      return std::string("OFF");
+    }
+    if (align_mode == "HW") {
+      return std::string("HW_D2C");
+    }
+    return align_target_stream == OB_STREAM_DEPTH ? std::string("SW_C2D") : std::string("SW_D2C");
+  };
+  const auto old_mode =
+      mode_from_state(old_depth_registration, old_align_mode, old_align_target_stream);
+
+  auto apply_image_registration_mode = [this](const std::string& mode) {
+    if (mode == "OFF") {
+      depth_registration_ = false;
+      align_mode_ = "HW";
+      align_target_stream_ = OB_STREAM_COLOR;
+    } else if (mode == "HW_D2C") {
+      depth_registration_ = true;
+      align_mode_ = "HW";
+      align_target_stream_ = OB_STREAM_COLOR;
+    } else {
+      depth_registration_ = true;
+      align_mode_ = "SW";
+      align_target_stream_ = mode == "SW_C2D" ? OB_STREAM_DEPTH : OB_STREAM_COLOR;
+    }
+    align_filter_.reset();
+    syncSoftwareAlignment();
+  };
+
+  auto restore_old_mode = [this, old_depth_registration, old_align_mode,
+                           old_align_target_stream]() {
+    depth_registration_ = old_depth_registration;
+    align_mode_ = old_align_mode;
+    align_target_stream_ = old_align_target_stream;
+    align_filter_.reset();
+    syncSoftwareAlignment();
+  };
+
+  auto rollback_after_error = [&](const std::string& error_message) {
+    try {
+      restore_old_mode();
+      if (was_running && !pipeline_started_.load()) {
+        startStreams();
+      }
+      response->message = "Failed to set image registration mode to " + mode + ": " +
+                          error_message + ". Rolled back to " + old_mode;
+    } catch (const std::exception& rollback_error) {
+      response->message = "Failed to set image registration mode to " + mode + ": " +
+                          error_message + ". Rollback to " + old_mode +
+                          " also failed: " + rollback_error.what();
+    } catch (...) {
+      response->message = "Failed to set image registration mode to " + mode + ": " +
+                          error_message + ". Rollback to " + old_mode + " also failed";
+    }
+    response->success = false;
+  };
+
+  try {
+    if (was_running) {
+      stopStreams();
+    }
+
+    apply_image_registration_mode(mode);
+
+    if (was_running) {
+      startStreams();
+      response->message = "Image registration mode changed from " + old_mode + " to " + mode +
+                          "; streams restarted";
+    } else {
+      response->message = "Image registration mode set to " + mode + "; streams remain stopped";
+    }
+    response->success = true;
+  } catch (const ob::Error& e) {
+    rollback_after_error(orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    rollback_after_error(e.what());
+  } catch (...) {
+    rollback_after_error("unknown error");
+  }
+}
+
 void OBCameraNode::getStreamsEnableCallback(
     const std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Request> request,
     std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Response> response) {