diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index 8db16ed1..26081dc8 100644 --- a/zivid_samples/CMakeLists.txt +++ b/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 diff --git a/zivid_samples/launch/sample.launch b/zivid_samples/launch/sample.launch index 791d79e1..dc56149d 100644 --- a/zivid_samples/launch/sample.launch +++ b/zivid_samples/launch/sample.launch @@ -1,6 +1,11 @@ + + + + + @@ -15,6 +20,12 @@ + + + + + + diff --git a/zivid_samples/launch/sample_with_rviz.launch b/zivid_samples/launch/sample_with_rviz.launch index 52d14915..d8037d3f 100644 --- a/zivid_samples/launch/sample_with_rviz.launch +++ b/zivid_samples/launch/sample_with_rviz.launch @@ -1,6 +1,11 @@ + + + + + @@ -15,6 +20,12 @@ + + + + + + diff --git a/zivid_samples/launch/zivid_camera.launch b/zivid_samples/launch/zivid_camera.launch index 6fa6fc36..933af18d 100644 --- a/zivid_samples/launch/zivid_camera.launch +++ b/zivid_samples/launch/zivid_camera.launch @@ -1,7 +1,18 @@ + + + + + + + + + + + diff --git a/zivid_samples/launch/zivid_camera_with_rviz.launch b/zivid_samples/launch/zivid_camera_with_rviz.launch index 6f9a90e6..cfa4f106 100644 --- a/zivid_samples/launch/zivid_camera_with_rviz.launch +++ b/zivid_samples/launch/zivid_camera_with_rviz.launch @@ -1,7 +1,18 @@ + + + + + + + + + + + diff --git a/zivid_samples/package.xml b/zivid_samples/package.xml index 47ba63fc..0af02da0 100644 --- a/zivid_samples/package.xml +++ b/zivid_samples/package.xml @@ -14,6 +14,7 @@ rclcpp_components sensor_msgs std_msgs + std_srvs image_transport builtin_interfaces zivid_interfaces diff --git a/zivid_samples/src/capture_2d_trigger.cpp b/zivid_samples/src/capture_2d_trigger.cpp new file mode 100644 index 00000000..3db43ab1 --- /dev/null +++ b/zivid_samples/src/capture_2d_trigger.cpp @@ -0,0 +1,133 @@ +#include +#include +#include +#include +#include +#include + +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("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(wait_param.get()); + 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(); + } 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("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::duration(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(); + capture_2d_client_->async_send_request( + request, [this](rclcpp::Client::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::SharedPtr capture_2d_client_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + if (node->waitForServiceAndStartTimer()) { + rclcpp::spin(node); + } + rclcpp::shutdown(); + return 0; +}