diff --git a/mission/vortex_yasmin_utils/CMakeLists.txt b/mission/vortex_yasmin_utils/CMakeLists.txt index 855f06e..5e8abd8 100644 --- a/mission/vortex_yasmin_utils/CMakeLists.txt +++ b/mission/vortex_yasmin_utils/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(std_srvs REQUIRED) add_library(vortex_yasmin_utils src/landmark_converge_state.cpp src/landmark_polling_state.cpp - src/trigger_wait_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..6e7d9b2 --- /dev/null +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_request_wait_state.hpp @@ -0,0 +1,126 @@ +#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 +#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/trigger_wait_state.hpp b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp similarity index 74% rename from mission/vortex_yasmin_utils/include/vortex_yasmin_utils/trigger_wait_state.hpp rename to mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp index 011c17e..0f8cde8 100644 --- a/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/trigger_wait_state.hpp +++ b/mission/vortex_yasmin_utils/include/vortex_yasmin_utils/service_trigger_wait_state.hpp @@ -1,5 +1,5 @@ -#ifndef VORTEX_YASMIN_UTILS__TRIGGER_WAIT_STATE_HPP_ -#define VORTEX_YASMIN_UTILS__TRIGGER_WAIT_STATE_HPP_ +#ifndef VORTEX_YASMIN_UTILS__SERVICE_TRIGGER_WAIT_STATE_HPP_ +#define VORTEX_YASMIN_UTILS__SERVICE_TRIGGER_WAIT_STATE_HPP_ #include #include @@ -24,11 +24,11 @@ namespace vortex_yasmin_utils { * @param timeout How long to wait before timing out. * A duration of zero (default) means wait indefinitely. */ -class TriggerWaitState : public yasmin::State { +class ServiceTriggerWaitState : public yasmin::State { public: - explicit TriggerWaitState(const std::string& service_name, - std::chrono::duration timeout = - std::chrono::duration(0)); + 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; @@ -51,4 +51,4 @@ class TriggerWaitState : public yasmin::State { } // namespace vortex_yasmin_utils -#endif // VORTEX_YASMIN_UTILS__TRIGGER_WAIT_STATE_HPP_ +#endif // VORTEX_YASMIN_UTILS__SERVICE_TRIGGER_WAIT_STATE_HPP_ diff --git a/mission/vortex_yasmin_utils/src/trigger_wait_state.cpp b/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp similarity index 69% rename from mission/vortex_yasmin_utils/src/trigger_wait_state.cpp rename to mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp index 2c76efa..ba4e91a 100644 --- a/mission/vortex_yasmin_utils/src/trigger_wait_state.cpp +++ b/mission/vortex_yasmin_utils/src/service_trigger_wait_state.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -9,21 +9,23 @@ 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(TriggerWaitState::TIMEOUT); + outcomes.insert(ServiceTriggerWaitState::TIMEOUT); return outcomes; } -TriggerWaitState::TriggerWaitState(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(&TriggerWaitState::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 TriggerWaitState::execute( +std::string ServiceTriggerWaitState::execute( yasmin::Blackboard::SharedPtr blackboard) { { std::lock_guard lock(mutex_); @@ -49,14 +51,14 @@ std::string TriggerWaitState::execute( return yasmin_ros::basic_outcomes::SUCCEED; } -void TriggerWaitState::cancel_state() { +void ServiceTriggerWaitState::cancel_state() { yasmin::State::cancel_state(); cv_.notify_all(); } -void TriggerWaitState::on_triggered(yasmin::Blackboard::SharedPtr) {} +void ServiceTriggerWaitState::on_triggered(yasmin::Blackboard::SharedPtr) {} -void TriggerWaitState::service_callback( +void ServiceTriggerWaitState::service_callback( std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr res) { std::lock_guard lock(mutex_);