From 5ffe3e84acc884c63c2b9590dd7d7452eb13d0c6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 26 Nov 2018 19:47:59 -0800 Subject: [PATCH 01/71] Commiting to back up work, does not function --- .../include/rclcpp_action/create_server.hpp | 9 +- .../include/rclcpp_action/server.hpp | 109 ++++++++++- rclcpp_action/src/server.cpp | 173 +++++++++++++++++- rclcpp_action/test/test_server.cpp | 47 ++++- 4 files changed, 326 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 50ad292516..952a3bb1d5 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -16,6 +16,7 @@ #define RCLCPP_ACTION__CREATE_SERVER_HPP_ #include +#include #include #include @@ -33,11 +34,17 @@ create_server( typename Server::GoalCallback handle_goal, typename Server::CancelCallback handle_cancel) { - return Server::make_shared( + auto action_server = Server::make_shared( node->get_node_base_interface(), + node->get_node_clock_interface(), name, handle_goal, handle_cancel); + + // TODO(sloretz) shared pointer destructor should remove self from node waitables + // TODO(sloretz) pass in callback group to this API + node->get_node_waitables_interface()->add_waitable(action_server, nullptr); + return action_server; } } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index fefa1c5a54..f38c74a8c7 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -31,28 +33,91 @@ namespace rclcpp_action // Forward declaration class ServerBaseImpl; +enum class GoalResponse : int8_t +{ + REJECT = 1, + ACCEPT = 2, +}; + +enum class CancelResponse : int8_t +{ + REJECT = 1, + ACCEPT = 2, +}; + /// Base Action Server implementation /// It is responsible for interfacing with the C action server API. -class ServerBase +class ServerBase : public rclcpp::Waitable { public: + using UUIDGetter = std::function (void * message)>; + // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node // TODO(sloretz) accept clock instance RCLCPP_ACTION_PUBLIC ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, - const rosidl_action_type_support_t * type_support); + const rosidl_action_type_support_t * type_support, + UUIDGetter uuid_getter); RCLCPP_ACTION_PUBLIC virtual ~ServerBase(); - // TODO(sloretz) add a link between this class and callbacks in the server class + // ServerBase will call this function when a goal request is received. + // The subclass should convert to the real type and call a user's callback. + virtual + std::pair> + base_handle_goal_(rcl_action_goal_info_t &, void *) = 0; + + // ServerBase will determine which goal ids are being cancelled, and then call this function for + // each goal id. + // The subclass should look up a goal handle and call the user's callback. + virtual + CancelResponse + base_handle_cancel_(rcl_action_goal_info_t &) = 0; + + // ------------- + // Waitables API + + size_t + get_number_of_ready_subscriptions() override; + + size_t + get_number_of_ready_timers() override; + + size_t + get_number_of_ready_clients() override; + + size_t + get_number_of_ready_services() override; + + size_t + get_number_of_ready_guard_conditions() override; + + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + bool + is_ready(rcl_wait_set_t *) override; + + void + execute() override; + + // End Waitables API + // ----------------- private: std::unique_ptr pimpl_; }; +template +std::array +get_goal_id_from_goal_request(void * message) +{ + return static_cast(message)->uuid; +} /// Templated Action Server class /// It is responsible for getting the C action type support struct from the C++ type, and @@ -63,30 +128,64 @@ class Server : public ServerBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) - using GoalCallback = std::function; + using GoalCallback = std::function; using CancelCallback = std::function>)>; // TODO(sloretz) accept clock instance Server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, GoalCallback handle_goal, CancelCallback handle_cancel ) : ServerBase( node_base, + node_clock, name, - rosidl_typesupport_cpp::get_action_type_support_handle()), + rosidl_typesupport_cpp::get_action_type_support_handle(), + get_goal_id_from_goal_request), handle_goal_(handle_goal), handle_cancel_(handle_cancel) { // TODO(sloretz) what's the link that causes `handle_goal_` and `handle_cancel_` to be called? + // Why, it's the Waitable::execute() method of course + // TODO(sloretz) what implements Waitable? + // Well, execute must be Server and not ServerBase because Server has the callbacks, + // unless of course functions are passed into server base + // + // Functions to pass into server base: + // Given goal request, return UUID and a type to pass to a user's callback } virtual ~Server() { } + std::pair> + base_handle_goal_(rcl_action_goal_info_t & info, void * message) override + { + // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type + static_assert( + std::is_same::value, + "Assuming user fields were merged with goal request fields"); + GoalResponse user_response = handle_goal_(info, static_cast(message)); + // TODO(sloretz) if goal is accepted then create a goal handle + + auto ros_response = std::make_shared(); + ros_response->accepted = GoalResponse::ACCEPT == user_response; + // TODO(sloretz) set timestamp in response and give that to ServerBase too + return std::make_pair(user_response, ros_response); + } + + CancelResponse + base_handle_cancel_(rcl_action_goal_info_t & info) + { + // TODO(sloretz) look up goal handle and call users' callback with it + (void)info; + return CancelResponse::REJECT; + } + private: GoalCallback handle_goal_; CancelCallback handle_cancel_; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index bac9d21996..03fbc0c877 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include #include #include @@ -22,20 +26,179 @@ namespace rclcpp_action { class ServerBaseImpl { +public: + rcl_action_server_t action_server_; + ServerBase::UUIDGetter uuid_getter_; + rclcpp::Clock::SharedPtr clock_; + + size_t num_subscriptions_ = 0; + size_t num_timers_ = 0; + size_t num_clients_ = 0; + size_t num_services_ = 0; + size_t num_guard_conditions_ = 0; + + bool goal_request_ready_ = false; + bool cancel_request_ready_ = false; + bool result_request_ready_ = false; }; } ServerBase::ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, - const rosidl_action_type_support_t * type_support) + const rosidl_action_type_support_t * type_support, + ServerBase::UUIDGetter uuid_getter + ) + : pimpl_(new ServerBaseImpl) { - // TODO(sloretz) use rcl_action API when available - (void)node_base; - (void)name; - (void)type_support; + rcl_ret_t ret; + // Create action server + pimpl_->uuid_getter_ = uuid_getter; + pimpl_->clock_ = node_clock->get_clock(); + pimpl_->action_server_ = rcl_action_get_zero_initialized_server(); + + // TODO(sloretz) pass options into API + const rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); + + ret = rcl_action_server_init( + &pimpl_->action_server_, rcl_node, rcl_clock, type_support, name.c_str(), &server_options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + ret = rcl_action_server_wait_set_get_num_entities( + &pimpl_->action_server_, + &pimpl_->num_subscriptions_, + &pimpl_->num_guard_conditions_, + &pimpl_->num_timers_, + &pimpl_->num_clients_, + &pimpl_->num_services_); + + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != rcl_action_server_fini(&pimpl_->action_server_, rcl_node)) { + // Ignoring error during finalization + } + rclcpp::exceptions::throw_from_rcl_error(ret); + } } ServerBase::~ServerBase() { } + + +size_t +ServerBase::get_number_of_ready_subscriptions() +{ + return pimpl_->num_subscriptions_; +} + +size_t +ServerBase::get_number_of_ready_timers() +{ + return pimpl_->num_timers_; +} + +size_t +ServerBase::get_number_of_ready_clients() +{ + return pimpl_->num_clients_; +} + +size_t +ServerBase::get_number_of_ready_services() +{ + return pimpl_->num_services_; +} + +size_t +ServerBase::get_number_of_ready_guard_conditions() +{ + return pimpl_->num_guard_conditions_; +} + +bool +ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, &pimpl_->action_server_, NULL); + return RCL_RET_OK == ret; +} + +bool +ServerBase::is_ready(rcl_wait_set_t * wait_set) +{ + std::cerr<< "Is_ready called\n"; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + &pimpl_->action_server_, + &pimpl_->goal_request_ready_, + &pimpl_->cancel_request_ready_, + &pimpl_->result_request_ready_); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + bool result = pimpl_->goal_request_ready_ + || pimpl_->cancel_request_ready_ + || pimpl_->result_request_ready_; + + if (result) { + std::cerr << "SOmething became ready\n"; + } + return result; +} + +void +ServerBase::execute() +{ + std::cerr << "Action server is executing\n"; + rcl_ret_t ret; + + if (pimpl_->goal_request_ready_) { + rcl_action_goal_info_t info; + rmw_request_id_t request_header; + void * message; + ret = rcl_action_take_goal_request( + &pimpl_->action_server_, + &request_header, + &message); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + pimpl_->goal_request_ready_ = false; + std::array uuid = pimpl_->uuid_getter_(message); + for (size_t i = 0; i < 16; ++i) { + info.uuid[i] = uuid[i]; + } + + auto response_pair = base_handle_goal_(info, message); + + // TODO(sloretz) if goal was accepted then does anything need to be stored here? + + ret = rcl_action_send_goal_response( + &pimpl_->action_server_, + &request_header, + response_pair.second.get()); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // TODO(sloretz) if goal was accepted then tell Server<> to begin execution + // maybe a base_handle_execute_ that sends a goal id so Server<> can look up the goal handle? + } else if (pimpl_->cancel_request_ready_) { + // TODO(sloretz) figure out which goals where canceled and notify Server<> + } else if (pimpl_->result_request_ready_) { + // TODO(sloretz) store the result request so it can be responded to later + } else { + throw std::runtime_error("Executing action server but nothing is ready"); + } +} diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index b6dea88077..5ed439c304 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -40,7 +40,52 @@ TEST_F(TestServer, construction_and_destruction) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - [](rcl_action_goal_info_t &, test_msgs::action::Fibonacci::Goal *) {}, + [](rcl_action_goal_info_t &, test_msgs::action::Fibonacci::Goal *) { + return rclcpp_action::GoalResponse::REJECT; + }, [](std::shared_ptr) {}); (void)as; } + +TEST_F(TestServer, handle_goal_called) +{ + auto node = std::make_shared("my_node", "/rclcpp_action/test/server"); + rcl_action_goal_info_t received_info; + + auto handle_goal = [&received_info]( + rcl_action_goal_info_t & info, test_msgs::action::Fibonacci::Goal *) + { + received_info = info; + return rclcpp_action::GoalResponse::REJECT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + [](std::shared_ptr) {}); + (void)as; + + // Create a client that calls the goal request service + // Make sure the UUID received is the same as the one sent + + auto client = node->create_client( + "fibonacci/_action/goal"); + + std::cerr << "About to wait for service\n"; + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + std::cerr << "service is ready\n"; + + auto request = std::make_shared(); + + const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + request->uuid = uuid; + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + for (size_t i = 0; i < 16; ++i) { + EXPECT_EQ(uuid[i], received_info.uuid[i]) << "at idx " << i; + } +} From 0967edabcbd46bf982bcaaaac5644e0bee00d909 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 09:11:57 -0800 Subject: [PATCH 02/71] Can call user callback when goal request received --- .../include/rclcpp_action/server.hpp | 83 ++++++++++--------- rclcpp_action/src/server.cpp | 16 ++-- rclcpp_action/test/test_server.cpp | 12 ++- 3 files changed, 56 insertions(+), 55 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index f38c74a8c7..b7e8851f86 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -50,34 +50,17 @@ enum class CancelResponse : int8_t class ServerBase : public rclcpp::Waitable { public: - using UUIDGetter = std::function (void * message)>; - // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node - // TODO(sloretz) accept clock instance RCLCPP_ACTION_PUBLIC ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, - const rosidl_action_type_support_t * type_support, - UUIDGetter uuid_getter); + const rosidl_action_type_support_t * type_support); RCLCPP_ACTION_PUBLIC virtual ~ServerBase(); - // ServerBase will call this function when a goal request is received. - // The subclass should convert to the real type and call a user's callback. - virtual - std::pair> - base_handle_goal_(rcl_action_goal_info_t &, void *) = 0; - - // ServerBase will determine which goal ids are being cancelled, and then call this function for - // each goal id. - // The subclass should look up a goal handle and call the user's callback. - virtual - CancelResponse - base_handle_cancel_(rcl_action_goal_info_t &) = 0; - // ------------- // Waitables API @@ -108,17 +91,34 @@ class ServerBase : public rclcpp::Waitable // End Waitables API // ----------------- +protected: + // ServerBase will call this function when a goal request is received. + // The subclass should convert to the real type and call a user's callback. + virtual + std::pair> + base_handle_goal_(rcl_action_goal_info_t &, std::shared_ptr request) = 0; + + // ServerBase will determine which goal ids are being cancelled, and then call this function for + // each goal id. + // The subclass should look up a goal handle and call the user's callback. + virtual + CancelResponse + base_handle_cancel_(rcl_action_goal_info_t &) = 0; + + /// Given a goal request message, return the UUID contained within. + virtual + std::array + get_goal_id_from_goal_request(void * message) = 0; + + /// Create an empty goal request message so it can be taken from a lower layer. + virtual + std::shared_ptr + create_goal_request() = 0; + private: std::unique_ptr pimpl_; }; -template -std::array -get_goal_id_from_goal_request(void * message) -{ - return static_cast(message)->uuid; -} - /// Templated Action Server class /// It is responsible for getting the C action type support struct from the C++ type, and /// calling user callbacks with goal handles of the appropriate type. @@ -128,7 +128,8 @@ class Server : public ServerBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) - using GoalCallback = std::function; + using GoalCallback = std::function)>; using CancelCallback = std::function>)>; // TODO(sloretz) accept clock instance @@ -143,33 +144,27 @@ class Server : public ServerBase node_base, node_clock, name, - rosidl_typesupport_cpp::get_action_type_support_handle(), - get_goal_id_from_goal_request), + rosidl_typesupport_cpp::get_action_type_support_handle()), handle_goal_(handle_goal), handle_cancel_(handle_cancel) { - // TODO(sloretz) what's the link that causes `handle_goal_` and `handle_cancel_` to be called? - // Why, it's the Waitable::execute() method of course - // TODO(sloretz) what implements Waitable? - // Well, execute must be Server and not ServerBase because Server has the callbacks, - // unless of course functions are passed into server base - // - // Functions to pass into server base: - // Given goal request, return UUID and a type to pass to a user's callback } virtual ~Server() { } +protected: std::pair> - base_handle_goal_(rcl_action_goal_info_t & info, void * message) override + base_handle_goal_(rcl_action_goal_info_t & info, std::shared_ptr message) override { // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type static_assert( std::is_same::value, "Assuming user fields were merged with goal request fields"); - GoalResponse user_response = handle_goal_(info, static_cast(message)); + GoalResponse user_response = handle_goal_( + info, std::static_pointer_cast(message)); + // TODO(sloretz) if goal is accepted then create a goal handle auto ros_response = std::make_shared(); @@ -186,6 +181,18 @@ class Server : public ServerBase return CancelResponse::REJECT; } + std::array + get_goal_id_from_goal_request(void * message) override + { + return static_cast(message)->uuid; + } + + std::shared_ptr + create_goal_request() override + { + return std::shared_ptr(new typename ACTION::GoalRequestService::Request()); + } + private: GoalCallback handle_goal_; CancelCallback handle_cancel_; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 03fbc0c877..748198874e 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -28,7 +28,6 @@ class ServerBaseImpl { public: rcl_action_server_t action_server_; - ServerBase::UUIDGetter uuid_getter_; rclcpp::Clock::SharedPtr clock_; size_t num_subscriptions_ = 0; @@ -47,14 +46,12 @@ ServerBase::ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, - const rosidl_action_type_support_t * type_support, - ServerBase::UUIDGetter uuid_getter + const rosidl_action_type_support_t * type_support ) : pimpl_(new ServerBaseImpl) { rcl_ret_t ret; // Create action server - pimpl_->uuid_getter_ = uuid_getter; pimpl_->clock_ = node_clock->get_clock(); pimpl_->action_server_ = rcl_action_get_zero_initialized_server(); @@ -132,7 +129,6 @@ ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) bool ServerBase::is_ready(rcl_wait_set_t * wait_set) { - std::cerr<< "Is_ready called\n"; rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( wait_set, &pimpl_->action_server_, @@ -149,7 +145,6 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) || pimpl_->result_request_ready_; if (result) { - std::cerr << "SOmething became ready\n"; } return result; } @@ -157,24 +152,25 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) void ServerBase::execute() { - std::cerr << "Action server is executing\n"; rcl_ret_t ret; if (pimpl_->goal_request_ready_) { rcl_action_goal_info_t info; rmw_request_id_t request_header; - void * message; + + // TODO this needs to be available here + std::shared_ptr message = create_goal_request(); ret = rcl_action_take_goal_request( &pimpl_->action_server_, &request_header, - &message); + message.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } pimpl_->goal_request_ready_ = false; - std::array uuid = pimpl_->uuid_getter_(message); + std::array uuid = get_goal_id_from_goal_request(message.get()); for (size_t i = 0; i < 16; ++i) { info.uuid[i] = uuid[i]; } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 5ed439c304..d8df7a9ca1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -36,11 +36,11 @@ class TestServer : public ::testing::Test TEST_F(TestServer, construction_and_destruction) { - auto node = std::make_shared("my_node", "/rclcpp_action/test/server"); + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - [](rcl_action_goal_info_t &, test_msgs::action::Fibonacci::Goal *) { + [](rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, [](std::shared_ptr) {}); @@ -49,11 +49,11 @@ TEST_F(TestServer, construction_and_destruction) TEST_F(TestServer, handle_goal_called) { - auto node = std::make_shared("my_node", "/rclcpp_action/test/server"); + auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); rcl_action_goal_info_t received_info; auto handle_goal = [&received_info]( - rcl_action_goal_info_t & info, test_msgs::action::Fibonacci::Goal *) + rcl_action_goal_info_t & info, std::shared_ptr) { received_info = info; return rclcpp_action::GoalResponse::REJECT; @@ -69,11 +69,9 @@ TEST_F(TestServer, handle_goal_called) // Make sure the UUID received is the same as the one sent auto client = node->create_client( - "fibonacci/_action/goal"); + "fibonacci/_action/send_goal"); - std::cerr << "About to wait for service\n"; ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - std::cerr << "service is ready\n"; auto request = std::make_shared(); From c2338f01a329358c1297e5445c2f6e678c495452 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 09:40:34 -0800 Subject: [PATCH 03/71] fini action server in destructor --- rclcpp_action/src/server.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 748198874e..bed5c553c3 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -27,6 +27,7 @@ namespace rclcpp_action class ServerBaseImpl { public: + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rcl_action_server_t action_server_; rclcpp::Clock::SharedPtr clock_; @@ -51,7 +52,7 @@ ServerBase::ServerBase( : pimpl_(new ServerBaseImpl) { rcl_ret_t ret; - // Create action server + pimpl_->node_base_ = node_base; pimpl_->clock_ = node_clock->get_clock(); pimpl_->action_server_ = rcl_action_get_zero_initialized_server(); @@ -86,9 +87,12 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { + rcl_node_t * rcl_node = pimpl_->node_base_->get_rcl_node_handle(); + rcl_ret_t ret = rcl_action_server_fini(&pimpl_->action_server_, rcl_node); + // TODO(sloretz) do something if error occurs during destruction + (void)ret; } - size_t ServerBase::get_number_of_ready_subscriptions() { From 8854af32ffe85701bdee2d8e455ac75d00befcd8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 09:44:19 -0800 Subject: [PATCH 04/71] rename user callback virtual functions --- rclcpp_action/include/rclcpp_action/server.hpp | 8 ++++---- rclcpp_action/src/server.cpp | 5 ++++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index b7e8851f86..b8a1db2a94 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -96,14 +96,14 @@ class ServerBase : public rclcpp::Waitable // The subclass should convert to the real type and call a user's callback. virtual std::pair> - base_handle_goal_(rcl_action_goal_info_t &, std::shared_ptr request) = 0; + call_handle_goal_callback(rcl_action_goal_info_t &, std::shared_ptr request) = 0; // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. // The subclass should look up a goal handle and call the user's callback. virtual CancelResponse - base_handle_cancel_(rcl_action_goal_info_t &) = 0; + call_handle_cancel_callback(rcl_action_goal_info_t &) = 0; /// Given a goal request message, return the UUID contained within. virtual @@ -156,7 +156,7 @@ class Server : public ServerBase protected: std::pair> - base_handle_goal_(rcl_action_goal_info_t & info, std::shared_ptr message) override + call_handle_goal_callback(rcl_action_goal_info_t & info, std::shared_ptr message) override { // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type static_assert( @@ -174,7 +174,7 @@ class Server : public ServerBase } CancelResponse - base_handle_cancel_(rcl_action_goal_info_t & info) + call_handle_cancel_callback(rcl_action_goal_info_t & info) { // TODO(sloretz) look up goal handle and call users' callback with it (void)info; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index bed5c553c3..148d109935 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -179,7 +179,10 @@ ServerBase::execute() info.uuid[i] = uuid[i]; } - auto response_pair = base_handle_goal_(info, message); + // Call user's callback, getting the user's response and a ros message to send back + auto response_pair = call_handle_goal_callback(info, message); + + // if goal is accepted, create a goal handle, and store it // TODO(sloretz) if goal was accepted then does anything need to be stored here? From f752de0f0403ef05875db9d7afb264524e6d4831 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 12:22:18 -0800 Subject: [PATCH 05/71] handle_execute test passes --- .../include/rclcpp_action/create_server.hpp | 6 +- .../include/rclcpp_action/server.hpp | 36 +++++++++- .../rclcpp_action/server_goal_handle.hpp | 67 ++++++++++++++---- rclcpp_action/src/server.cpp | 70 ++++++++++++------- rclcpp_action/src/server_goal_handle.cpp | 32 +++++++++ rclcpp_action/test/test_server.cpp | 49 +++++++++++++ 6 files changed, 218 insertions(+), 42 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 952a3bb1d5..df64926074 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -32,14 +32,16 @@ create_server( rclcpp::Node * node, const std::string & name, typename Server::GoalCallback handle_goal, - typename Server::CancelCallback handle_cancel) + typename Server::CancelCallback handle_cancel, + typename Server::ExecuteCallback handle_execute) { auto action_server = Server::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), name, handle_goal, - handle_cancel); + handle_cancel, + handle_execute); // TODO(sloretz) shared pointer destructor should remove self from node waitables // TODO(sloretz) pass in callback group to this API diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index b8a1db2a94..23b4e11e8a 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -115,6 +115,14 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr create_goal_request() = 0; + /// Call user callback to begin execution + virtual + void + call_begin_execution_callback( + std::shared_ptr rcl_server, + std::shared_ptr rcl_goal_handle, + std::array uuid, std::shared_ptr goal_request_message) = 0; + private: std::unique_ptr pimpl_; }; @@ -131,6 +139,7 @@ class Server : public ServerBase using GoalCallback = std::function)>; using CancelCallback = std::function>)>; + using ExecuteCallback = std::function>)>; // TODO(sloretz) accept clock instance Server( @@ -138,7 +147,8 @@ class Server : public ServerBase rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, GoalCallback handle_goal, - CancelCallback handle_cancel + CancelCallback handle_cancel, + ExecuteCallback handle_execute ) : ServerBase( node_base, @@ -146,7 +156,8 @@ class Server : public ServerBase name, rosidl_typesupport_cpp::get_action_type_support_handle()), handle_goal_(handle_goal), - handle_cancel_(handle_cancel) + handle_cancel_(handle_cancel), + handle_execute_(handle_execute) { } @@ -181,6 +192,26 @@ class Server : public ServerBase return CancelResponse::REJECT; } + void + call_begin_execution_callback( + std::shared_ptr rcl_server, + std::shared_ptr rcl_goal_handle, + std::array uuid, std::shared_ptr goal_request_message) + { + // TODO(sloretz) pass in action server, and use this to create ServerGoalHandleImpl instance + // Also make action server a shared pointer so it can live longer than this class if handle lives on + // (void)rcl_server; + // (void)rcl_goal_handle; + // (void)uuid; + // (void)goal_request_message; + std::shared_ptr> goal_handle; + goal_handle.reset( + new ServerGoalHandle( + rcl_server, rcl_goal_handle, + uuid, std::static_pointer_cast(goal_request_message))); + handle_execute_(goal_handle); + } + std::array get_goal_id_from_goal_request(void * message) override { @@ -196,6 +227,7 @@ class Server : public ServerBase private: GoalCallback handle_goal_; CancelCallback handle_cancel_; + ExecuteCallback handle_execute_; }; } // namespace rclcpp_action #endif // RCLCPP_ACTION__SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index e2ee1cba94..dd25848324 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -24,32 +24,73 @@ namespace rclcpp_action { -template -class ServerGoalHandle + +class ServerGoalHandleBase { public: - virtual ~ServerGoalHandle(); - /// Indicate if client has requested this goal be cancelled. /// \return true if a cancelation request has been accepted for this goal. - virtual bool - is_cancel_request() const = 0; + bool + is_cancel_request() const; + + virtual + ~ServerGoalHandleBase(); + +protected: + ServerGoalHandleBase( + std::shared_ptr rcl_server, + std::shared_ptr rcl_handle + ) + : rcl_server_(rcl_server), rcl_handle_(rcl_handle) + { + } + + void + publish_feedback(std::shared_ptr feedback_msg); + +private: + std::shared_ptr rcl_server_; + std::shared_ptr rcl_handle_; +}; + +// Forward declar server +template +class Server; + +template +class ServerGoalHandle : public ServerGoalHandleBase +{ +public: + virtual ~ServerGoalHandle() = default; /// Send an update about the progress of a goal. - virtual void - publish_feedback(const typename ACTION::Feedback * feedback_msg) = 0; + void + publish_feedback(std::shared_ptr feedback_msg) + { + publish_feedback(std::static_pointer_cast(feedback_msg)); + } // TODO(sloretz) `set_cancelled`, `set_succeeded`, `set_aborted` - // TODO(sloretz) examples has this attribute as 'goal' /// The original request message describing the goal. - const typename ACTION::Goal goal_; + const std::shared_ptr goal_; + + /// A unique id for the goal request. + const std::array uuid_; protected: - explicit ServerGoalHandle(const typename ACTION::Goal goal) - : goal_(goal) {} + ServerGoalHandle( + std::shared_ptr rcl_server, + std::shared_ptr rcl_handle, + std::array uuid, + std::shared_ptr goal + ) + : ServerGoalHandleBase(rcl_server, rcl_handle), goal_(goal), uuid_(uuid) + { + } + + friend Server; }; } // namespace rclcpp_action -#include // NOLINT(build/include_order) #endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 148d109935..d3857b3ba2 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -27,8 +27,7 @@ namespace rclcpp_action class ServerBaseImpl { public: - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rcl_action_server_t action_server_; + std::shared_ptr action_server_; rclcpp::Clock::SharedPtr clock_; size_t num_subscriptions_ = 0; @@ -52,9 +51,21 @@ ServerBase::ServerBase( : pimpl_(new ServerBaseImpl) { rcl_ret_t ret; - pimpl_->node_base_ = node_base; pimpl_->clock_ = node_clock->get_clock(); - pimpl_->action_server_ = rcl_action_get_zero_initialized_server(); + + auto deleter = [node_base](rcl_action_server_t * ptr) + { + if (nullptr != ptr) { + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); + // TODO(sloretz) do something if error occurs during destruction + (void)ret; + } + delete ptr; + }; + + pimpl_->action_server_.reset(new rcl_action_server_t, deleter); + *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server(); // TODO(sloretz) pass options into API const rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); @@ -63,14 +74,14 @@ ServerBase::ServerBase( rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); ret = rcl_action_server_init( - &pimpl_->action_server_, rcl_node, rcl_clock, type_support, name.c_str(), &server_options); + pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &server_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } ret = rcl_action_server_wait_set_get_num_entities( - &pimpl_->action_server_, + pimpl_->action_server_.get(), &pimpl_->num_subscriptions_, &pimpl_->num_guard_conditions_, &pimpl_->num_timers_, @@ -78,7 +89,7 @@ ServerBase::ServerBase( &pimpl_->num_services_); if (RCL_RET_OK != ret) { - if (RCL_RET_OK != rcl_action_server_fini(&pimpl_->action_server_, rcl_node)) { + if (RCL_RET_OK != rcl_action_server_fini(pimpl_->action_server_.get(), rcl_node)) { // Ignoring error during finalization } rclcpp::exceptions::throw_from_rcl_error(ret); @@ -87,10 +98,6 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { - rcl_node_t * rcl_node = pimpl_->node_base_->get_rcl_node_handle(); - rcl_ret_t ret = rcl_action_server_fini(&pimpl_->action_server_, rcl_node); - // TODO(sloretz) do something if error occurs during destruction - (void)ret; } size_t @@ -126,7 +133,7 @@ ServerBase::get_number_of_ready_guard_conditions() bool ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, &pimpl_->action_server_, NULL); + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, pimpl_->action_server_.get(), NULL); return RCL_RET_OK == ret; } @@ -135,7 +142,7 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( wait_set, - &pimpl_->action_server_, + pimpl_->action_server_.get(), &pimpl_->goal_request_ready_, &pimpl_->cancel_request_ready_, &pimpl_->result_request_ready_); @@ -159,13 +166,12 @@ ServerBase::execute() rcl_ret_t ret; if (pimpl_->goal_request_ready_) { - rcl_action_goal_info_t info; + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); rmw_request_id_t request_header; - // TODO this needs to be available here std::shared_ptr message = create_goal_request(); ret = rcl_action_take_goal_request( - &pimpl_->action_server_, + pimpl_->action_server_.get(), &request_header, message.get()); @@ -176,18 +182,14 @@ ServerBase::execute() pimpl_->goal_request_ready_ = false; std::array uuid = get_goal_id_from_goal_request(message.get()); for (size_t i = 0; i < 16; ++i) { - info.uuid[i] = uuid[i]; + goal_info.uuid[i] = uuid[i]; } // Call user's callback, getting the user's response and a ros message to send back - auto response_pair = call_handle_goal_callback(info, message); - - // if goal is accepted, create a goal handle, and store it - - // TODO(sloretz) if goal was accepted then does anything need to be stored here? + auto response_pair = call_handle_goal_callback(goal_info, message); ret = rcl_action_send_goal_response( - &pimpl_->action_server_, + pimpl_->action_server_.get(), &request_header, response_pair.second.get()); @@ -195,8 +197,26 @@ ServerBase::execute() rclcpp::exceptions::throw_from_rcl_error(ret); } - // TODO(sloretz) if goal was accepted then tell Server<> to begin execution - // maybe a base_handle_execute_ that sends a goal id so Server<> can look up the goal handle? + // if goal is accepted, create a goal handle, and store it + if (GoalResponse::ACCEPT == response_pair.first) { + // rcl_action will set time stamp + auto deleter = [](rcl_action_goal_handle_t * ptr) + { + if (nullptr != ptr) { + rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); + (void)fail_ret; // TODO(sloretz) do something with error during cleanup + } + }; + std::shared_ptr handle; + handle.reset(rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info), deleter); + if (!handle) { + throw std::runtime_error("Failed to accept new goal\n"); + } + // TODO publish status since a goal's state has changed + + // Tell user to start executing action + call_begin_execution_callback(pimpl_->action_server_, handle, uuid, message); + } } else if (pimpl_->cancel_request_ready_) { // TODO(sloretz) figure out which goals where canceled and notify Server<> } else if (pimpl_->result_request_ready_) { diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index eae7f88981..51f3023f10 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -13,3 +13,35 @@ // limitations under the License. #include + +#include + +namespace rclcpp_action +{ +ServerGoalHandleBase::~ServerGoalHandleBase() +{ +} + +bool +ServerGoalHandleBase::is_cancel_request() const +{ + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_CANCELING == state; +} + +void +ServerGoalHandleBase::publish_feedback(std::shared_ptr feedback_msg) +{ + (void)feedback_msg; + // TODO(sloretz) what is ros_message and how does IntraProcessmessage come into play? + // if (RCL_RET_OK != rcl_action_publish_feedback(rcl_server_, ros_message) { + // // TODO(sloretz) more specific exception + // throw std::runtime_error("Failed to publish feedback"); + // } + throw std::runtime_error("Failed to publish feedback"); +} +} // namespace rclcpp_action diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index d8df7a9ca1..fdb4f0a7f3 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -43,6 +43,7 @@ TEST_F(TestServer, construction_and_destruction) [](rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, + [](std::shared_ptr) {}, [](std::shared_ptr) {}); (void)as; } @@ -62,6 +63,7 @@ TEST_F(TestServer, handle_goal_called) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, + [](std::shared_ptr) {}, [](std::shared_ptr) {}); (void)as; @@ -87,3 +89,50 @@ TEST_F(TestServer, handle_goal_called) EXPECT_EQ(uuid[i], received_info.uuid[i]) << "at idx " << i; } } + +TEST_F(TestServer, handle_execute_called) +{ + auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_execute"); + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + [](std::shared_ptr) {}, + handle_execute); + (void)as; + + // Create a client that calls the goal request service + // Make sure the UUID received is the same as the one sent + + auto client = node->create_client( + "fibonacci/_action/send_goal"); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + + auto request = std::make_shared(); + + const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + request->uuid = uuid; + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->uuid_); + EXPECT_EQ(*request, *(received_handle->goal_)); +} From 6962024f6ddea35140c780ec1708fd4ebe17878d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 13:22:46 -0800 Subject: [PATCH 06/71] Remove out of date comment --- rclcpp_action/include/rclcpp_action/server.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 23b4e11e8a..35e7efed86 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -198,12 +198,6 @@ class Server : public ServerBase std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) { - // TODO(sloretz) pass in action server, and use this to create ServerGoalHandleImpl instance - // Also make action server a shared pointer so it can live longer than this class if handle lives on - // (void)rcl_server; - // (void)rcl_goal_handle; - // (void)uuid; - // (void)goal_request_message; std::shared_ptr> goal_handle; goal_handle.reset( new ServerGoalHandle( From 5a4cc1bdb0c9f5abfe26039c685bb39705c5b939 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 13:56:01 -0800 Subject: [PATCH 07/71] Refactor execute into three functions --- .../include/rclcpp_action/server.hpp | 11 +- rclcpp_action/src/server.cpp | 116 ++++++++++-------- 2 files changed, 76 insertions(+), 51 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 35e7efed86..848dd684fa 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -124,6 +124,15 @@ class ServerBase : public rclcpp::Waitable std::array uuid, std::shared_ptr goal_request_message) = 0; private: + void + execute_goal_request_received(); + + void + execute_cancel_request_received(); + + void + execute_result_request_received(); + std::unique_ptr pimpl_; }; @@ -138,7 +147,7 @@ class Server : public ServerBase using GoalCallback = std::function)>; - using CancelCallback = std::function>)>; + using CancelCallback = std::function>)>; using ExecuteCallback = std::function>)>; // TODO(sloretz) accept clock instance diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d3857b3ba2..be30087146 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -163,65 +163,81 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) void ServerBase::execute() { - rcl_ret_t ret; - if (pimpl_->goal_request_ready_) { - rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); - rmw_request_id_t request_header; + execute_goal_request_received(); + } else if (pimpl_->cancel_request_ready_) { + execute_cancel_request_received(); + } else if (pimpl_->result_request_ready_) { + execute_result_request_received(); + } else { + throw std::runtime_error("Executing action server but nothing is ready"); + } +} - std::shared_ptr message = create_goal_request(); - ret = rcl_action_take_goal_request( - pimpl_->action_server_.get(), - &request_header, - message.get()); +void +ServerBase::execute_goal_request_received() +{ + rcl_ret_t ret; + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rmw_request_id_t request_header; - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } + std::shared_ptr message = create_goal_request(); + ret = rcl_action_take_goal_request( + pimpl_->action_server_.get(), + &request_header, + message.get()); - pimpl_->goal_request_ready_ = false; - std::array uuid = get_goal_id_from_goal_request(message.get()); - for (size_t i = 0; i < 16; ++i) { - goal_info.uuid[i] = uuid[i]; - } + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } - // Call user's callback, getting the user's response and a ros message to send back - auto response_pair = call_handle_goal_callback(goal_info, message); + pimpl_->goal_request_ready_ = false; + std::array uuid = get_goal_id_from_goal_request(message.get()); + for (size_t i = 0; i < 16; ++i) { + goal_info.uuid[i] = uuid[i]; + } - ret = rcl_action_send_goal_response( - pimpl_->action_server_.get(), - &request_header, - response_pair.second.get()); + // Call user's callback, getting the user's response and a ros message to send back + auto response_pair = call_handle_goal_callback(goal_info, message); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } + ret = rcl_action_send_goal_response( + pimpl_->action_server_.get(), + &request_header, + response_pair.second.get()); - // if goal is accepted, create a goal handle, and store it - if (GoalResponse::ACCEPT == response_pair.first) { - // rcl_action will set time stamp - auto deleter = [](rcl_action_goal_handle_t * ptr) - { - if (nullptr != ptr) { - rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); - (void)fail_ret; // TODO(sloretz) do something with error during cleanup - } - }; - std::shared_ptr handle; - handle.reset(rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info), deleter); - if (!handle) { - throw std::runtime_error("Failed to accept new goal\n"); - } - // TODO publish status since a goal's state has changed + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } - // Tell user to start executing action - call_begin_execution_callback(pimpl_->action_server_, handle, uuid, message); + // if goal is accepted, create a goal handle, and store it + if (GoalResponse::ACCEPT == response_pair.first) { + // rcl_action will set time stamp + auto deleter = [](rcl_action_goal_handle_t * ptr) + { + if (nullptr != ptr) { + rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); + (void)fail_ret; // TODO(sloretz) do something with error during cleanup + } + }; + std::shared_ptr handle; + handle.reset(rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info), deleter); + if (!handle) { + throw std::runtime_error("Failed to accept new goal\n"); } - } else if (pimpl_->cancel_request_ready_) { - // TODO(sloretz) figure out which goals where canceled and notify Server<> - } else if (pimpl_->result_request_ready_) { - // TODO(sloretz) store the result request so it can be responded to later - } else { - throw std::runtime_error("Executing action server but nothing is ready"); + // TODO publish status since a goal's state has changed + + // Tell user to start executing action + call_begin_execution_callback(pimpl_->action_server_, handle, uuid, message); } } + +void +ServerBase::execute_cancel_request_received() +{ +} + +void +ServerBase::execute_result_request_received() +{ + // TODO(sloretz) store the result request so it can be responded to later +} From 9e5d0d675ea7744d1c976bfae2bd71178d732354 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 14:08:09 -0800 Subject: [PATCH 08/71] Remove unused file --- .../rclcpp_action/server_goal_handle_impl.hpp | 76 ------------------- 1 file changed, 76 deletions(-) delete mode 100644 rclcpp_action/include/rclcpp_action/server_goal_handle_impl.hpp diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle_impl.hpp deleted file mode 100644 index 132de08f16..0000000000 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle_impl.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_ -#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_ - -#include -#include - -#include - -namespace rclcpp_action -{ -template -class Server; - -template -class ServerGoalHandleImpl : public ServerGoalHandle -{ -public: - virtual ~ServerGoalHandleImpl() - { - } - - bool - is_cancel_request() const override - { - rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; - if (RCL_RET_OK != rcl_action_goal_handle_get_status(rcl_handle_.get(), &state)) { - // TODO(sloretz) more specific exception - throw std::runtime_error("Failed to get goal handle state"); - } - return GOAL_STATE_CANCELING == state; - } - - void - publish_feedback(const typename ACTION::Feedback * feedback_msg) override - { - (void)feedback_msg; - // TODO(sloretz) what is ros_message and how does IntraProcessmessage come into play? - // if (RCL_RET_OK != rcl_action_publish_feedback(rcl_server_, ros_message) { - // // TODO(sloretz) more specific exception - // throw std::runtime_error("Failed to publish feedback"); - // } - throw std::runtime_error("Failed to publish feedback"); - } - -protected: - ServerGoalHandleImpl( - rcl_action_server_t * rcl_server, - const typename ACTION::Goal goal, - rcl_action_goal_handle_t * rcl_handle - ) - : rcl_server_(rcl_server), rcl_handle_(rcl_handle), ServerGoalHandle(goal) - { - } - -private: - friend Server; - std::shared_ptr rcl_server_; - std::shared_ptr rcl_handle_; -}; -} // namespace rclcpp_action - -#endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_ From 36c3825bd1046a8e958eb8687386feb1d4b76a5f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 14:08:36 -0800 Subject: [PATCH 09/71] Add failing cancel test --- rclcpp_action/test/test_server.cpp | 74 ++++++++++++++++++++++++++++-- 1 file changed, 71 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index fdb4f0a7f3..27d373657f 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -43,7 +43,9 @@ TEST_F(TestServer, construction_and_destruction) [](rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, - [](std::shared_ptr) {}, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, [](std::shared_ptr) {}); (void)as; } @@ -63,7 +65,9 @@ TEST_F(TestServer, handle_goal_called) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, - [](std::shared_ptr) {}, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, [](std::shared_ptr) {}); (void)as; @@ -110,7 +114,9 @@ TEST_F(TestServer, handle_execute_called) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, - [](std::shared_ptr) {}, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, handle_execute); (void)as; @@ -136,3 +142,65 @@ TEST_F(TestServer, handle_execute_called) EXPECT_EQ(uuid, received_handle->uuid_); EXPECT_EQ(*request, *(received_handle->goal_)); } + +TEST_F(TestServer, handle_cancel_called) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const std::array uuid = {10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Send goal request + { + auto client = node->create_client( + "fibonacci/_action/send_goal"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + auto request = std::make_shared(); + request->uuid = uuid; + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + } + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->uuid_); + EXPECT_FALSE(received_handle->is_cancel_request()); + + // Send cancel request + { + auto cancel_client = node->create_client( + "fibonacci/_action/cancel_goal"); + ASSERT_TRUE(cancel_client->wait_for_service(std::chrono::seconds(20))); + auto request = std::make_shared(); + request->goal_info.uuid = uuid; + auto future = cancel_client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + } + EXPECT_TRUE(received_handle->is_cancel_request()); +} From c508c6db0b47484fb157cdd3e2650d15d0f2a2b5 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 16:53:56 -0800 Subject: [PATCH 10/71] Cancel test passes --- .../include/rclcpp_action/server.hpp | 43 ++++++++++-- .../rclcpp_action/server_goal_handle.hpp | 3 + rclcpp_action/src/server.cpp | 69 +++++++++++++++++++ rclcpp_action/src/server_goal_handle.cpp | 6 ++ 4 files changed, 114 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 848dd684fa..94fe00a903 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -21,9 +21,11 @@ #include #include +#include #include #include #include +#include #include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/server_goal_handle.hpp" @@ -102,8 +104,8 @@ class ServerBase : public rclcpp::Waitable // each goal id. // The subclass should look up a goal handle and call the user's callback. virtual - CancelResponse - call_handle_cancel_callback(rcl_action_goal_info_t &) = 0; + std::pair> + call_handle_cancel_callback(const std::array & uuid) = 0; /// Given a goal request message, return the UUID contained within. virtual @@ -136,6 +138,21 @@ class ServerBase : public rclcpp::Waitable std::unique_ptr pimpl_; }; +struct UUIDHash +{ + size_t operator()(std::array const & uuid) const noexcept + { + // TODO(sloretz) Use someone else's hash function and cite it + size_t result = 0; + for (size_t i = 0; i < 16; ++i) { + for (size_t b = 0; b < sizeof(size_t); ++b) { + result ^= uuid[i] << CHAR_BIT * b; + } + } + return result; + } +}; + /// Templated Action Server class /// It is responsible for getting the C action type support struct from the C++ type, and /// calling user callbacks with goal handles of the appropriate type. @@ -193,12 +210,20 @@ class Server : public ServerBase return std::make_pair(user_response, ros_response); } - CancelResponse - call_handle_cancel_callback(rcl_action_goal_info_t & info) + std::pair> + call_handle_cancel_callback(const std::array & uuid) { - // TODO(sloretz) look up goal handle and call users' callback with it - (void)info; - return CancelResponse::REJECT; + CancelResponse resp = CancelResponse::REJECT; + std::shared_ptr rcl_handle; + auto element = goal_handles_.find(uuid); + if (element != goal_handles_.end()) { + std::shared_ptr> goal_handle = element->second.lock(); + if (goal_handle) { + resp = handle_cancel_(goal_handle); + rcl_handle = goal_handle->get_rcl_handle(); + } + } + return std::make_pair(resp, rcl_handle); } void @@ -212,6 +237,7 @@ class Server : public ServerBase new ServerGoalHandle( rcl_server, rcl_goal_handle, uuid, std::static_pointer_cast(goal_request_message))); + goal_handles_[uuid] = goal_handle; handle_execute_(goal_handle); } @@ -231,6 +257,9 @@ class Server : public ServerBase GoalCallback handle_goal_; CancelCallback handle_cancel_; ExecuteCallback handle_execute_; + + using GoalHandleWeakPtr = std::weak_ptr>; + std::unordered_map, GoalHandleWeakPtr, UUIDHash> goal_handles_; }; } // namespace rclcpp_action #endif // RCLCPP_ACTION__SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index dd25848324..01e500f112 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -36,6 +36,9 @@ class ServerGoalHandleBase virtual ~ServerGoalHandleBase(); + std::shared_ptr + get_rcl_handle() const; + protected: ServerGoalHandleBase( std::shared_ptr rcl_server, diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index be30087146..25aa32bf60 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -234,6 +235,74 @@ ServerBase::execute_goal_request_received() void ServerBase::execute_cancel_request_received() { + rcl_ret_t ret; + rmw_request_id_t request_header; + + // Initialize cancel request + auto request = std::make_shared(); + + ret = rcl_action_take_cancel_request( + pimpl_->action_server_.get(), + &request_header, + request.get()); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // Convert c++ message to C message + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + for (size_t i = 0; i < 16; ++i) { + cancel_request.goal_info.uuid[i] = request->goal_info.uuid[i]; + } + cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; + cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; + + // Get a list of goal info that should be attempted to be cancelled + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + ret = rcl_action_process_cancel_request( + pimpl_->action_server_.get(), + &cancel_request, + &cancel_response); + + auto response = std::make_shared(); + + auto & goals = cancel_response.msg.goals_canceling; + // For each canceled goal, call cancel callback + for (size_t i = 0; i < goals.size; ++i) { + const rcl_action_goal_info_t & goal_info = goals.data[i]; + std::array uuid; + for (size_t i = 0; i < 16; ++i) { + uuid[i] = goal_info.uuid[i]; + } + auto response_pair = call_handle_cancel_callback(uuid); + if (CancelResponse::ACCEPT == response_pair.first) { + ret = rcl_action_update_goal_state(response_pair.second.get(), GOAL_EVENT_CANCEL); + if (RCL_RET_OK != ret) { + rcl_ret_t fail_ret = rcl_action_cancel_response_fini(&cancel_response); + (void)fail_ret; // TODO(sloretz) do something with error during cleanup + rclcpp::exceptions::throw_from_rcl_error(ret); + } else { + action_msgs::msg::GoalInfo cpp_info; + cpp_info.uuid = uuid; + cpp_info.stamp.sec = goal_info.stamp.sec; + cpp_info.stamp.nanosec = goal_info.stamp.nanosec; + response->goals_canceling.push_back(cpp_info); + } + } + } + + // TODO(sloretz) make this fini happen in an exception safe way + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + ret = rcl_action_send_cancel_response( + pimpl_->action_server_.get(), &request_header, response.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } void diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 51f3023f10..002e27c15a 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -33,6 +33,12 @@ ServerGoalHandleBase::is_cancel_request() const return GOAL_STATE_CANCELING == state; } +std::shared_ptr +ServerGoalHandleBase::get_rcl_handle() const +{ + return rcl_handle_; +} + void ServerGoalHandleBase::publish_feedback(std::shared_ptr feedback_msg) { From 6e603e96c6ce89daf74badd16adbfed66a8c2c5f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 17:21:15 -0800 Subject: [PATCH 11/71] Remove out of date comments --- rclcpp_action/include/rclcpp_action/server.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 94fe00a903..ddb47005c8 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -202,11 +202,8 @@ class Server : public ServerBase GoalResponse user_response = handle_goal_( info, std::static_pointer_cast(message)); - // TODO(sloretz) if goal is accepted then create a goal handle - auto ros_response = std::make_shared(); ros_response->accepted = GoalResponse::ACCEPT == user_response; - // TODO(sloretz) set timestamp in response and give that to ServerBase too return std::make_pair(user_response, ros_response); } From 9565203644a01865803bdba9cfd1be16f954cd89 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 19:10:55 -0800 Subject: [PATCH 12/71] Make sure server publishes status when accepting a goal --- .../include/rclcpp_action/server.hpp | 3 + .../rclcpp_action/server_goal_handle.hpp | 12 ++++ rclcpp_action/src/server.cpp | 56 +++++++++++++++- rclcpp_action/src/server_goal_handle.cpp | 27 ++++++++ rclcpp_action/test/test_server.cpp | 65 +++++++++++++++++++ 5 files changed, 162 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index ddb47005c8..643d244214 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -135,6 +135,9 @@ class ServerBase : public rclcpp::Waitable void execute_result_request_received(); + void + publish_status(); + std::unique_ptr pimpl_; }; diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 01e500f112..2b919791b8 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -33,6 +33,18 @@ class ServerGoalHandleBase bool is_cancel_request() const; + /// Indicate that a goal could not be reached and has been aborted. + void + set_aborted(); + + /// Indicate that a goal has been reached. + void + set_succeeded(); + + /// Indicate that a goal has been canceled. + void + set_cancelled(); + virtual ~ServerGoalHandleBase(); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 25aa32bf60..db5a140ec5 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -225,7 +226,9 @@ ServerBase::execute_goal_request_received() if (!handle) { throw std::runtime_error("Failed to accept new goal\n"); } - // TODO publish status since a goal's state has changed + + // publish status since a goal's state has changed (was accepted) + publish_status(); // Tell user to start executing action call_begin_execution_callback(pimpl_->action_server_, handle, uuid, message); @@ -310,3 +313,54 @@ ServerBase::execute_result_request_received() { // TODO(sloretz) store the result request so it can be responded to later } + + +void +ServerBase::publish_status() +{ + rcl_ret_t ret; + + // Get all goal handles known to C action server + rcl_action_goal_handle_t ** goal_handles = NULL; + size_t num_goals = 0; + ret = rcl_action_server_get_goal_handles(pimpl_->action_server_.get(), &goal_handles, &num_goals); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + auto status_msg = std::make_shared(); + status_msg->status_list.reserve(num_goals); + // Populate a c++ status message with the goals and their statuses + for (size_t i = 0; i < num_goals; ++i) { + rcl_action_goal_handle_t * goal_handle = goal_handles[i]; + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rcl_action_goal_state_t goal_state; + + ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + ret = rcl_action_goal_handle_get_status(goal_handle, &goal_state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + action_msgs::msg::GoalStatus msg; + msg.status = goal_state; + // Convert C goal info to C++ goal info + for (size_t i = 0; i < 16; ++i) { + msg.goal_info.uuid[i] = goal_info.uuid[i]; + } + msg.goal_info.stamp.sec = goal_info.stamp.sec; + msg.goal_info.stamp.nanosec = goal_info.stamp.nanosec; + + status_msg->status_list.push_back(msg); + } + + // Publish the message through the status publisher + ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 002e27c15a..206442a9d1 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -33,6 +33,33 @@ ServerGoalHandleBase::is_cancel_request() const return GOAL_STATE_CANCELING == state; } +void +ServerGoalHandleBase::set_aborted() +{ + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::set_succeeded() +{ + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::set_cancelled() +{ + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + std::shared_ptr ServerGoalHandleBase::get_rcl_handle() const { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 27d373657f..f25ee044b1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -204,3 +204,68 @@ TEST_F(TestServer, handle_cancel_called) } EXPECT_TRUE(received_handle->is_cancel_request()); } + +TEST_F(TestServer, publish_status_accepted) +{ + auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); + const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + // Send goal request + { + auto client = node->create_client( + "fibonacci/_action/send_goal"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + auto request = std::make_shared(); + request->uuid = uuid; + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + } + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() == 0u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_EQ(1u, received_msgs.size()); + ASSERT_EQ(1u, received_msgs.at(0)->status_list.size()); + EXPECT_EQ( + action_msgs::msg::GoalStatus::STATUS_ACCEPTED, received_msgs.at(0)->status_list.at(0).status); + EXPECT_EQ(uuid, received_msgs.at(0)->status_list.at(0).goal_info.uuid); +} From d33fe6542dca388bc8af624c9afaab13c59fa545 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 19:25:18 -0800 Subject: [PATCH 13/71] Send status when goals transition to cancelling --- rclcpp_action/src/server.cpp | 6 +++ rclcpp_action/test/test_server.cpp | 78 ++++++++++++++++++++++++++++++ 2 files changed, 84 insertions(+) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index db5a140ec5..af1b42f250 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -295,6 +295,11 @@ ServerBase::execute_cancel_request_received() } } + if (!response->goals_canceling.empty()) { + // at least one goal state changed, publish a new status message + publish_status(); + } + // TODO(sloretz) make this fini happen in an exception safe way ret = rcl_action_cancel_response_fini(&cancel_response); if (RCL_RET_OK != ret) { @@ -306,6 +311,7 @@ ServerBase::execute_cancel_request_received() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + } void diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index f25ee044b1..4777298c4c 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -269,3 +269,81 @@ TEST_F(TestServer, publish_status_accepted) action_msgs::msg::GoalStatus::STATUS_ACCEPTED, received_msgs.at(0)->status_list.at(0).status); EXPECT_EQ(uuid, received_msgs.at(0)->status_list.at(0).goal_info.uuid); } + +TEST_F(TestServer, publish_status_canceling) +{ + auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); + const std::array uuid = {1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + // Send goal request + { + auto client = node->create_client( + "fibonacci/_action/send_goal"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + auto request = std::make_shared(); + request->uuid = uuid; + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + } + + // Send cancel request + { + auto cancel_client = node->create_client( + "fibonacci/_action/cancel_goal"); + ASSERT_TRUE(cancel_client->wait_for_service(std::chrono::seconds(20))); + auto request = std::make_shared(); + request->goal_info.uuid = uuid; + auto future = cancel_client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + } + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() != 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_EQ(2u, received_msgs.size()); + ASSERT_EQ(1u, received_msgs.at(0)->status_list.size()); + EXPECT_EQ( + action_msgs::msg::GoalStatus::STATUS_CANCELING, received_msgs.at(1)->status_list.at(0).status); + EXPECT_EQ(uuid, received_msgs.at(1)->status_list.at(0).goal_info.uuid); +} From f795ca8401003b2d9311fefba767843e18f646e3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 19:34:27 -0800 Subject: [PATCH 14/71] Refactored sending goal request to its own function --- rclcpp_action/test/test_server.cpp | 76 +++++++++--------------------- 1 file changed, 23 insertions(+), 53 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 4777298c4c..fc3e69458e 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -32,6 +32,24 @@ class TestServer : public ::testing::Test { rclcpp::init(0, nullptr); } + + std::shared_ptr + send_goal_request(rclcpp::Node::SharedPtr node, std::array uuid) + { + auto client = node->create_client( + "fibonacci/_action/send_goal"); + if (!client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("send goal service didn't become available"); + } + auto request = std::make_shared(); + request->uuid = uuid; + auto future = client->async_send_request(request); + if (rclcpp::executor::FutureReturnCode::SUCCESS != + rclcpp::spin_until_future_complete(node, future)) { + throw std::runtime_error("send goal future didn't complete succesfully"); + } + return request; + } }; TEST_F(TestServer, construction_and_destruction) @@ -97,6 +115,7 @@ TEST_F(TestServer, handle_goal_called) TEST_F(TestServer, handle_execute_called) { auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_execute"); + const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -120,23 +139,7 @@ TEST_F(TestServer, handle_execute_called) handle_execute); (void)as; - // Create a client that calls the goal request service - // Make sure the UUID received is the same as the one sent - - auto client = node->create_client( - "fibonacci/_action/send_goal"); - - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - - auto request = std::make_shared(); - - const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; - request->uuid = uuid; - - auto future = client->async_send_request(request); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + auto request = send_goal_request(node, uuid); ASSERT_TRUE(received_handle); EXPECT_EQ(uuid, received_handle->uuid_); @@ -173,18 +176,7 @@ TEST_F(TestServer, handle_cancel_called) handle_execute); (void)as; - // Send goal request - { - auto client = node->create_client( - "fibonacci/_action/send_goal"); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); - request->uuid = uuid; - auto future = client->async_send_request(request); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - } + send_goal_request(node, uuid); ASSERT_TRUE(received_handle); EXPECT_EQ(uuid, received_handle->uuid_); @@ -243,18 +235,7 @@ TEST_F(TestServer, publish_status_accepted) received_msgs.push_back(list); }); - // Send goal request - { - auto client = node->create_client( - "fibonacci/_action/send_goal"); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); - request->uuid = uuid; - auto future = client->async_send_request(request); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - } + send_goal_request(node, uuid); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -308,18 +289,7 @@ TEST_F(TestServer, publish_status_canceling) received_msgs.push_back(list); }); - // Send goal request - { - auto client = node->create_client( - "fibonacci/_action/send_goal"); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); - request->uuid = uuid; - auto future = client->async_send_request(request); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - } + send_goal_request(node, uuid); // Send cancel request { From 387701d15551d8c44fb107d845d02cfd81832ed8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 19:36:54 -0800 Subject: [PATCH 15/71] Refactor cancel request into it's own function --- rclcpp_action/test/test_server.cpp | 44 +++++++++++++----------------- 1 file changed, 19 insertions(+), 25 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index fc3e69458e..e3f40a8b83 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -50,6 +50,23 @@ class TestServer : public ::testing::Test } return request; } + + void + send_cancel_request(rclcpp::Node::SharedPtr node, std::array uuid) + { + auto cancel_client = node->create_client( + "fibonacci/_action/cancel_goal"); + if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("cancel goal service didn't become available"); + } + auto request = std::make_shared(); + request->goal_info.uuid = uuid; + auto future = cancel_client->async_send_request(request); + if (rclcpp::executor::FutureReturnCode::SUCCESS != + rclcpp::spin_until_future_complete(node, future)) { + throw std::runtime_error("cancel goal future didn't complete succesfully"); + } + } }; TEST_F(TestServer, construction_and_destruction) @@ -182,18 +199,7 @@ TEST_F(TestServer, handle_cancel_called) EXPECT_EQ(uuid, received_handle->uuid_); EXPECT_FALSE(received_handle->is_cancel_request()); - // Send cancel request - { - auto cancel_client = node->create_client( - "fibonacci/_action/cancel_goal"); - ASSERT_TRUE(cancel_client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); - request->goal_info.uuid = uuid; - auto future = cancel_client->async_send_request(request); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - } + send_cancel_request(node, uuid); EXPECT_TRUE(received_handle->is_cancel_request()); } @@ -290,19 +296,7 @@ TEST_F(TestServer, publish_status_canceling) }); send_goal_request(node, uuid); - - // Send cancel request - { - auto cancel_client = node->create_client( - "fibonacci/_action/cancel_goal"); - ASSERT_TRUE(cancel_client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); - request->goal_info.uuid = uuid; - auto future = cancel_client->async_send_request(request); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - } + send_cancel_request(node, uuid); // 10 seconds const size_t max_tries = 10 * 1000 / 100; From ec7f1eb2d34c30c0f829ed32e2b06fe8dcc92c4f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Nov 2018 19:38:22 -0800 Subject: [PATCH 16/71] Comment with remaining tests --- rclcpp_action/test/test_server.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index e3f40a8b83..a15924e4bb 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -311,3 +311,6 @@ TEST_F(TestServer, publish_status_canceling) action_msgs::msg::GoalStatus::STATUS_CANCELING, received_msgs.at(1)->status_list.at(0).status); EXPECT_EQ(uuid, received_msgs.at(1)->status_list.at(0).goal_info.uuid); } + +// TODO publish_status set_aborted, set_suceeded, set_cancelled +// TODO request result service From 04de5f4bc4edad57b550f4f4fbebeab53692520c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 09:21:58 -0800 Subject: [PATCH 17/71] Executing and terminal state statuses --- .../include/rclcpp_action/server.hpp | 14 +- .../rclcpp_action/server_goal_handle.hpp | 13 +- rclcpp_action/src/server.cpp | 10 +- rclcpp_action/src/server_goal_handle.cpp | 5 +- rclcpp_action/test/test_server.cpp | 199 ++++++++++++++++-- 5 files changed, 216 insertions(+), 25 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 643d244214..a0f1be84f0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -125,6 +125,9 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) = 0; + void + publish_status(); + private: void execute_goal_request_received(); @@ -135,9 +138,6 @@ class ServerBase : public rclcpp::Waitable void execute_result_request_received(); - void - publish_status(); - std::unique_ptr pimpl_; }; @@ -233,9 +233,15 @@ class Server : public ServerBase std::array uuid, std::shared_ptr goal_request_message) { std::shared_ptr> goal_handle; + // TODO(sloretz) how to make sure this lambda is not called beyond lifetime of this? + std::function on_terminal_state = [this]() + { + // Publish a status message any time a goal handle changes state + publish_status(); + }; goal_handle.reset( new ServerGoalHandle( - rcl_server, rcl_goal_handle, + rcl_server, rcl_goal_handle, on_terminal_state, uuid, std::static_pointer_cast(goal_request_message))); goal_handles_[uuid] = goal_handle; handle_execute_(goal_handle); diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 2b919791b8..b1ffda87ee 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -43,7 +43,7 @@ class ServerGoalHandleBase /// Indicate that a goal has been canceled. void - set_cancelled(); + set_canceled(); virtual ~ServerGoalHandleBase(); @@ -54,9 +54,10 @@ class ServerGoalHandleBase protected: ServerGoalHandleBase( std::shared_ptr rcl_server, - std::shared_ptr rcl_handle + std::shared_ptr rcl_handle, + std::function on_terminal_state ) - : rcl_server_(rcl_server), rcl_handle_(rcl_handle) + : rcl_server_(rcl_server), rcl_handle_(rcl_handle), on_terminal_state_(on_terminal_state) { } @@ -66,6 +67,7 @@ class ServerGoalHandleBase private: std::shared_ptr rcl_server_; std::shared_ptr rcl_handle_; + std::function on_terminal_state_; }; // Forward declar server @@ -85,8 +87,6 @@ class ServerGoalHandle : public ServerGoalHandleBase publish_feedback(std::static_pointer_cast(feedback_msg)); } - // TODO(sloretz) `set_cancelled`, `set_succeeded`, `set_aborted` - /// The original request message describing the goal. const std::shared_ptr goal_; @@ -97,10 +97,11 @@ class ServerGoalHandle : public ServerGoalHandleBase ServerGoalHandle( std::shared_ptr rcl_server, std::shared_ptr rcl_handle, + std::function on_terminal_state, std::array uuid, std::shared_ptr goal ) - : ServerGoalHandleBase(rcl_server, rcl_handle), goal_(goal), uuid_(uuid) + : ServerGoalHandleBase(rcl_server, rcl_handle, on_terminal_state), goal_(goal), uuid_(uuid) { } diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index af1b42f250..3905b33ee5 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -230,6 +230,15 @@ ServerBase::execute_goal_request_received() // publish status since a goal's state has changed (was accepted) publish_status(); + // Change status to executing + ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // publish status since a goal's state has changed (now executing) + publish_status(); + // Tell user to start executing action call_begin_execution_callback(pimpl_->action_server_, handle, uuid, message); } @@ -320,7 +329,6 @@ ServerBase::execute_result_request_received() // TODO(sloretz) store the result request so it can be responded to later } - void ServerBase::publish_status() { diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 206442a9d1..3b34c28713 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -40,6 +40,7 @@ ServerGoalHandleBase::set_aborted() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + on_terminal_state_(); } void @@ -49,15 +50,17 @@ ServerGoalHandleBase::set_succeeded() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + on_terminal_state_(); } void -ServerGoalHandleBase::set_cancelled() +ServerGoalHandleBase::set_canceled() { rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + on_terminal_state_(); } std::shared_ptr diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index a15924e4bb..eec08914b3 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -245,16 +245,23 @@ TEST_F(TestServer, publish_status_accepted) // 10 seconds const size_t max_tries = 10 * 1000 / 100; - for (size_t retry = 0; retry < max_tries && received_msgs.size() == 0u; ++retry) { + for (size_t retry = 0; retry < max_tries && received_msgs.size() != 1u; ++retry) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node); } - ASSERT_EQ(1u, received_msgs.size()); - ASSERT_EQ(1u, received_msgs.at(0)->status_list.size()); - EXPECT_EQ( - action_msgs::msg::GoalStatus::STATUS_ACCEPTED, received_msgs.at(0)->status_list.at(0).status); - EXPECT_EQ(uuid, received_msgs.at(0)->status_list.at(0).goal_info.uuid); + ASSERT_LT(0u, received_msgs.size()); + // Not sure whether accepted will come through because not sure when subscriber will match + for (auto & msg : received_msgs) { + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); + auto status = msg->status_list.at(0).status; + if (action_msgs::msg::GoalStatus::STATUS_ACCEPTED == status) { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ACCEPTED, status); + } else { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_EXECUTING, status); + } + } } TEST_F(TestServer, publish_status_canceling) @@ -300,17 +307,183 @@ TEST_F(TestServer, publish_status_canceling) // 10 seconds const size_t max_tries = 10 * 1000 / 100; - for (size_t retry = 0; retry < max_tries && received_msgs.size() != 2u; ++retry) { + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELING, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); +} + +TEST_F(TestServer, publish_status_canceled) +{ + auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); + const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + send_cancel_request(node, uuid); + + received_handle->set_canceled(); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 3u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); +} + +TEST_F(TestServer, publish_status_succeeded) +{ + auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); + const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + received_handle->set_succeeded(); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); +} + +TEST_F(TestServer, publish_status_aborted) +{ + auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); + const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + received_handle->set_aborted(); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node); } - ASSERT_EQ(2u, received_msgs.size()); - ASSERT_EQ(1u, received_msgs.at(0)->status_list.size()); - EXPECT_EQ( - action_msgs::msg::GoalStatus::STATUS_CANCELING, received_msgs.at(1)->status_list.at(0).status); - EXPECT_EQ(uuid, received_msgs.at(1)->status_list.at(0).goal_info.uuid); + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); } -// TODO publish_status set_aborted, set_suceeded, set_cancelled // TODO request result service From e1d1006351b4e92eda410a3bfc01c7829e98b1c3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 10:02:13 -0800 Subject: [PATCH 18/71] publish feedback works --- .../rclcpp_action/server_goal_handle.hpp | 5 +- rclcpp_action/src/server_goal_handle.cpp | 16 +++--- rclcpp_action/test/test_server.cpp | 57 +++++++++++++++++++ 3 files changed, 68 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index b1ffda87ee..a7e5e3eae1 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ +#include #include #include @@ -62,7 +63,7 @@ class ServerGoalHandleBase } void - publish_feedback(std::shared_ptr feedback_msg); + _publish_feedback(std::shared_ptr feedback_msg); private: std::shared_ptr rcl_server_; @@ -84,7 +85,7 @@ class ServerGoalHandle : public ServerGoalHandleBase void publish_feedback(std::shared_ptr feedback_msg) { - publish_feedback(std::static_pointer_cast(feedback_msg)); + _publish_feedback(std::static_pointer_cast(feedback_msg)); } /// The original request message describing the goal. diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 3b34c28713..c1e9447444 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include #include @@ -70,14 +73,11 @@ ServerGoalHandleBase::get_rcl_handle() const } void -ServerGoalHandleBase::publish_feedback(std::shared_ptr feedback_msg) +ServerGoalHandleBase::_publish_feedback(std::shared_ptr feedback_msg) { - (void)feedback_msg; - // TODO(sloretz) what is ros_message and how does IntraProcessmessage come into play? - // if (RCL_RET_OK != rcl_action_publish_feedback(rcl_server_, ros_message) { - // // TODO(sloretz) more specific exception - // throw std::runtime_error("Failed to publish feedback"); - // } - throw std::runtime_error("Failed to publish feedback"); + rcl_ret_t ret = rcl_action_publish_feedback(rcl_server_.get(), feedback_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); + } } } // namespace rclcpp_action diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index eec08914b3..9c289240f8 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -486,4 +486,61 @@ TEST_F(TestServer, publish_status_aborted) EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); } +TEST_F(TestServer, publish_feedback) +{ + auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); + const std::array uuid = {1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + // Subscribe to feedback messages + using FeedbackT = test_msgs::action::Fibonacci::Feedback; + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg) + { + received_msgs.push_back(msg); + }); + + send_goal_request(node, uuid); + + auto sent_message = std::make_shared(); + sent_message->sequence = {1, 1, 2, 3, 5}; + received_handle->publish_feedback(sent_message); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 1u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_EQ(1u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(sent_message->sequence, msg->sequence); +} + // TODO request result service From 20fe2ac675948f7ff304f87de93245bf26d5e3b0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 16:28:05 -0800 Subject: [PATCH 19/71] server sends result to clients that request it --- .../include/rclcpp_action/server.hpp | 50 +++++++++++- .../rclcpp_action/server_goal_handle.hpp | 65 ++++++++++----- rclcpp_action/src/server.cpp | 79 ++++++++++++++++++- rclcpp_action/src/server_goal_handle.cpp | 9 +-- rclcpp_action/test/test_server.cpp | 62 ++++++++++++++- 5 files changed, 232 insertions(+), 33 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index a0f1be84f0..c237e90d65 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -125,9 +125,27 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) = 0; + /// Given a result request message, return the UUID contained within. + virtual + std::array + get_goal_id_from_result_request(void * message) = 0; + + /// Create an empty goal request message so it can be taken from a lower layer. + virtual + std::shared_ptr + create_result_request() = 0; + + /// Create an empty goal result message so it can be sent as a reply in a lower layer + virtual + std::shared_ptr + create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + void publish_status(); + void + publish_result(const std::array & uuid, std::shared_ptr result_msg); + private: void execute_goal_request_received(); @@ -234,15 +252,21 @@ class Server : public ServerBase { std::shared_ptr> goal_handle; // TODO(sloretz) how to make sure this lambda is not called beyond lifetime of this? - std::function on_terminal_state = [this]() + std::function &, std::shared_ptr)> on_terminal_state = + [this](const std::array & uuid, std::shared_ptr result_message) { + // Send result message to anyone that asked + publish_result(uuid, result_message); // Publish a status message any time a goal handle changes state publish_status(); + // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) + goal_handles_.erase(uuid); }; goal_handle.reset( new ServerGoalHandle( - rcl_server, rcl_goal_handle, on_terminal_state, - uuid, std::static_pointer_cast(goal_request_message))); + rcl_server, rcl_goal_handle, + uuid, std::static_pointer_cast(goal_request_message), + on_terminal_state)); goal_handles_[uuid] = goal_handle; handle_execute_(goal_handle); } @@ -259,6 +283,26 @@ class Server : public ServerBase return std::shared_ptr(new typename ACTION::GoalRequestService::Request()); } + std::array + get_goal_id_from_result_request(void * message) override + { + return static_cast(message)->uuid; + } + + std::shared_ptr + create_result_request() override + { + return std::shared_ptr(new typename ACTION::GoalResultService::Request()); + } + + std::shared_ptr + create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override + { + auto result = std::make_shared(); + result->status = status; + return std::static_pointer_cast(result); + } + private: GoalCallback handle_goal_; CancelCallback handle_cancel_; diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index a7e5e3eae1..f3bd2f5cf4 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include @@ -34,18 +36,6 @@ class ServerGoalHandleBase bool is_cancel_request() const; - /// Indicate that a goal could not be reached and has been aborted. - void - set_aborted(); - - /// Indicate that a goal has been reached. - void - set_succeeded(); - - /// Indicate that a goal has been canceled. - void - set_canceled(); - virtual ~ServerGoalHandleBase(); @@ -55,20 +45,27 @@ class ServerGoalHandleBase protected: ServerGoalHandleBase( std::shared_ptr rcl_server, - std::shared_ptr rcl_handle, - std::function on_terminal_state + std::shared_ptr rcl_handle ) - : rcl_server_(rcl_server), rcl_handle_(rcl_handle), on_terminal_state_(on_terminal_state) + : rcl_server_(rcl_server), rcl_handle_(rcl_handle) { } void _publish_feedback(std::shared_ptr feedback_msg); + void + _set_aborted(); + + void + _set_succeeded(); + + void + _set_canceled(); + private: std::shared_ptr rcl_server_; std::shared_ptr rcl_handle_; - std::function on_terminal_state_; }; // Forward declar server @@ -88,6 +85,33 @@ class ServerGoalHandle : public ServerGoalHandleBase _publish_feedback(std::static_pointer_cast(feedback_msg)); } + /// Indicate that a goal could not be reached and has been aborted. + void + set_aborted(typename ACTION::Result::SharedPtr result_msg) + { + _set_aborted(); + result_msg->status = action_msgs::msg::GoalStatus::STATUS_ABORTED; + on_terminal_state_(uuid_, result_msg); + } + + /// Indicate that a goal has been reached. + void + set_succeeded(typename ACTION::Result::SharedPtr result_msg) + { + _set_succeeded(); + result_msg->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + on_terminal_state_(uuid_, result_msg); + } + + /// Indicate that a goal has been canceled. + void + set_canceled(typename ACTION::Result::SharedPtr result_msg) + { + _set_canceled(); + result_msg->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + on_terminal_state_(uuid_, result_msg); + } + /// The original request message describing the goal. const std::shared_ptr goal_; @@ -98,15 +122,18 @@ class ServerGoalHandle : public ServerGoalHandleBase ServerGoalHandle( std::shared_ptr rcl_server, std::shared_ptr rcl_handle, - std::function on_terminal_state, std::array uuid, - std::shared_ptr goal + std::shared_ptr goal, + std::function &, std::shared_ptr)> on_terminal_state ) - : ServerGoalHandleBase(rcl_server, rcl_handle, on_terminal_state), goal_(goal), uuid_(uuid) + : ServerGoalHandleBase(rcl_server, rcl_handle), goal_(goal), uuid_(uuid), + on_terminal_state_(on_terminal_state) { } friend Server; + + std::function &, std::shared_ptr)> on_terminal_state_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 3905b33ee5..bddc3dfcc9 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -41,6 +41,12 @@ class ServerBaseImpl bool goal_request_ready_ = false; bool cancel_request_ready_ = false; bool result_request_ready_ = false; + + // Results to be kept until the goal expires after reaching a terminal state + std::unordered_map, std::shared_ptr, UUIDHash> goal_results_; + // Requests for results are kept until a result becomes available + std::unordered_map, std::vector, UUIDHash> + result_requests_; }; } @@ -326,7 +332,51 @@ ServerBase::execute_cancel_request_received() void ServerBase::execute_result_request_received() { - // TODO(sloretz) store the result request so it can be responded to later + rcl_ret_t ret; + // Get the result request message + rmw_request_id_t request_header; + std::shared_ptr result_request = create_result_request(); + ret = rcl_action_take_result_request( + pimpl_->action_server_.get(), &request_header, result_request.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + std::shared_ptr result_response; + + // check if the goal exists + std::array uuid = get_goal_id_from_result_request(result_request.get()); + rcl_action_goal_info_t goal_info; + for (size_t i = 0; i < 16; ++i) { + goal_info.uuid[i] = uuid[i]; + } + if (!rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info)) { + // Goal does not exists + result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); + } else { + // Goal exists, check if a result is already available + auto iter = pimpl_->goal_results_.find(uuid); + if (iter != pimpl_->goal_results_.end()) { + result_response = iter->second; + } + } + + if (result_response) { + // Send the result now + ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_response.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } else { + // Store the request so it can be responded to later + auto iter = pimpl_->result_requests_.find(uuid); + if (iter == pimpl_->result_requests_.end()) { + pimpl_->result_requests_[uuid] = std::vector{request_header}; + } else { + iter->second.push_back(request_header); + } + } } void @@ -378,3 +428,30 @@ ServerBase::publish_status() rclcpp::exceptions::throw_from_rcl_error(ret); } } + +void +ServerBase::publish_result(const std::array & uuid, std::shared_ptr result_msg) +{ + // Check that the goal exists + rcl_action_goal_info_t goal_info; + for (size_t i = 0; i < 16; ++i) { + goal_info.uuid[i] = uuid[i]; + } + if (!rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info)) { + throw std::runtime_error("Asked to publish result for goal that does not exist"); + } + + pimpl_->goal_results_[uuid] = result_msg; + + // if there are clients who already asked for the result, send it to them + auto iter = pimpl_->result_requests_.find(uuid); + if (iter != pimpl_->result_requests_.end()) { + for (auto & request_header : iter->second) { + rcl_ret_t ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + } +} diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index c1e9447444..a5daa9e79f 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -37,33 +37,30 @@ ServerGoalHandleBase::is_cancel_request() const } void -ServerGoalHandleBase::set_aborted() +ServerGoalHandleBase::_set_aborted() { rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - on_terminal_state_(); } void -ServerGoalHandleBase::set_succeeded() +ServerGoalHandleBase::_set_succeeded() { rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - on_terminal_state_(); } void -ServerGoalHandleBase::set_canceled() +ServerGoalHandleBase::_set_canceled() { rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - on_terminal_state_(); } std::shared_ptr diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 9c289240f8..71718a617b 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -360,7 +360,7 @@ TEST_F(TestServer, publish_status_canceled) send_goal_request(node, uuid); send_cancel_request(node, uuid); - received_handle->set_canceled(); + received_handle->set_canceled(std::make_shared()); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -415,7 +415,7 @@ TEST_F(TestServer, publish_status_succeeded) }); send_goal_request(node, uuid); - received_handle->set_succeeded(); + received_handle->set_succeeded(std::make_shared()); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -470,7 +470,7 @@ TEST_F(TestServer, publish_status_aborted) }); send_goal_request(node, uuid); - received_handle->set_aborted(); + received_handle->set_aborted(std::make_shared()); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -543,4 +543,58 @@ TEST_F(TestServer, publish_feedback) ASSERT_EQ(sent_message->sequence, msg->sequence); } -// TODO request result service +TEST_F(TestServer, get_result) +{ + auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); + const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}; + + auto handle_goal = []( + rcl_action_goal_info_t &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_execute = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_execute); + (void)as; + + send_goal_request(node, uuid); + + // Send result request + auto result_client = node->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->uuid = uuid; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->set_succeeded(result); + + // Wait for the result request to be received + ASSERT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->sequence); +} From 8dc5b8412155d29b719886d1c6948c622cf33d63 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 16:38:08 -0800 Subject: [PATCH 20/71] Remove out of date comment --- rclcpp_action/include/rclcpp_action/server.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index c237e90d65..28bc3e88b4 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -188,7 +188,6 @@ class Server : public ServerBase using CancelCallback = std::function>)>; using ExecuteCallback = std::function>)>; - // TODO(sloretz) accept clock instance Server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, From 4a9819d442e7ec90e27dc6c5eeb0e04bb246c1e7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 16:48:41 -0800 Subject: [PATCH 21/71] Add ServerGoalHandle::is_active() --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 5 +++++ rclcpp_action/src/server_goal_handle.cpp | 6 ++++++ rclcpp_action/test/test_server.cpp | 1 + 3 files changed, 12 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index f3bd2f5cf4..a90426a834 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -36,6 +36,11 @@ class ServerGoalHandleBase bool is_cancel_request() const; + /// Indicate if goal is being worked on. + /// \return false if goal has reached a terminal state. + bool + is_active() const; + virtual ~ServerGoalHandleBase(); diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index a5daa9e79f..e1281484c6 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -36,6 +36,12 @@ ServerGoalHandleBase::is_cancel_request() const return GOAL_STATE_CANCELING == state; } +bool +ServerGoalHandleBase::is_active() const +{ + return rcl_action_goal_handle_is_active(rcl_handle_.get()); +} + void ServerGoalHandleBase::_set_aborted() { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 71718a617b..7df3608d7a 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -159,6 +159,7 @@ TEST_F(TestServer, handle_execute_called) auto request = send_goal_request(node, uuid); ASSERT_TRUE(received_handle); + ASSERT_TRUE(received_handle->is_active()); EXPECT_EQ(uuid, received_handle->uuid_); EXPECT_EQ(*request, *(received_handle->goal_)); } From a2c06a4e26efddca90832375f5056c38fdf50dae Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 18:41:15 -0800 Subject: [PATCH 22/71] Cleanup when goals expire --- .../include/rclcpp_action/server.hpp | 3 ++ rclcpp_action/src/server.cpp | 31 ++++++++++++++++++- rclcpp_action/src/server_goal_handle.cpp | 15 +++++++++ 3 files changed, 48 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 28bc3e88b4..3d4fd57547 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -156,6 +156,9 @@ class ServerBase : public rclcpp::Waitable void execute_result_request_received(); + void + execute_check_expired_goals(); + std::unique_ptr pimpl_; }; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index bddc3dfcc9..79828cd386 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -41,6 +41,7 @@ class ServerBaseImpl bool goal_request_ready_ = false; bool cancel_request_ready_ = false; bool result_request_ready_ = false; + bool goal_expired_ = false; // Results to be kept until the goal expires after reaching a terminal state std::unordered_map, std::shared_ptr, UUIDHash> goal_results_; @@ -153,7 +154,8 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) pimpl_->action_server_.get(), &pimpl_->goal_request_ready_, &pimpl_->cancel_request_ready_, - &pimpl_->result_request_ready_); + &pimpl_->result_request_ready_, + &pimpl_->goal_expired_); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -177,6 +179,8 @@ ServerBase::execute() execute_cancel_request_received(); } else if (pimpl_->result_request_ready_) { execute_result_request_received(); + } else if (pimpl_->goal_expired_) { + execute_check_expired_goals(); } else { throw std::runtime_error("Executing action server but nothing is ready"); } @@ -379,6 +383,31 @@ ServerBase::execute_result_request_received() } } +void +ServerBase::execute_check_expired_goals() +{ + // Allocate expecting only one goal to expire at a time + rcl_action_goal_info_t expired_goals[1]; + size_t num_expired = 1; + + // Loop in case more than 1 goal expired + while (num_expired > 0u) { + rcl_ret_t ret = rcl_action_expire_goals( + pimpl_->action_server_.get(), expired_goals, 1, &num_expired); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } else if (num_expired) { + // A goal expired! + std::array uuid; + for (size_t i = 0; i < 16; ++i) { + uuid[i] = expired_goals[0].uuid[i]; + } + pimpl_->goal_results_.erase(uuid); + pimpl_->result_requests_.erase(uuid); + } + } +} + void ServerBase::publish_status() { diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index e1281484c6..4822e6e68a 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -49,6 +49,11 @@ ServerGoalHandleBase::_set_aborted() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + + ret = rcl_action_notify_goal_done(rcl_server_.get(), rcl_handle_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } void @@ -58,6 +63,11 @@ ServerGoalHandleBase::_set_succeeded() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + + ret = rcl_action_notify_goal_done(rcl_server_.get(), rcl_handle_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } void @@ -67,6 +77,11 @@ ServerGoalHandleBase::_set_canceled() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } + + ret = rcl_action_notify_goal_done(rcl_server_.get(), rcl_handle_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } std::shared_ptr From da520be164f04828ef2b6a82b53932b7b519029c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 18:48:10 -0800 Subject: [PATCH 23/71] Can pass in action server options --- rclcpp_action/include/rclcpp_action/create_server.hpp | 6 +++++- rclcpp_action/include/rclcpp_action/server.hpp | 7 +++++-- rclcpp_action/src/server.cpp | 8 +++----- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index df64926074..9775b5a3e3 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_ #define RCLCPP_ACTION__CREATE_SERVER_HPP_ +#include + #include #include @@ -33,12 +35,14 @@ create_server( const std::string & name, typename Server::GoalCallback handle_goal, typename Server::CancelCallback handle_cancel, - typename Server::ExecuteCallback handle_execute) + typename Server::ExecuteCallback handle_execute, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) { auto action_server = Server::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), name, + options, handle_goal, handle_cancel, handle_execute); diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 3d4fd57547..23a1f43ed9 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -58,7 +58,8 @@ class ServerBase : public rclcpp::Waitable rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, - const rosidl_action_type_support_t * type_support); + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options); RCLCPP_ACTION_PUBLIC virtual ~ServerBase(); @@ -195,6 +196,7 @@ class Server : public ServerBase rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, + const rcl_action_server_options_t & options, GoalCallback handle_goal, CancelCallback handle_cancel, ExecuteCallback handle_execute @@ -203,7 +205,8 @@ class Server : public ServerBase node_base, node_clock, name, - rosidl_typesupport_cpp::get_action_type_support_handle()), + rosidl_typesupport_cpp::get_action_type_support_handle(), + options), handle_goal_(handle_goal), handle_cancel_(handle_cancel), handle_execute_(handle_execute) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 79828cd386..5743353e62 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -55,7 +55,8 @@ ServerBase::ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::string & name, - const rosidl_action_type_support_t * type_support + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options ) : pimpl_(new ServerBaseImpl) { @@ -76,14 +77,11 @@ ServerBase::ServerBase( pimpl_->action_server_.reset(new rcl_action_server_t, deleter); *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server(); - // TODO(sloretz) pass options into API - const rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); ret = rcl_action_server_init( - pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &server_options); + pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); From 199d01512c8fc09182975b7b6652cbfff4b8ba35 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Nov 2018 19:07:29 -0800 Subject: [PATCH 24/71] cpplint and uncrustify fixes --- .../include/rclcpp_action/server.hpp | 9 +- .../rclcpp_action/server_goal_handle.hpp | 4 +- rclcpp_action/src/server.cpp | 25 ++-- rclcpp_action/src/server_goal_handle.cpp | 3 +- rclcpp_action/test/test_server.cpp | 131 +++++++++--------- 5 files changed, 88 insertions(+), 84 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 23a1f43ed9..85eb8300e4 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/server_goal_handle.hpp" @@ -187,9 +188,9 @@ class Server : public ServerBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) - using GoalCallback = std::function)>; - using CancelCallback = std::function>)>; + using GoalCallback = std::function)>; + using CancelCallback = std::function>)>; using ExecuteCallback = std::function>)>; Server( @@ -257,7 +258,7 @@ class Server : public ServerBase { std::shared_ptr> goal_handle; // TODO(sloretz) how to make sure this lambda is not called beyond lifetime of this? - std::function &, std::shared_ptr)> on_terminal_state = + std::function&, std::shared_ptr)> on_terminal_state = [this](const std::array & uuid, std::shared_ptr result_message) { // Send result message to anyone that asked diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index a90426a834..8b75f2ea21 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -129,7 +129,7 @@ class ServerGoalHandle : public ServerGoalHandleBase std::shared_ptr rcl_handle, std::array uuid, std::shared_ptr goal, - std::function &, std::shared_ptr)> on_terminal_state + std::function&, std::shared_ptr)> on_terminal_state ) : ServerGoalHandleBase(rcl_server, rcl_handle), goal_(goal), uuid_(uuid), on_terminal_state_(on_terminal_state) @@ -138,7 +138,7 @@ class ServerGoalHandle : public ServerGoalHandleBase friend Server; - std::function &, std::shared_ptr)> on_terminal_state_; + std::function&, std::shared_ptr)> on_terminal_state_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 5743353e62..b641063f2d 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -20,7 +20,10 @@ #include #include +#include #include +#include +#include using rclcpp_action::ServerBase; @@ -47,9 +50,9 @@ class ServerBaseImpl std::unordered_map, std::shared_ptr, UUIDHash> goal_results_; // Requests for results are kept until a result becomes available std::unordered_map, std::vector, UUIDHash> - result_requests_; + result_requests_; }; -} +} // namespace rclcpp_action ServerBase::ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -57,8 +60,8 @@ ServerBase::ServerBase( const std::string & name, const rosidl_action_type_support_t * type_support, const rcl_action_server_options_t & options - ) - : pimpl_(new ServerBaseImpl) +) +: pimpl_(new ServerBaseImpl) { rcl_ret_t ret; pimpl_->clock_ = node_clock->get_clock(); @@ -140,7 +143,8 @@ ServerBase::get_number_of_ready_guard_conditions() bool ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, pimpl_->action_server_.get(), NULL); + rcl_ret_t ret = rcl_action_wait_set_add_action_server( + wait_set, pimpl_->action_server_.get(), NULL); return RCL_RET_OK == ret; } @@ -159,13 +163,9 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) rclcpp::exceptions::throw_from_rcl_error(ret); } - bool result = pimpl_->goal_request_ready_ - || pimpl_->cancel_request_ready_ - || pimpl_->result_request_ready_; - - if (result) { - } - return result; + return pimpl_->goal_request_ready_ || + pimpl_->cancel_request_ready_ || + pimpl_->result_request_ready_; } void @@ -328,7 +328,6 @@ ServerBase::execute_cancel_request_received() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - } void diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 4822e6e68a..6167cd2995 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -16,9 +16,10 @@ #include #include - #include +#include + namespace rclcpp_action { ServerGoalHandleBase::~ServerGoalHandleBase() diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7df3608d7a..39208c982b 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -20,6 +20,7 @@ #include #include +#include #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server.hpp" @@ -45,7 +46,8 @@ class TestServer : public ::testing::Test request->uuid = uuid; auto future = client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != - rclcpp::spin_until_future_complete(node, future)) { + rclcpp::spin_until_future_complete(node, future)) + { throw std::runtime_error("send goal future didn't complete succesfully"); } return request; @@ -63,7 +65,8 @@ class TestServer : public ::testing::Test request->goal_info.uuid = uuid; auto future = cancel_client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != - rclcpp::spin_until_future_complete(node, future)) { + rclcpp::spin_until_future_complete(node, future)) + { throw std::runtime_error("cancel goal future didn't complete succesfully"); } } @@ -91,7 +94,7 @@ TEST_F(TestServer, handle_goal_called) rcl_action_goal_info_t received_info; auto handle_goal = [&received_info]( - rcl_action_goal_info_t & info, std::shared_ptr) + rcl_action_goal_info_t & info, std::shared_ptr) { received_info = info; return rclcpp_action::GoalResponse::REJECT; @@ -99,11 +102,11 @@ TEST_F(TestServer, handle_goal_called) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - [](std::shared_ptr) {}); + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); (void)as; // Create a client that calls the goal request service @@ -135,7 +138,7 @@ TEST_F(TestServer, handle_execute_called) const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -149,11 +152,11 @@ TEST_F(TestServer, handle_execute_called) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - handle_execute); + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + handle_execute); (void)as; auto request = send_goal_request(node, uuid); @@ -170,7 +173,7 @@ TEST_F(TestServer, handle_cancel_called) const std::array uuid = {10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -189,9 +192,9 @@ TEST_F(TestServer, handle_cancel_called) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; send_goal_request(node, uuid); @@ -210,7 +213,7 @@ TEST_F(TestServer, publish_status_accepted) const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -229,18 +232,18 @@ TEST_F(TestServer, publish_status_accepted) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { - received_msgs.push_back(list); - }); + { + received_msgs.push_back(list); + }); send_goal_request(node, uuid); @@ -271,7 +274,7 @@ TEST_F(TestServer, publish_status_canceling) const std::array uuid = {1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -290,18 +293,18 @@ TEST_F(TestServer, publish_status_canceling) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { - received_msgs.push_back(list); - }); + { + received_msgs.push_back(list); + }); send_goal_request(node, uuid); send_cancel_request(node, uuid); @@ -326,7 +329,7 @@ TEST_F(TestServer, publish_status_canceled) const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -345,18 +348,18 @@ TEST_F(TestServer, publish_status_canceled) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { - received_msgs.push_back(list); - }); + { + received_msgs.push_back(list); + }); send_goal_request(node, uuid); send_cancel_request(node, uuid); @@ -383,7 +386,7 @@ TEST_F(TestServer, publish_status_succeeded) const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -402,18 +405,18 @@ TEST_F(TestServer, publish_status_succeeded) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { - received_msgs.push_back(list); - }); + { + received_msgs.push_back(list); + }); send_goal_request(node, uuid); received_handle->set_succeeded(std::make_shared()); @@ -438,7 +441,7 @@ TEST_F(TestServer, publish_status_aborted) const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -457,18 +460,18 @@ TEST_F(TestServer, publish_status_aborted) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { - received_msgs.push_back(list); - }); + { + received_msgs.push_back(list); + }); send_goal_request(node, uuid); received_handle->set_aborted(std::make_shared()); @@ -493,7 +496,7 @@ TEST_F(TestServer, publish_feedback) const std::array uuid = {1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -512,9 +515,9 @@ TEST_F(TestServer, publish_feedback) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; // Subscribe to feedback messages @@ -522,9 +525,9 @@ TEST_F(TestServer, publish_feedback) std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg) - { - received_msgs.push_back(msg); - }); + { + received_msgs.push_back(msg); + }); send_goal_request(node, uuid); @@ -550,7 +553,7 @@ TEST_F(TestServer, get_result) const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + rcl_action_goal_info_t &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -569,9 +572,9 @@ TEST_F(TestServer, get_result) }; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - handle_goal, - handle_cancel, - handle_execute); + handle_goal, + handle_cancel, + handle_execute); (void)as; send_goal_request(node, uuid); From 586b8616948c8019fe1ef74f3480388e637bc327 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 11:30:53 -0800 Subject: [PATCH 25/71] Fix clang warnings --- .../include/rclcpp_action/server.hpp | 4 ++-- rclcpp_action/test/test_server.cpp | 20 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 85eb8300e4..646e1c2d3e 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -235,7 +235,7 @@ class Server : public ServerBase } std::pair> - call_handle_cancel_callback(const std::array & uuid) + call_handle_cancel_callback(const std::array & uuid) override { CancelResponse resp = CancelResponse::REJECT; std::shared_ptr rcl_handle; @@ -254,7 +254,7 @@ class Server : public ServerBase call_begin_execution_callback( std::shared_ptr rcl_server, std::shared_ptr rcl_goal_handle, - std::array uuid, std::shared_ptr goal_request_message) + std::array uuid, std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; // TODO(sloretz) how to make sure this lambda is not called beyond lifetime of this? diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 39208c982b..45c74fd060 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -119,7 +119,7 @@ TEST_F(TestServer, handle_goal_called) auto request = std::make_shared(); - const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + const std::array uuid{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; request->uuid = uuid; auto future = client->async_send_request(request); @@ -135,7 +135,7 @@ TEST_F(TestServer, handle_goal_called) TEST_F(TestServer, handle_execute_called) { auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_execute"); - const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + const std::array uuid{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -170,7 +170,7 @@ TEST_F(TestServer, handle_execute_called) TEST_F(TestServer, handle_cancel_called) { auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); - const std::array uuid = {10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + const std::array uuid{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -210,7 +210,7 @@ TEST_F(TestServer, handle_cancel_called) TEST_F(TestServer, publish_status_accepted) { auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); - const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}; + const std::array uuid{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -271,7 +271,7 @@ TEST_F(TestServer, publish_status_accepted) TEST_F(TestServer, publish_status_canceling) { auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); - const std::array uuid = {1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}; + const std::array uuid{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -326,7 +326,7 @@ TEST_F(TestServer, publish_status_canceling) TEST_F(TestServer, publish_status_canceled) { auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); - const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + const std::array uuid{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -383,7 +383,7 @@ TEST_F(TestServer, publish_status_canceled) TEST_F(TestServer, publish_status_succeeded) { auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); - const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + const std::array uuid{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -438,7 +438,7 @@ TEST_F(TestServer, publish_status_succeeded) TEST_F(TestServer, publish_status_aborted) { auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); - const std::array uuid = {1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + const std::array uuid{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -493,7 +493,7 @@ TEST_F(TestServer, publish_status_aborted) TEST_F(TestServer, publish_feedback) { auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); - const std::array uuid = {1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}; + const std::array uuid{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -550,7 +550,7 @@ TEST_F(TestServer, publish_feedback) TEST_F(TestServer, get_result) { auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); - const std::array uuid = {1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}; + const std::array uuid{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) From d03e6513583c18c0833d506ae9e7e01c3023e65d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 12:06:32 -0800 Subject: [PATCH 26/71] Copy rcl goal handle --- rclcpp_action/src/server.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index b641063f2d..2cc9ca038b 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -227,14 +227,19 @@ ServerBase::execute_goal_request_received() if (nullptr != ptr) { rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); (void)fail_ret; // TODO(sloretz) do something with error during cleanup + delete ptr; } }; - std::shared_ptr handle; - handle.reset(rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info), deleter); - if (!handle) { + rcl_action_goal_handle_t * rcl_handle = rcl_action_accept_new_goal( + pimpl_->action_server_.get(), &goal_info); + if (!rcl_handle) { throw std::runtime_error("Failed to accept new goal\n"); } + std::shared_ptr handle(new rcl_action_goal_handle_t, deleter); + // Copy out goal handle since action server storage disappears when it is fini'd + *handle = *rcl_handle; + // publish status since a goal's state has changed (was accepted) publish_status(); From ef65970cd9da33abf173673b487b8ba573f27fa1 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 13:17:49 -0800 Subject: [PATCH 27/71] Fix clang warning --- rclcpp_action/test/test_server.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 45c74fd060..d77943f87e 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -119,7 +119,7 @@ TEST_F(TestServer, handle_goal_called) auto request = std::make_shared(); - const std::array uuid{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; request->uuid = uuid; auto future = client->async_send_request(request); @@ -135,7 +135,7 @@ TEST_F(TestServer, handle_goal_called) TEST_F(TestServer, handle_execute_called) { auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_execute"); - const std::array uuid{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -170,7 +170,7 @@ TEST_F(TestServer, handle_execute_called) TEST_F(TestServer, handle_cancel_called) { auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); - const std::array uuid{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + const std::array uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -210,7 +210,7 @@ TEST_F(TestServer, handle_cancel_called) TEST_F(TestServer, publish_status_accepted) { auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); - const std::array uuid{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}; + const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -271,7 +271,7 @@ TEST_F(TestServer, publish_status_accepted) TEST_F(TestServer, publish_status_canceling) { auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); - const std::array uuid{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}; + const std::array uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -326,7 +326,7 @@ TEST_F(TestServer, publish_status_canceling) TEST_F(TestServer, publish_status_canceled) { auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); - const std::array uuid{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -383,7 +383,7 @@ TEST_F(TestServer, publish_status_canceled) TEST_F(TestServer, publish_status_succeeded) { auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); - const std::array uuid{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -438,7 +438,7 @@ TEST_F(TestServer, publish_status_succeeded) TEST_F(TestServer, publish_status_aborted) { auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); - const std::array uuid{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}; + const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -493,7 +493,7 @@ TEST_F(TestServer, publish_status_aborted) TEST_F(TestServer, publish_feedback) { auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); - const std::array uuid{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}; + const std::array uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) @@ -550,7 +550,7 @@ TEST_F(TestServer, publish_feedback) TEST_F(TestServer, get_result) { auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); - const std::array uuid{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}; + const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; auto handle_goal = []( rcl_action_goal_info_t &, std::shared_ptr) From 48c951d00c18dfeac126e506d443949ea2bd31ca Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 14:02:05 -0800 Subject: [PATCH 28/71] Use intermediate value to avoid left shift on 32bit integer --- rclcpp_action/include/rclcpp_action/server.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 646e1c2d3e..52469bc52e 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -172,7 +172,9 @@ struct UUIDHash size_t result = 0; for (size_t i = 0; i < 16; ++i) { for (size_t b = 0; b < sizeof(size_t); ++b) { - result ^= uuid[i] << CHAR_BIT * b; + size_t part = uuid[i]; + part <<= CHAR_BIT * b; + result ^= part; } } return result; From 283e39f5b1b07bf4c3fbbfade57353acbe3e1302 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 15:15:52 -0800 Subject: [PATCH 29/71] RCLCPP_ACTION_PUBLIC everwhere --- .../include/rclcpp_action/server.hpp | 22 +++++++++++++++++++ .../rclcpp_action/server_goal_handle.hpp | 9 ++++++++ 2 files changed, 31 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 52469bc52e..52e4328d07 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -68,27 +68,35 @@ class ServerBase : public rclcpp::Waitable // ------------- // Waitables API + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_subscriptions() override; + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_timers() override; + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_clients() override; + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_services() override; + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_guard_conditions() override; + RCLCPP_ACTION_PUBLIC bool add_to_wait_set(rcl_wait_set_t * wait_set) override; + RCLCPP_ACTION_PUBLIC bool is_ready(rcl_wait_set_t *) override; + RCLCPP_ACTION_PUBLIC void execute() override; @@ -98,6 +106,7 @@ class ServerBase : public rclcpp::Waitable protected: // ServerBase will call this function when a goal request is received. // The subclass should convert to the real type and call a user's callback. + RCLCPP_ACTION_PUBLIC virtual std::pair> call_handle_goal_callback(rcl_action_goal_info_t &, std::shared_ptr request) = 0; @@ -105,21 +114,25 @@ class ServerBase : public rclcpp::Waitable // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. // The subclass should look up a goal handle and call the user's callback. + RCLCPP_ACTION_PUBLIC virtual std::pair> call_handle_cancel_callback(const std::array & uuid) = 0; /// Given a goal request message, return the UUID contained within. + RCLCPP_ACTION_PUBLIC virtual std::array get_goal_id_from_goal_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. + RCLCPP_ACTION_PUBLIC virtual std::shared_ptr create_goal_request() = 0; /// Call user callback to begin execution + RCLCPP_ACTION_PUBLIC virtual void call_begin_execution_callback( @@ -128,36 +141,45 @@ class ServerBase : public rclcpp::Waitable std::array uuid, std::shared_ptr goal_request_message) = 0; /// Given a result request message, return the UUID contained within. + RCLCPP_ACTION_PUBLIC virtual std::array get_goal_id_from_result_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. + RCLCPP_ACTION_PUBLIC virtual std::shared_ptr create_result_request() = 0; /// Create an empty goal result message so it can be sent as a reply in a lower layer + RCLCPP_ACTION_PUBLIC virtual std::shared_ptr create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + RCLCPP_ACTION_PUBLIC void publish_status(); + RCLCPP_ACTION_PUBLIC void publish_result(const std::array & uuid, std::shared_ptr result_msg); private: + RCLCPP_ACTION_PUBLIC void execute_goal_request_received(); + RCLCPP_ACTION_PUBLIC void execute_cancel_request_received(); + RCLCPP_ACTION_PUBLIC void execute_result_request_received(); + RCLCPP_ACTION_PUBLIC void execute_check_expired_goals(); diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 8b75f2ea21..205e30c571 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -33,21 +33,26 @@ class ServerGoalHandleBase public: /// Indicate if client has requested this goal be cancelled. /// \return true if a cancelation request has been accepted for this goal. + RCLCPP_ACTION_PUBLIC bool is_cancel_request() const; /// Indicate if goal is being worked on. /// \return false if goal has reached a terminal state. + RCLCPP_ACTION_PUBLIC bool is_active() const; + RCLCPP_ACTION_PUBLIC virtual ~ServerGoalHandleBase(); + RCLCPP_ACTION_PUBLIC std::shared_ptr get_rcl_handle() const; protected: + RCLCPP_ACTION_PUBLIC ServerGoalHandleBase( std::shared_ptr rcl_server, std::shared_ptr rcl_handle @@ -56,15 +61,19 @@ class ServerGoalHandleBase { } + RCLCPP_ACTION_PUBLIC void _publish_feedback(std::shared_ptr feedback_msg); + RCLCPP_ACTION_PUBLIC void _set_aborted(); + RCLCPP_ACTION_PUBLIC void _set_succeeded(); + RCLCPP_ACTION_PUBLIC void _set_canceled(); From a13933a85dd490b2236247cabe56fce077c1533b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 17:30:40 -0800 Subject: [PATCH 30/71] Change callback parameter from C type to C++ --- .../include/rclcpp_action/create_server.hpp | 14 ++++++++ .../include/rclcpp_action/server.hpp | 8 ++--- rclcpp_action/src/server.cpp | 2 +- rclcpp_action/test/test_server.cpp | 32 +++++++++---------- 4 files changed, 34 insertions(+), 22 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 9775b5a3e3..914c0965cd 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -52,5 +52,19 @@ create_server( node->get_node_waitables_interface()->add_waitable(action_server, nullptr); return action_server; } + +template +typename Server::SharedPtr +create_server( + rclcpp::Node::SharedPtr node, + const std::string & name, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::ExecuteCallback handle_execute, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) +{ + return create_server( + node.get(), name, handle_goal, handle_cancel, handle_execute, options); +} } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 52e4328d07..00cf38bd40 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -109,7 +109,7 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual std::pair> - call_handle_goal_callback(rcl_action_goal_info_t &, std::shared_ptr request) = 0; + call_handle_goal_callback(std::array &, std::shared_ptr request) = 0; // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. @@ -213,7 +213,7 @@ class Server : public ServerBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) using GoalCallback = std::function)>; + std::array&, std::shared_ptr)>; using CancelCallback = std::function>)>; using ExecuteCallback = std::function>)>; @@ -244,14 +244,14 @@ class Server : public ServerBase protected: std::pair> - call_handle_goal_callback(rcl_action_goal_info_t & info, std::shared_ptr message) override + call_handle_goal_callback(std::array & uuid, std::shared_ptr message) override { // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type static_assert( std::is_same::value, "Assuming user fields were merged with goal request fields"); GoalResponse user_response = handle_goal_( - info, std::static_pointer_cast(message)); + uuid, std::static_pointer_cast(message)); auto ros_response = std::make_shared(); ros_response->accepted = GoalResponse::ACCEPT == user_response; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 2cc9ca038b..8b352cba30 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -208,7 +208,7 @@ ServerBase::execute_goal_request_received() } // Call user's callback, getting the user's response and a ros message to send back - auto response_pair = call_handle_goal_callback(goal_info, message); + auto response_pair = call_handle_goal_callback(uuid, message); ret = rcl_action_send_goal_response( pimpl_->action_server_.get(), diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index d77943f87e..08c4410c48 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -78,7 +78,7 @@ TEST_F(TestServer, construction_and_destruction) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node.get(), "fibonacci", - [](rcl_action_goal_info_t &, std::shared_ptr) { + [](std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, [](std::shared_ptr) { @@ -91,12 +91,12 @@ TEST_F(TestServer, construction_and_destruction) TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); - rcl_action_goal_info_t received_info; + std::array received_uuid; - auto handle_goal = [&received_info]( - rcl_action_goal_info_t & info, std::shared_ptr) + auto handle_goal = [&received_uuid]( + std::array & uuid, std::shared_ptr) { - received_info = info; + received_uuid = uuid; return rclcpp_action::GoalResponse::REJECT; }; @@ -127,9 +127,7 @@ TEST_F(TestServer, handle_goal_called) rclcpp::executor::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); - for (size_t i = 0; i < 16; ++i) { - EXPECT_EQ(uuid[i], received_info.uuid[i]) << "at idx " << i; - } + ASSERT_EQ(uuid, received_uuid); } TEST_F(TestServer, handle_execute_called) @@ -138,7 +136,7 @@ TEST_F(TestServer, handle_execute_called) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -173,7 +171,7 @@ TEST_F(TestServer, handle_cancel_called) const std::array uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -213,7 +211,7 @@ TEST_F(TestServer, publish_status_accepted) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -274,7 +272,7 @@ TEST_F(TestServer, publish_status_canceling) const std::array uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -329,7 +327,7 @@ TEST_F(TestServer, publish_status_canceled) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -386,7 +384,7 @@ TEST_F(TestServer, publish_status_succeeded) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -441,7 +439,7 @@ TEST_F(TestServer, publish_status_aborted) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -496,7 +494,7 @@ TEST_F(TestServer, publish_feedback) const std::array uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; @@ -553,7 +551,7 @@ TEST_F(TestServer, get_result) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; auto handle_goal = []( - rcl_action_goal_info_t &, std::shared_ptr) + std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT; }; From 06d3842c38b323c311df31f80397712a2740deab Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 17:37:06 -0800 Subject: [PATCH 31/71] Add accessors for request and uuid --- .../rclcpp_action/server_goal_handle.hpp | 23 +++++++++++++++---- rclcpp_action/test/test_server.cpp | 6 ++--- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 205e30c571..d370662604 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -126,11 +126,19 @@ class ServerGoalHandle : public ServerGoalHandleBase on_terminal_state_(uuid_, result_msg); } - /// The original request message describing the goal. - const std::shared_ptr goal_; + /// Get the original request message describing the goal. + const std::shared_ptr + get_goal() const + { + return goal_; + } - /// A unique id for the goal request. - const std::array uuid_; + /// Get the unique identifier of the goal + const std::array + get_uuid() const + { + return uuid_; + } protected: ServerGoalHandle( @@ -145,6 +153,13 @@ class ServerGoalHandle : public ServerGoalHandleBase { } + /// The original request message describing the goal. + const std::shared_ptr goal_; + + /// A unique id for the goal request. + const std::array uuid_; + + friend Server; std::function&, std::shared_ptr)> on_terminal_state_; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 08c4410c48..15b6e3d97c 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -161,8 +161,8 @@ TEST_F(TestServer, handle_execute_called) ASSERT_TRUE(received_handle); ASSERT_TRUE(received_handle->is_active()); - EXPECT_EQ(uuid, received_handle->uuid_); - EXPECT_EQ(*request, *(received_handle->goal_)); + EXPECT_EQ(uuid, received_handle->get_uuid()); + EXPECT_EQ(*request, *(received_handle->get_goal())); } TEST_F(TestServer, handle_cancel_called) @@ -198,7 +198,7 @@ TEST_F(TestServer, handle_cancel_called) send_goal_request(node, uuid); ASSERT_TRUE(received_handle); - EXPECT_EQ(uuid, received_handle->uuid_); + EXPECT_EQ(uuid, received_handle->get_uuid()); EXPECT_FALSE(received_handle->is_cancel_request()); send_cancel_request(node, uuid); From eac6dddb2098a607d30e6dace7451ae6487bab48 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 19:42:11 -0800 Subject: [PATCH 32/71] Feedback must include goal id --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 1 + rclcpp_action/test/test_server.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d370662604..d49e17ab77 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -96,6 +96,7 @@ class ServerGoalHandle : public ServerGoalHandleBase void publish_feedback(std::shared_ptr feedback_msg) { + feedback_msg->uuid = uuid_; _publish_feedback(std::static_pointer_cast(feedback_msg)); } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 15b6e3d97c..fbcfad14c6 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -543,6 +543,7 @@ TEST_F(TestServer, publish_feedback) ASSERT_EQ(1u, received_msgs.size()); auto & msg = received_msgs.back(); ASSERT_EQ(sent_message->sequence, msg->sequence); + ASSERT_EQ(uuid, msg->uuid); } TEST_F(TestServer, get_result) From 4038b1558a7d4fae0be126bf65c4c657e2039de8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 12:03:57 -0800 Subject: [PATCH 33/71] Document Server<> and ServerBase<> --- .../include/rclcpp_action/server.hpp | 119 ++++++++++++++++-- 1 file changed, 112 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 00cf38bd40..7fe576f01e 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -36,20 +36,32 @@ namespace rclcpp_action // Forward declaration class ServerBaseImpl; +/// A response returned by an action server callback when a goal is requested. enum class GoalResponse : int8_t { + /// Returned when the action server decides it will not act on the requested goal. REJECT = 1, + /// Returned when the action server decides it will try to reach the requested goal. ACCEPT = 2, }; +/// A response returned by an action server callback when a goal has been asked to be canceled. enum class CancelResponse : int8_t { + /// Returned when the server decides it is impossible to cancel a goal. REJECT = 1, + /// Returned when the server agrees to try to cancel a goal. ACCEPT = 2, }; /// Base Action Server implementation -/// It is responsible for interfacing with the C action server API. +/// \internal +/** + * This class should not be used directly by users writing an action server. + * Instead users should use `rclcpp_action::Server<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ class ServerBase : public rclcpp::Waitable { public: @@ -68,34 +80,50 @@ class ServerBase : public rclcpp::Waitable // ------------- // Waitables API + /// Return the number of subscriptions used to implement an action server + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_subscriptions() override; + /// Return the number of timers used to implement an action server + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_timers() override; + /// Return the number of service clients used to implement an action server + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_clients() override; + /// Return the number of service servers used to implement an action server + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_services() override; + /// Return the number of guard conditions used to implement an action server + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_guard_conditions() override; + /// Add all entities to a wait set. + /// \internal RCLCPP_ACTION_PUBLIC bool add_to_wait_set(rcl_wait_set_t * wait_set) override; + /// Return true if any entity belonging to the action server is ready to be executed. + /// \internal RCLCPP_ACTION_PUBLIC bool is_ready(rcl_wait_set_t *) override; + /// Act on entities in the wait set which are ready to be acted upon. + /// \internal RCLCPP_ACTION_PUBLIC void execute() override; @@ -104,8 +132,12 @@ class ServerBase : public rclcpp::Waitable // ----------------- protected: + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + // ServerBase will call this function when a goal request is received. // The subclass should convert to the real type and call a user's callback. + /// \internal RCLCPP_ACTION_PUBLIC virtual std::pair> @@ -114,24 +146,28 @@ class ServerBase : public rclcpp::Waitable // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. // The subclass should look up a goal handle and call the user's callback. + /// \internal RCLCPP_ACTION_PUBLIC virtual std::pair> call_handle_cancel_callback(const std::array & uuid) = 0; /// Given a goal request message, return the UUID contained within. + /// \internal RCLCPP_ACTION_PUBLIC virtual std::array get_goal_id_from_goal_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. + /// \internal RCLCPP_ACTION_PUBLIC virtual std::shared_ptr create_goal_request() = 0; /// Call user callback to begin execution + /// \internal RCLCPP_ACTION_PUBLIC virtual void @@ -141,51 +177,71 @@ class ServerBase : public rclcpp::Waitable std::array uuid, std::shared_ptr goal_request_message) = 0; /// Given a result request message, return the UUID contained within. + /// \internal RCLCPP_ACTION_PUBLIC virtual std::array get_goal_id_from_result_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. + /// \internal RCLCPP_ACTION_PUBLIC virtual std::shared_ptr create_result_request() = 0; /// Create an empty goal result message so it can be sent as a reply in a lower layer + /// \internal RCLCPP_ACTION_PUBLIC virtual std::shared_ptr create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + /// \internal RCLCPP_ACTION_PUBLIC void publish_status(); + /// \internal RCLCPP_ACTION_PUBLIC void publish_result(const std::array & uuid, std::shared_ptr result_msg); + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + private: + /// Handle a request to add a new goal to the server + /// \internal RCLCPP_ACTION_PUBLIC void execute_goal_request_received(); + /// Handle a request to cancel goals on the server + /// \internal RCLCPP_ACTION_PUBLIC void execute_cancel_request_received(); + /// Handle a request to get the result of an action + /// \internal RCLCPP_ACTION_PUBLIC void execute_result_request_received(); + /// Handle a timeout indicating a completed goal should be forgotten by the server + /// \internal RCLCPP_ACTION_PUBLIC void execute_check_expired_goals(); + /// Private implementation + /// \internal std::unique_ptr pimpl_; }; + +/// Hash a goal id so it can be used as a key in std::unordered_map struct UUIDHash { size_t operator()(std::array const & uuid) const noexcept @@ -203,20 +259,55 @@ struct UUIDHash } }; -/// Templated Action Server class -/// It is responsible for getting the C action type support struct from the C++ type, and -/// calling user callbacks with goal handles of the appropriate type. +/// Action Server +/** + * This class creates an action server. + * + * Create an instance of this server using `rclcpp_action::create_server()`. + * + * Internally, this class is responsible for: + * - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and + * - calling user callbacks. + */ template class Server : public ServerBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) + /// Signature of a callback that accepts or rejects goal requests. using GoalCallback = std::function&, std::shared_ptr)>; + /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; + /// Signature of a callback that is used to notify when execution of a goal should begin. using ExecuteCallback = std::function>)>; + /// Construct an action server. + /** + * This constructs an action server, but it will not work until it has been added to a node. + * Use `rclcpp_action::create_server()` to both construct and add to a node. + * + * Three callbacks must be provided: + * - one to accept or reject goals sent to the server, + * - one to accept or reject requests to cancel a goal, + * - one to be notified when execution of the goal should begin. + * All callbacks must be non-blocking. + * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle<>`. + * + * \param[in] node_base a pointer to the base interface of a node. + * \param[in] node_clock a pointer to an interface that allows getting a node's clock. + * \param[in] name the name of an action. + * The same name and type must be used by both the action client and action server to + * communicate. + * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param[in] handle_goal a callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_execute a callback that is called to notify when a goal should begin + * execution. + */ Server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, @@ -238,11 +329,13 @@ class Server : public ServerBase { } - virtual ~Server() - { - } + virtual ~Server() = default; protected: + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + + /// \internal std::pair> call_handle_goal_callback(std::array & uuid, std::shared_ptr message) override { @@ -258,6 +351,7 @@ class Server : public ServerBase return std::make_pair(user_response, ros_response); } + /// \internal std::pair> call_handle_cancel_callback(const std::array & uuid) override { @@ -274,6 +368,7 @@ class Server : public ServerBase return std::make_pair(resp, rcl_handle); } + /// \internal void call_begin_execution_callback( std::shared_ptr rcl_server, @@ -301,30 +396,35 @@ class Server : public ServerBase handle_execute_(goal_handle); } + /// \internal std::array get_goal_id_from_goal_request(void * message) override { return static_cast(message)->uuid; } + /// \internal std::shared_ptr create_goal_request() override { return std::shared_ptr(new typename ACTION::GoalRequestService::Request()); } + /// \internal std::array get_goal_id_from_result_request(void * message) override { return static_cast(message)->uuid; } + /// \internal std::shared_ptr create_result_request() override { return std::shared_ptr(new typename ACTION::GoalResultService::Request()); } + /// \internal std::shared_ptr create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override { @@ -333,12 +433,17 @@ class Server : public ServerBase return std::static_pointer_cast(result); } + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + private: GoalCallback handle_goal_; CancelCallback handle_cancel_; ExecuteCallback handle_execute_; 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, GoalHandleWeakPtr, UUIDHash> goal_handles_; }; } // namespace rclcpp_action From 0606831db0d2cb07ec1225952cdaa416fa6bd5d5 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 13:46:31 -0800 Subject: [PATCH 34/71] handle_execute -> handle_accepted --- .../include/rclcpp_action/create_server.hpp | 8 +-- .../include/rclcpp_action/server.hpp | 37 ++++++------ .../rclcpp_action/server_goal_handle.hpp | 20 +++++++ rclcpp_action/src/server.cpp | 22 +++---- rclcpp_action/test/test_server.cpp | 58 +++++++++---------- 5 files changed, 84 insertions(+), 61 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 914c0965cd..ecc0ad433e 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -35,7 +35,7 @@ create_server( const std::string & name, typename Server::GoalCallback handle_goal, typename Server::CancelCallback handle_cancel, - typename Server::ExecuteCallback handle_execute, + typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) { auto action_server = Server::make_shared( @@ -45,7 +45,7 @@ create_server( options, handle_goal, handle_cancel, - handle_execute); + handle_accepted); // TODO(sloretz) shared pointer destructor should remove self from node waitables // TODO(sloretz) pass in callback group to this API @@ -60,11 +60,11 @@ create_server( const std::string & name, typename Server::GoalCallback handle_goal, typename Server::CancelCallback handle_cancel, - typename Server::ExecuteCallback handle_execute, + typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) { return create_server( - node.get(), name, handle_goal, handle_cancel, handle_execute, options); + node.get(), name, handle_goal, handle_cancel, handle_accepted, options); } } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 7fe576f01e..f60d034df0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -39,18 +39,20 @@ class ServerBaseImpl; /// A response returned by an action server callback when a goal is requested. enum class GoalResponse : int8_t { - /// Returned when the action server decides it will not act on the requested goal. + /// The goal is rejected and will not be executed. REJECT = 1, - /// Returned when the action server decides it will try to reach the requested goal. - ACCEPT = 2, + /// The server accepts the goal, and is going to begin execution immediately. + ACCEPT_AND_EXECUTE = 2, + /// The server accepts the goal, and is going to execute it later. + ACCEPT_AND_DEFER = 3, }; /// A response returned by an action server callback when a goal has been asked to be canceled. enum class CancelResponse : int8_t { - /// Returned when the server decides it is impossible to cancel a goal. + /// The server will not try to cancel the goal. REJECT = 1, - /// Returned when the server agrees to try to cancel a goal. + /// The server has agreed to try to cancel the goal. ACCEPT = 2, }; @@ -166,12 +168,12 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr create_goal_request() = 0; - /// Call user callback to begin execution + /// Call user callback to inform them a goal has been accepted. /// \internal RCLCPP_ACTION_PUBLIC virtual void - call_begin_execution_callback( + call_goal_accepted_callback( std::shared_ptr rcl_server, std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) = 0; @@ -280,8 +282,8 @@ class Server : public ServerBase std::array&, std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; - /// Signature of a callback that is used to notify when execution of a goal should begin. - using ExecuteCallback = std::function>)>; + /// Signature of a callback that is used to notify when the goal has been accepted. + using AcceptedCallback = std::function>)>; /// Construct an action server. /** @@ -291,7 +293,7 @@ class Server : public ServerBase * Three callbacks must be provided: * - one to accept or reject goals sent to the server, * - one to accept or reject requests to cancel a goal, - * - one to be notified when execution of the goal should begin. + * - one to given a goal handle after a goal has been accepted. * All callbacks must be non-blocking. * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle<>`. * @@ -305,7 +307,7 @@ class Server : public ServerBase * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled. * The return from this callback only indicates if the server will try to cancel a goal. * It does not indicate if the goal was actually canceled. - * \param[in] handle_execute a callback that is called to notify when a goal should begin + * \param[in] handle_accepted a callback that is called to give the user a handle to the goal. * execution. */ Server( @@ -315,7 +317,7 @@ class Server : public ServerBase const rcl_action_server_options_t & options, GoalCallback handle_goal, CancelCallback handle_cancel, - ExecuteCallback handle_execute + AcceptedCallback handle_accepted ) : ServerBase( node_base, @@ -325,7 +327,7 @@ class Server : public ServerBase options), handle_goal_(handle_goal), handle_cancel_(handle_cancel), - handle_execute_(handle_execute) + handle_accepted_(handle_accepted) { } @@ -347,7 +349,8 @@ class Server : public ServerBase uuid, std::static_pointer_cast(message)); auto ros_response = std::make_shared(); - ros_response->accepted = GoalResponse::ACCEPT == user_response; + ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || + GoalResponse::ACCEPT_AND_DEFER == user_response; return std::make_pair(user_response, ros_response); } @@ -370,7 +373,7 @@ class Server : public ServerBase /// \internal void - call_begin_execution_callback( + call_goal_accepted_callback( std::shared_ptr rcl_server, std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) override @@ -393,7 +396,7 @@ class Server : public ServerBase uuid, std::static_pointer_cast(goal_request_message), on_terminal_state)); goal_handles_[uuid] = goal_handle; - handle_execute_(goal_handle); + handle_accepted_(goal_handle); } /// \internal @@ -439,7 +442,7 @@ class Server : public ServerBase private: GoalCallback handle_goal_; CancelCallback handle_cancel_; - ExecuteCallback handle_execute_; + AcceptedCallback handle_accepted_; using GoalHandleWeakPtr = std::weak_ptr>; /// A map of goal id to goal handle weak pointers. diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d49e17ab77..c2e249d707 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -28,6 +28,14 @@ namespace rclcpp_action { +/// Base class to interact with goals on a server. +/** + * + * This class should not be used directly by users writing an action server. + * Instead users should use `rclcpp_action::ServerGoalHandle<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ class ServerGoalHandleBase { public: @@ -47,11 +55,16 @@ class ServerGoalHandleBase virtual ~ServerGoalHandleBase(); + /// Return a shared pointer to the C struct this class wraps. RCLCPP_ACTION_PUBLIC std::shared_ptr get_rcl_handle() const; protected: + // ------------------------------------------------------------------------- + // API for communication between ServerGoalHandleBase and ServerGoalHandle<> + + /// \internal RCLCPP_ACTION_PUBLIC ServerGoalHandleBase( std::shared_ptr rcl_server, @@ -61,22 +74,29 @@ class ServerGoalHandleBase { } + /// \internal RCLCPP_ACTION_PUBLIC void _publish_feedback(std::shared_ptr feedback_msg); + /// \internal RCLCPP_ACTION_PUBLIC void _set_aborted(); + /// \internal RCLCPP_ACTION_PUBLIC void _set_succeeded(); + /// \internal RCLCPP_ACTION_PUBLIC void _set_canceled(); + // End API for communication between ServerGoalHandleBase and ServerGoalHandle<> + // ----------------------------------------------------------------------------- + private: std::shared_ptr rcl_server_; std::shared_ptr rcl_handle_; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 8b352cba30..3c02a7354c 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -219,8 +219,10 @@ ServerBase::execute_goal_request_received() rclcpp::exceptions::throw_from_rcl_error(ret); } + const auto status = response_pair.first; + // if goal is accepted, create a goal handle, and store it - if (GoalResponse::ACCEPT == response_pair.first) { + if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) { // rcl_action will set time stamp auto deleter = [](rcl_action_goal_handle_t * ptr) { @@ -240,20 +242,18 @@ ServerBase::execute_goal_request_received() // Copy out goal handle since action server storage disappears when it is fini'd *handle = *rcl_handle; - // publish status since a goal's state has changed (was accepted) - publish_status(); - - // Change status to executing - ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + if (GoalResponse::ACCEPT_AND_EXECUTE == status) { + // Change status to executing + ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } - - // publish status since a goal's state has changed (now executing) + // publish status since a goal's state has changed (was accepted or has begun execution) publish_status(); // Tell user to start executing action - call_begin_execution_callback(pimpl_->action_server_, handle, uuid, message); + call_goal_accepted_callback(pimpl_->action_server_, handle, uuid, message); } } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index fbcfad14c6..72a7a3b70b 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -130,21 +130,21 @@ TEST_F(TestServer, handle_goal_called) ASSERT_EQ(uuid, received_uuid); } -TEST_F(TestServer, handle_execute_called) +TEST_F(TestServer, handle_accepted_called) { - auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_execute"); + auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -154,7 +154,7 @@ TEST_F(TestServer, handle_execute_called) [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; }, - handle_execute); + handle_accepted); (void)as; auto request = send_goal_request(node, uuid); @@ -173,7 +173,7 @@ TEST_F(TestServer, handle_cancel_called) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -184,7 +184,7 @@ TEST_F(TestServer, handle_cancel_called) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -192,7 +192,7 @@ TEST_F(TestServer, handle_cancel_called) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; send_goal_request(node, uuid); @@ -213,7 +213,7 @@ TEST_F(TestServer, publish_status_accepted) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -224,7 +224,7 @@ TEST_F(TestServer, publish_status_accepted) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -232,7 +232,7 @@ TEST_F(TestServer, publish_status_accepted) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; // Subscribe to status messages @@ -274,7 +274,7 @@ TEST_F(TestServer, publish_status_canceling) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -285,7 +285,7 @@ TEST_F(TestServer, publish_status_canceling) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -293,7 +293,7 @@ TEST_F(TestServer, publish_status_canceling) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; // Subscribe to status messages @@ -329,7 +329,7 @@ TEST_F(TestServer, publish_status_canceled) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -340,7 +340,7 @@ TEST_F(TestServer, publish_status_canceled) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -348,7 +348,7 @@ TEST_F(TestServer, publish_status_canceled) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; // Subscribe to status messages @@ -386,7 +386,7 @@ TEST_F(TestServer, publish_status_succeeded) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -397,7 +397,7 @@ TEST_F(TestServer, publish_status_succeeded) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -405,7 +405,7 @@ TEST_F(TestServer, publish_status_succeeded) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; // Subscribe to status messages @@ -441,7 +441,7 @@ TEST_F(TestServer, publish_status_aborted) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -452,7 +452,7 @@ TEST_F(TestServer, publish_status_aborted) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -460,7 +460,7 @@ TEST_F(TestServer, publish_status_aborted) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; // Subscribe to status messages @@ -496,7 +496,7 @@ TEST_F(TestServer, publish_feedback) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -507,7 +507,7 @@ TEST_F(TestServer, publish_feedback) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -515,7 +515,7 @@ TEST_F(TestServer, publish_feedback) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; // Subscribe to feedback messages @@ -554,7 +554,7 @@ TEST_F(TestServer, get_result) auto handle_goal = []( std::array &, std::shared_ptr) { - return rclcpp_action::GoalResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; using GoalHandle = rclcpp_action::ServerGoalHandle; @@ -565,7 +565,7 @@ TEST_F(TestServer, get_result) }; std::shared_ptr received_handle; - auto handle_execute = [&received_handle](std::shared_ptr handle) + auto handle_accepted = [&received_handle](std::shared_ptr handle) { received_handle = handle; }; @@ -573,7 +573,7 @@ TEST_F(TestServer, get_result) auto as = rclcpp_action::create_server(node.get(), "fibonacci", handle_goal, handle_cancel, - handle_execute); + handle_accepted); (void)as; send_goal_request(node, uuid); From 5922b591975a8bfd969173122a872e60afc00247 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 14:16:19 -0800 Subject: [PATCH 35/71] Test deferred execution --- .../include/rclcpp_action/server.hpp | 12 +++++- .../rclcpp_action/server_goal_handle.hpp | 26 +++++++++++-- rclcpp_action/src/server_goal_handle.cpp | 20 ++++++++++ rclcpp_action/test/test_server.cpp | 38 +++++++++++++++++++ 4 files changed, 92 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index f60d034df0..59efd60f00 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -380,6 +380,7 @@ class Server : public ServerBase { std::shared_ptr> goal_handle; // TODO(sloretz) how to make sure this lambda is not called beyond lifetime of this? + std::function&, std::shared_ptr)> on_terminal_state = [this](const std::array & uuid, std::shared_ptr result_message) { @@ -390,11 +391,20 @@ class Server : public ServerBase // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) goal_handles_.erase(uuid); }; + + std::function&)> on_executing = + [this](const std::array & uuid) + { + (void)uuid; + // Publish a status message any time a goal handle changes state + publish_status(); + }; + goal_handle.reset( new ServerGoalHandle( rcl_server, rcl_goal_handle, uuid, std::static_pointer_cast(goal_request_message), - on_terminal_state)); + on_terminal_state, on_executing)); goal_handles_[uuid] = goal_handle; handle_accepted_(goal_handle); } diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index c2e249d707..bee2e8cc51 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -45,12 +45,18 @@ class ServerGoalHandleBase bool is_cancel_request() const; - /// Indicate if goal is being worked on. + /// Indicate if goal is pending or executing. /// \return false if goal has reached a terminal state. RCLCPP_ACTION_PUBLIC bool is_active() const; + /// Indicate if goal is executing. + /// \return true only if the goal is in an executing state. + RCLCPP_ACTION_PUBLIC + bool + is_executing() const; + RCLCPP_ACTION_PUBLIC virtual ~ServerGoalHandleBase(); @@ -94,6 +100,11 @@ class ServerGoalHandleBase void _set_canceled(); + /// \internal + RCLCPP_ACTION_PUBLIC + void + _set_executing(); + // End API for communication between ServerGoalHandleBase and ServerGoalHandle<> // ----------------------------------------------------------------------------- @@ -147,6 +158,13 @@ class ServerGoalHandle : public ServerGoalHandleBase on_terminal_state_(uuid_, result_msg); } + void + set_executing() + { + _set_executing(); + on_executing_(uuid_); + } + /// Get the original request message describing the goal. const std::shared_ptr get_goal() const @@ -167,10 +185,11 @@ class ServerGoalHandle : public ServerGoalHandleBase std::shared_ptr rcl_handle, std::array uuid, std::shared_ptr goal, - std::function&, std::shared_ptr)> on_terminal_state + std::function&, std::shared_ptr)> on_terminal_state, + std::function&)> on_executing ) : ServerGoalHandleBase(rcl_server, rcl_handle), goal_(goal), uuid_(uuid), - on_terminal_state_(on_terminal_state) + on_terminal_state_(on_terminal_state), on_executing_(on_executing) { } @@ -184,6 +203,7 @@ class ServerGoalHandle : public ServerGoalHandleBase friend Server; std::function&, std::shared_ptr)> on_terminal_state_; + std::function&)> on_executing_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 6167cd2995..98db2bc3b3 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -43,6 +43,17 @@ ServerGoalHandleBase::is_active() const return rcl_action_goal_handle_is_active(rcl_handle_.get()); } +bool +ServerGoalHandleBase::is_executing() const +{ + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_EXECUTING == state; +} + void ServerGoalHandleBase::_set_aborted() { @@ -85,6 +96,15 @@ ServerGoalHandleBase::_set_canceled() } } +void +ServerGoalHandleBase::_set_executing() +{ + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + std::shared_ptr ServerGoalHandleBase::get_rcl_handle() const { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 72a7a3b70b..4e88220864 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -601,3 +601,41 @@ TEST_F(TestServer, get_result) EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); EXPECT_EQ(result->sequence, response->sequence); } + +TEST_F(TestServer, deferred_execution) +{ + auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); + const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + std::array &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node.get(), "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + EXPECT_TRUE(received_handle->is_active()); + EXPECT_FALSE(received_handle->is_executing()); + received_handle->set_executing(); + EXPECT_TRUE(received_handle->is_executing()); +} From 8a4c4cfcd2558e95674491709d3cc4bc1acf5b15 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 14:20:01 -0800 Subject: [PATCH 36/71] only publish feedback if goal is executing --- rclcpp_action/src/server_goal_handle.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 98db2bc3b3..a23f9ca6fe 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -114,6 +114,10 @@ ServerGoalHandleBase::get_rcl_handle() const void ServerGoalHandleBase::_publish_feedback(std::shared_ptr feedback_msg) { + if (!is_executing()) { + // TODO(sloretz) custom exception + throw std::runtime_error("Publishing feedback for goal that's not executing\n"); + } rcl_ret_t ret = rcl_action_publish_feedback(rcl_server_.get(), feedback_msg.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); From e75ce644761ff11e825fc7a5e9ed87cb2c46d153 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 14:37:37 -0800 Subject: [PATCH 37/71] Documentation for ServerGoalHandle --- .../rclcpp_action/server_goal_handle.hpp | 56 +++++++++++++++++-- 1 file changed, 51 insertions(+), 5 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index bee2e8cc51..d015589b59 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -29,10 +29,11 @@ namespace rclcpp_action { /// Base class to interact with goals on a server. +/// \internal /** * - * This class should not be used directly by users writing an action server. - * Instead users should use `rclcpp_action::ServerGoalHandle<>`. + * This class in not be used directly by users writing an action server. + * Instead users will be given an instance of `rclcpp_action::ServerGoalHandle<>`. * * Internally, this class is responsible for interfacing with the `rcl_action` API. */ @@ -61,7 +62,11 @@ class ServerGoalHandleBase virtual ~ServerGoalHandleBase(); - /// Return a shared pointer to the C struct this class wraps. + /// Get a shared pointer to the C struct this class wraps. + /** + * It is not thread safe to use the returned pointer in any `rcl_action` functions while calling + * any of the other functions on this class. + */ RCLCPP_ACTION_PUBLIC std::shared_ptr get_rcl_handle() const; @@ -113,10 +118,21 @@ class ServerGoalHandleBase std::shared_ptr rcl_handle_; }; -// Forward declar server +// Forward declare server template class Server; +/// Class to interact with goals on a server. +/** + * Use this class to check the status of a goal as well as set the result. + * This class is not meant to be created by a user, instead it is created when a goal has been + * accepted. + * The class `rclcpp_action::Server<>` will create an instance and give it to the user in their + * `handle_accepted` callback. + * + * Internally, this class is responsible for coverting between the C++ action type and generic + * types for `rclcpp_action::ServerGoalHandleBase`. + */ template class ServerGoalHandle : public ServerGoalHandleBase { @@ -124,6 +140,12 @@ class ServerGoalHandle : public ServerGoalHandleBase virtual ~ServerGoalHandle() = default; /// Send an update about the progress of a goal. + /** + * This must only be called when the goal is executing. + * If execution of a goal is deferred then `ServerGoalHandle<>::set_executing()` must be called + * first. + * `std::runtime_error` is raised if the goal is in any state besides executing. + */ void publish_feedback(std::shared_ptr feedback_msg) { @@ -131,7 +153,14 @@ class ServerGoalHandle : public ServerGoalHandleBase _publish_feedback(std::static_pointer_cast(feedback_msg)); } + // TODO(sloretz) which exception is raised? /// Indicate that a goal could not be reached and has been aborted. + /** + * Only call this if the goal was executing but cannot be completed. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * An exception is raised if the goal is in any state besides executing. + */ void set_aborted(typename ACTION::Result::SharedPtr result_msg) { @@ -141,6 +170,12 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// Indicate that a goal has been reached. + /** + * Only call this if the goal is executing and has reached the desired final state. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * An exception is raised if the goal is in any state besides executing. + */ void set_succeeded(typename ACTION::Result::SharedPtr result_msg) { @@ -150,6 +185,12 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// Indicate that a goal has been canceled. + /** + * Only call this if the goal is executing or pending, but has been canceled. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * An exception is raised if the goal is in any state besides executing or pending. + */ void set_canceled(typename ACTION::Result::SharedPtr result_msg) { @@ -158,6 +199,11 @@ class ServerGoalHandle : public ServerGoalHandleBase on_terminal_state_(uuid_, result_msg); } + /// Indicate that the server is starting to execute a goal. + /** + * Only call this if the goal is pending. + * An exception is raised if the goal is in any state besides pending. + */ void set_executing() { @@ -180,6 +226,7 @@ class ServerGoalHandle : public ServerGoalHandleBase } protected: + /// \internal ServerGoalHandle( std::shared_ptr rcl_server, std::shared_ptr rcl_handle, @@ -199,7 +246,6 @@ class ServerGoalHandle : public ServerGoalHandleBase /// A unique id for the goal request. const std::array uuid_; - friend Server; std::function&, std::shared_ptr)> on_terminal_state_; From ff542aac99e0998cadd6b6fe23be58634d3ae5d8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 14:42:37 -0800 Subject: [PATCH 38/71] document msg parameters --- .../include/rclcpp_action/server_goal_handle.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d015589b59..6c465a672e 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -145,6 +145,8 @@ class ServerGoalHandle : public ServerGoalHandleBase * If execution of a goal is deferred then `ServerGoalHandle<>::set_executing()` must be called * first. * `std::runtime_error` is raised if the goal is in any state besides executing. + * + * \param[in] feedback_msg the message to publish to clients. */ void publish_feedback(std::shared_ptr feedback_msg) @@ -160,6 +162,8 @@ class ServerGoalHandle : public ServerGoalHandleBase * This is a terminal state, no more methods should be called on a goal handle after this is * called. * An exception is raised if the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. */ void set_aborted(typename ACTION::Result::SharedPtr result_msg) @@ -175,6 +179,8 @@ class ServerGoalHandle : public ServerGoalHandleBase * This is a terminal state, no more methods should be called on a goal handle after this is * called. * An exception is raised if the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. */ void set_succeeded(typename ACTION::Result::SharedPtr result_msg) @@ -190,6 +196,8 @@ class ServerGoalHandle : public ServerGoalHandleBase * This is a terminal state, no more methods should be called on a goal handle after this is * called. * An exception is raised if the goal is in any state besides executing or pending. + * + * \param[in] result_msg the final result to send to clients. */ void set_canceled(typename ACTION::Result::SharedPtr result_msg) From 0005862cd761408981a31847320ffdf71bf4b9f1 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 18:21:08 -0800 Subject: [PATCH 39/71] remove unnecessary fini --- rclcpp_action/src/server.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 3c02a7354c..1abb85e6cd 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -99,9 +99,6 @@ ServerBase::ServerBase( &pimpl_->num_services_); if (RCL_RET_OK != ret) { - if (RCL_RET_OK != rcl_action_server_fini(pimpl_->action_server_.get(), rcl_node)) { - // Ignoring error during finalization - } rclcpp::exceptions::throw_from_rcl_error(ret); } } From 40b5dbdf69b8bd0103d64c8b13cd806380d1e8a4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 18:27:38 -0800 Subject: [PATCH 40/71] notify_goal_done only takes server --- rclcpp_action/src/server_goal_handle.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index a23f9ca6fe..de6650bce4 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -62,7 +62,7 @@ ServerGoalHandleBase::_set_aborted() rclcpp::exceptions::throw_from_rcl_error(ret); } - ret = rcl_action_notify_goal_done(rcl_server_.get(), rcl_handle_.get()); + ret = rcl_action_notify_goal_done(rcl_server_.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -76,7 +76,7 @@ ServerGoalHandleBase::_set_succeeded() rclcpp::exceptions::throw_from_rcl_error(ret); } - ret = rcl_action_notify_goal_done(rcl_server_.get(), rcl_handle_.get()); + ret = rcl_action_notify_goal_done(rcl_server_.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -90,7 +90,7 @@ ServerGoalHandleBase::_set_canceled() rclcpp::exceptions::throw_from_rcl_error(ret); } - ret = rcl_action_notify_goal_done(rcl_server_.get(), rcl_handle_.get()); + ret = rcl_action_notify_goal_done(rcl_server_.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } From 994539fc3e15aef39464a1cff8da3a18339ffaf2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 12:12:11 -0800 Subject: [PATCH 41/71] Use unique_indentifier_msgs --- rclcpp_action/src/server.cpp | 16 ++++++++-------- rclcpp_action/test/test_server.cpp | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 1abb85e6cd..68ef6dfc3d 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -201,7 +201,7 @@ ServerBase::execute_goal_request_received() pimpl_->goal_request_ready_ = false; std::array uuid = get_goal_id_from_goal_request(message.get()); for (size_t i = 0; i < 16; ++i) { - goal_info.uuid[i] = uuid[i]; + goal_info.goal_id.uuid[i] = uuid[i]; } // Call user's callback, getting the user's response and a ros message to send back @@ -275,7 +275,7 @@ ServerBase::execute_cancel_request_received() // Convert c++ message to C message rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); for (size_t i = 0; i < 16; ++i) { - cancel_request.goal_info.uuid[i] = request->goal_info.uuid[i]; + cancel_request.goal_info.goal_id.uuid[i] = request->goal_info.goal_id.uuid[i]; } cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; @@ -295,7 +295,7 @@ ServerBase::execute_cancel_request_received() const rcl_action_goal_info_t & goal_info = goals.data[i]; std::array uuid; for (size_t i = 0; i < 16; ++i) { - uuid[i] = goal_info.uuid[i]; + uuid[i] = goal_info.goal_id.uuid[i]; } auto response_pair = call_handle_cancel_callback(uuid); if (CancelResponse::ACCEPT == response_pair.first) { @@ -306,7 +306,7 @@ ServerBase::execute_cancel_request_received() rclcpp::exceptions::throw_from_rcl_error(ret); } else { action_msgs::msg::GoalInfo cpp_info; - cpp_info.uuid = uuid; + cpp_info.goal_id.uuid = uuid; cpp_info.stamp.sec = goal_info.stamp.sec; cpp_info.stamp.nanosec = goal_info.stamp.nanosec; response->goals_canceling.push_back(cpp_info); @@ -351,7 +351,7 @@ ServerBase::execute_result_request_received() std::array uuid = get_goal_id_from_result_request(result_request.get()); rcl_action_goal_info_t goal_info; for (size_t i = 0; i < 16; ++i) { - goal_info.uuid[i] = uuid[i]; + goal_info.goal_id.uuid[i] = uuid[i]; } if (!rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info)) { // Goal does not exists @@ -399,7 +399,7 @@ ServerBase::execute_check_expired_goals() // A goal expired! std::array uuid; for (size_t i = 0; i < 16; ++i) { - uuid[i] = expired_goals[0].uuid[i]; + uuid[i] = expired_goals[0].goal_id.uuid[i]; } pimpl_->goal_results_.erase(uuid); pimpl_->result_requests_.erase(uuid); @@ -442,7 +442,7 @@ ServerBase::publish_status() msg.status = goal_state; // Convert C goal info to C++ goal info for (size_t i = 0; i < 16; ++i) { - msg.goal_info.uuid[i] = goal_info.uuid[i]; + msg.goal_info.goal_id.uuid[i] = goal_info.goal_id.uuid[i]; } msg.goal_info.stamp.sec = goal_info.stamp.sec; msg.goal_info.stamp.nanosec = goal_info.stamp.nanosec; @@ -463,7 +463,7 @@ ServerBase::publish_result(const std::array & uuid, std::shared_ptr // Check that the goal exists rcl_action_goal_info_t goal_info; for (size_t i = 0; i < 16; ++i) { - goal_info.uuid[i] = uuid[i]; + goal_info.goal_id.uuid[i] = uuid[i]; } if (!rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info)) { throw std::runtime_error("Asked to publish result for goal that does not exist"); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 4e88220864..748f4f75fb 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -62,7 +62,7 @@ class TestServer : public ::testing::Test throw std::runtime_error("cancel goal service didn't become available"); } auto request = std::make_shared(); - request->goal_info.uuid = uuid; + request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != rclcpp::spin_until_future_complete(node, future)) @@ -256,7 +256,7 @@ TEST_F(TestServer, publish_status_accepted) // Not sure whether accepted will come through because not sure when subscriber will match for (auto & msg : received_msgs) { ASSERT_EQ(1u, msg->status_list.size()); - EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); auto status = msg->status_list.at(0).status; if (action_msgs::msg::GoalStatus::STATUS_ACCEPTED == status) { EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ACCEPTED, status); @@ -318,7 +318,7 @@ TEST_F(TestServer, publish_status_canceling) auto & msg = received_msgs.back(); ASSERT_EQ(1u, msg->status_list.size()); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELING, msg->status_list.at(0).status); - EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); } TEST_F(TestServer, publish_status_canceled) @@ -375,7 +375,7 @@ TEST_F(TestServer, publish_status_canceled) auto & msg = received_msgs.back(); ASSERT_EQ(1u, msg->status_list.size()); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELED, msg->status_list.at(0).status); - EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); } TEST_F(TestServer, publish_status_succeeded) @@ -430,7 +430,7 @@ TEST_F(TestServer, publish_status_succeeded) auto & msg = received_msgs.back(); ASSERT_EQ(1u, msg->status_list.size()); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, msg->status_list.at(0).status); - EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); } TEST_F(TestServer, publish_status_aborted) @@ -485,7 +485,7 @@ TEST_F(TestServer, publish_status_aborted) auto & msg = received_msgs.back(); ASSERT_EQ(1u, msg->status_list.size()); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status); - EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.uuid); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); } TEST_F(TestServer, publish_feedback) From 057afa992f0e960e2e02917bc8e52a2e90535e39 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 15:29:38 -0800 Subject: [PATCH 42/71] create_server accepts group and removes waitable --- .../include/rclcpp_action/create_server.hpp | 57 ++++++++++++------- rclcpp_action/test/test_server.cpp | 24 ++++---- 2 files changed, 48 insertions(+), 33 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index ecc0ad433e..0857737118 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -31,40 +31,55 @@ namespace rclcpp_action template typename Server::SharedPtr create_server( - rclcpp::Node * node, + rclcpp::Node::SharedPtr node, const std::string & name, typename Server::GoalCallback handle_goal, typename Server::CancelCallback handle_cancel, typename Server::AcceptedCallback handle_accepted, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) { - auto action_server = Server::make_shared( + 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](Server * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (!shared_node) { + return; + } + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [] (Server *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specfic group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + delete ptr; + }; + + std::shared_ptr> action_server(new Server( node->get_node_base_interface(), node->get_node_clock_interface(), name, options, handle_goal, handle_cancel, - handle_accepted); + handle_accepted), deleter); - // TODO(sloretz) shared pointer destructor should remove self from node waitables - // TODO(sloretz) pass in callback group to this API - node->get_node_waitables_interface()->add_waitable(action_server, nullptr); + node->get_node_waitables_interface()->add_waitable(action_server, group); return action_server; } - -template -typename Server::SharedPtr -create_server( - rclcpp::Node::SharedPtr node, - const std::string & name, - typename Server::GoalCallback handle_goal, - typename Server::CancelCallback handle_cancel, - typename Server::AcceptedCallback handle_accepted, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) -{ - return create_server( - node.get(), name, handle_goal, handle_cancel, handle_accepted, options); -} } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 748f4f75fb..d406fa09aa 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -77,7 +77,7 @@ TEST_F(TestServer, construction_and_destruction) auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", [](std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, @@ -101,7 +101,7 @@ TEST_F(TestServer, handle_goal_called) }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; @@ -149,7 +149,7 @@ TEST_F(TestServer, handle_accepted_called) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; @@ -189,7 +189,7 @@ TEST_F(TestServer, handle_cancel_called) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -229,7 +229,7 @@ TEST_F(TestServer, publish_status_accepted) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -290,7 +290,7 @@ TEST_F(TestServer, publish_status_canceling) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -345,7 +345,7 @@ TEST_F(TestServer, publish_status_canceled) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -402,7 +402,7 @@ TEST_F(TestServer, publish_status_succeeded) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -457,7 +457,7 @@ TEST_F(TestServer, publish_status_aborted) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -512,7 +512,7 @@ TEST_F(TestServer, publish_feedback) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -570,7 +570,7 @@ TEST_F(TestServer, get_result) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -626,7 +626,7 @@ TEST_F(TestServer, deferred_execution) received_handle = handle; }; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); From 34050770bc3cceacf7a48a3904c539e80e76d4c8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 15:32:12 -0800 Subject: [PATCH 43/71] uncrustify --- .../include/rclcpp_action/create_server.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 0857737118..bf4ad102e8 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -54,7 +54,7 @@ create_server( return; } // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [] (Server *) {}); + std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); if (group_is_null) { // Was added to default group @@ -70,13 +70,13 @@ create_server( }; std::shared_ptr> action_server(new Server( - node->get_node_base_interface(), - node->get_node_clock_interface(), - name, - options, - handle_goal, - handle_cancel, - handle_accepted), deleter); + node->get_node_base_interface(), + node->get_node_clock_interface(), + name, + options, + handle_goal, + handle_cancel, + handle_accepted), deleter); node->get_node_waitables_interface()->add_waitable(action_server, group); return action_server; From a0a1a91caab6a146bc30079b429baee89d0df36d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 17:52:16 -0800 Subject: [PATCH 44/71] Use weak ptr to avoid crash if goal handle lives longer than server --- .../include/rclcpp_action/server.hpp | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 59efd60f00..d2b394e00b 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -272,7 +272,7 @@ struct UUIDHash * - calling user callbacks. */ template -class Server : public ServerBase +class Server : public ServerBase, public std::enable_shared_from_this> { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) @@ -379,25 +379,33 @@ class Server : public ServerBase std::array uuid, std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; - // TODO(sloretz) how to make sure this lambda is not called beyond lifetime of this? + std::weak_ptr> weak_this = this->shared_from_this(); std::function&, std::shared_ptr)> on_terminal_state = - [this](const std::array & uuid, std::shared_ptr result_message) + [weak_this](const std::array & 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 - publish_result(uuid, result_message); + shared_this->publish_result(uuid, result_message); // Publish a status message any time a goal handle changes state - publish_status(); + shared_this->publish_status(); // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) - goal_handles_.erase(uuid); + shared_this->goal_handles_.erase(uuid); }; std::function&)> on_executing = - [this](const std::array & uuid) + [weak_this](const std::array & uuid) { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } (void)uuid; // Publish a status message any time a goal handle changes state - publish_status(); + shared_this->publish_status(); }; goal_handle.reset( From 29652744b726565058f9a95a72ab594b3c87ba9d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 18:02:46 -0800 Subject: [PATCH 45/71] Handle goal callback const message --- .../include/rclcpp_action/server.hpp | 2 +- rclcpp_action/test/test_server.cpp | 99 ++++++++++--------- 2 files changed, 51 insertions(+), 50 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index d2b394e00b..08f1610d44 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -279,7 +279,7 @@ class Server : public ServerBase, public std::enable_shared_from_this&, std::shared_ptr)>; + const std::array&, std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index d406fa09aa..d78bc92e64 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -25,6 +25,7 @@ #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server.hpp" +using Fibonacci = test_msgs::action::Fibonacci; class TestServer : public ::testing::Test { @@ -34,15 +35,15 @@ class TestServer : public ::testing::Test rclcpp::init(0, nullptr); } - std::shared_ptr + std::shared_ptr send_goal_request(rclcpp::Node::SharedPtr node, std::array uuid) { - auto client = node->create_client( + auto client = node->create_client( "fibonacci/_action/send_goal"); if (!client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("send goal service didn't become available"); } - auto request = std::make_shared(); + auto request = std::make_shared(); request->uuid = uuid; auto future = client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != @@ -56,12 +57,12 @@ class TestServer : public ::testing::Test void send_cancel_request(rclcpp::Node::SharedPtr node, std::array uuid) { - auto cancel_client = node->create_client( + auto cancel_client = node->create_client( "fibonacci/_action/cancel_goal"); if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("cancel goal service didn't become available"); } - auto request = std::make_shared(); + auto request = std::make_shared(); request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != @@ -76,9 +77,9 @@ TEST_F(TestServer, construction_and_destruction) { auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); - using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node, "fibonacci", - [](std::array &, std::shared_ptr) { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server(node, "fibonacci", + [](const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, [](std::shared_ptr) { @@ -94,14 +95,14 @@ TEST_F(TestServer, handle_goal_called) std::array received_uuid; auto handle_goal = [&received_uuid]( - std::array & uuid, std::shared_ptr) + const std::array & uuid, std::shared_ptr) { received_uuid = uuid; return rclcpp_action::GoalResponse::REJECT; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node, "fibonacci", + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; @@ -112,12 +113,12 @@ TEST_F(TestServer, handle_goal_called) // Create a client that calls the goal request service // Make sure the UUID received is the same as the one sent - auto client = node->create_client( + auto client = node->create_client( "fibonacci/_action/send_goal"); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); + auto request = std::make_shared(); const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; request->uuid = uuid; @@ -136,12 +137,12 @@ TEST_F(TestServer, handle_accepted_called) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; std::shared_ptr received_handle; auto handle_accepted = [&received_handle](std::shared_ptr handle) @@ -149,7 +150,7 @@ TEST_F(TestServer, handle_accepted_called) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; @@ -171,12 +172,12 @@ TEST_F(TestServer, handle_cancel_called) const std::array uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -189,7 +190,7 @@ TEST_F(TestServer, handle_cancel_called) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -211,12 +212,12 @@ TEST_F(TestServer, publish_status_accepted) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -229,7 +230,7 @@ TEST_F(TestServer, publish_status_accepted) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -272,12 +273,12 @@ TEST_F(TestServer, publish_status_canceling) const std::array uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -290,7 +291,7 @@ TEST_F(TestServer, publish_status_canceling) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -327,12 +328,12 @@ TEST_F(TestServer, publish_status_canceled) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -345,7 +346,7 @@ TEST_F(TestServer, publish_status_canceled) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -362,7 +363,7 @@ TEST_F(TestServer, publish_status_canceled) send_goal_request(node, uuid); send_cancel_request(node, uuid); - received_handle->set_canceled(std::make_shared()); + received_handle->set_canceled(std::make_shared()); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -384,12 +385,12 @@ TEST_F(TestServer, publish_status_succeeded) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -402,7 +403,7 @@ TEST_F(TestServer, publish_status_succeeded) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -417,7 +418,7 @@ TEST_F(TestServer, publish_status_succeeded) }); send_goal_request(node, uuid); - received_handle->set_succeeded(std::make_shared()); + received_handle->set_succeeded(std::make_shared()); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -439,12 +440,12 @@ TEST_F(TestServer, publish_status_aborted) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -457,7 +458,7 @@ TEST_F(TestServer, publish_status_aborted) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -472,7 +473,7 @@ TEST_F(TestServer, publish_status_aborted) }); send_goal_request(node, uuid); - received_handle->set_aborted(std::make_shared()); + received_handle->set_aborted(std::make_shared()); // 10 seconds const size_t max_tries = 10 * 1000 / 100; @@ -494,12 +495,12 @@ TEST_F(TestServer, publish_feedback) const std::array uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -512,14 +513,14 @@ TEST_F(TestServer, publish_feedback) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to feedback messages - using FeedbackT = test_msgs::action::Fibonacci::Feedback; + using FeedbackT = Fibonacci::Feedback; std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg) @@ -552,12 +553,12 @@ TEST_F(TestServer, get_result) const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { @@ -570,7 +571,7 @@ TEST_F(TestServer, get_result) received_handle = handle; }; - auto as = rclcpp_action::create_server(node, "fibonacci", + auto as = rclcpp_action::create_server(node, "fibonacci", handle_goal, handle_cancel, handle_accepted); @@ -579,17 +580,17 @@ TEST_F(TestServer, get_result) send_goal_request(node, uuid); // Send result request - auto result_client = node->create_client( + auto result_client = node->create_client( "fibonacci/_action/get_result"); if (!result_client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("get result service didn't become available"); } - auto request = std::make_shared(); + auto request = std::make_shared(); request->uuid = uuid; auto future = result_client->async_send_request(request); // Send a result - auto result = std::make_shared(); + auto result = std::make_shared(); result->sequence = {5, 8, 13, 21}; received_handle->set_succeeded(result); @@ -608,12 +609,12 @@ TEST_F(TestServer, deferred_execution) const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - std::array &, std::shared_ptr) + const std::array &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; }; - using GoalHandle = rclcpp_action::ServerGoalHandle; + using GoalHandle = rclcpp_action::ServerGoalHandle; auto handle_cancel = [](std::shared_ptr) { From 1003eed9920892da303f3096662075163c862bd9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 18:34:17 -0800 Subject: [PATCH 46/71] Goal handle doesn't have server pointer anymore --- .../include/rclcpp_action/server.hpp | 30 +++++++++++++++---- .../rclcpp_action/server_goal_handle.hpp | 21 +++++-------- rclcpp_action/src/server.cpp | 20 ++++++++++++- rclcpp_action/src/server_goal_handle.cpp | 28 ----------------- 4 files changed, 52 insertions(+), 47 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 08f1610d44..c47384da27 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -174,7 +174,6 @@ class ServerBase : public rclcpp::Waitable virtual void call_goal_accepted_callback( - std::shared_ptr rcl_server, std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) = 0; @@ -204,11 +203,21 @@ class ServerBase : public rclcpp::Waitable void publish_status(); + /// \internal + RCLCPP_ACTION_PUBLIC + void + notify_goal_terminal_state(); + /// \internal RCLCPP_ACTION_PUBLIC void publish_result(const std::array & uuid, std::shared_ptr result_msg); + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_feedback(std::shared_ptr feedback_msg); + // End API for communication between ServerBase and Server<> // --------------------------------------------------------- @@ -374,7 +383,6 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_server, std::shared_ptr rcl_goal_handle, std::array uuid, std::shared_ptr goal_request_message) override { @@ -392,6 +400,8 @@ class Server : public ServerBase, public std::enable_shared_from_thispublish_result(uuid, 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) shared_this->goal_handles_.erase(uuid); }; @@ -408,11 +418,21 @@ class Server : public ServerBase, public std::enable_shared_from_thispublish_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::static_pointer_cast(feedback_msg)); + }; + goal_handle.reset( new ServerGoalHandle( - rcl_server, rcl_goal_handle, - uuid, std::static_pointer_cast(goal_request_message), - on_terminal_state, on_executing)); + rcl_goal_handle, uuid, + std::static_pointer_cast(goal_request_message), + on_terminal_state, on_executing, publish_feedback)); goal_handles_[uuid] = goal_handle; handle_accepted_(goal_handle); } diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 6c465a672e..7c4832be4b 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -78,18 +78,12 @@ class ServerGoalHandleBase /// \internal RCLCPP_ACTION_PUBLIC ServerGoalHandleBase( - std::shared_ptr rcl_server, std::shared_ptr rcl_handle ) - : rcl_server_(rcl_server), rcl_handle_(rcl_handle) + : rcl_handle_(rcl_handle) { } - /// \internal - RCLCPP_ACTION_PUBLIC - void - _publish_feedback(std::shared_ptr feedback_msg); - /// \internal RCLCPP_ACTION_PUBLIC void @@ -114,7 +108,6 @@ class ServerGoalHandleBase // ----------------------------------------------------------------------------- private: - std::shared_ptr rcl_server_; std::shared_ptr rcl_handle_; }; @@ -152,7 +145,7 @@ class ServerGoalHandle : public ServerGoalHandleBase publish_feedback(std::shared_ptr feedback_msg) { feedback_msg->uuid = uuid_; - _publish_feedback(std::static_pointer_cast(feedback_msg)); + publish_feedback_(feedback_msg); } // TODO(sloretz) which exception is raised? @@ -236,15 +229,16 @@ class ServerGoalHandle : public ServerGoalHandleBase protected: /// \internal ServerGoalHandle( - std::shared_ptr rcl_server, std::shared_ptr rcl_handle, std::array uuid, std::shared_ptr goal, std::function&, std::shared_ptr)> on_terminal_state, - std::function&)> on_executing + std::function&)> on_executing, + std::function)> publish_feedback ) - : ServerGoalHandleBase(rcl_server, rcl_handle), goal_(goal), uuid_(uuid), - on_terminal_state_(on_terminal_state), on_executing_(on_executing) + : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), + on_terminal_state_(on_terminal_state), on_executing_(on_executing), + publish_feedback_(publish_feedback) { } @@ -258,6 +252,7 @@ class ServerGoalHandle : public ServerGoalHandleBase std::function&, std::shared_ptr)> on_terminal_state_; std::function&)> on_executing_; + std::function)> publish_feedback_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 68ef6dfc3d..1ee4da15dc 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -250,7 +250,7 @@ ServerBase::execute_goal_request_received() publish_status(); // Tell user to start executing action - call_goal_accepted_callback(pimpl_->action_server_, handle, uuid, message); + call_goal_accepted_callback(handle, uuid, message); } } @@ -483,3 +483,21 @@ ServerBase::publish_result(const std::array & uuid, std::shared_ptr } } } + +void +ServerBase::notify_goal_terminal_state() +{ + rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::publish_feedback(std::shared_ptr feedback_msg) +{ + rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); + } +} diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index de6650bce4..b5662ea29a 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -61,11 +61,6 @@ ServerGoalHandleBase::_set_aborted() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - - ret = rcl_action_notify_goal_done(rcl_server_.get()); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } } void @@ -75,11 +70,6 @@ ServerGoalHandleBase::_set_succeeded() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - - ret = rcl_action_notify_goal_done(rcl_server_.get()); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } } void @@ -89,11 +79,6 @@ ServerGoalHandleBase::_set_canceled() if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - - ret = rcl_action_notify_goal_done(rcl_server_.get()); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } } void @@ -110,17 +95,4 @@ ServerGoalHandleBase::get_rcl_handle() const { return rcl_handle_; } - -void -ServerGoalHandleBase::_publish_feedback(std::shared_ptr feedback_msg) -{ - if (!is_executing()) { - // TODO(sloretz) custom exception - throw std::runtime_error("Publishing feedback for goal that's not executing\n"); - } - rcl_ret_t ret = rcl_action_publish_feedback(rcl_server_.get(), feedback_msg.get()); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); - } -} } // namespace rclcpp_action From 379459febdcabc30709c67f37f14f39a7a4ea563 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 18:39:26 -0800 Subject: [PATCH 47/71] Lock goal_handles_ on Server<> --- rclcpp_action/include/rclcpp_action/server.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index c47384da27..dd4e791ce3 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -367,6 +368,7 @@ class Server : public ServerBase, public std::enable_shared_from_this> call_handle_cancel_callback(const std::array & uuid) override { + std::lock_guard lock(goal_handles_mutex_); CancelResponse resp = CancelResponse::REJECT; std::shared_ptr rcl_handle; auto element = goal_handles_.find(uuid); @@ -403,6 +405,7 @@ class Server : public ServerBase, public std::enable_shared_from_thisnotify_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(uuid); }; @@ -433,7 +436,10 @@ class Server : public ServerBase, public std::enable_shared_from_this(goal_request_message), on_terminal_state, on_executing, publish_feedback)); - goal_handles_[uuid] = goal_handle; + { + std::lock_guard lock(goal_handles_mutex_); + goal_handles_[uuid] = goal_handle; + } handle_accepted_(goal_handle); } @@ -486,6 +492,7 @@ class Server : public ServerBase, public std::enable_shared_from_this, GoalHandleWeakPtr, UUIDHash> goal_handles_; + std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action #endif // RCLCPP_ACTION__SERVER_HPP_ From 2085366a9f304fdd8592f04ea1a8be6a2283905c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 18:49:59 -0800 Subject: [PATCH 48/71] rcl_action_server_t protected with mutex --- rclcpp_action/src/server.cpp | 114 ++++++++++++++++++++++++++--------- 1 file changed, 84 insertions(+), 30 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 1ee4da15dc..a4a79acf57 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -32,7 +33,9 @@ namespace rclcpp_action class ServerBaseImpl { public: + std::mutex server_mutex_; std::shared_ptr action_server_; + rclcpp::Clock::SharedPtr clock_; size_t num_subscriptions_ = 0; @@ -46,6 +49,7 @@ class ServerBaseImpl bool result_request_ready_ = false; bool goal_expired_ = false; + std::mutex results_mutex_; // Results to be kept until the goal expires after reaching a terminal state std::unordered_map, std::shared_ptr, UUIDHash> goal_results_; // Requests for results are kept until a result becomes available @@ -140,6 +144,7 @@ ServerBase::get_number_of_ready_guard_conditions() bool ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { + std::lock_guard lock(pimpl_->server_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( wait_set, pimpl_->action_server_.get(), NULL); return RCL_RET_OK == ret; @@ -148,6 +153,7 @@ ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) bool ServerBase::is_ready(rcl_wait_set_t * wait_set) { + std::lock_guard lock(pimpl_->server_mutex_); rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( wait_set, pimpl_->action_server_.get(), @@ -189,10 +195,13 @@ ServerBase::execute_goal_request_received() rmw_request_id_t request_header; std::shared_ptr message = create_goal_request(); - ret = rcl_action_take_goal_request( - pimpl_->action_server_.get(), - &request_header, - message.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_take_goal_request( + pimpl_->action_server_.get(), + &request_header, + message.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -207,10 +216,13 @@ ServerBase::execute_goal_request_received() // Call user's callback, getting the user's response and a ros message to send back auto response_pair = call_handle_goal_callback(uuid, message); - ret = rcl_action_send_goal_response( - pimpl_->action_server_.get(), - &request_header, - response_pair.second.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_send_goal_response( + pimpl_->action_server_.get(), + &request_header, + response_pair.second.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -229,8 +241,11 @@ ServerBase::execute_goal_request_received() delete ptr; } }; - rcl_action_goal_handle_t * rcl_handle = rcl_action_accept_new_goal( - pimpl_->action_server_.get(), &goal_info); + rcl_action_goal_handle_t * rcl_handle; + { + std::lock_guard lock(pimpl_->server_mutex_); + rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); + } if (!rcl_handle) { throw std::runtime_error("Failed to accept new goal\n"); } @@ -263,10 +278,13 @@ ServerBase::execute_cancel_request_received() // Initialize cancel request auto request = std::make_shared(); - ret = rcl_action_take_cancel_request( - pimpl_->action_server_.get(), - &request_header, - request.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_take_cancel_request( + pimpl_->action_server_.get(), + &request_header, + request.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -282,10 +300,14 @@ ServerBase::execute_cancel_request_received() // Get a list of goal info that should be attempted to be cancelled rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); - ret = rcl_action_process_cancel_request( - pimpl_->action_server_.get(), - &cancel_request, - &cancel_response); + + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_process_cancel_request( + pimpl_->action_server_.get(), + &cancel_request, + &cancel_response); + } auto response = std::make_shared(); @@ -325,8 +347,11 @@ ServerBase::execute_cancel_request_received() rclcpp::exceptions::throw_from_rcl_error(ret); } - ret = rcl_action_send_cancel_response( - pimpl_->action_server_.get(), &request_header, response.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_send_cancel_response( + pimpl_->action_server_.get(), &request_header, response.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -339,8 +364,11 @@ ServerBase::execute_result_request_received() // Get the result request message rmw_request_id_t request_header; std::shared_ptr result_request = create_result_request(); - ret = rcl_action_take_result_request( - pimpl_->action_server_.get(), &request_header, result_request.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_take_result_request( + pimpl_->action_server_.get(), &request_header, result_request.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -353,7 +381,12 @@ ServerBase::execute_result_request_received() for (size_t i = 0; i < 16; ++i) { goal_info.goal_id.uuid[i] = uuid[i]; } - if (!rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info)) { + bool goal_exists; + { + std::lock_guard lock(pimpl_->server_mutex_); + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + } + if (!goal_exists) { // Goal does not exists result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); } else { @@ -366,8 +399,11 @@ ServerBase::execute_result_request_received() if (result_response) { // Send the result now - ret = rcl_action_send_result_response( - pimpl_->action_server_.get(), &request_header, result_response.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_response.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -391,8 +427,11 @@ ServerBase::execute_check_expired_goals() // Loop in case more than 1 goal expired while (num_expired > 0u) { - rcl_ret_t ret = rcl_action_expire_goals( - pimpl_->action_server_.get(), expired_goals, 1, &num_expired); + rcl_ret_t ret; + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } else if (num_expired) { @@ -415,7 +454,11 @@ ServerBase::publish_status() // Get all goal handles known to C action server rcl_action_goal_handle_t ** goal_handles = NULL; size_t num_goals = 0; - ret = rcl_action_server_get_goal_handles(pimpl_->action_server_.get(), &goal_handles, &num_goals); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_server_get_goal_handles( + pimpl_->action_server_.get(), &goal_handles, &num_goals); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -451,7 +494,10 @@ ServerBase::publish_status() } // Publish the message through the status publisher - ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + { + std::lock_guard lock(pimpl_->server_mutex_); + ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -465,7 +511,12 @@ ServerBase::publish_result(const std::array & uuid, std::shared_ptr for (size_t i = 0; i < 16; ++i) { goal_info.goal_id.uuid[i] = uuid[i]; } - if (!rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info)) { + bool goal_exists; + { + std::lock_guard lock(pimpl_->server_mutex_); + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + } + if (!goal_exists) { throw std::runtime_error("Asked to publish result for goal that does not exist"); } @@ -475,6 +526,7 @@ ServerBase::publish_result(const std::array & uuid, std::shared_ptr auto iter = pimpl_->result_requests_.find(uuid); if (iter != pimpl_->result_requests_.end()) { for (auto & request_header : iter->second) { + std::lock_guard lock(pimpl_->server_mutex_); rcl_ret_t ret = rcl_action_send_result_response( pimpl_->action_server_.get(), &request_header, result_msg.get()); if (RCL_RET_OK != ret) { @@ -487,6 +539,7 @@ ServerBase::publish_result(const std::array & uuid, std::shared_ptr void ServerBase::notify_goal_terminal_state() { + std::lock_guard lock(pimpl_->server_mutex_); rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -496,6 +549,7 @@ ServerBase::notify_goal_terminal_state() void ServerBase::publish_feedback(std::shared_ptr feedback_msg) { + std::lock_guard lock(pimpl_->server_mutex_); rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); From 466439887b052cd9a0c1b2ece616af8636ed90ce Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 18:52:11 -0800 Subject: [PATCH 49/71] ServerBase results protected with mutex --- rclcpp_action/src/server.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index a4a79acf57..ce0f1a7810 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -391,6 +391,7 @@ ServerBase::execute_result_request_received() result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); } else { // Goal exists, check if a result is already available + std::lock_guard lock(pimpl_->results_mutex_); auto iter = pimpl_->goal_results_.find(uuid); if (iter != pimpl_->goal_results_.end()) { result_response = iter->second; @@ -409,6 +410,7 @@ ServerBase::execute_result_request_received() } } else { // Store the request so it can be responded to later + std::lock_guard lock(pimpl_->results_mutex_); auto iter = pimpl_->result_requests_.find(uuid); if (iter == pimpl_->result_requests_.end()) { pimpl_->result_requests_[uuid] = std::vector{request_header}; @@ -440,6 +442,7 @@ ServerBase::execute_check_expired_goals() for (size_t i = 0; i < 16; ++i) { uuid[i] = expired_goals[0].goal_id.uuid[i]; } + std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_.erase(uuid); pimpl_->result_requests_.erase(uuid); } @@ -520,6 +523,7 @@ ServerBase::publish_result(const std::array & uuid, std::shared_ptr throw std::runtime_error("Asked to publish result for goal that does not exist"); } + std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_[uuid] = result_msg; // if there are clients who already asked for the result, send it to them From 48a709e58529bc1a5b95518b8aa7c19e08bf1f59 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 19:41:46 -0800 Subject: [PATCH 50/71] protect rcl goal handle with mutex --- .../include/rclcpp_action/server.hpp | 10 ++++---- .../rclcpp_action/server_goal_handle.hpp | 16 ++++++------- rclcpp_action/src/server.cpp | 21 ++++++++--------- rclcpp_action/src/server_goal_handle.cpp | 23 ++++++++++++++----- 4 files changed, 39 insertions(+), 31 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index dd4e791ce3..91d3e17725 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -152,7 +152,7 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC virtual - std::pair> + CancelResponse call_handle_cancel_callback(const std::array & uuid) = 0; /// Given a goal request message, return the UUID contained within. @@ -365,7 +365,7 @@ class Server : public ServerBase, public std::enable_shared_from_this> + CancelResponse call_handle_cancel_callback(const std::array & uuid) override { std::lock_guard lock(goal_handles_mutex_); @@ -376,10 +376,12 @@ class Server : public ServerBase, public std::enable_shared_from_this> goal_handle = element->second.lock(); if (goal_handle) { resp = handle_cancel_(goal_handle); - rcl_handle = goal_handle->get_rcl_handle(); + if (CancelResponse::ACCEPT == resp) { + goal_handle->_set_canceling(); + } } } - return std::make_pair(resp, rcl_handle); + return resp; } /// \internal diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 7c4832be4b..e68aab575b 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -22,6 +22,7 @@ #include #include +#include #include "rclcpp_action/visibility_control.hpp" @@ -62,15 +63,6 @@ class ServerGoalHandleBase virtual ~ServerGoalHandleBase(); - /// Get a shared pointer to the C struct this class wraps. - /** - * It is not thread safe to use the returned pointer in any `rcl_action` functions while calling - * any of the other functions on this class. - */ - RCLCPP_ACTION_PUBLIC - std::shared_ptr - get_rcl_handle() const; - protected: // ------------------------------------------------------------------------- // API for communication between ServerGoalHandleBase and ServerGoalHandle<> @@ -94,6 +86,11 @@ class ServerGoalHandleBase void _set_succeeded(); + /// \internal + RCLCPP_ACTION_PUBLIC + void + _set_canceling(); + /// \internal RCLCPP_ACTION_PUBLIC void @@ -109,6 +106,7 @@ class ServerGoalHandleBase private: std::shared_ptr rcl_handle_; + mutable std::mutex rcl_handle_mutex_; }; // Forward declare server diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index ce0f1a7810..28da409ebd 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -319,20 +319,17 @@ ServerBase::execute_cancel_request_received() for (size_t i = 0; i < 16; ++i) { uuid[i] = goal_info.goal_id.uuid[i]; } - auto response_pair = call_handle_cancel_callback(uuid); - if (CancelResponse::ACCEPT == response_pair.first) { - ret = rcl_action_update_goal_state(response_pair.second.get(), GOAL_EVENT_CANCEL); - if (RCL_RET_OK != ret) { - rcl_ret_t fail_ret = rcl_action_cancel_response_fini(&cancel_response); - (void)fail_ret; // TODO(sloretz) do something with error during cleanup + auto response_code = call_handle_cancel_callback(uuid); + if (CancelResponse::ACCEPT == response_code) { + rcl_ret_t fail_ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != fail_ret) { rclcpp::exceptions::throw_from_rcl_error(ret); - } else { - action_msgs::msg::GoalInfo cpp_info; - cpp_info.goal_id.uuid = uuid; - cpp_info.stamp.sec = goal_info.stamp.sec; - cpp_info.stamp.nanosec = goal_info.stamp.nanosec; - response->goals_canceling.push_back(cpp_info); } + action_msgs::msg::GoalInfo cpp_info; + cpp_info.goal_id.uuid = uuid; + cpp_info.stamp.sec = goal_info.stamp.sec; + cpp_info.stamp.nanosec = goal_info.stamp.nanosec; + response->goals_canceling.push_back(cpp_info); } } diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index b5662ea29a..9e6999a550 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -29,6 +29,7 @@ ServerGoalHandleBase::~ServerGoalHandleBase() bool ServerGoalHandleBase::is_cancel_request() const { + std::lock_guard lock(rcl_handle_mutex_); rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); if (RCL_RET_OK != ret) { @@ -40,12 +41,14 @@ ServerGoalHandleBase::is_cancel_request() const bool ServerGoalHandleBase::is_active() const { + std::lock_guard lock(rcl_handle_mutex_); return rcl_action_goal_handle_is_active(rcl_handle_.get()); } bool ServerGoalHandleBase::is_executing() const { + std::lock_guard lock(rcl_handle_mutex_); rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); if (RCL_RET_OK != ret) { @@ -57,6 +60,7 @@ ServerGoalHandleBase::is_executing() const void ServerGoalHandleBase::_set_aborted() { + std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -66,15 +70,27 @@ ServerGoalHandleBase::_set_aborted() void ServerGoalHandleBase::_set_succeeded() { + std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } +void +ServerGoalHandleBase::_set_canceling() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + void ServerGoalHandleBase::_set_canceled() { + std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -84,15 +100,10 @@ ServerGoalHandleBase::_set_canceled() void ServerGoalHandleBase::_set_executing() { + std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } - -std::shared_ptr -ServerGoalHandleBase::get_rcl_handle() const -{ - return rcl_handle_; -} } // namespace rclcpp_action From e54b8a5207647fb9aa0f83d585507be91bd876b9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 19:46:38 -0800 Subject: [PATCH 51/71] is_cancel_request -> is_canceling --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 2 +- rclcpp_action/src/server_goal_handle.cpp | 2 +- rclcpp_action/test/test_server.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index e68aab575b..33d87252c6 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -45,7 +45,7 @@ class ServerGoalHandleBase /// \return true if a cancelation request has been accepted for this goal. RCLCPP_ACTION_PUBLIC bool - is_cancel_request() const; + is_canceling() const; /// Indicate if goal is pending or executing. /// \return false if goal has reached a terminal state. diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 9e6999a550..9308296bbf 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -27,7 +27,7 @@ ServerGoalHandleBase::~ServerGoalHandleBase() } bool -ServerGoalHandleBase::is_cancel_request() const +ServerGoalHandleBase::is_canceling() const { std::lock_guard lock(rcl_handle_mutex_); rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index d78bc92e64..68c60455bd 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -200,10 +200,10 @@ TEST_F(TestServer, handle_cancel_called) ASSERT_TRUE(received_handle); EXPECT_EQ(uuid, received_handle->get_uuid()); - EXPECT_FALSE(received_handle->is_cancel_request()); + EXPECT_FALSE(received_handle->is_canceling()); send_cancel_request(node, uuid); - EXPECT_TRUE(received_handle->is_cancel_request()); + EXPECT_TRUE(received_handle->is_canceling()); } TEST_F(TestServer, publish_status_accepted) From 69fba0c017b83b19acd81c9a4db68ed80ea1022f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 19:49:26 -0800 Subject: [PATCH 52/71] Add missing include --- rclcpp_action/include/rclcpp_action/server.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 91d3e17725..501c94d81b 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__SERVER_HPP_ #define RCLCPP_ACTION__SERVER_HPP_ +#include #include #include #include From c3f1ec61b5b120f24cf33d507d170a81e852407c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 08:26:56 -0800 Subject: [PATCH 53/71] use GoalID and change uuid -> goal_id --- .../include/rclcpp_action/server.hpp | 37 ++++++++++--------- .../rclcpp_action/server_goal_handle.hpp | 17 +++++---- rclcpp_action/src/server.cpp | 14 +++---- rclcpp_action/test/test_server.cpp | 4 +- 4 files changed, 37 insertions(+), 35 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 501c94d81b..6bfe9bc076 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -32,6 +32,7 @@ #include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_action/types.hpp" namespace rclcpp_action { @@ -145,7 +146,7 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual std::pair> - call_handle_goal_callback(std::array &, std::shared_ptr request) = 0; + call_handle_goal_callback(GoalID &, std::shared_ptr request) = 0; // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. @@ -154,13 +155,13 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual CancelResponse - call_handle_cancel_callback(const std::array & uuid) = 0; + call_handle_cancel_callback(const GoalID & uuid) = 0; /// Given a goal request message, return the UUID contained within. /// \internal RCLCPP_ACTION_PUBLIC virtual - std::array + GoalID get_goal_id_from_goal_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. @@ -177,13 +178,13 @@ class ServerBase : public rclcpp::Waitable void call_goal_accepted_callback( std::shared_ptr rcl_goal_handle, - std::array uuid, std::shared_ptr goal_request_message) = 0; + GoalID uuid, std::shared_ptr goal_request_message) = 0; /// Given a result request message, return the UUID contained within. /// \internal RCLCPP_ACTION_PUBLIC virtual - std::array + GoalID get_goal_id_from_result_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. @@ -213,7 +214,7 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - publish_result(const std::array & uuid, std::shared_ptr result_msg); + publish_result(const GoalID & uuid, std::shared_ptr result_msg); /// \internal RCLCPP_ACTION_PUBLIC @@ -257,7 +258,7 @@ class ServerBase : public rclcpp::Waitable /// Hash a goal id so it can be used as a key in std::unordered_map struct UUIDHash { - size_t operator()(std::array const & uuid) const noexcept + size_t operator()(GoalID const & uuid) const noexcept { // TODO(sloretz) Use someone else's hash function and cite it size_t result = 0; @@ -290,7 +291,7 @@ class Server : public ServerBase, public std::enable_shared_from_this&, std::shared_ptr)>; + const GoalID&, std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. @@ -350,7 +351,7 @@ class Server : public ServerBase, public std::enable_shared_from_this> - call_handle_goal_callback(std::array & uuid, std::shared_ptr message) override + call_handle_goal_callback(GoalID & uuid, std::shared_ptr message) override { // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type static_assert( @@ -367,7 +368,7 @@ class Server : public ServerBase, public std::enable_shared_from_this & uuid) override + call_handle_cancel_callback(const GoalID & uuid) override { std::lock_guard lock(goal_handles_mutex_); CancelResponse resp = CancelResponse::REJECT; @@ -389,13 +390,13 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_goal_handle, - std::array uuid, std::shared_ptr goal_request_message) override + GoalID uuid, std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); - std::function&, std::shared_ptr)> on_terminal_state = - [weak_this](const std::array & uuid, std::shared_ptr result_message) + std::function)> on_terminal_state = + [weak_this](const GoalID & uuid, std::shared_ptr result_message) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { @@ -412,8 +413,8 @@ class Server : public ServerBase, public std::enable_shared_from_thisgoal_handles_.erase(uuid); }; - std::function&)> on_executing = - [weak_this](const std::array & uuid) + std::function on_executing = + [weak_this](const GoalID & uuid) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { @@ -447,7 +448,7 @@ class Server : public ServerBase, public std::enable_shared_from_this + GoalID get_goal_id_from_goal_request(void * message) override { return static_cast(message)->uuid; @@ -461,7 +462,7 @@ class Server : public ServerBase, public std::enable_shared_from_this + GoalID get_goal_id_from_result_request(void * message) override { return static_cast(message)->uuid; @@ -494,7 +495,7 @@ class Server : public ServerBase, public std::enable_shared_from_this>; /// A map of goal id to goal handle weak pointers. /// This is used to provide a goal handle to handle_cancel. - std::unordered_map, GoalHandleWeakPtr, UUIDHash> goal_handles_; + std::unordered_map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 33d87252c6..97844661a0 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -25,6 +25,7 @@ #include #include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/types.hpp" namespace rclcpp_action { @@ -218,8 +219,8 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// Get the unique identifier of the goal - const std::array - get_uuid() const + const GoalID + get_goal_id() const { return uuid_; } @@ -228,10 +229,10 @@ class ServerGoalHandle : public ServerGoalHandleBase /// \internal ServerGoalHandle( std::shared_ptr rcl_handle, - std::array uuid, + GoalID uuid, std::shared_ptr goal, - std::function&, std::shared_ptr)> on_terminal_state, - std::function&)> on_executing, + std::function)> on_terminal_state, + std::function on_executing, std::function)> publish_feedback ) : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), @@ -244,12 +245,12 @@ class ServerGoalHandle : public ServerGoalHandleBase const std::shared_ptr goal_; /// A unique id for the goal request. - const std::array uuid_; + const GoalID uuid_; friend Server; - std::function&, std::shared_ptr)> on_terminal_state_; - std::function&)> on_executing_; + std::function)> on_terminal_state_; + std::function on_executing_; std::function)> publish_feedback_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 28da409ebd..7b64f17ca4 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -51,9 +51,9 @@ class ServerBaseImpl std::mutex results_mutex_; // Results to be kept until the goal expires after reaching a terminal state - std::unordered_map, std::shared_ptr, UUIDHash> goal_results_; + std::unordered_map, UUIDHash> goal_results_; // Requests for results are kept until a result becomes available - std::unordered_map, std::vector, UUIDHash> + std::unordered_map, UUIDHash> result_requests_; }; } // namespace rclcpp_action @@ -208,7 +208,7 @@ ServerBase::execute_goal_request_received() } pimpl_->goal_request_ready_ = false; - std::array uuid = get_goal_id_from_goal_request(message.get()); + GoalID uuid = get_goal_id_from_goal_request(message.get()); for (size_t i = 0; i < 16; ++i) { goal_info.goal_id.uuid[i] = uuid[i]; } @@ -315,7 +315,7 @@ ServerBase::execute_cancel_request_received() // For each canceled goal, call cancel callback for (size_t i = 0; i < goals.size; ++i) { const rcl_action_goal_info_t & goal_info = goals.data[i]; - std::array uuid; + GoalID uuid; for (size_t i = 0; i < 16; ++i) { uuid[i] = goal_info.goal_id.uuid[i]; } @@ -373,7 +373,7 @@ ServerBase::execute_result_request_received() std::shared_ptr result_response; // check if the goal exists - std::array uuid = get_goal_id_from_result_request(result_request.get()); + GoalID uuid = get_goal_id_from_result_request(result_request.get()); rcl_action_goal_info_t goal_info; for (size_t i = 0; i < 16; ++i) { goal_info.goal_id.uuid[i] = uuid[i]; @@ -435,7 +435,7 @@ ServerBase::execute_check_expired_goals() rclcpp::exceptions::throw_from_rcl_error(ret); } else if (num_expired) { // A goal expired! - std::array uuid; + GoalID uuid; for (size_t i = 0; i < 16; ++i) { uuid[i] = expired_goals[0].goal_id.uuid[i]; } @@ -504,7 +504,7 @@ ServerBase::publish_status() } void -ServerBase::publish_result(const std::array & uuid, std::shared_ptr result_msg) +ServerBase::publish_result(const GoalID & uuid, std::shared_ptr result_msg) { // Check that the goal exists rcl_action_goal_info_t goal_info; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 68c60455bd..43953de0b5 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -162,7 +162,7 @@ TEST_F(TestServer, handle_accepted_called) ASSERT_TRUE(received_handle); ASSERT_TRUE(received_handle->is_active()); - EXPECT_EQ(uuid, received_handle->get_uuid()); + EXPECT_EQ(uuid, received_handle->get_goal_id()); EXPECT_EQ(*request, *(received_handle->get_goal())); } @@ -199,7 +199,7 @@ TEST_F(TestServer, handle_cancel_called) send_goal_request(node, uuid); ASSERT_TRUE(received_handle); - EXPECT_EQ(uuid, received_handle->get_uuid()); + EXPECT_EQ(uuid, received_handle->get_goal_id()); EXPECT_FALSE(received_handle->is_canceling()); send_cancel_request(node, uuid); From 1c36fb8c3cd69464adf4a13f2d2072c9bc6682ec Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 13:49:33 -0800 Subject: [PATCH 54/71] Keep rcl goal handle alive until it expires on server --- rclcpp_action/src/server.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 7b64f17ca4..39b02696c4 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -53,8 +53,9 @@ class ServerBaseImpl // Results to be kept until the goal expires after reaching a terminal state std::unordered_map, UUIDHash> goal_results_; // Requests for results are kept until a result becomes available - std::unordered_map, UUIDHash> - result_requests_; + std::unordered_map, UUIDHash> result_requests_; + // rcl goal handles are kept so api to send result doesn't try to access freed memory + std::unordered_map, UUIDHash> goal_handles_; }; } // namespace rclcpp_action @@ -254,6 +255,11 @@ ServerBase::execute_goal_request_received() // Copy out goal handle since action server storage disappears when it is fini'd *handle = *rcl_handle; + { + std::lock_guard lock(pimpl_->results_mutex_); + pimpl_->goal_handles_[uuid] = handle; + } + if (GoalResponse::ACCEPT_AND_EXECUTE == status) { // Change status to executing ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); @@ -442,6 +448,7 @@ ServerBase::execute_check_expired_goals() std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_.erase(uuid); pimpl_->result_requests_.erase(uuid); + pimpl_->goal_handles_.erase(uuid); } } } From 63eff6c0d0f45b51d334456e8072dce431bf25d8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 13:54:45 -0800 Subject: [PATCH 55/71] uncrustify --- rclcpp_action/include/rclcpp_action/server.hpp | 6 +++--- .../include/rclcpp_action/server_goal_handle.hpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 6bfe9bc076..2da21d37cb 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -291,7 +291,7 @@ class Server : public ServerBase, public std::enable_shared_from_this)>; + const GoalID &, std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. @@ -395,7 +395,7 @@ class Server : public ServerBase, public std::enable_shared_from_this> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); - std::function)> on_terminal_state = + std::function)> on_terminal_state = [weak_this](const GoalID & uuid, std::shared_ptr result_message) { std::shared_ptr> shared_this = weak_this.lock(); @@ -413,7 +413,7 @@ class Server : public ServerBase, public std::enable_shared_from_thisgoal_handles_.erase(uuid); }; - std::function on_executing = + std::function on_executing = [weak_this](const GoalID & uuid) { std::shared_ptr> shared_this = weak_this.lock(); diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 97844661a0..a6b446fbb2 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -231,8 +231,8 @@ class ServerGoalHandle : public ServerGoalHandleBase std::shared_ptr rcl_handle, GoalID uuid, std::shared_ptr goal, - std::function)> on_terminal_state, - std::function on_executing, + std::function)> on_terminal_state, + std::function on_executing, std::function)> publish_feedback ) : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), @@ -249,8 +249,8 @@ class ServerGoalHandle : public ServerGoalHandleBase friend Server; - std::function)> on_terminal_state_; - std::function on_executing_; + std::function)> on_terminal_state_; + std::function on_executing_; std::function)> publish_feedback_; }; } // namespace rclcpp_action From 054b8181bebc9aa7ef781d57cd11e6f4ef664701 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 14:58:11 -0800 Subject: [PATCH 56/71] Move UUID hash --- .../include/rclcpp_action/server.hpp | 22 +------------------ rclcpp_action/include/rclcpp_action/types.hpp | 20 +++++++++++++++++ rclcpp_action/src/server.cpp | 6 ++--- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 2da21d37cb..b6fcd86b15 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -22,7 +22,6 @@ #include #include -#include #include #include #include @@ -254,25 +253,6 @@ class ServerBase : public rclcpp::Waitable std::unique_ptr pimpl_; }; - -/// Hash a goal id so it can be used as a key in std::unordered_map -struct UUIDHash -{ - size_t operator()(GoalID const & uuid) const noexcept - { - // TODO(sloretz) Use someone else's hash function and cite it - size_t result = 0; - for (size_t i = 0; i < 16; ++i) { - for (size_t b = 0; b < sizeof(size_t); ++b) { - size_t part = uuid[i]; - part <<= CHAR_BIT * b; - result ^= part; - } - } - return result; - } -}; - /// Action Server /** * This class creates an action server. @@ -495,7 +475,7 @@ class Server : public ServerBase, public std::enable_shared_from_this>; /// 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::unordered_map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 3ab9f4ce19..e6bc9c0d37 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -20,6 +20,7 @@ #include #include +#include #include @@ -44,5 +45,24 @@ struct less return lhs < rhs; } }; + +/// Hash a goal id so it can be used as a key in std::unordered_map +template<> +struct hash +{ + size_t operator()(const rclcpp_action::GoalID & uuid) const noexcept + { + // TODO(sloretz) Use someone else's hash function and cite it + size_t result = 0; + for (size_t i = 0; i < 16; ++i) { + for (size_t b = 0; b < sizeof(size_t); ++b) { + size_t part = uuid[i]; + part <<= CHAR_BIT * b; + result ^= part; + } + } + return result; + } +}; } // namespace std #endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 39b02696c4..6e568edd17 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -51,11 +51,11 @@ class ServerBaseImpl std::mutex results_mutex_; // Results to be kept until the goal expires after reaching a terminal state - std::unordered_map, UUIDHash> goal_results_; + std::unordered_map> goal_results_; // Requests for results are kept until a result becomes available - std::unordered_map, UUIDHash> result_requests_; + std::unordered_map> result_requests_; // rcl goal handles are kept so api to send result doesn't try to access freed memory - std::unordered_map, UUIDHash> goal_handles_; + std::unordered_map> goal_handles_; }; } // namespace rclcpp_action From ac82e363960fa23899ab2169c79ea6de3654eed4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 15:42:57 -0800 Subject: [PATCH 57/71] Log messages in server --- rclcpp_action/CMakeLists.txt | 1 + .../include/rclcpp_action/create_server.hpp | 4 +++ .../include/rclcpp_action/server.hpp | 22 +++++++------ rclcpp_action/include/rclcpp_action/types.hpp | 8 ++++- rclcpp_action/src/server.cpp | 24 +++++++++++--- rclcpp_action/src/types.cpp | 32 +++++++++++++++++++ 6 files changed, 77 insertions(+), 14 deletions(-) create mode 100644 rclcpp_action/src/types.cpp diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index cb5c8c1d22..29f85680c3 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -22,6 +22,7 @@ set(${PROJECT_NAME}_SRCS src/client.cpp src/server.cpp src/server_goal_handle.cpp + src/types.cpp ) add_library(${PROJECT_NAME} diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index bf4ad102e8..ff3dc7e73c 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -18,6 +18,9 @@ #include #include +#include +#include +#include #include #include @@ -72,6 +75,7 @@ create_server( std::shared_ptr> action_server(new Server( node->get_node_base_interface(), node->get_node_clock_interface(), + node->get_node_logging_interface(), name, options, handle_goal, diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index b6fcd86b15..e101386cab 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -69,15 +70,6 @@ enum class CancelResponse : int8_t class ServerBase : public rclcpp::Waitable { public: - // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node - RCLCPP_ACTION_PUBLIC - ServerBase( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, - const std::string & name, - const rosidl_action_type_support_t * type_support, - const rcl_action_server_options_t & options); - RCLCPP_ACTION_PUBLIC virtual ~ServerBase(); @@ -136,6 +128,15 @@ class ServerBase : public rclcpp::Waitable // ----------------- protected: + RCLCPP_ACTION_PUBLIC + ServerBase( + 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, + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options); + // ----------------------------------------------------- // API for communication between ServerBase and Server<> @@ -291,6 +292,7 @@ class Server : public ServerBase, public std::enable_shared_from_this(), options), diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index e6bc9c0d37..15d1f1ead3 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -22,7 +22,9 @@ #include #include +#include +#include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action { @@ -31,6 +33,10 @@ using GoalID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; +/// Convert a goal id to a human readable string. +RCLCPP_ACTION_PUBLIC +std::string +to_string(const GoalID & goal_id); } // namespace rclcpp_action namespace std @@ -54,7 +60,7 @@ struct hash { // TODO(sloretz) Use someone else's hash function and cite it size_t result = 0; - for (size_t i = 0; i < 16; ++i) { + for (size_t i = 0; i < uuid.size(); ++i) { for (size_t b = 0; b < sizeof(size_t); ++b) { size_t part = uuid[i]; part <<= CHAR_BIT * b; diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 6e568edd17..dde2834a76 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -33,6 +33,14 @@ namespace rclcpp_action class ServerBaseImpl { public: + ServerBaseImpl( + rclcpp::Clock::SharedPtr clock, + rclcpp::Logger logger + ) + : clock_(clock), logger_(logger) + { + } + std::mutex server_mutex_; std::shared_ptr action_server_; @@ -56,28 +64,32 @@ class ServerBaseImpl std::unordered_map> result_requests_; // rcl goal handles are kept so api to send result doesn't try to access freed memory std::unordered_map> goal_handles_; + + rclcpp::Logger logger_; }; } // namespace rclcpp_action ServerBase::ServerBase( 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, const rosidl_action_type_support_t * type_support, const rcl_action_server_options_t & options ) -: pimpl_(new ServerBaseImpl) +: pimpl_(new ServerBaseImpl( + node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) { rcl_ret_t ret; - pimpl_->clock_ = node_clock->get_clock(); auto deleter = [node_base](rcl_action_server_t * ptr) { if (nullptr != ptr) { rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); - // TODO(sloretz) do something if error occurs during destruction (void)ret; + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_server_t in deleter"); } delete ptr; }; @@ -233,12 +245,15 @@ ServerBase::execute_goal_request_received() // if goal is accepted, create a goal handle, and store it if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) { + RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str()); // rcl_action will set time stamp auto deleter = [](rcl_action_goal_handle_t * ptr) { if (nullptr != ptr) { rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); - (void)fail_ret; // TODO(sloretz) do something with error during cleanup + (void)fail_ret; + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_goal_handle_t in deleter"); delete ptr; } }; @@ -445,6 +460,7 @@ ServerBase::execute_check_expired_goals() for (size_t i = 0; i < 16; ++i) { uuid[i] = expired_goals[0].goal_id.uuid[i]; } + RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_.erase(uuid); pimpl_->result_requests_.erase(uuid); diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp new file mode 100644 index 0000000000..fd6c783297 --- /dev/null +++ b/rclcpp_action/src/types.cpp @@ -0,0 +1,32 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp_action/types.hpp" + +#include +#include + +namespace rclcpp_action +{ +std::string +to_string(const GoalID & goal_id) +{ + std::stringstream stream; + stream << std::hex; + for (const auto & element : goal_id) { + stream << static_cast(element); + } + return stream.str(); +} +} // namespace rclcpp_action From ad570dc4215eb96ac38295177efdc91838751d1a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 15:44:34 -0800 Subject: [PATCH 58/71] ACTION -> ActionT --- .../include/rclcpp_action/create_server.hpp | 16 +++--- .../include/rclcpp_action/server.hpp | 50 +++++++++---------- .../rclcpp_action/server_goal_handle.hpp | 24 ++++----- 3 files changed, 45 insertions(+), 45 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index ff3dc7e73c..ecd557ca48 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -31,14 +31,14 @@ namespace rclcpp_action { -template -typename Server::SharedPtr +template +typename Server::SharedPtr create_server( rclcpp::Node::SharedPtr node, const std::string & name, - typename Server::GoalCallback handle_goal, - typename Server::CancelCallback handle_cancel, - typename Server::AcceptedCallback handle_accepted, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) { @@ -47,7 +47,7 @@ create_server( std::weak_ptr weak_group = group; bool group_is_null = (nullptr == group.get()); - auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) + auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) { if (nullptr == ptr) { return; @@ -57,7 +57,7 @@ create_server( return; } // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); + std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); if (group_is_null) { // Was added to default group @@ -72,7 +72,7 @@ create_server( delete ptr; }; - std::shared_ptr> action_server(new Server( + std::shared_ptr> action_server(new Server( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index e101386cab..6f9f20ac0d 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -264,19 +264,19 @@ class ServerBase : public rclcpp::Waitable * - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and * - calling user callbacks. */ -template -class Server : public ServerBase, public std::enable_shared_from_this> +template +class Server : public ServerBase, public std::enable_shared_from_this> { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) /// Signature of a callback that accepts or rejects goal requests. using GoalCallback = std::function)>; + const GoalID &, std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. - using CancelCallback = std::function>)>; + using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. - using AcceptedCallback = std::function>)>; + using AcceptedCallback = std::function>)>; /// Construct an action server. /** @@ -319,7 +319,7 @@ class Server : public ServerBase, public std::enable_shared_from_this(), + rosidl_typesupport_cpp::get_action_type_support_handle(), options), handle_goal_(handle_goal), handle_cancel_(handle_cancel), @@ -339,12 +339,12 @@ class Server : public ServerBase, public std::enable_shared_from_this::value, + std::is_same::value, "Assuming user fields were merged with goal request fields"); GoalResponse user_response = handle_goal_( - uuid, std::static_pointer_cast(message)); + uuid, std::static_pointer_cast(message)); - auto ros_response = std::make_shared(); + auto ros_response = std::make_shared(); ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || GoalResponse::ACCEPT_AND_DEFER == user_response; return std::make_pair(user_response, ros_response); @@ -359,7 +359,7 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_handle; auto element = goal_handles_.find(uuid); if (element != goal_handles_.end()) { - std::shared_ptr> goal_handle = element->second.lock(); + std::shared_ptr> goal_handle = element->second.lock(); if (goal_handle) { resp = handle_cancel_(goal_handle); if (CancelResponse::ACCEPT == resp) { @@ -376,13 +376,13 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_goal_handle, GoalID uuid, std::shared_ptr goal_request_message) override { - std::shared_ptr> goal_handle; - std::weak_ptr> weak_this = this->shared_from_this(); + std::shared_ptr> goal_handle; + std::weak_ptr> weak_this = this->shared_from_this(); std::function)> on_terminal_state = [weak_this](const GoalID & uuid, std::shared_ptr result_message) { - std::shared_ptr> shared_this = weak_this.lock(); + std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; } @@ -400,7 +400,7 @@ class Server : public ServerBase, public std::enable_shared_from_this on_executing = [weak_this](const GoalID & uuid) { - std::shared_ptr> shared_this = weak_this.lock(); + std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; } @@ -409,10 +409,10 @@ class Server : public ServerBase, public std::enable_shared_from_thispublish_status(); }; - std::function)> publish_feedback = - [weak_this](std::shared_ptr feedback_msg) + std::function)> publish_feedback = + [weak_this](std::shared_ptr feedback_msg) { - std::shared_ptr> shared_this = weak_this.lock(); + std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; } @@ -420,9 +420,9 @@ class Server : public ServerBase, public std::enable_shared_from_this( + new ServerGoalHandle( rcl_goal_handle, uuid, - std::static_pointer_cast(goal_request_message), + std::static_pointer_cast(goal_request_message), on_terminal_state, on_executing, publish_feedback)); { std::lock_guard lock(goal_handles_mutex_); @@ -435,35 +435,35 @@ class Server : public ServerBase, public std::enable_shared_from_this(message)->uuid; + return static_cast(message)->uuid; } /// \internal std::shared_ptr create_goal_request() override { - return std::shared_ptr(new typename ACTION::GoalRequestService::Request()); + return std::shared_ptr(new typename ActionT::GoalRequestService::Request()); } /// \internal GoalID get_goal_id_from_result_request(void * message) override { - return static_cast(message)->uuid; + return static_cast(message)->uuid; } /// \internal std::shared_ptr create_result_request() override { - return std::shared_ptr(new typename ACTION::GoalResultService::Request()); + return std::shared_ptr(new typename ActionT::GoalResultService::Request()); } /// \internal std::shared_ptr create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override { - auto result = std::make_shared(); + auto result = std::make_shared(); result->status = status; return std::static_pointer_cast(result); } @@ -476,7 +476,7 @@ class Server : public ServerBase, public std::enable_shared_from_this>; + 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_; diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index a6b446fbb2..b39ce2b234 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -111,7 +111,7 @@ class ServerGoalHandleBase }; // Forward declare server -template +template class Server; /// Class to interact with goals on a server. @@ -125,7 +125,7 @@ class Server; * Internally, this class is responsible for coverting between the C++ action type and generic * types for `rclcpp_action::ServerGoalHandleBase`. */ -template +template class ServerGoalHandle : public ServerGoalHandleBase { public: @@ -141,7 +141,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * \param[in] feedback_msg the message to publish to clients. */ void - publish_feedback(std::shared_ptr feedback_msg) + publish_feedback(std::shared_ptr feedback_msg) { feedback_msg->uuid = uuid_; publish_feedback_(feedback_msg); @@ -158,7 +158,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * \param[in] result_msg the final result to send to clients. */ void - set_aborted(typename ACTION::Result::SharedPtr result_msg) + set_aborted(typename ActionT::Result::SharedPtr result_msg) { _set_aborted(); result_msg->status = action_msgs::msg::GoalStatus::STATUS_ABORTED; @@ -175,7 +175,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * \param[in] result_msg the final result to send to clients. */ void - set_succeeded(typename ACTION::Result::SharedPtr result_msg) + set_succeeded(typename ActionT::Result::SharedPtr result_msg) { _set_succeeded(); result_msg->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; @@ -192,7 +192,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * \param[in] result_msg the final result to send to clients. */ void - set_canceled(typename ACTION::Result::SharedPtr result_msg) + set_canceled(typename ActionT::Result::SharedPtr result_msg) { _set_canceled(); result_msg->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; @@ -212,7 +212,7 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// Get the original request message describing the goal. - const std::shared_ptr + const std::shared_ptr get_goal() const { return goal_; @@ -230,10 +230,10 @@ class ServerGoalHandle : public ServerGoalHandleBase ServerGoalHandle( std::shared_ptr rcl_handle, GoalID uuid, - std::shared_ptr goal, + std::shared_ptr goal, std::function)> on_terminal_state, std::function on_executing, - std::function)> publish_feedback + std::function)> publish_feedback ) : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), on_terminal_state_(on_terminal_state), on_executing_(on_executing), @@ -242,16 +242,16 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// The original request message describing the goal. - const std::shared_ptr goal_; + const std::shared_ptr goal_; /// A unique id for the goal request. const GoalID uuid_; - friend Server; + friend Server; std::function)> on_terminal_state_; std::function on_executing_; - std::function)> publish_feedback_; + std::function)> publish_feedback_; }; } // namespace rclcpp_action From ec70b67de9692acd55da8db3175af368514edc54 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:11:12 -0800 Subject: [PATCH 59/71] Cancel abandoned goal handles --- .../rclcpp_action/server_goal_handle.hpp | 18 +++++++- rclcpp_action/src/server_goal_handle.cpp | 42 +++++++++++++++++++ 2 files changed, 58 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index b39ce2b234..ba66cfcb30 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -102,6 +102,12 @@ class ServerGoalHandleBase void _set_executing(); + /// Transition the goal to canceled state if it never reached a terminal state. + /// \internal + RCLCPP_ACTION_PUBLIC + bool + try_canceling() noexcept; + // End API for communication between ServerGoalHandleBase and ServerGoalHandle<> // ----------------------------------------------------------------------------- @@ -129,8 +135,6 @@ template class ServerGoalHandle : public ServerGoalHandleBase { public: - virtual ~ServerGoalHandle() = default; - /// Send an update about the progress of a goal. /** * This must only be called when the goal is executing. @@ -225,6 +229,16 @@ class ServerGoalHandle : public ServerGoalHandleBase return uuid_; } + virtual ~ServerGoalHandle() + { + // Cancel goal if handle was allowed to destruct without reaching a terminal state + if (try_canceling()) { + auto null_result = std::make_shared(); + null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + on_terminal_state_(uuid_, null_result); + } + } + protected: /// \internal ServerGoalHandle( diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index 9308296bbf..14a6279352 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -106,4 +106,46 @@ ServerGoalHandleBase::_set_executing() rclcpp::exceptions::throw_from_rcl_error(ret); } } + +bool +ServerGoalHandleBase::try_canceling() noexcept +{ + std::lock_guard lock(rcl_handle_mutex_); + // Check if the goal reached a terminal state already + const bool active = rcl_action_goal_handle_is_active(rcl_handle_.get()); + if (!active) { + return false; + } + + rcl_ret_t ret; + + // Get the current state + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + return false; + } + + // If it's not already canceling then transition to that state + if (GOAL_STATE_CANCELING != state) { + ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL); + if (RCL_RET_OK != ret) { + return false; + } + } + + // Get the state again + ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + return false; + } + + // If it's canceling, cancel it + if (GOAL_STATE_CANCELING == state) { + ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED); + return RCL_RET_OK == ret; + } + + return false; +} } // namespace rclcpp_action From f717e9b5fcda8d36b3a7fb5d3557682b6bac745e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:32:26 -0800 Subject: [PATCH 60/71] Add convert() for C and C++ goal id --- rclcpp_action/include/rclcpp_action/types.hpp | 10 +++++++ rclcpp_action/src/server.cpp | 28 +++++-------------- rclcpp_action/src/types.cpp | 16 +++++++++++ 3 files changed, 33 insertions(+), 21 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 15d1f1ead3..badfdbdff3 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -37,6 +37,16 @@ using GoalInfo = action_msgs::msg::GoalInfo; RCLCPP_ACTION_PUBLIC std::string to_string(const GoalID & goal_id); + +// Convert C++ GoalID to rcl_action_goal_info_t +RCLCPP_ACTION_PUBLIC +void +convert(const GoalID & goal_id, rcl_action_goal_info_t * info); + +// Convert rcl_action_goal_info_t to C++ GoalID +RCLCPP_ACTION_PUBLIC +void +convert(const rcl_action_goal_info_t & info, GoalID * goal_id); } // namespace rclcpp_action namespace std diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index dde2834a76..20b830b7cc 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -222,9 +222,7 @@ ServerBase::execute_goal_request_received() pimpl_->goal_request_ready_ = false; GoalID uuid = get_goal_id_from_goal_request(message.get()); - for (size_t i = 0; i < 16; ++i) { - goal_info.goal_id.uuid[i] = uuid[i]; - } + convert(uuid, &goal_info); // Call user's callback, getting the user's response and a ros message to send back auto response_pair = call_handle_goal_callback(uuid, message); @@ -313,9 +311,7 @@ ServerBase::execute_cancel_request_received() // Convert c++ message to C message rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); - for (size_t i = 0; i < 16; ++i) { - cancel_request.goal_info.goal_id.uuid[i] = request->goal_info.goal_id.uuid[i]; - } + convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info); cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; @@ -337,9 +333,7 @@ ServerBase::execute_cancel_request_received() for (size_t i = 0; i < goals.size; ++i) { const rcl_action_goal_info_t & goal_info = goals.data[i]; GoalID uuid; - for (size_t i = 0; i < 16; ++i) { - uuid[i] = goal_info.goal_id.uuid[i]; - } + convert(goal_info, &uuid); auto response_code = call_handle_cancel_callback(uuid); if (CancelResponse::ACCEPT == response_code) { rcl_ret_t fail_ret = rcl_action_cancel_response_fini(&cancel_response); @@ -396,9 +390,7 @@ ServerBase::execute_result_request_received() // check if the goal exists GoalID uuid = get_goal_id_from_result_request(result_request.get()); rcl_action_goal_info_t goal_info; - for (size_t i = 0; i < 16; ++i) { - goal_info.goal_id.uuid[i] = uuid[i]; - } + convert(uuid, &goal_info); bool goal_exists; { std::lock_guard lock(pimpl_->server_mutex_); @@ -457,9 +449,7 @@ ServerBase::execute_check_expired_goals() } else if (num_expired) { // A goal expired! GoalID uuid; - for (size_t i = 0; i < 16; ++i) { - uuid[i] = expired_goals[0].goal_id.uuid[i]; - } + convert(expired_goals[0], &uuid); RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_.erase(uuid); @@ -507,9 +497,7 @@ ServerBase::publish_status() action_msgs::msg::GoalStatus msg; msg.status = goal_state; // Convert C goal info to C++ goal info - for (size_t i = 0; i < 16; ++i) { - msg.goal_info.goal_id.uuid[i] = goal_info.goal_id.uuid[i]; - } + convert(goal_info, &msg.goal_info.goal_id.uuid); msg.goal_info.stamp.sec = goal_info.stamp.sec; msg.goal_info.stamp.nanosec = goal_info.stamp.nanosec; @@ -531,9 +519,7 @@ ServerBase::publish_result(const GoalID & uuid, std::shared_ptr result_msg { // Check that the goal exists rcl_action_goal_info_t goal_info; - for (size_t i = 0; i < 16; ++i) { - goal_info.goal_id.uuid[i] = uuid[i]; - } + convert(uuid, &goal_info); bool goal_exists; { std::lock_guard lock(pimpl_->server_mutex_); diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp index fd6c783297..cc2779bd18 100644 --- a/rclcpp_action/src/types.cpp +++ b/rclcpp_action/src/types.cpp @@ -29,4 +29,20 @@ to_string(const GoalID & goal_id) } return stream.str(); } + +void +convert(const GoalID & goal_id, rcl_action_goal_info_t * info) +{ + for (size_t i = 0; i < 16; ++i) { + info->goal_id.uuid[i] = goal_id[i]; + } +} + +void +convert(const rcl_action_goal_info_t & info, GoalID * goal_id) +{ + for (size_t i = 0; i < 16; ++i) { + (*goal_id)[i] = info.goal_id.uuid[i]; + } +} } // namespace rclcpp_action From 03c9cf0fda35ccf0d74d34dfc546927948f61f58 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:34:21 -0800 Subject: [PATCH 61/71] Remove unused variable --- rclcpp_action/include/rclcpp_action/server.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 6f9f20ac0d..4b81ae7461 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -356,7 +356,6 @@ class Server : public ServerBase, public std::enable_shared_from_this lock(goal_handles_mutex_); CancelResponse resp = CancelResponse::REJECT; - std::shared_ptr rcl_handle; auto element = goal_handles_.find(uuid); if (element != goal_handles_.end()) { std::shared_ptr> goal_handle = element->second.lock(); From ea33c8810128ce955615d05eb1649c58993816b7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:35:31 -0800 Subject: [PATCH 62/71] Constant reference --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index ba66cfcb30..df3c83ef14 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -223,7 +223,7 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// Get the unique identifier of the goal - const GoalID + const GoalID & get_goal_id() const { return uuid_; From 8a21dec3d19ec00c91b5b595da67c6980ec1d712 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:36:57 -0800 Subject: [PATCH 63/71] Move variable declaration down --- rclcpp_action/src/server.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 20b830b7cc..19eac90de3 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -80,8 +80,6 @@ ServerBase::ServerBase( : pimpl_(new ServerBaseImpl( node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) { - rcl_ret_t ret; - auto deleter = [node_base](rcl_action_server_t * ptr) { if (nullptr != ptr) { @@ -100,7 +98,7 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); - ret = rcl_action_server_init( + rcl_ret_t ret = rcl_action_server_init( pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options); if (RCL_RET_OK != ret) { From bf4797a7a6af486f9f425afa055bcc7aeaacaffe Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:38:01 -0800 Subject: [PATCH 64/71] is_ready if goal expired --- rclcpp_action/src/server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 19eac90de3..2243dfb0b5 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -179,7 +179,8 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) return pimpl_->goal_request_ready_ || pimpl_->cancel_request_ready_ || - pimpl_->result_request_ready_; + pimpl_->result_request_ready_ || + pimpl_->goal_expired_; } void From ae676d84ec27615e17cbd2284b7edee711f5eb18 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 16:44:13 -0800 Subject: [PATCH 65/71] map[] default constructs if it doesn't exist --- rclcpp_action/src/server.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 2243dfb0b5..ba048321b1 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -420,12 +420,7 @@ ServerBase::execute_result_request_received() } else { // Store the request so it can be responded to later std::lock_guard lock(pimpl_->results_mutex_); - auto iter = pimpl_->result_requests_.find(uuid); - if (iter == pimpl_->result_requests_.end()) { - pimpl_->result_requests_[uuid] = std::vector{request_header}; - } else { - iter->second.push_back(request_header); - } + pimpl_->result_requests_[uuid].push_back(request_header); } } From 039854d3762e01f1d09e3ae8d6c7d6809e4c5a18 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 17:07:14 -0800 Subject: [PATCH 66/71] Use rcl_action_get_goal_status_array() --- rclcpp_action/src/server.cpp | 31 +++++++++++++++++-------------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index ba048321b1..8f98fb76c7 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -473,27 +474,29 @@ ServerBase::publish_status() auto status_msg = std::make_shared(); status_msg->status_list.reserve(num_goals); // Populate a c++ status message with the goals and their statuses - for (size_t i = 0; i < num_goals; ++i) { - rcl_action_goal_handle_t * goal_handle = goal_handles[i]; - rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); - rcl_action_goal_state_t goal_state; + rcl_action_goal_status_array_t c_status_array = + rcl_action_get_zero_initialized_goal_status_array(); + ret = rcl_action_get_goal_status_array(pimpl_->action_server_.get(), &c_status_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } - ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info); + RCLCPP_SCOPE_EXIT({ + ret = rcl_action_goal_status_array_fini(&c_status_array); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message"); } + }); - ret = rcl_action_goal_handle_get_status(goal_handle, &goal_state); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } + for (size_t i = 0; i < c_status_array.msg.status_list.size; ++i) { + auto & c_status_msg = c_status_array.msg.status_list.data[i]; action_msgs::msg::GoalStatus msg; - msg.status = goal_state; + msg.status = c_status_msg.status; // Convert C goal info to C++ goal info - convert(goal_info, &msg.goal_info.goal_id.uuid); - msg.goal_info.stamp.sec = goal_info.stamp.sec; - msg.goal_info.stamp.nanosec = goal_info.stamp.nanosec; + convert(c_status_msg.goal_info, &msg.goal_info.goal_id.uuid); + msg.goal_info.stamp.sec = c_status_msg.goal_info.stamp.sec; + msg.goal_info.stamp.nanosec = c_status_msg.goal_info.stamp.nanosec; status_msg->status_list.push_back(msg); } From 71c7100707cfe93b6804d6446b860ac8609536c6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 17:10:13 -0800 Subject: [PATCH 67/71] Array -> GoalID --- rclcpp_action/test/test_server.cpp | 53 +++++++++++++++--------------- 1 file changed, 27 insertions(+), 26 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 43953de0b5..3fde3e71ad 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -26,6 +26,7 @@ #include "rclcpp_action/server.hpp" using Fibonacci = test_msgs::action::Fibonacci; +using GoalID = rclcpp_action::GoalID; class TestServer : public ::testing::Test { @@ -36,7 +37,7 @@ class TestServer : public ::testing::Test } std::shared_ptr - send_goal_request(rclcpp::Node::SharedPtr node, std::array uuid) + send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid) { auto client = node->create_client( "fibonacci/_action/send_goal"); @@ -55,7 +56,7 @@ class TestServer : public ::testing::Test } void - send_cancel_request(rclcpp::Node::SharedPtr node, std::array uuid) + send_cancel_request(rclcpp::Node::SharedPtr node, GoalID uuid) { auto cancel_client = node->create_client( "fibonacci/_action/cancel_goal"); @@ -79,7 +80,7 @@ TEST_F(TestServer, construction_and_destruction) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node, "fibonacci", - [](const std::array &, std::shared_ptr) { + [](const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, [](std::shared_ptr) { @@ -92,10 +93,10 @@ TEST_F(TestServer, construction_and_destruction) TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); - std::array received_uuid; + GoalID received_uuid; auto handle_goal = [&received_uuid]( - const std::array & uuid, std::shared_ptr) + const GoalID & uuid, std::shared_ptr) { received_uuid = uuid; return rclcpp_action::GoalResponse::REJECT; @@ -120,7 +121,7 @@ TEST_F(TestServer, handle_goal_called) auto request = std::make_shared(); - const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; request->uuid = uuid; auto future = client->async_send_request(request); @@ -134,10 +135,10 @@ TEST_F(TestServer, handle_goal_called) TEST_F(TestServer, handle_accepted_called) { auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); - const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -169,10 +170,10 @@ TEST_F(TestServer, handle_accepted_called) TEST_F(TestServer, handle_cancel_called) { auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); - const std::array uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -209,10 +210,10 @@ TEST_F(TestServer, handle_cancel_called) TEST_F(TestServer, publish_status_accepted) { auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); - const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -270,10 +271,10 @@ TEST_F(TestServer, publish_status_accepted) TEST_F(TestServer, publish_status_canceling) { auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); - const std::array uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; + const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -325,10 +326,10 @@ TEST_F(TestServer, publish_status_canceling) TEST_F(TestServer, publish_status_canceled) { auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); - const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -382,10 +383,10 @@ TEST_F(TestServer, publish_status_canceled) TEST_F(TestServer, publish_status_succeeded) { auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); - const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -437,10 +438,10 @@ TEST_F(TestServer, publish_status_succeeded) TEST_F(TestServer, publish_status_aborted) { auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); - const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -492,10 +493,10 @@ TEST_F(TestServer, publish_status_aborted) TEST_F(TestServer, publish_feedback) { auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); - const std::array uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; + const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -550,10 +551,10 @@ TEST_F(TestServer, publish_feedback) TEST_F(TestServer, get_result) { auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); - const std::array uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -606,10 +607,10 @@ TEST_F(TestServer, get_result) TEST_F(TestServer, deferred_execution) { auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); - const std::array uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const std::array &, std::shared_ptr) + const GoalID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; }; From b7a3de1ace4fbf8754c30cf3064b493f4ad4cdaa Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 17:38:32 -0800 Subject: [PATCH 68/71] Use reentrant mutex for everything --- rclcpp_action/src/server.cpp | 130 +++++++++++++---------------------- 1 file changed, 47 insertions(+), 83 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 8f98fb76c7..6f12924e44 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -42,7 +42,9 @@ class ServerBaseImpl { } - std::mutex server_mutex_; + // Lock everything except user callbacks + std::recursive_mutex reentrant_mutex_; + std::shared_ptr action_server_; rclcpp::Clock::SharedPtr clock_; @@ -58,7 +60,6 @@ class ServerBaseImpl bool result_request_ready_ = false; bool goal_expired_ = false; - std::mutex results_mutex_; // Results to be kept until the goal expires after reaching a terminal state std::unordered_map> goal_results_; // Requests for results are kept until a result becomes available @@ -156,7 +157,7 @@ ServerBase::get_number_of_ready_guard_conditions() bool ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - std::lock_guard lock(pimpl_->server_mutex_); + std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( wait_set, pimpl_->action_server_.get(), NULL); return RCL_RET_OK == ret; @@ -165,7 +166,7 @@ ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) bool ServerBase::is_ready(rcl_wait_set_t * wait_set) { - std::lock_guard lock(pimpl_->server_mutex_); + std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( wait_set, pimpl_->action_server_.get(), @@ -207,14 +208,13 @@ ServerBase::execute_goal_request_received() rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); rmw_request_id_t request_header; + std::lock_guard lock(pimpl_->reentrant_mutex_); + std::shared_ptr message = create_goal_request(); - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_take_goal_request( - pimpl_->action_server_.get(), - &request_header, - message.get()); - } + ret = rcl_action_take_goal_request( + pimpl_->action_server_.get(), + &request_header, + message.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -227,13 +227,10 @@ ServerBase::execute_goal_request_received() // Call user's callback, getting the user's response and a ros message to send back auto response_pair = call_handle_goal_callback(uuid, message); - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_send_goal_response( - pimpl_->action_server_.get(), - &request_header, - response_pair.second.get()); - } + ret = rcl_action_send_goal_response( + pimpl_->action_server_.get(), + &request_header, + response_pair.second.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -256,10 +253,7 @@ ServerBase::execute_goal_request_received() } }; rcl_action_goal_handle_t * rcl_handle; - { - std::lock_guard lock(pimpl_->server_mutex_); - rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); - } + rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); if (!rcl_handle) { throw std::runtime_error("Failed to accept new goal\n"); } @@ -268,10 +262,7 @@ ServerBase::execute_goal_request_received() // Copy out goal handle since action server storage disappears when it is fini'd *handle = *rcl_handle; - { - std::lock_guard lock(pimpl_->results_mutex_); - pimpl_->goal_handles_[uuid] = handle; - } + pimpl_->goal_handles_[uuid] = handle; if (GoalResponse::ACCEPT_AND_EXECUTE == status) { // Change status to executing @@ -297,13 +288,11 @@ ServerBase::execute_cancel_request_received() // Initialize cancel request auto request = std::make_shared(); - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_take_cancel_request( - pimpl_->action_server_.get(), - &request_header, - request.get()); - } + std::lock_guard lock(pimpl_->reentrant_mutex_); + ret = rcl_action_take_cancel_request( + pimpl_->action_server_.get(), + &request_header, + request.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -318,13 +307,10 @@ ServerBase::execute_cancel_request_received() // Get a list of goal info that should be attempted to be cancelled rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_process_cancel_request( - pimpl_->action_server_.get(), - &cancel_request, - &cancel_response); - } + ret = rcl_action_process_cancel_request( + pimpl_->action_server_.get(), + &cancel_request, + &cancel_response); auto response = std::make_shared(); @@ -359,11 +345,8 @@ ServerBase::execute_cancel_request_received() rclcpp::exceptions::throw_from_rcl_error(ret); } - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_send_cancel_response( - pimpl_->action_server_.get(), &request_header, response.get()); - } + ret = rcl_action_send_cancel_response( + pimpl_->action_server_.get(), &request_header, response.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -376,11 +359,10 @@ ServerBase::execute_result_request_received() // Get the result request message rmw_request_id_t request_header; std::shared_ptr result_request = create_result_request(); - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_take_result_request( - pimpl_->action_server_.get(), &request_header, result_request.get()); - } + std::lock_guard lock(pimpl_->reentrant_mutex_); + ret = rcl_action_take_result_request( + pimpl_->action_server_.get(), &request_header, result_request.get()); + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -392,16 +374,12 @@ ServerBase::execute_result_request_received() rcl_action_goal_info_t goal_info; convert(uuid, &goal_info); bool goal_exists; - { - std::lock_guard lock(pimpl_->server_mutex_); - goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); - } + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); if (!goal_exists) { // Goal does not exists result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); } else { // Goal exists, check if a result is already available - std::lock_guard lock(pimpl_->results_mutex_); auto iter = pimpl_->goal_results_.find(uuid); if (iter != pimpl_->goal_results_.end()) { result_response = iter->second; @@ -410,17 +388,13 @@ ServerBase::execute_result_request_received() if (result_response) { // Send the result now - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_send_result_response( - pimpl_->action_server_.get(), &request_header, result_response.get()); - } + ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_response.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } else { // Store the request so it can be responded to later - std::lock_guard lock(pimpl_->results_mutex_); pimpl_->result_requests_[uuid].push_back(request_header); } } @@ -434,11 +408,9 @@ ServerBase::execute_check_expired_goals() // Loop in case more than 1 goal expired while (num_expired > 0u) { + std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret; - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired); - } + ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } else if (num_expired) { @@ -446,7 +418,6 @@ ServerBase::execute_check_expired_goals() GoalID uuid; convert(expired_goals[0], &uuid); RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); - std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_.erase(uuid); pimpl_->result_requests_.erase(uuid); pimpl_->goal_handles_.erase(uuid); @@ -462,11 +433,9 @@ ServerBase::publish_status() // Get all goal handles known to C action server rcl_action_goal_handle_t ** goal_handles = NULL; size_t num_goals = 0; - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_server_get_goal_handles( - pimpl_->action_server_.get(), &goal_handles, &num_goals); - } + ret = rcl_action_server_get_goal_handles( + pimpl_->action_server_.get(), &goal_handles, &num_goals); + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -502,10 +471,8 @@ ServerBase::publish_status() } // Publish the message through the status publisher - { - std::lock_guard lock(pimpl_->server_mutex_); - ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); - } + ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -517,23 +484,20 @@ ServerBase::publish_result(const GoalID & uuid, std::shared_ptr result_msg // Check that the goal exists rcl_action_goal_info_t goal_info; convert(uuid, &goal_info); + std::lock_guard lock(pimpl_->reentrant_mutex_); bool goal_exists; - { - std::lock_guard lock(pimpl_->server_mutex_); - goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); - } + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + if (!goal_exists) { throw std::runtime_error("Asked to publish result for goal that does not exist"); } - std::lock_guard lock(pimpl_->results_mutex_); pimpl_->goal_results_[uuid] = result_msg; // if there are clients who already asked for the result, send it to them auto iter = pimpl_->result_requests_.find(uuid); if (iter != pimpl_->result_requests_.end()) { for (auto & request_header : iter->second) { - std::lock_guard lock(pimpl_->server_mutex_); rcl_ret_t ret = rcl_action_send_result_response( pimpl_->action_server_.get(), &request_header, result_msg.get()); if (RCL_RET_OK != ret) { @@ -546,7 +510,7 @@ ServerBase::publish_result(const GoalID & uuid, std::shared_ptr result_msg void ServerBase::notify_goal_terminal_state() { - std::lock_guard lock(pimpl_->server_mutex_); + std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -556,7 +520,7 @@ ServerBase::notify_goal_terminal_state() void ServerBase::publish_feedback(std::shared_ptr feedback_msg) { - std::lock_guard lock(pimpl_->server_mutex_); + std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); From de1510b2d36b0337e2042b73837c3c7b966ca3f4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 17:40:30 -0800 Subject: [PATCH 69/71] comment --- rclcpp_action/include/rclcpp_action/server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 4b81ae7461..68479c5414 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -286,7 +286,7 @@ class Server : public ServerBase, public std::enable_shared_from_this`. * From 1c8c86d88da333c21b0de6fc35118775fa6970fa Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 17:44:23 -0800 Subject: [PATCH 70/71] scope exit to fini cancel response --- rclcpp_action/src/server.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 6f12924e44..0181a1e1be 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -312,6 +312,13 @@ ServerBase::execute_cancel_request_received() &cancel_request, &cancel_response); + RCLCPP_SCOPE_EXIT({ + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response"); + } + }); + auto response = std::make_shared(); auto & goals = cancel_response.msg.goals_canceling; @@ -322,10 +329,6 @@ ServerBase::execute_cancel_request_received() convert(goal_info, &uuid); auto response_code = call_handle_cancel_callback(uuid); if (CancelResponse::ACCEPT == response_code) { - rcl_ret_t fail_ret = rcl_action_cancel_response_fini(&cancel_response); - if (RCL_RET_OK != fail_ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } action_msgs::msg::GoalInfo cpp_info; cpp_info.goal_id.uuid = uuid; cpp_info.stamp.sec = goal_info.stamp.sec; @@ -339,12 +342,6 @@ ServerBase::execute_cancel_request_received() publish_status(); } - // TODO(sloretz) make this fini happen in an exception safe way - ret = rcl_action_cancel_response_fini(&cancel_response); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - ret = rcl_action_send_cancel_response( pimpl_->action_server_.get(), &request_header, response.get()); if (RCL_RET_OK != ret) { From 9ec3f39d27147e142b54392d68af9398e73ef005 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 18:35:43 -0800 Subject: [PATCH 71/71] using GoalID --- rclcpp_action/src/server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 0181a1e1be..d06d627685 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -28,6 +28,7 @@ #include using rclcpp_action::ServerBase; +using rclcpp_action::GoalID; namespace rclcpp_action {