From 2f4e3215ec3cf1e2142bda8ee7896c0498e04aca Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 16 Sep 2026 11:29:47 +0800 Subject: [PATCH] feat: add action command trigger modes --- .../examples/gige_action_command/README.md | 45 ++++++-- .../multi_gige_action_command.launch.py | 2 + .../src/gige_action_command_node.cpp | 100 +++++++++++++++++- orbbec_camera_msgs/srv/SendActionCommand.srv | 15 ++- 4 files changed, 149 insertions(+), 13 deletions(-) diff --git a/orbbec_camera/examples/gige_action_command/README.md b/orbbec_camera/examples/gige_action_command/README.md index a2663fef..1526c9de 100644 --- a/orbbec_camera/examples/gige_action_command/README.md +++ b/orbbec_camera/examples/gige_action_command/README.md @@ -53,16 +53,47 @@ ros2 service call /camera_01/get_action_config \ ## Trigger the group -Send an immediate broadcast command. Every camera whose device key, group key, and group mask -match the request will be triggered: +The service exposes three trigger modes. Every camera whose device key, group key, and group mask +match the request will be triggered. + +### Immediate trigger + +Set `trigger_mode` to `0`. The delay and scheduled time fields must be zero: ```bash ros2 service call /gige_action_command_node/send_action_command \ orbbec_camera_msgs/srv/SendActionCommand \ - "{device_key: 1, group_key: 1, group_mask: 1, destination_ip: '255.255.255.255', scheduled_time: 0}" + "{device_key: 1, group_key: 1, group_mask: 1, broadcast_ip: '255.255.255.255', trigger_mode: 0, delay_ms: 0, scheduled_time: 0}" ``` -`success: true` means the host dispatched the GVCP command; the protocol does not return a device -acknowledgment. A nonzero `scheduled_time` uses a GVCP/PTP timestamp, with seconds in the upper -32 bits and nanoseconds in the lower 32 bits. Scheduled triggering requires the cameras and sender -host to use synchronized time. +### Relative-delay trigger + +Set `trigger_mode` to `1` and provide a positive delay in milliseconds. The node reads the host +system clock, adds the delay, and converts the result to the absolute GVCP/PTP timestamp expected by +the SDK. This example schedules the command one second in the future: + +```bash +ros2 service call /gige_action_command_node/send_action_command \ + orbbec_camera_msgs/srv/SendActionCommand \ + "{device_key: 1, group_key: 1, group_mask: 1, broadcast_ip: '255.255.255.255', trigger_mode: 1, delay_ms: 1000, scheduled_time: 0}" +``` + +The host `CLOCK_REALTIME` must be synchronized to the same PTP domain as the cameras, for example +by using `phc2sys`. The launch file enables camera PTP synchronization, but it does not configure +the host PTP services. Choose a delay long enough for the command to reach the cameras before its +target time. + +### Absolute PTP-time trigger + +Set `trigger_mode` to `2`, leave `delay_ms` at zero, and provide a future encoded PTP timestamp. The +upper 32 bits contain seconds and the lower 32 bits contain nanoseconds: + +```bash +ros2 service call /gige_action_command_node/send_action_command \ + orbbec_camera_msgs/srv/SendActionCommand \ + "{device_key: 1, group_key: 1, group_mask: 1, broadcast_ip: '255.255.255.255', trigger_mode: 2, delay_ms: 0, scheduled_time: }" +``` + +The response returns `encoded_scheduled_time`, the exact 64-bit value sent to the SDK. For delayed +triggering this is the timestamp calculated by the node. `success: true` means the host dispatched +the GVCP command; the protocol does not return a device acknowledgment. diff --git a/orbbec_camera/examples/gige_action_command/multi_gige_action_command.launch.py b/orbbec_camera/examples/gige_action_command/multi_gige_action_command.launch.py index 3efeb0bf..686f16b0 100644 --- a/orbbec_camera/examples/gige_action_command/multi_gige_action_command.launch.py +++ b/orbbec_camera/examples/gige_action_command/multi_gige_action_command.launch.py @@ -21,6 +21,7 @@ def generate_launch_description(): "net_device_ip": "192.168.1.10", "net_device_port": "8090", "sync_mode": "group_actions", + "enable_ptp_config": "true", "log_file_name": "camera_01.log", }.items(), ) @@ -33,6 +34,7 @@ def generate_launch_description(): "net_device_ip": "192.168.1.11", "net_device_port": "8090", "sync_mode": "group_actions", + "enable_ptp_config": "true", "log_file_name": "camera_02.log", }.items(), ) diff --git a/orbbec_camera/src/gige_action_command_node.cpp b/orbbec_camera/src/gige_action_command_node.cpp index f1e201b5..6cc0dbbb 100644 --- a/orbbec_camera/src/gige_action_command_node.cpp +++ b/orbbec_camera/src/gige_action_command_node.cpp @@ -16,8 +16,11 @@ #include "orbbec_camera/gige_action_command_node.h" +#include +#include #include #include +#include #include #include @@ -26,6 +29,10 @@ namespace orbbec_camera { namespace { +constexpr uint64_t kNanosecondsPerSecond = 1000000000ULL; +constexpr uint64_t kMillisecondsPerSecond = 1000ULL; +constexpr uint64_t kNanosecondsPerMillisecond = 1000000ULL; + std::string formatObError(const ob::Error& error) { std::ostringstream stream; stream << (error.getMessage() ? error.getMessage() : "Unknown OB error") @@ -33,6 +40,18 @@ std::string formatObError(const ob::Error& error) { return stream.str(); } +bool getSystemTimeMilliseconds(uint64_t* milliseconds, std::string* error_message) { + const auto now = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + if (now < 0) { + *error_message = "System clock returned a time before its epoch"; + return false; + } + *milliseconds = static_cast(now); + return true; +} + } // namespace GigEActionCommandNode::GigEActionCommandNode(const rclcpp::NodeOptions& node_options) @@ -53,12 +72,87 @@ void GigEActionCommandNode::sendActionCommandCallback( return; } - const std::string destination_ip = - request->destination_ip.empty() ? "255.255.255.255" : request->destination_ip; + const std::string broadcast_ip = + request->broadcast_ip.empty() ? "255.255.255.255" : request->broadcast_ip; + using Request = orbbec_camera_msgs::srv::SendActionCommand::Request; + uint64_t action_time = 0; + std::string validation_error; + + switch (request->trigger_mode) { + case Request::TRIGGER_MODE_IMMEDIATE: + if (request->delay_ms != 0 || request->scheduled_time != 0) { + validation_error = "Immediate trigger requires zero delay and zero scheduled time"; + } + break; + + case Request::TRIGGER_MODE_DELAYED: { + if (request->delay_ms == 0) { + validation_error = "Action Command delay must be greater than zero"; + break; + } + if (request->scheduled_time != 0) { + validation_error = "Delayed trigger requires zero scheduled time"; + break; + } + + uint64_t now_milliseconds = 0; + if (!getSystemTimeMilliseconds(&now_milliseconds, &validation_error)) { + break; + } + const uint64_t delay_milliseconds = request->delay_ms; + if (delay_milliseconds > (std::numeric_limits::max)() - now_milliseconds) { + validation_error = "Action Command target time overflows the timestamp range"; + break; + } + const uint64_t target_milliseconds = now_milliseconds + delay_milliseconds; + const uint64_t seconds = target_milliseconds / kMillisecondsPerSecond; + if (seconds > (std::numeric_limits::max)()) { + validation_error = "PTP seconds exceed the 32-bit GVCP timestamp range"; + break; + } + const uint64_t nanoseconds = + (target_milliseconds % kMillisecondsPerSecond) * kNanosecondsPerMillisecond; + action_time = (seconds << 32) | nanoseconds; + break; + } + + case Request::TRIGGER_MODE_TIMESTAMP: { + if (request->delay_ms != 0) { + validation_error = "PTP timestamp trigger requires zero delay"; + break; + } + if (request->scheduled_time == 0) { + validation_error = "Action Command scheduled time must be greater than zero"; + break; + } + + const uint64_t nanoseconds = request->scheduled_time & 0xFFFFFFFFULL; + if (nanoseconds >= kNanosecondsPerSecond) { + validation_error = "PTP nanoseconds must be less than 1000000000"; + break; + } + + action_time = request->scheduled_time; + break; + } + + default: + validation_error = "Unsupported Action Command trigger mode"; + break; + } + + if (!validation_error.empty()) { + response->success = false; + response->encoded_scheduled_time = 0; + response->message = validation_error; + return; + } + + response->encoded_scheduled_time = action_time; try { response->success = context_->sendActionCommand(request->device_key, request->group_key, request->group_mask, - destination_ip.c_str(), request->scheduled_time); + broadcast_ip.c_str(), action_time); response->message = response->success ? "Action Command dispatched" : "SDK failed to send Action Command"; } catch (const ob::Error& error) { diff --git a/orbbec_camera_msgs/srv/SendActionCommand.srv b/orbbec_camera_msgs/srv/SendActionCommand.srv index c8a9628a..2c4cbb4c 100644 --- a/orbbec_camera_msgs/srv/SendActionCommand.srv +++ b/orbbec_camera_msgs/srv/SendActionCommand.srv @@ -1,12 +1,21 @@ +uint8 TRIGGER_MODE_IMMEDIATE=0 +uint8 TRIGGER_MODE_DELAYED=1 +uint8 TRIGGER_MODE_TIMESTAMP=2 + uint32 device_key uint32 group_key uint32 group_mask # IPv4 destination. Leave empty to use the SDK broadcast address (255.255.255.255). -string destination_ip -# GVCP/PTP timestamp: upper 32 bits are seconds and lower 32 bits are nanoseconds. -# Zero sends the command immediately. +string broadcast_ip +# Select immediate, relative-delay, or absolute-PTP-time triggering. +uint8 trigger_mode +# Used only with TRIGGER_MODE_DELAYED. Must be positive. +uint32 delay_ms +# Used only with TRIGGER_MODE_TIMESTAMP. Upper 32 bits are seconds and lower 32 bits are nanoseconds. uint64 scheduled_time --- # Success means that the SDK dispatched the command; Action Command has no device acknowledgment. bool success +# Exact GVCP/PTP value sent to the SDK. Zero is used for an immediate command. +uint64 encoded_scheduled_time string message