feat(camera): support GigE Action Commands

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -53,6 +53,7 @@
 #include "orbbec_camera_msgs/msg/depth_filter_state.hpp"
 #include "orbbec_camera_msgs/msg/depth_filters_status.hpp"
 #include "orbbec_camera_msgs/srv/get_device_config.hpp"
+#include "orbbec_camera_msgs/srv/get_action_config.hpp"
 #include "orbbec_camera_msgs/srv/get_device_info.hpp"
 #include "orbbec_camera_msgs/srv/get_awb_gain.hpp"
 #include "orbbec_camera_msgs/msg/extrinsics.hpp"
@@ -65,6 +66,7 @@
 #include "orbbec_camera_msgs/srv/get_bool.hpp"
 #include "orbbec_camera_msgs/srv/set_string.hpp"
 #include "orbbec_camera_msgs/srv/set_filter.hpp"
+#include "orbbec_camera_msgs/srv/set_action_config.hpp"
 #include "orbbec_camera_msgs/srv/set_arrays.hpp"
 #include "orbbec_camera_msgs/srv/set_stream_profile.hpp"
 #include "orbbec_camera_msgs/srv/get_user_calib_params.hpp"
@@ -135,6 +137,7 @@
 
 namespace orbbec_camera {
 using GetDeviceConfig = orbbec_camera_msgs::srv::GetDeviceConfig;
+using GetActionConfig = orbbec_camera_msgs::srv::GetActionConfig;
 using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo;
 using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
 using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
@@ -146,6 +149,7 @@ using SetString = orbbec_camera_msgs::srv::SetString;
 using SetBool = std_srvs::srv::SetBool;
 using GetBool = orbbec_camera_msgs::srv::GetBool;
 using SetFilter = orbbec_camera_msgs::srv::SetFilter;
+using SetActionConfig = orbbec_camera_msgs::srv::SetActionConfig;
 using SetArrays = orbbec_camera_msgs::srv::SetArrays;
 using SetStreamProfile = orbbec_camera_msgs::srv::SetStreamProfile;
 using SetUserCalibParams = orbbec_camera_msgs::srv::SetUserCalibParams;
@@ -481,6 +485,12 @@ class OBCameraNode {
   void getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig::Request>& request,
                                std::shared_ptr<GetDeviceConfig::Response>& response);
 
+  void getActionConfigCallback(const std::shared_ptr<GetActionConfig::Request>& request,
+                               std::shared_ptr<GetActionConfig::Response>& response);
+
+  void setActionConfigCallback(const std::shared_ptr<SetActionConfig::Request>& request,
+                               std::shared_ptr<SetActionConfig::Response>& response);
+
   void getSDKVersion(const std::shared_ptr<GetString::Request>& request,
                      std::shared_ptr<GetString::Response>& response);
 
@@ -778,6 +788,8 @@ class OBCameraNode {
   std::map<stream_index_pair, rclcpp::Service<SetArrays>::SharedPtr> set_ae_roi_srv_;
   rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
   rclcpp::Service<GetDeviceConfig>::SharedPtr get_device_config_srv_;
+  rclcpp::Service<GetActionConfig>::SharedPtr get_action_config_srv_;
+  rclcpp::Service<SetActionConfig>::SharedPtr set_action_config_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -33,6 +33,7 @@
 #include "libobsensor/hpp/Device.hpp"
 #include "libobsensor/hpp/RecordPlayback.hpp"
 #include "orbbec_camera_msgs/srv/set_bag_recording.hpp"
+#include "orbbec_camera_msgs/srv/send_action_command.hpp"
 
 namespace orbbec_camera {
 
@@ -87,6 +88,9 @@ class OBCameraNodeDriver : public rclcpp::Node {
   void setBagRecordingCallback(
       const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
       std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response);
+  void sendActionCommandCallback(
+      const std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
+      std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response);
   void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message,
                             uint8_t percent);
   void updatePresetFirmware(std::string path);
@@ -148,6 +152,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
   rclcpp::Service<orbbec_camera_msgs::srv::SetBagRecording>::SharedPtr set_bag_recording_srv_ =
       nullptr;
+  rclcpp::Service<orbbec_camera_msgs::srv::SendActionCommand>::SharedPtr send_action_command_srv_ =
+      nullptr;
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   std::string extension_path_;
   static backward::SignalHandling sh;  // for stack trace

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -386,6 +386,9 @@ void OBCameraNodeDriver::init() {
   set_bag_recording_srv_ = this->create_service<orbbec_camera_msgs::srv::SetBagRecording>(
       "set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this,
                                      std::placeholders::_1, std::placeholders::_2));
+  send_action_command_srv_ = this->create_service<orbbec_camera_msgs::srv::SendActionCommand>(
+      "send_action_command", std::bind(&OBCameraNodeDriver::sendActionCommandCallback, this,
+                                       std::placeholders::_1, std::placeholders::_2));
   pthread_mutexattr_init(&orb_device_lock_attr_);
   pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
   orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
@@ -972,6 +975,44 @@ void OBCameraNodeDriver::rebootDeviceCallback(
   return;
 }
 
+void OBCameraNodeDriver::sendActionCommandCallback(
+    const std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
+    std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response) {
+  if (!request) {
+    response->success = false;
+    response->message = "Invalid request";
+    return;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  if (!ctx_) {
+    response->success = false;
+    response->message = "SDK context is not available";
+    return;
+  }
+
+  const std::string destination_ip =
+      request->destination_ip.empty() ? "255.255.255.255" : request->destination_ip;
+  try {
+    const auto send_command = [this, &request, &destination_ip]() {
+      return ctx_->sendActionCommand(request->device_key, request->group_key, request->group_mask,
+                                     destination_ip.c_str(), request->scheduled_time);
+    };
+    response->success =
+        ob_camera_node_ ? ob_camera_node_->withDeviceLock(send_command) : send_command();
+    response->message = response->success ? "OK" : "SDK failed to send Action Command";
+  } catch (const ob::Error &e) {
+    response->success = false;
+    response->message = orbbec_camera::formatObErrorWithStatus(e);
+  } catch (const std::exception &e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
 std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
     const std::shared_ptr<ob::DeviceList> &list) {
   std::shared_ptr<ob::Device> device = nullptr;

orbbec_camera/src/ros_service.cpp

@@ -19,6 +19,7 @@
 #include <cctype>
 #include <rclcpp/rclcpp.hpp>
 #include <nlohmann/json.hpp>
+#include <stdexcept>
 #include <thread>
 
 #include "orbbec_camera/utils.h"
@@ -112,6 +113,8 @@ std::string OBSyncModeToString(const OBMultiDeviceSyncMode& mode) {
       return "SOFTWARE_TRIGGERING";
     case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING:
       return "HARDWARE_TRIGGERING";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_GROUP_ACTIONS:
+      return "GROUP_ACTIONS";
     default:
       return "FREE_RUN";
   }
@@ -334,6 +337,28 @@ void OBCameraNode::setupCameraCtrlServices() {
                                   std::shared_ptr<GetDeviceConfig::Response> response) {
         getDeviceConfigCallback(request, response);
       });
+  if (isPropertyReadable(device_, OB_PROP_ACTION_SIGNAL_COUNT_INT) &&
+      isPropertyReadable(device_, OB_PROP_ACTION_DEVICE_KEY_INT) &&
+      isPropertyWritable(device_, OB_PROP_ACTION_SELECTOR_INT) &&
+      isPropertyReadable(device_, OB_PROP_ACTION_GROUP_KEY_INT) &&
+      isPropertyReadable(device_, OB_PROP_ACTION_GROUP_MASK_INT)) {
+    get_action_config_srv_ = node_->create_service<GetActionConfig>(
+        "get_action_config", [this](const std::shared_ptr<GetActionConfig::Request> request,
+                                    std::shared_ptr<GetActionConfig::Response> response) {
+          getActionConfigCallback(request, response);
+        });
+  }
+  if (isPropertyReadable(device_, OB_PROP_ACTION_SIGNAL_COUNT_INT) &&
+      isPropertyWritable(device_, OB_PROP_ACTION_DEVICE_KEY_INT) &&
+      isPropertyWritable(device_, OB_PROP_ACTION_SELECTOR_INT) &&
+      isPropertyWritable(device_, OB_PROP_ACTION_GROUP_KEY_INT) &&
+      isPropertyWritable(device_, OB_PROP_ACTION_GROUP_MASK_INT)) {
+    set_action_config_srv_ = node_->create_service<SetActionConfig>(
+        "set_action_config", [this](const std::shared_ptr<SetActionConfig::Request> request,
+                                    std::shared_ptr<SetActionConfig::Response> response) {
+          setActionConfigCallback(request, response);
+        });
+  }
   get_sdk_version_srv_ = node_->create_service<GetString>(
       "get_sdk_version",
       [this](const std::shared_ptr<GetString::Request> request,
@@ -1598,6 +1623,100 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Re
   }
 }
 
+void OBCameraNode::getActionConfigCallback(const std::shared_ptr<GetActionConfig::Request>& request,
+                                           std::shared_ptr<GetActionConfig::Response>& response) {
+  if (!request) {
+    response->success = false;
+    response->message = "Invalid request";
+    return;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  try {
+    const int action_signal_count = device_->getIntProperty(OB_PROP_ACTION_SIGNAL_COUNT_INT);
+    if (action_signal_count <= 0 ||
+        request->selector >= static_cast<uint32_t>(action_signal_count)) {
+      response->success = false;
+      response->message = "selector must be less than action signal count " +
+                          std::to_string(std::max(action_signal_count, 0));
+      return;
+    }
+
+    device_->setIntProperty(OB_PROP_ACTION_SELECTOR_INT, static_cast<int32_t>(request->selector));
+    response->action_signal_count = static_cast<uint32_t>(action_signal_count);
+    response->device_key =
+        static_cast<uint32_t>(device_->getIntProperty(OB_PROP_ACTION_DEVICE_KEY_INT));
+    response->group_key =
+        static_cast<uint32_t>(device_->getIntProperty(OB_PROP_ACTION_GROUP_KEY_INT));
+    response->group_mask =
+        static_cast<uint32_t>(device_->getIntProperty(OB_PROP_ACTION_GROUP_MASK_INT));
+    response->success = true;
+    response->message = "OK";
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = orbbec_camera::formatObErrorWithStatus(e);
+  } catch (const std::exception& e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
+void OBCameraNode::setActionConfigCallback(const std::shared_ptr<SetActionConfig::Request>& request,
+                                           std::shared_ptr<SetActionConfig::Response>& response) {
+  if (!request) {
+    response->success = false;
+    response->message = "Invalid request";
+    return;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  try {
+    const int action_signal_count = device_->getIntProperty(OB_PROP_ACTION_SIGNAL_COUNT_INT);
+    if (action_signal_count <= 0 ||
+        request->selector >= static_cast<uint32_t>(action_signal_count)) {
+      response->success = false;
+      response->message = "selector must be less than action signal count " +
+                          std::to_string(std::max(action_signal_count, 0));
+      return;
+    }
+
+    const auto validate_property = [this](OBPropertyID property_id, uint32_t value,
+                                          const char* property_name) {
+      const auto range = device_->getIntPropertyRange(property_id);
+      const int64_t signed_value = static_cast<int64_t>(value);
+      if (signed_value < range.min || signed_value > range.max) {
+        throw std::out_of_range(std::string(property_name) + " must be in [" +
+                                std::to_string(range.min) + ", " + std::to_string(range.max) + "]");
+      }
+    };
+    validate_property(OB_PROP_ACTION_DEVICE_KEY_INT, request->device_key, "device_key");
+    validate_property(OB_PROP_ACTION_SELECTOR_INT, request->selector, "selector");
+    validate_property(OB_PROP_ACTION_GROUP_KEY_INT, request->group_key, "group_key");
+    validate_property(OB_PROP_ACTION_GROUP_MASK_INT, request->group_mask, "group_mask");
+
+    device_->setIntProperty(OB_PROP_ACTION_DEVICE_KEY_INT,
+                            static_cast<int32_t>(request->device_key));
+    device_->setIntProperty(OB_PROP_ACTION_SELECTOR_INT, static_cast<int32_t>(request->selector));
+    device_->setIntProperty(OB_PROP_ACTION_GROUP_KEY_INT, static_cast<int32_t>(request->group_key));
+    device_->setIntProperty(OB_PROP_ACTION_GROUP_MASK_INT,
+                            static_cast<int32_t>(request->group_mask));
+    response->success = true;
+    response->message = "OK";
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = orbbec_camera::formatObErrorWithStatus(e);
+  } catch (const std::exception& e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
 void OBCameraNode::getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig::Request>& request,
                                            std::shared_ptr<GetDeviceConfig::Response>& response) {
   (void)request;

orbbec_camera/src/utils.cpp

@@ -679,6 +679,8 @@ OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
     return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING;
   } else if (mode == "HARDWARE_TRIGGERING") {
     return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING;
+  } else if (mode == "GROUP_ACTIONS") {
+    return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_GROUP_ACTIONS;
   } else {
     return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
   }