i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(camera): support GigE Action Commands

Browse Source
This commit is contained in:
slz
2026-09-03 15:06:58 +08:00
parent 0caafc5c20
commit 8e8c183e03
5 changed files with 180 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+12
View File
@@ -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
+6
View File
@@ -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
+41
View File
@@ -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
+119
View File
@@ -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
+2
View File
@@ -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;
}