From 8e8c183e03fba9793d5cdce4911c2952efd0e2d2 Mon Sep 17 00:00:00 2001 From: slz Date: Thu, 3 Sep 2026 10:56:30 +0800 Subject: [PATCH] feat(camera): support GigE Action Commands --- .../include/orbbec_camera/ob_camera_node.h | 12 ++ .../orbbec_camera/ob_camera_node_driver.h | 6 + orbbec_camera/src/ob_camera_node_driver.cpp | 41 ++++++ orbbec_camera/src/ros_service.cpp | 119 ++++++++++++++++++ orbbec_camera/src/utils.cpp | 2 + 5 files changed, 180 insertions(+) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 30427d15..0e5a067d 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/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& request, std::shared_ptr& response); + void getActionConfigCallback(const std::shared_ptr& request, + std::shared_ptr& response); + + void setActionConfigCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void getSDKVersion(const std::shared_ptr& request, std::shared_ptr& response); @@ -778,6 +788,8 @@ class OBCameraNode { std::map::SharedPtr> set_ae_roi_srv_; rclcpp::Service::SharedPtr get_device_srv_; rclcpp::Service::SharedPtr get_device_config_srv_; + rclcpp::Service::SharedPtr get_action_config_srv_; + rclcpp::Service::SharedPtr set_action_config_srv_; rclcpp::Service::SharedPtr set_laser_enable_srv_; rclcpp::Service::SharedPtr set_ldp_enable_srv_; rclcpp::Service::SharedPtr get_ldp_status_srv_; 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 a132d177..3949f5be 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/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 request, std::shared_ptr response); + void sendActionCommandCallback( + const std::shared_ptr request, + std::shared_ptr 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::SharedPtr reboot_device_srv_ = nullptr; rclcpp::Service::SharedPtr set_bag_recording_srv_ = nullptr; + rclcpp::Service::SharedPtr send_action_command_srv_ = + nullptr; std::chrono::time_point start_time_; std::string extension_path_; static backward::SignalHandling sh; // for stack trace diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index a22d6d09..e146a3e9 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -386,6 +386,9 @@ void OBCameraNodeDriver::init() { set_bag_recording_srv_ = this->create_service( "set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this, std::placeholders::_1, std::placeholders::_2)); + send_action_command_srv_ = this->create_service( + "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 request, + std::shared_ptr response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + std::lock_guard 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 OBCameraNodeDriver::selectDevice( const std::shared_ptr &list) { std::shared_ptr device = nullptr; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 4fd14f55..5814728e 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #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 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( + "get_action_config", [this](const std::shared_ptr request, + std::shared_ptr 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( + "set_action_config", [this](const std::shared_ptr request, + std::shared_ptr response) { + setActionConfigCallback(request, response); + }); + } get_sdk_version_srv_ = node_->create_service( "get_sdk_version", [this](const std::shared_ptr request, @@ -1598,6 +1623,100 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + std::lock_guard 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(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(request->selector)); + response->action_signal_count = static_cast(action_signal_count); + response->device_key = + static_cast(device_->getIntProperty(OB_PROP_ACTION_DEVICE_KEY_INT)); + response->group_key = + static_cast(device_->getIntProperty(OB_PROP_ACTION_GROUP_KEY_INT)); + response->group_mask = + static_cast(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& request, + std::shared_ptr& response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + std::lock_guard 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(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(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(request->device_key)); + device_->setIntProperty(OB_PROP_ACTION_SELECTOR_INT, static_cast(request->selector)); + device_->setIntProperty(OB_PROP_ACTION_GROUP_KEY_INT, static_cast(request->group_key)); + device_->setIntProperty(OB_PROP_ACTION_GROUP_MASK_INT, + static_cast(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& request, std::shared_ptr& response) { (void)request; diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index 3cb41255..674991f1 100644 --- a/orbbec_camera/src/utils.cpp +++ b/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; }