add switch IR camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -47,7 +47,7 @@
 #include "orbbec_camera_msgs/srv/get_string.hpp"
 #include "orbbec_camera_msgs/srv/set_int32.hpp"
 #include "orbbec_camera_msgs/srv/get_bool.hpp"
-
+#include "orbbec_camera_msgs/srv/set_string.hpp"
 #include "orbbec_camera/constants.h"
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
@@ -83,6 +83,7 @@ using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
 using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
 using GetInt32 = orbbec_camera_msgs::srv::GetInt32;
 using GetString = orbbec_camera_msgs::srv::GetString;
+using SetString = orbbec_camera_msgs::srv::SetString;
 using SetBool = std_srvs::srv::SetBool;
 using GetBool = orbbec_camera_msgs::srv::GetBool;
 
@@ -224,6 +225,9 @@ class OBCameraNode {
   void savePointCloudCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
                               std::shared_ptr<std_srvs::srv::Empty::Response>& response);
 
+  void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
+                              std::shared_ptr<SetString::Response>& response);
+
   void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
   void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
@@ -293,6 +297,7 @@ class OBCameraNode {
   rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
   rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
   rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
+  rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
   std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
       set_auto_exposure_srv_;
   rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;

orbbec_camera/src/ros_service.cpp

@@ -150,6 +150,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                  std::shared_ptr<std_srvs::srv::Empty::Response> response) {
         savePointCloudCallback(request, response);
       });
+  switch_ir_camera_srv_ = node_->create_service<SetString>(
+      "switch_ir", [this](const std::shared_ptr<SetString::Request> request,
+                                 std::shared_ptr<SetString::Response> response) {
+        switchIRCameraCallback(request, response);
+      });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -639,4 +644,27 @@ void OBCameraNode::savePointCloudCallback(
   }
 }
 
+void OBCameraNode::switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
+                                          std::shared_ptr<SetString::Response>& response) {
+  if (request->data != "left" && request->data != "right") {
+    response->success = false;
+    response->message = "invalid ir camera name";
+    return;
+  }
+  try {
+    int data = request->data == "left" ? 0 : 1;
+    device_->setIntProperty(OB_PROP_IR_CHANNEL_DATA_SOURCE_INT, data);
+    response->success = true;
+    return;
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+    response->success = false;
+  } catch (const std::exception& e) {
+    response->message = e.what();
+    response->success = false;
+  } catch (...) {
+    response->message = "unknown error";
+    response->success = false;
+  }
+}
 }  // namespace orbbec_camera

orbbec_camera_msgs/CMakeLists.txt

@@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/GetInt32.srv"
   "srv/GetString.srv"
   "srv/SetInt32.srv"
+  "srv/SetString.srv"
   DEPENDENCIES
   sensor_msgs
   std_msgs)

orbbec_camera_msgs/srv/SetString.srv

@@ -0,0 +1,4 @@
+string data
+---
+bool success
+string message