From 07746826e54dc0e004c88efefc0c2e1df1de8c85 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 26 Mar 2026 11:29:35 +0100 Subject: [PATCH 1/2] service request state, renamed trigger state --- mission/vortex_yasmin_utils/CMakeLists.txt | 1 + .../service_request_wait_state.hpp | 128 ++++++++++++++++++ .../service_trigger_wait_state.hpp | 54 ++++++++ .../src/service_trigger_wait_state.cpp | 70 ++++++++++ 4 files changed, 253 insertions(+) create mode 100644 mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp create mode 100644 mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp create mode 100644 mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp diff --git a/mission/vortex_yasmin_utils/CMakeLists.txt b/mission/vortex_yasmin_utils/CMakeLists.txt index f473212..33af65b 100644 --- a/mission/vortex_yasmin_utils/CMakeLists.txt +++ b/mission/vortex_yasmin_utils/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(geometry_msgs REQUIRED) add_library(vortex_yasmin_utils src/converge_state.cpp src/landmark_polling_state.cpp + src/service_trigger_wait_state.cpp ) target_include_directories(vortex_yasmin_utils PUBLIC diff --git a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp new file mode 100644 index 0000000..e6df164 --- /dev/null +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp @@ -0,0 +1,128 @@ +#ifndef VORTEX_YASMIN_UTILS__SERVICE_REQUEST_WAIT_STATE_HPP_ +#define VORTEX_YASMIN_UTILS__SERVICE_REQUEST_WAIT_STATE_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace vortex_yasmin_utils { + +/** + * @brief A YASMIN state that blocks until a ROS service of type ServiceT is + * called, then stores the request on the blackboard. + * + * This is a generic version of ServiceTriggerWaitState that works with any service + * type, making the service request data available to downstream states via the + * blackboard. + * + * @tparam ServiceT The ROS service type (e.g. your_msgs::srv::SetPose). + * The response type must have a `success` bool field. + * + * @param service_name The fully-qualified ROS service name to advertise. + * @param request_bb_key Blackboard key under which the request is stored. + * @param timeout How long to wait before timing out. + * A duration of zero (default) means wait indefinitely. + */ +template +class ServiceRequestWaitState : public yasmin::State { + public: + explicit ServiceRequestWaitState( + const std::string& service_name, + const std::string& request_bb_key, + std::chrono::duration timeout = + std::chrono::duration(0)) + : yasmin::State(make_outcomes(timeout.count() > 0)), + request_bb_key_(request_bb_key), + timeout_(timeout) { + auto node = yasmin_ros::YasminNode::get_instance(); + + service_ = node->create_service( + service_name, + std::bind(&ServiceRequestWaitState::service_callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + std::string execute(yasmin::Blackboard::SharedPtr blackboard) override { + { + std::lock_guard lock(mutex_); + triggered_ = false; + } + + std::unique_lock lock(mutex_); + + auto predicate = [this] { return triggered_ || is_canceled(); }; + + if (timeout_.count() > 0) { + if (!cv_.wait_for(lock, timeout_, predicate)) { + return TIMEOUT; + } + } else { + cv_.wait(lock, predicate); + } + + if (is_canceled()) + return yasmin_ros::basic_outcomes::CANCEL; + + blackboard->set( + request_bb_key_, last_request_); + + on_triggered(blackboard, last_request_); + return yasmin_ros::basic_outcomes::SUCCEED; + } + + void cancel_state() override { + yasmin::State::cancel_state(); + cv_.notify_all(); + } + + static constexpr const char* TIMEOUT = "timeout"; + + protected: + virtual void on_triggered( + yasmin::Blackboard::SharedPtr blackboard, + typename ServiceT::Request::SharedPtr request) { + (void)blackboard; + (void)request; + } + + private: + static std::set make_outcomes(bool has_timeout) { + std::set outcomes{yasmin_ros::basic_outcomes::SUCCEED, + yasmin_ros::basic_outcomes::CANCEL}; + if (has_timeout) + outcomes.insert(TIMEOUT); + return outcomes; + } + + void service_callback( + typename ServiceT::Request::SharedPtr req, + typename ServiceT::Response::SharedPtr res) { + std::lock_guard lock(mutex_); + + last_request_ = req; + triggered_ = true; + res->success = true; + + cv_.notify_all(); + } + + typename rclcpp::Service::SharedPtr service_; + typename ServiceT::Request::SharedPtr last_request_; + std::string request_bb_key_; + std::condition_variable cv_; + std::mutex mutex_; + bool triggered_{false}; + std::chrono::duration timeout_; +}; + +} // namespace vortex_yasmin_utils + +#endif // VORTEX_YASMIN_UTILS__SERVICE_REQUEST_WAIT_STATE_HPP_ diff --git a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp new file mode 100644 index 0000000..1156b9f --- /dev/null +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp @@ -0,0 +1,54 @@ +#ifndef VORTEX_YASMIN_UTILS__SERVICE_TRIGGER_WAIT_STATE_HPP_ +#define VORTEX_YASMIN_UTILS__SERVICE_TRIGGER_WAIT_STATE_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include + +namespace vortex_yasmin_utils { + +/** + * @brief A YASMIN state that blocks until a std_srvs/Trigger service is called. + * + * Creates a ROS service server and waits for it to be triggered. + * Returns SUCCEED when the service is called, CANCEL if the state is canceled, + * or TIMEOUT if the optional timeout expires. + * + * @param service_name The fully-qualified ROS service name to advertise. + * @param timeout How long to wait before timing out. + * A duration of zero (default) means wait indefinitely. + */ +class ServiceTriggerWaitState : public yasmin::State { + public: + explicit ServiceTriggerWaitState(const std::string& service_name, + std::chrono::duration timeout = + std::chrono::duration(0)); + + std::string execute(yasmin::Blackboard::SharedPtr blackboard) override; + void cancel_state() override; + + static constexpr const char* TIMEOUT = "timeout"; + + protected: + virtual void on_triggered(yasmin::Blackboard::SharedPtr blackboard); + + private: + void service_callback(std_srvs::srv::Trigger::Request::SharedPtr req, + std_srvs::srv::Trigger::Response::SharedPtr res); + + rclcpp::Service::SharedPtr service_; + std::condition_variable cv_; + std::mutex mutex_; + bool triggered_{false}; + std::chrono::duration timeout_; +}; + +} // namespace vortex_yasmin_utils + +#endif // VORTEX_YASMIN_UTILS__SERVICE_TRIGGER_WAIT_STATE_HPP_ diff --git a/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp b/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp new file mode 100644 index 0000000..e05e93c --- /dev/null +++ b/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp @@ -0,0 +1,70 @@ +#include + +#include +#include + +namespace vortex_yasmin_utils { + +static std::set make_outcomes(bool has_timeout) { + std::set outcomes{yasmin_ros::basic_outcomes::SUCCEED, + yasmin_ros::basic_outcomes::CANCEL}; + if (has_timeout) + outcomes.insert(ServiceTriggerWaitState::TIMEOUT); + return outcomes; +} + +ServiceTriggerWaitState::ServiceTriggerWaitState(const std::string& service_name, + std::chrono::duration timeout) + : yasmin::State(make_outcomes(timeout.count() > 0)), timeout_(timeout) { + auto node = yasmin_ros::YasminNode::get_instance(); + + service_ = node->create_service( + service_name, std::bind(&ServiceTriggerWaitState::service_callback, this, + std::placeholders::_1, std::placeholders::_2)); +} + +std::string ServiceTriggerWaitState::execute( + yasmin::Blackboard::SharedPtr blackboard) { + { + std::lock_guard lock(mutex_); + triggered_ = false; + } + + std::unique_lock lock(mutex_); + + auto predicate = [this] { return triggered_ || is_canceled(); }; + + if (timeout_.count() > 0) { + if (!cv_.wait_for(lock, timeout_, predicate)) { + return TIMEOUT; + } + } else { + cv_.wait(lock, predicate); + } + + if (is_canceled()) + return yasmin_ros::basic_outcomes::CANCEL; + + on_triggered(blackboard); + return yasmin_ros::basic_outcomes::SUCCEED; +} + +void ServiceTriggerWaitState::cancel_state() { + yasmin::State::cancel_state(); + cv_.notify_all(); +} + +void ServiceTriggerWaitState::on_triggered(yasmin::Blackboard::SharedPtr) {} + +void ServiceTriggerWaitState::service_callback( + std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr res) { + std::lock_guard lock(mutex_); + + triggered_ = true; + res->success = true; + + cv_.notify_all(); +} + +} // namespace vortex_yasmin_utils From eea0adb362faaaf39635504c2ebac02d15c83784 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 26 Mar 2026 11:31:09 +0100 Subject: [PATCH 2/2] pre-commit --- .../landmark_polling_state.hpp | 5 ++-- .../service_request_wait_state.hpp | 30 +++++++++---------- .../service_trigger_wait_state.hpp | 4 +-- .../src/converge_state.cpp | 7 ++--- .../src/service_trigger_wait_state.cpp | 10 ++++--- 5 files changed, 27 insertions(+), 29 deletions(-) diff --git a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_polling_state.hpp b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_polling_state.hpp index 7e77890..ac49a27 100644 --- a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_polling_state.hpp +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_polling_state.hpp @@ -31,9 +31,8 @@ class LandmarkPollingState LandmarkPollingAction::Goal create_goal( yasmin::Blackboard::SharedPtr blackboard); - std::string result_handler( - yasmin::Blackboard::SharedPtr blackboard, - LandmarkPollingAction::Result::SharedPtr result); + std::string result_handler(yasmin::Blackboard::SharedPtr blackboard, + LandmarkPollingAction::Result::SharedPtr result); private: vortex_msgs::msg::LandmarkType landmark_type_; diff --git a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp index e6df164..6e7d9b2 100644 --- a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -19,9 +20,9 @@ namespace vortex_yasmin_utils { * @brief A YASMIN state that blocks until a ROS service of type ServiceT is * called, then stores the request on the blackboard. * - * This is a generic version of ServiceTriggerWaitState that works with any service - * type, making the service request data available to downstream states via the - * blackboard. + * This is a generic version of ServiceTriggerWaitState that works with any + * service type, making the service request data available to downstream states + * via the blackboard. * * @tparam ServiceT The ROS service type (e.g. your_msgs::srv::SetPose). * The response type must have a `success` bool field. @@ -34,11 +35,10 @@ namespace vortex_yasmin_utils { template class ServiceRequestWaitState : public yasmin::State { public: - explicit ServiceRequestWaitState( - const std::string& service_name, - const std::string& request_bb_key, - std::chrono::duration timeout = - std::chrono::duration(0)) + explicit ServiceRequestWaitState(const std::string& service_name, + const std::string& request_bb_key, + std::chrono::duration timeout = + std::chrono::duration(0)) : yasmin::State(make_outcomes(timeout.count() > 0)), request_bb_key_(request_bb_key), timeout_(timeout) { @@ -71,8 +71,8 @@ class ServiceRequestWaitState : public yasmin::State { if (is_canceled()) return yasmin_ros::basic_outcomes::CANCEL; - blackboard->set( - request_bb_key_, last_request_); + blackboard->set(request_bb_key_, + last_request_); on_triggered(blackboard, last_request_); return yasmin_ros::basic_outcomes::SUCCEED; @@ -86,9 +86,8 @@ class ServiceRequestWaitState : public yasmin::State { static constexpr const char* TIMEOUT = "timeout"; protected: - virtual void on_triggered( - yasmin::Blackboard::SharedPtr blackboard, - typename ServiceT::Request::SharedPtr request) { + virtual void on_triggered(yasmin::Blackboard::SharedPtr blackboard, + typename ServiceT::Request::SharedPtr request) { (void)blackboard; (void)request; } @@ -102,9 +101,8 @@ class ServiceRequestWaitState : public yasmin::State { return outcomes; } - void service_callback( - typename ServiceT::Request::SharedPtr req, - typename ServiceT::Response::SharedPtr res) { + void service_callback(typename ServiceT::Request::SharedPtr req, + typename ServiceT::Response::SharedPtr res) { std::lock_guard lock(mutex_); last_request_ = req; diff --git a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp index 1156b9f..0f8cde8 100644 --- a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp @@ -27,8 +27,8 @@ namespace vortex_yasmin_utils { class ServiceTriggerWaitState : public yasmin::State { public: explicit ServiceTriggerWaitState(const std::string& service_name, - std::chrono::duration timeout = - std::chrono::duration(0)); + std::chrono::duration timeout = + std::chrono::duration(0)); std::string execute(yasmin::Blackboard::SharedPtr blackboard) override; void cancel_state() override; diff --git a/mission/vortex_yasmin_utils/src/converge_state.cpp b/mission/vortex_yasmin_utils/src/converge_state.cpp index 1004144..cba046a 100644 --- a/mission/vortex_yasmin_utils/src/converge_state.cpp +++ b/mission/vortex_yasmin_utils/src/converge_state.cpp @@ -11,10 +11,9 @@ namespace vortex_yasmin_utils { ConvergeState::ConvergeState( const std::string& action_server_name, vortex::utils::waypoints::LandmarkConvergenceGoal convergence_goal) - : ActionState(action_server_name, - std::bind(&ConvergeState::create_goal, - this, - std::placeholders::_1)), + : ActionState( + action_server_name, + std::bind(&ConvergeState::create_goal, this, std::placeholders::_1)), convergence_goal_(std::move(convergence_goal)) {} WaypointManagerAction::Goal ConvergeState::create_goal( diff --git a/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp b/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp index e05e93c..ba4e91a 100644 --- a/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp +++ b/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp @@ -13,14 +13,16 @@ static std::set make_outcomes(bool has_timeout) { return outcomes; } -ServiceTriggerWaitState::ServiceTriggerWaitState(const std::string& service_name, - std::chrono::duration timeout) +ServiceTriggerWaitState::ServiceTriggerWaitState( + const std::string& service_name, + std::chrono::duration timeout) : yasmin::State(make_outcomes(timeout.count() > 0)), timeout_(timeout) { auto node = yasmin_ros::YasminNode::get_instance(); service_ = node->create_service( - service_name, std::bind(&ServiceTriggerWaitState::service_callback, this, - std::placeholders::_1, std::placeholders::_2)); + service_name, + std::bind(&ServiceTriggerWaitState::service_callback, this, + std::placeholders::_1, std::placeholders::_2)); } std::string ServiceTriggerWaitState::execute(