Move capture 2D trigger node to samples

zivid_samples/CMakeLists.txt

@@ -120,6 +120,13 @@ zivid_add_cpp_sample(sample_project_and_capture
     ${zivid_interfaces_TARGETS}
 )
 
+add_executable(capture_2d_trigger src/capture_2d_trigger.cpp)
+target_link_libraries(capture_2d_trigger PUBLIC
+  rclcpp::rclcpp
+  ${std_srvs_TARGETS}
+)
+install(TARGETS capture_2d_trigger DESTINATION lib/${PROJECT_NAME})
+
 zivid_add_cpp_sample(sample_with_sdk_capture_and_load_frame
   LINK_TARGETS
     rclcpp::rclcpp

zivid_samples/launch/sample.launch

@@ -1,6 +1,11 @@
 <launch>
   <arg name="sample"/>
   <arg name="serial_number" default="" description="Serial number of camera to connect to"/>
+  <arg name="settings_2d_file_path" default="" description="Path to a 2D settings file for capture_2d"/>
+  <arg name="settings_2d_yaml" default="" description="Inline 2D settings YAML for capture_2d"/>
+  <arg name="capture_2d_wait_for_service_timeout_s" default="0.0" description="Seconds to wait for capture_2d service (&lt;= 0 means wait forever)"/>
+  <arg name="auto_capture_2d" default="false" description="Continuously trigger capture_2d when available"/>
+  <arg name="capture_2d_rate" default="30.0" description="Rate in Hz for auto capture_2d triggers"/>
 
   <!-- For infield correction -->
   <arg name="operation" default=""/>
@@ -15,6 +20,12 @@
 
   <node pkg="zivid_camera" exec="zivid_camera" name="zivid_camera">
     <param name="serial_number" value="$(var serial_number)"/>
+    <param name="settings_2d_file_path" value="$(var settings_2d_file_path)"/>
+    <param name="settings_2d_yaml" value="$(var settings_2d_yaml)"/>
+  </node>
+  <node pkg="zivid_samples" exec="capture_2d_trigger" name="capture_2d_trigger" if="$(var auto_capture_2d)">
+    <param name="rate_hz" value="$(var capture_2d_rate)"/>
+    <param name="wait_for_service_timeout_s" value="$(var capture_2d_wait_for_service_timeout_s)"/>
   </node>
 
   <node pkg="zivid_samples" exec="$(var sample)" name="zivid_sample">

zivid_samples/launch/sample_with_rviz.launch

@@ -1,6 +1,11 @@
 <launch>
   <arg name="sample"/>
   <arg name="serial_number" default="" description="Serial number of camera to connect to"/>
+  <arg name="settings_2d_file_path" default="" description="Path to a 2D settings file for capture_2d"/>
+  <arg name="settings_2d_yaml" default="" description="Inline 2D settings YAML for capture_2d"/>
+  <arg name="capture_2d_wait_for_service_timeout_s" default="0.0" description="Seconds to wait for capture_2d service (&lt;= 0 means wait forever)"/>
+  <arg name="auto_capture_2d" default="false" description="Continuously trigger capture_2d when available"/>
+  <arg name="capture_2d_rate" default="30.0" description="Rate in Hz for auto capture_2d triggers"/>
 
   <!-- For infield correction -->
   <arg name="operation" default=""/>
@@ -15,6 +20,12 @@
 
   <node pkg="zivid_camera" exec="zivid_camera" name="zivid_camera">
     <param name="serial_number" value="$(var serial_number)"/>
+    <param name="settings_2d_file_path" value="$(var settings_2d_file_path)"/>
+    <param name="settings_2d_yaml" value="$(var settings_2d_yaml)"/>
+  </node>
+  <node pkg="zivid_samples" exec="capture_2d_trigger" name="capture_2d_trigger" if="$(var auto_capture_2d)">
+    <param name="rate_hz" value="$(var capture_2d_rate)"/>
+    <param name="wait_for_service_timeout_s" value="$(var capture_2d_wait_for_service_timeout_s)"/>
   </node>
 
   <node pkg="zivid_samples" exec="$(var sample)" name="zivid_sample">

zivid_samples/launch/zivid_camera.launch

@@ -1,7 +1,18 @@
 <launch>
   <arg name="serial_number" default="" description="Serial number of camera to connect to"/>
+  <arg name="settings_2d_file_path" default="" description="Path to a 2D settings file for capture_2d"/>
+  <arg name="settings_2d_yaml" default="" description="Inline 2D settings YAML for capture_2d"/>
+  <arg name="auto_capture_2d" default="false" description="Continuously trigger capture_2d when available"/>
+  <arg name="capture_2d_rate" default="30.0" description="Rate in Hz for auto capture_2d triggers"/>
+  <arg name="capture_2d_wait_for_service_timeout_s" default="0.0" description="Seconds to wait for capture_2d service (&lt;= 0 means wait forever)"/>
   <node pkg="zivid_camera" exec="zivid_camera" name="zivid_camera">
     <param name="serial_number" value="$(var serial_number)"/>
+    <param name="settings_2d_file_path" value="$(var settings_2d_file_path)"/>
+    <param name="settings_2d_yaml" value="$(var settings_2d_yaml)"/>
+  </node>
+  <node pkg="zivid_samples" exec="capture_2d_trigger" name="capture_2d_trigger" if="$(var auto_capture_2d)">
+    <param name="rate_hz" value="$(var capture_2d_rate)"/>
+    <param name="wait_for_service_timeout_s" value="$(var capture_2d_wait_for_service_timeout_s)"/>
   </node>
 
   <!-- Specify the Zivid camera model for the URDF. See available options in `README.md`. -->

zivid_samples/launch/zivid_camera_with_rviz.launch

@@ -1,7 +1,18 @@
 <launch>
   <arg name="serial_number" default="" description="Serial number of camera to connect to"/>
+  <arg name="settings_2d_file_path" default="" description="Path to a 2D settings file for capture_2d"/>
+  <arg name="settings_2d_yaml" default="" description="Inline 2D settings YAML for capture_2d"/>
+  <arg name="auto_capture_2d" default="false" description="Continuously trigger capture_2d when available"/>
+  <arg name="capture_2d_rate" default="30.0" description="Rate in Hz for auto capture_2d triggers"/>
+  <arg name="capture_2d_wait_for_service_timeout_s" default="0.0" description="Seconds to wait for capture_2d service (&lt;= 0 means wait forever)"/>
   <node pkg="zivid_camera" exec="zivid_camera" name="zivid_camera">
     <param name="serial_number" value="$(var serial_number)"/>
+    <param name="settings_2d_file_path" value="$(var settings_2d_file_path)"/>
+    <param name="settings_2d_yaml" value="$(var settings_2d_yaml)"/>
+  </node>
+  <node pkg="zivid_samples" exec="capture_2d_trigger" name="capture_2d_trigger" if="$(var auto_capture_2d)">
+    <param name="rate_hz" value="$(var capture_2d_rate)"/>
+    <param name="wait_for_service_timeout_s" value="$(var capture_2d_wait_for_service_timeout_s)"/>
   </node>
 
   <node pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share zivid_camera)/config/rviz/zivid.rviz" />

zivid_samples/package.xml

@@ -14,6 +14,7 @@
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>image_transport</depend>
   <depend>builtin_interfaces</depend>
   <depend>zivid_interfaces</depend>

zivid_samples/src/capture_2d_trigger.cpp

@@ -0,0 +1,133 @@
+#include <atomic>
+#include <chrono>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <string>
+
+namespace
+{
+constexpr double default_rate_hz = 30.0;
+constexpr double default_wait_for_service_timeout_s = 0.0;
+}  // namespace
+
+class Capture2DTrigger final : public rclcpp::Node
+{
+public:
+  Capture2DTrigger() : Node("capture_2d_trigger"), start_time_(now())
+  {
+    rate_hz_ = declare_parameter<double>("rate_hz", default_rate_hz);
+    const auto wait_param =
+      declare_parameter("wait_for_service_timeout_s", rclcpp::ParameterValue{});
+    if (rate_hz_ <= 0.0) {
+      RCLCPP_WARN(
+        get_logger(), "Parameter rate_hz must be positive. Falling back to default %.1f Hz.",
+        default_rate_hz);
+      rate_hz_ = default_rate_hz;
+    }
+
+    if (wait_param.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      wait_for_service_timeout_s_ = default_wait_for_service_timeout_s;
+    } else if (wait_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) {
+      wait_for_service_timeout_s_ = static_cast<double>(wait_param.get<int64_t>());
+      RCLCPP_WARN(
+        get_logger(),
+        "Parameter wait_for_service_timeout_s was provided as integer. Converting to %.1f s.",
+        wait_for_service_timeout_s_);
+    } else if (wait_param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
+      wait_for_service_timeout_s_ = wait_param.get<double>();
+    } else {
+      throw rclcpp::exceptions::InvalidParameterTypeException(
+        "wait_for_service_timeout_s", "double or integer");
+    }
+
+    if (wait_for_service_timeout_s_ < 0.0) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Parameter wait_for_service_timeout_s must be >= 0. Falling back to default %.1f s.",
+        default_wait_for_service_timeout_s);
+      wait_for_service_timeout_s_ = default_wait_for_service_timeout_s;
+    }
+
+    capture_2d_client_ = create_client<std_srvs::srv::Trigger>("capture_2d");
+  }
+
+  bool waitForServiceAndStartTimer()
+  {
+    while (rclcpp::ok()) {
+      if (capture_2d_client_->wait_for_service(std::chrono::seconds(1))) {
+        break;
+      }
+
+      if (!waiting_for_service_.exchange(true)) {
+        RCLCPP_INFO(get_logger(), "Waiting for capture_2d service to become available...");
+      }
+
+      if (wait_for_service_timeout_s_ > 0.0) {
+        const auto elapsed = now() - start_time_;
+        if (elapsed.seconds() >= wait_for_service_timeout_s_) {
+          RCLCPP_ERROR(
+            get_logger(), "capture_2d service was not available after %.1f s. Shutting down.",
+            wait_for_service_timeout_s_);
+          rclcpp::shutdown();
+          return false;
+        }
+      }
+    }
+
+    if (!rclcpp::ok()) {
+      return false;
+    }
+
+    const auto period = std::chrono::duration_cast<std::chrono::milliseconds>(
+      std::chrono::duration<double>(1.0 / rate_hz_));
+    timer_ = create_wall_timer(period, std::bind(&Capture2DTrigger::onTimer, this));
+    return true;
+  }
+
+private:
+  void onTimer()
+  {
+    if (!capture_2d_client_->service_is_ready()) {
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 2000, "capture_2d service not ready; skipping tick");
+      return;
+    }
+
+    waiting_for_service_.store(false);
+
+    if (request_in_flight_.exchange(true)) {
+      return;
+    }
+
+    auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
+    capture_2d_client_->async_send_request(
+      request, [this](rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture future) {
+        request_in_flight_.store(false);
+        const auto response = future.get();
+        if (!response->success) {
+          RCLCPP_WARN_THROTTLE(
+            get_logger(), *get_clock(), 2000, "capture_2d failed: %s", response->message.c_str());
+        }
+      });
+  }
+
+  double rate_hz_{default_rate_hz};
+  double wait_for_service_timeout_s_{default_wait_for_service_timeout_s};
+  std::atomic_bool request_in_flight_{false};
+  std::atomic_bool waiting_for_service_{false};
+  rclcpp::Time start_time_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr capture_2d_client_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<Capture2DTrigger>();
+  if (node->waitForServiceAndStartTimer()) {
+    rclcpp::spin(node);
+  }
+  rclcpp::shutdown();
+  return 0;
+}