Merge branch 'feat/gige-action-trigger-modes' into v2-main

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
+The service exposes three trigger modes. Every camera whose device key, group key, and group mask
-match the request will be triggered:
+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
+### Relative-delay trigger
-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
+Set `trigger_mode` to `1` and provide a positive delay in milliseconds. The node reads the host
-host to use synchronized time.
+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: <PTP_TIMESTAMP>}"
+```
+
+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.

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(),
     )

orbbec_camera/src/gige_action_command_node.cpp

@@ -16,8 +16,11 @@
 
 #include "orbbec_camera/gige_action_command_node.h"
 
+#include <chrono>
+#include <cstdint>
 #include <exception>
 #include <functional>
+#include <limits>
 #include <sstream>
 #include <string>
 
@@ -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::milliseconds>(
+                       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<uint64_t>(now);
+  return true;
+}
+
 }  // namespace
 
 GigEActionCommandNode::GigEActionCommandNode(const rclcpp::NodeOptions& node_options)
@@ -53,12 +72,87 @@ void GigEActionCommandNode::sendActionCommandCallback(
     return;
   }
 
-  const std::string destination_ip =
+  const std::string broadcast_ip =
-      request->destination_ip.empty() ? "255.255.255.255" : request->destination_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<uint64_t>::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<uint32_t>::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) {

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
+string broadcast_ip
-# GVCP/PTP timestamp: upper 32 bits are seconds and lower 32 bits are nanoseconds.
+# Select immediate, relative-delay, or absolute-PTP-time triggering.
-# Zero sends the command immediately.
+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