From 7c15da27971a954e56b5b70dd9852bf7232e2f5e Mon Sep 17 00:00:00 2001 From: Stefan Fabian Date: Fri, 25 Oct 2024 15:13:01 +0200 Subject: [PATCH] Added BabelFishActionServer with tests. --- ros_babel_fish/CMakeLists.txt | 5 + .../include/ros_babel_fish/babel_fish.hpp | 18 ++ .../detail/babel_fish_action_server.hpp | 131 +++++++- ros_babel_fish/src/babel_fish.cpp | 55 +++- .../src/detail/babel_fish_action_server.cpp | 281 ++++++++++++++++++ ros_babel_fish/test/action_server.cpp | 176 +++++++++++ 6 files changed, 650 insertions(+), 16 deletions(-) create mode 100644 ros_babel_fish/src/detail/babel_fish_action_server.cpp create mode 100644 ros_babel_fish/test/action_server.cpp diff --git a/ros_babel_fish/CMakeLists.txt b/ros_babel_fish/CMakeLists.txt index b2161e7..a3505a4 100644 --- a/ros_babel_fish/CMakeLists.txt +++ b/ros_babel_fish/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(rosidl_typesupport_introspection_cpp REQUIRED) set(SOURCES src/detail/babel_fish_action_client.cpp + src/detail/babel_fish_action_server.cpp src/detail/babel_fish_publisher.cpp src/detail/babel_fish_service.cpp src/detail/babel_fish_service_client.cpp @@ -155,6 +156,10 @@ if (BUILD_TESTING) ament_add_gtest(test_action_client test/action_client.cpp) ament_target_dependencies(test_action_client example_interfaces ros_babel_fish_test_msgs) target_link_libraries(test_action_client ${PROJECT_NAME}) + + ament_add_gtest(test_action_server test/action_server.cpp) + ament_target_dependencies(test_action_server example_interfaces ros_babel_fish_test_msgs) + target_link_libraries(test_action_server ${PROJECT_NAME}) endif () # to run: catkin build ros_babel_fish --no-deps -DENABLE_COVERAGE_TESTING=ON -DCMAKE_BUILD_TYPE=Debug -v --catkin-make-args ros_babel_fish_coverage diff --git a/ros_babel_fish/include/ros_babel_fish/babel_fish.hpp b/ros_babel_fish/include/ros_babel_fish/babel_fish.hpp index 3467566..d7d6e2b 100644 --- a/ros_babel_fish/include/ros_babel_fish/babel_fish.hpp +++ b/ros_babel_fish/include/ros_babel_fish/babel_fish.hpp @@ -5,6 +5,7 @@ #define ROS_BABEL_FISH_BABEL_FISH_HPP #include "ros_babel_fish/detail/babel_fish_action_client.hpp" +#include "ros_babel_fish/detail/babel_fish_action_server.hpp" #include "ros_babel_fish/detail/babel_fish_publisher.hpp" #include "ros_babel_fish/detail/babel_fish_service.hpp" #include "ros_babel_fish/detail/babel_fish_service_client.hpp" @@ -150,6 +151,23 @@ class BabelFish : public std::enable_shared_from_this const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr ); + /*! + * Creates an action server for the given name and type. + * @param name The name under which the action server is registered. + * @param type They type of the action. + * @param handle_goal Callback when a new goal was received. + * @param handle_cancel Callback when a cancel request was received. + * @param handle_accepted Callback when a goal was accepted. Should start executing the goal. + * @return An action server that goals can be sent to for processing. + */ + BabelFishActionServer::SharedPtr create_action_server( + rclcpp::Node &node, const std::string &name, const std::string &type, + BabelFishActionServer::GoalCallback handle_goal, + BabelFishActionServer::CancelCallback handle_cancel, + BabelFishActionServer::AcceptedCallback handle_accepted, + const rcl_action_server_options_t &options = rcl_action_server_get_default_options(), + rclcpp::CallbackGroup::SharedPtr group = nullptr ); + /*! * Creates an action client for the given name and type. * @param name The name under which the action server is registered. diff --git a/ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action_server.hpp b/ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action_server.hpp index a16dcd9..739fa8e 100644 --- a/ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action_server.hpp +++ b/ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action_server.hpp @@ -1,33 +1,138 @@ // Copyright (c) 2021 Stefan Fabian. All rights reserved. // Licensed under the MIT license. See LICENSE file in the project root for full license information. -#ifndef ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP -#define ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP +#ifndef ROS_BABEL_FISH_BABEL_FISH_ACTION_SERVER_HPP +#define ROS_BABEL_FISH_BABEL_FISH_ACTION_SERVER_HPP -#include +#include "ros_babel_fish/detail/babel_fish_action.hpp" +#include #include #include -namespace ros_babel_fish +namespace rclcpp_action { -class BabelFishService +template<> +class ServerGoalHandle + : public ServerGoalHandleBase, + public std::enable_shared_from_this> { public: - RCLCPP_SMART_PTR_DEFINITIONS( BabelFishService ) + using ActionT = ros_babel_fish::impl::BabelFishAction; + + /// @see ServerGoalHandle::publish_feedback + void publish_feedback( const ActionT::Feedback &feedback_msg ) const; + + /// @see ServerGoalHandle::abort + void abort( const ActionT::Result &result_msg ); + + /// @see ServerGoalHandle::succeed + void succeed( const ActionT::Result &result_msg ); + + // @see ServerGoalHandle::canceled + void canceled( const ActionT::Result &result_msg ); - BabelFishService( - rclcpp::Node *node, const std::string &name, ServiceTypeSupport::ConstSharedPtr type_support, - std::function callback, - rcl_service_options_t options ); + /// @see ServerGoalHandle::execute + void execute(); - rclcpp::ServiceBase::ConstSharedPtr getService() const; + /// Get the user provided message describing the goal. + const std::shared_ptr &get_goal() const { return goal_; } - rclcpp::ServiceBase::SharedPtr getService(); + /// Get the unique identifier of the goal + const GoalUUID &get_goal_id() const { return uuid_; } + + ~ServerGoalHandle() override; + + ActionT::Result create_result_message() const; + + ActionT::Feedback create_feedback_message() const; private: - rclcpp::ServiceBase::SharedPtr service_; + ServerGoalHandle( ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support, + std::shared_ptr rcl_handle, GoalUUID uuid, + std::shared_ptr goal, + std::function )> on_terminal_state, + std::function on_executing, + std::function )> publish_feedback ); + + ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support_; + const std::shared_ptr goal_; + const GoalUUID uuid_; + + friend class Server; + + std::function )> on_terminal_state_; + std::function on_executing_; + std::function )> publish_feedback_; }; + +template<> +class Server + : public ServerBase, + public std::enable_shared_from_this> +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE( Server ) + using ActionT = ros_babel_fish::impl::BabelFishAction; + using GoalHandle = ServerGoalHandle; + + using GoalCallback = + std::function )>; + + using CancelCallback = + std::function> )>; + + using AcceptedCallback = std::function> )>; + + //! Do not call directly, this is private API and might change. Use BabelFish::create_action_server. + Server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string &name, ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support, + const rcl_action_server_options_t &options, GoalCallback handle_goal, + CancelCallback handle_cancel, AcceptedCallback handle_accepted ); + + ~Server() override; + +private: + std::pair> + call_handle_goal_callback( GoalUUID &uuid, std::shared_ptr message ) override; + + CancelResponse call_handle_cancel_callback( const GoalUUID &uuid ) override; + + void call_goal_accepted_callback( std::shared_ptr rcl_goal_handle, + GoalUUID uuid, + std::shared_ptr goal_request_message ) override; + + GoalUUID get_goal_id_from_goal_request( void *message ) override; + + std::shared_ptr create_goal_request() override; + + GoalUUID get_goal_id_from_result_request( void *message ) override; + + std::shared_ptr create_result_request() override; + + std::shared_ptr + create_result_response( decltype( action_msgs::msg::GoalStatus::status ) status ) override; + + ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support_; + + GoalCallback handle_goal_; + CancelCallback handle_cancel_; + AcceptedCallback handle_accepted_; + + using GoalHandleWeakPtr = std::weak_ptr>; + /// A map of goal id to goal handle weak pointers. + /// This is used to provide a goal handle to handle_cancel. + std::unordered_map goal_handles_; + std::mutex goal_handles_mutex_; +}; +} // namespace rclcpp_action + +namespace ros_babel_fish +{ +using BabelFishActionServer = rclcpp_action::Server; +using BabelFishActionServerGoalHandle = rclcpp_action::ServerGoalHandle; } // namespace ros_babel_fish #endif // ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP diff --git a/ros_babel_fish/src/babel_fish.cpp b/ros_babel_fish/src/babel_fish.cpp index 40e1a5b..559ae9f 100644 --- a/ros_babel_fish/src/babel_fish.cpp +++ b/ros_babel_fish/src/babel_fish.cpp @@ -152,6 +152,55 @@ BabelFish::create_service_client( rclcpp::Node &node, const std::string &service return result; } +BabelFishActionServer::SharedPtr BabelFish::create_action_server( + rclcpp::Node &node, const std::string &name, const std::string &type, + BabelFishActionServer::GoalCallback handle_goal, + BabelFishActionServer::CancelCallback handle_cancel, + BabelFishActionServer::AcceptedCallback handle_accepted, + const rcl_action_server_options_t &options, rclcpp::CallbackGroup::SharedPtr group ) +{ + ActionTypeSupport::ConstSharedPtr type_support = get_action_type_support( type ); + if ( type_support == nullptr ) { + throw BabelFishException( "Failed to create an action client for type: " + type + + ". Type not found!" ); + } + std::weak_ptr weak_node = + node.get_node_waitables_interface(); + std::weak_ptr weak_group = group; + bool group_is_null = ( nullptr == group.get() ); + + auto deleter = [weak_node, weak_group, group_is_null]( BabelFishActionServer *ptr ) { + if ( nullptr == ptr ) { + return; + } + if ( auto shared_node = weak_node.lock(); shared_node != nullptr ) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr fake_shared_ptr( + ptr, []( const BabelFishActionServer * ) { /* do nothing */ } ); + + if ( group_is_null ) { + // Was added to default group + shared_node->remove_waitable( fake_shared_ptr, nullptr ); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if ( shared_group ) { + shared_node->remove_waitable( fake_shared_ptr, shared_group ); + } + } + } + delete ptr; + }; + std::shared_ptr server( + new BabelFishActionServer( node.get_node_base_interface(), node.get_node_clock_interface(), + node.get_node_logging_interface(), name, type_support, options, + std::move( handle_goal ), std::move( handle_cancel ), + std::move( handle_accepted ) ), + deleter ); + node.get_node_waitables_interface()->add_waitable( server, std::move( group ) ); + return server; +} + BabelFishActionClient::SharedPtr BabelFish::create_action_client( rclcpp::Node &node, const std::string &name, const std::string &type, const rcl_action_client_options_t &options, @@ -171,10 +220,10 @@ BabelFish::create_action_client( rclcpp::Node &node, const std::string &name, if ( nullptr == ptr ) { return; } - auto shared_node = weak_node.lock(); - if ( shared_node ) { + if ( auto shared_node = weak_node.lock(); shared_node != nullptr ) { // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr fake_shared_ptr( ptr, []( BabelFishActionClient * ) {} ); + std::shared_ptr fake_shared_ptr( + ptr, []( const BabelFishActionClient * ) { /* not ours to delete */ } ); if ( group_is_null ) { // Was added to default group diff --git a/ros_babel_fish/src/detail/babel_fish_action_server.cpp b/ros_babel_fish/src/detail/babel_fish_action_server.cpp new file mode 100644 index 0000000..91834f1 --- /dev/null +++ b/ros_babel_fish/src/detail/babel_fish_action_server.cpp @@ -0,0 +1,281 @@ +// Copyright (c) 2024 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license information. + +#include "../logging.hpp" + +#include "ros_babel_fish/detail/babel_fish_action_server.hpp" +#include "ros_babel_fish/idl/serialization.hpp" +#include "ros_babel_fish/messages/array_message.hpp" + +using CompoundMessage = ros_babel_fish::CompoundMessage; +using UUIDMessage = ros_babel_fish::FixedLengthArrayMessage; + +namespace rclcpp_action +{ + +ServerGoalHandle::ServerGoalHandle( + ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support, + std::shared_ptr rcl_handle, GoalUUID uuid, + std::shared_ptr goal, + std::function )> on_terminal_state, + std::function on_executing, + std::function )> publish_feedback ) + : ServerGoalHandleBase( std::move( rcl_handle ) ), type_support_( std::move( type_support ) ), + goal_( std::move( goal ) ), uuid_( uuid ), + on_terminal_state_( std::move( on_terminal_state ) ), + on_executing_( std::move( on_executing ) ), publish_feedback_( std::move( publish_feedback ) ) +{ +} + +void ServerGoalHandle::publish_feedback( + const ActionT::Feedback &feedback_msg ) const +{ + auto feedback_message = CompoundMessage( *type_support_->feedback_message_type_support ); + feedback_message["goal_id"]["uuid"].as() = uuid_; + feedback_message["feedback"].as() = feedback_msg; + publish_feedback_( feedback_message.type_erased_message() ); +} + +void ServerGoalHandle::abort( const ActionT::Result &result_msg ) +{ + _abort(); + auto response = CompoundMessage( type_support_->result_service_type_support->response() ); + response["status"] = action_msgs::msg::GoalStatus::STATUS_ABORTED; + response["result"].as() = result_msg; + on_terminal_state_( uuid_, response.type_erased_message() ); +} + +void ServerGoalHandle::succeed( const ActionT::Result &result_msg ) +{ + _succeed(); + auto response = CompoundMessage( type_support_->result_service_type_support->response() ); + response["status"] = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + response["result"].as() = result_msg; + on_terminal_state_( uuid_, response.type_erased_message() ); +} + +void ServerGoalHandle::canceled( const ActionT::Result &result_msg ) +{ + _canceled(); + auto response = CompoundMessage( type_support_->result_service_type_support->response() ); + response["status"] = action_msgs::msg::GoalStatus::STATUS_CANCELED; + response["result"].as() = result_msg; + on_terminal_state_( uuid_, response.type_erased_message() ); +} + +void ServerGoalHandle::execute() +{ + _execute(); + on_executing_( uuid_ ); +} + +CompoundMessage ServerGoalHandle::create_result_message() const +{ + auto response_type_support = type_support_->result_service_type_support->response(); + // Find result in response message + size_t i; + for ( i = 0; i < response_type_support->member_count_; ++i ) { + if ( response_type_support.getMember( i )->name_ == std::string( "result" ) ) + break; + } + if ( i == response_type_support->member_count_ ) { + throw ros_babel_fish::BabelFishException( + "Could not create action result message: Result response message does not contain a " + "'result' field" ); + } + auto msg = ros_babel_fish::createContainer( response_type_support.getMember( i ) ); + return { response_type_support.getMember( i ), msg }; +} + +CompoundMessage ServerGoalHandle::create_feedback_message() const +{ + auto feedback_type_support = + ros_babel_fish::MessageMembersIntrospection( *type_support_->feedback_message_type_support ); + // Find feedback in FeedbackMessage + size_t i; + for ( i = 0; i < feedback_type_support->member_count_; ++i ) { + if ( feedback_type_support.getMember( i )->name_ == std::string( "feedback" ) ) + break; + } + if ( i == feedback_type_support->member_count_ ) { + throw ros_babel_fish::BabelFishException( + "Could not create action feedback message: FeedbackMessage does not contain a 'feedback' " + "field" ); + } + auto msg = ros_babel_fish::createContainer( feedback_type_support.getMember( i ) ); + return { feedback_type_support.getMember( i ), msg }; +} + +ServerGoalHandle::~ServerGoalHandle() +{ + // Cancel goal if handle was allowed to destruct without reaching a terminal state + if ( try_canceling() ) { + auto null_result = + CompoundMessage::make_shared( type_support_->result_service_type_support->response() ); + ( *null_result )["status"] = action_msgs::msg::GoalStatus::STATUS_CANCELED; + on_terminal_state_( uuid_, null_result ); + } +} + +// ================================================================================================= +// Server +// ================================================================================================= + +Server::Server( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string &name, + ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support, + const rcl_action_server_options_t &options, + Server::GoalCallback handle_goal, + Server::CancelCallback handle_cancel, + Server::AcceptedCallback handle_accepted ) + : ServerBase( std::move( node_base ), std::move( node_clock ), std::move( node_logging ), name, + &type_support->type_support_handle, options ), + type_support_( std::move( type_support ) ), handle_goal_( std::move( handle_goal ) ), + handle_cancel_( std::move( handle_cancel ) ), handle_accepted_( std::move( handle_accepted ) ) +{ +} + +Server::~Server() = default; + +std::pair> +Server::call_handle_goal_callback( GoalUUID &uuid, + std::shared_ptr message ) +{ + const auto &request_type_support = type_support_->goal_service_type_support->request(); + auto request = CompoundMessage::make_shared( request_type_support, message ); + auto goal = std::shared_ptr( request, &( *request )["goal"].as() ); + GoalResponse user_response = handle_goal_( uuid, goal ); + + const auto &response_type_support = type_support_->goal_service_type_support->response(); + auto response = CompoundMessage( response_type_support ); + response["accepted"] = GoalResponse::ACCEPT_AND_EXECUTE == user_response || + GoalResponse::ACCEPT_AND_DEFER == user_response; + return std::make_pair( user_response, response.type_erased_message() ); +} + +CancelResponse +Server::call_handle_cancel_callback( const GoalUUID &uuid ) +{ + std::shared_ptr> goal_handle; + std::unique_lock lock( goal_handles_mutex_ ); + if ( auto element = goal_handles_.find( uuid ); element != goal_handles_.end() ) { + goal_handle = element->second.lock(); + } + lock.unlock(); + + CancelResponse response = CancelResponse::REJECT; + if ( goal_handle ) { + response = handle_cancel_( goal_handle ); + if ( CancelResponse::ACCEPT == response ) { + try { + goal_handle->_cancel_goal(); + } catch ( const rclcpp::exceptions::RCLError &ex ) { + RBF2_DEBUG( "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what() ); + return CancelResponse::REJECT; + } + } + } + return response; +} + +void Server::call_goal_accepted_callback( + std::shared_ptr rcl_goal_handle, GoalUUID uuid, + std::shared_ptr goal_request_message ) +{ + std::shared_ptr> goal_handle; + std::weak_ptr> weak_this = this->shared_from_this(); + + std::function )> on_terminal_state = + [weak_this]( const GoalUUID &goal_uuid, std::shared_ptr result_message ) { + std::shared_ptr> shared_this = weak_this.lock(); + if ( !shared_this ) { + return; + } + // Send result message to anyone that asked + shared_this->publish_result( goal_uuid, std::move( result_message ) ); + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + // notify base so it can recalculate the expired goal timer + shared_this->notify_goal_terminal_state(); + // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) + std::lock_guard lock( shared_this->goal_handles_mutex_ ); + shared_this->goal_handles_.erase( goal_uuid ); + }; + + std::function on_executing = [weak_this]( const GoalUUID &goal_uuid ) { + std::shared_ptr> shared_this = weak_this.lock(); + if ( !shared_this ) { + return; + } + (void)goal_uuid; + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + }; + + std::function )> publish_feedback = + [weak_this]( std::shared_ptr feedback_msg ) { + std::shared_ptr> shared_this = weak_this.lock(); + if ( !shared_this ) { + return; + } + shared_this->publish_feedback( std::move( feedback_msg ) ); + }; + + const auto &request_type_support = type_support_->goal_service_type_support->request(); + auto request = CompoundMessage::make_shared( request_type_support, goal_request_message ); + auto goal = + std::shared_ptr( request, &( *request )["goal"].as() ); + goal_handle.reset( new ServerGoalHandle( type_support_, rcl_goal_handle, uuid, goal, + on_terminal_state, on_executing, + publish_feedback ) ); + std::unique_lock lock( goal_handles_mutex_ ); + goal_handles_[uuid] = goal_handle; + lock.unlock(); + handle_accepted_( goal_handle ); +} + +GoalUUID Server::get_goal_id_from_goal_request( void *message ) +{ + GoalUUID result; + auto compound = CompoundMessage( + type_support_->goal_service_type_support->request(), + std::shared_ptr( message, []( const void * ) { /* not ours to delete */ } ) ); + const auto &uuid = compound["goal_id"]["uuid"].as(); + for ( size_t i = 0; i < uuid.size(); ++i ) { result[i] = uuid[i]; } + return result; +} + +std::shared_ptr Server::create_goal_request() +{ + return ros_babel_fish::createContainer( type_support_->goal_service_type_support->request() ); +} + +GoalUUID Server::get_goal_id_from_result_request( void *message ) +{ + GoalUUID result; + auto compound = CompoundMessage( + type_support_->result_service_type_support->request(), + std::shared_ptr( message, []( const void * ) { /* not ours to delete */ } ) ); + const auto &uuid = compound["goal_id"]["uuid"].as(); + for ( size_t i = 0; i < uuid.size(); ++i ) { result[i] = uuid[i]; } + return result; +} + +std::shared_ptr Server::create_result_request() +{ + return ros_babel_fish::createContainer( type_support_->result_service_type_support->request() ); +} + +std::shared_ptr Server::create_result_response( + decltype( action_msgs::msg::GoalStatus::status ) status ) +{ + auto message = + ros_babel_fish::createContainer( type_support_->result_service_type_support->response() ); + auto result = CompoundMessage( type_support_->result_service_type_support->response(), message ); + result["status"] = status; + return std::static_pointer_cast( message ); +} + +} // namespace rclcpp_action diff --git a/ros_babel_fish/test/action_server.cpp b/ros_babel_fish/test/action_server.cpp new file mode 100644 index 0000000..55b335f --- /dev/null +++ b/ros_babel_fish/test/action_server.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2024 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license information. + +#include "message_comparison.hpp" +#include + +#include +#include +#include + +#include +#include + +using namespace ros_babel_fish; +using namespace ros_babel_fish_test_msgs; + +rclcpp::Node::SharedPtr node; + +::testing::AssertionResult Equal( const rosidl_message_type_support_t &a, + const rosidl_message_type_support_t &b ) +{ + if ( a.data != b.data ) + return ::testing::AssertionFailure() << "Type support maps are not equal!"; + return ::testing::AssertionSuccess(); +} + +::testing::AssertionResult Equal( const rosidl_action_type_support_t &a, + const rosidl_action_type_support_t &b ) +{ + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + if ( !( result = Equal( *a.status_message_type_support, *b.status_message_type_support ) ) ) + return result << std::endl << "Action/StatusMessage"; + if ( !( result = Equal( *a.feedback_message_type_support, *b.feedback_message_type_support ) ) ) + return result << std::endl << "Action/StatusMessage"; + return result; +} + +TEST( ActionClientTest, actionServer ) +{ + using namespace std::chrono_literals; + using Action = ros_babel_fish_test_msgs::action::SimpleTest; + BabelFish fish; + auto server = fish.create_action_server( + *node, "ros_babel_fish_server_test_server", "ros_babel_fish_test_msgs/action/SimpleTest", + []( const rclcpp_action::GoalUUID &, const CompoundMessage::ConstSharedPtr &msg ) { + if ( ( *msg )["target"] == 13 ) // 13 is rejected + return rclcpp_action::GoalResponse::REJECT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + []( const std::shared_ptr &handle ) { + if ( ( *handle->get_goal() )["target"].value() == 7 ) + return rclcpp_action::CancelResponse::REJECT; + return rclcpp_action::CancelResponse::ACCEPT; + }, + []( const std::shared_ptr &handle ) { + std::thread t( [handle]() { + auto result = handle->create_result_message(); + int goal = ( *handle->get_goal() )["target"].value(); + for ( int i = 0; i < goal; ++i ) { + if ( i >= 10 && i >= goal / 2 ) { + result["final_value"] = i; + handle->abort( result ); + return; + } + std::this_thread::sleep_for( 50ms ); + auto feedback = handle->create_feedback_message(); + feedback["current_value"] = i; + handle->publish_feedback( feedback ); + if ( handle->is_canceling() ) { + result["final_value"] = i; + handle->canceled( result ); + return; + } + } + result["final_value"] = goal; + handle->succeed( result ); + } ); + t.detach(); + } ); + auto client = rclcpp_action::create_client( node, "ros_babel_fish_server_test_server" ); + ASSERT_TRUE( client->wait_for_action_server( 5s ) ); + ASSERT_TRUE( client->action_server_is_ready() ); + + // This goal should succeed + Action::Goal goal; + goal.target = 2; + auto gh_future = client->async_send_goal( goal ); + ASSERT_EQ( gh_future.wait_for( 30s ), std::future_status::ready ); + auto goal_handle = gh_future.get(); + ASSERT_NE( goal_handle, nullptr ); + ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ACCEPTED ); + auto result = client->async_get_result( goal_handle ); + ASSERT_EQ( result.wait_for( 3s ), std::future_status::ready ); + ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_SUCCEEDED ); + auto result_msg = result.get(); + EXPECT_EQ( result_msg.result->final_value, 2 ); + + // This goal should abort after 10 + goal.target = 20; + std::vector feedback_values; + rclcpp_action::Client::SendGoalOptions options; + options.feedback_callback = + [&feedback_values]( auto, const std::shared_ptr &feedback ) { + feedback_values.push_back( feedback->current_value ); + }; + gh_future = client->async_send_goal( goal, options ); + ASSERT_EQ( gh_future.wait_for( 3s ), std::future_status::ready ); + goal_handle = gh_future.get(); + ASSERT_NE( goal_handle, nullptr ); + ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ACCEPTED ); + result = client->async_get_result( goal_handle ); + ASSERT_EQ( result.wait_for( 10s ), std::future_status::ready ); + ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ABORTED ); + result_msg = result.get(); + EXPECT_EQ( result_msg.result->final_value, 10 ); + + ASSERT_EQ( feedback_values.size(), 10U ); + for ( int i = 0; i < 10; ++i ) { + if ( feedback_values[i] != i ) // cppcheck-suppress containerOutOfBounds + FAIL() << "Feedback at " << i << " should be " << i << "!"; + } + + // This goal should be preempted + goal.target = 200; + gh_future = client->async_send_goal( goal ); + ASSERT_EQ( gh_future.wait_for( 3s ), std::future_status::ready ); + goal_handle = gh_future.get(); + ASSERT_NE( goal_handle, nullptr ); + std::this_thread::sleep_for( 200ms ); + auto cancel_response_future = client->async_cancel_goal( goal_handle ); + ASSERT_EQ( cancel_response_future.wait_for( 1s ), std::future_status::ready ) + << "ActionServer did not cancel in 1 second!"; + auto cancel_response = cancel_response_future.get(); + ASSERT_EQ( cancel_response->return_code, action_msgs::srv::CancelGoal::Response::ERROR_NONE ); + result = client->async_get_result( goal_handle ); + ASSERT_EQ( result.wait_for( 10s ), std::future_status::ready ); + EXPECT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_CANCELED ); + result_msg = result.get(); + EXPECT_LT( result_msg.result->final_value, 200 ); + + // 13 should be rejected + goal.target = 13; + gh_future = client->async_send_goal( goal ); + ASSERT_EQ( gh_future.wait_for( 3s ), std::future_status::ready ); + goal_handle = gh_future.get(); + ASSERT_EQ( goal_handle, nullptr ); + + // 7 can't be canceled + goal.target = 7; + gh_future = client->async_send_goal( goal ); + ASSERT_EQ( gh_future.wait_for( 3s ), std::future_status::ready ); + goal_handle = gh_future.get(); + ASSERT_NE( goal_handle, nullptr ); + ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ACCEPTED ); + std::this_thread::sleep_for( 50ms ); + cancel_response_future = client->async_cancel_goal( goal_handle ); + ASSERT_EQ( cancel_response_future.wait_for( 1s ), std::future_status::ready ) + << "ActionServer did not cancel in 1 second!"; + cancel_response = cancel_response_future.get(); + ASSERT_EQ( cancel_response->return_code, action_msgs::srv::CancelGoal::Response::ERROR_REJECTED ); + result = client->async_get_result( goal_handle ); + ASSERT_EQ( result.wait_for( 10s ), std::future_status::ready ); +} + +int main( int argc, char **argv ) +{ + testing::InitGoogleTest( &argc, argv ); + rclcpp::init( argc, argv ); + node = std::make_shared( "action_server_test" ); + std::thread spinner( []() { rclcpp::spin( node ); } ); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + spinner.join(); + node.reset(); + return result; +}