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 50ad292516..ecd557ca48 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -15,7 +15,13 @@ #ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_ #define RCLCPP_ACTION__CREATE_SERVER_HPP_ +#include + #include +#include +#include +#include +#include #include #include @@ -25,19 +31,59 @@ namespace rclcpp_action { -template -typename Server::SharedPtr +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::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) { - return Server::make_shared( - node->get_node_base_interface(), - name, - handle_goal, - handle_cancel); + 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(), + node->get_node_logging_interface(), + name, + options, + handle_goal, + handle_cancel, + handle_accepted), deleter); + + node->get_node_waitables_interface()->add_waitable(action_server, group); + 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..68479c5414 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -15,81 +15,471 @@ #ifndef RCLCPP_ACTION__SERVER_HPP_ #define RCLCPP_ACTION__SERVER_HPP_ +#include #include #include #include +#include +#include +#include #include #include +#include #include +#include +#include #include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_action/types.hpp" 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 +{ + /// The goal is rejected and will not be executed. + REJECT = 1, + /// 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 +{ + /// The server will not try to cancel the goal. + REJECT = 1, + /// The server has agreed to try to cancel the goal. + ACCEPT = 2, +}; + /// Base Action Server implementation -/// It is responsible for interfacing with the C action server API. -class ServerBase +/// \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: - // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node - // TODO(sloretz) accept clock instance + RCLCPP_ACTION_PUBLIC + virtual ~ServerBase(); + + // ------------- + // 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; + + // End Waitables API + // ----------------- + +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 rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options); + + // ----------------------------------------------------- + // 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 ~ServerBase(); + virtual + std::pair> + 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. + // The subclass should look up a goal handle and call the user's callback. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + CancelResponse + call_handle_cancel_callback(const GoalID & uuid) = 0; + + /// Given a goal request message, return the UUID contained within. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + 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. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_goal_request() = 0; + + /// Call user callback to inform them a goal has been accepted. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + call_goal_accepted_callback( + std::shared_ptr rcl_goal_handle, + GoalID uuid, std::shared_ptr goal_request_message) = 0; + + /// Given a result request message, return the UUID contained within. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + 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. + /// \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(); - // TODO(sloretz) add a link between this class and callbacks in the server class + /// \internal + RCLCPP_ACTION_PUBLIC + void + notify_goal_terminal_state(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_result(const GoalID & 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<> + // --------------------------------------------------------- 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_; }; - -/// 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. -template -class Server : public ServerBase +/// 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 std::enable_shared_from_this> { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) - using GoalCallback = std::function; - using CancelCallback = std::function>)>; + /// Signature of a callback that accepts or rejects goal requests. + using GoalCallback = std::function)>; + /// 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. + using AcceptedCallback = std::function>)>; - // TODO(sloretz) accept clock instance + /// 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 receive 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<>`. + * + * \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] node_logging a pointer to an interface that allows getting a node's logger. + * \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_accepted a callback that is called to give the user a handle to the goal. + * execution. + */ Server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & name, + const rcl_action_server_options_t & options, GoalCallback handle_goal, - CancelCallback handle_cancel + CancelCallback handle_cancel, + AcceptedCallback handle_accepted ) : ServerBase( node_base, + node_clock, + node_logging, 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_cancel_(handle_cancel), + handle_accepted_(handle_accepted) { - // TODO(sloretz) what's the link that causes `handle_goal_` and `handle_cancel_` to be called? } - virtual ~Server() + virtual ~Server() = default; + +protected: + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + + /// \internal + std::pair> + 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( + std::is_same::value, + "Assuming user fields were merged with goal request fields"); + GoalResponse user_response = handle_goal_( + uuid, std::static_pointer_cast(message)); + + 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); } + /// \internal + CancelResponse + call_handle_cancel_callback(const GoalID & uuid) override + { + std::lock_guard lock(goal_handles_mutex_); + CancelResponse resp = CancelResponse::REJECT; + 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); + if (CancelResponse::ACCEPT == resp) { + goal_handle->_set_canceling(); + } + } + } + return resp; + } + + /// \internal + void + call_goal_accepted_callback( + std::shared_ptr 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::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) { + return; + } + // Send result message to anyone that asked + shared_this->publish_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) + std::lock_guard lock(shared_this->goal_handles_mutex_); + shared_this->goal_handles_.erase(uuid); + }; + + std::function on_executing = + [weak_this](const GoalID & 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 + shared_this->publish_status(); + }; + + std::function)> publish_feedback = + [weak_this](std::shared_ptr feedback_msg) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + shared_this->publish_feedback(std::static_pointer_cast(feedback_msg)); + }; + + goal_handle.reset( + new ServerGoalHandle( + rcl_goal_handle, uuid, + std::static_pointer_cast(goal_request_message), + on_terminal_state, on_executing, publish_feedback)); + { + std::lock_guard lock(goal_handles_mutex_); + goal_handles_[uuid] = goal_handle; + } + handle_accepted_(goal_handle); + } + + /// \internal + GoalID + 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 ActionT::GoalRequestService::Request()); + } + + /// \internal + GoalID + 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 ActionT::GoalResultService::Request()); + } + + /// \internal + 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); + } + + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + private: GoalCallback handle_goal_; CancelCallback handle_cancel_; + AcceptedCallback handle_accepted_; + + using GoalHandleWeakPtr = std::weak_ptr>; + /// A map of goal id to goal handle weak pointers. + /// This is used to provide a goal handle to handle_cancel. + std::unordered_map goal_handles_; + std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action #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..df3c83ef14 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -15,41 +15,258 @@ #ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ +#include #include +#include + #include #include +#include #include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/types.hpp" namespace rclcpp_action { -template -class ServerGoalHandle + +/// Base class to interact with goals on a server. +/// \internal +/** + * + * 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. + */ +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; + RCLCPP_ACTION_PUBLIC + bool + is_canceling() const; + + /// 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(); + +protected: + // ------------------------------------------------------------------------- + // API for communication between ServerGoalHandleBase and ServerGoalHandle<> + + /// \internal + RCLCPP_ACTION_PUBLIC + ServerGoalHandleBase( + std::shared_ptr rcl_handle + ) + : rcl_handle_(rcl_handle) + { + } + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _set_aborted(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _set_succeeded(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _set_canceling(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _set_canceled(); + + /// \internal + RCLCPP_ACTION_PUBLIC + 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<> + // ----------------------------------------------------------------------------- +private: + std::shared_ptr rcl_handle_; + mutable std::mutex rcl_handle_mutex_; +}; + +// 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 +{ +public: /// Send an update about the progress of a goal. - virtual void - publish_feedback(const typename ACTION::Feedback * feedback_msg) = 0; + /** + * 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. + * + * \param[in] feedback_msg the message to publish to clients. + */ + void + publish_feedback(std::shared_ptr feedback_msg) + { + feedback_msg->uuid = uuid_; + publish_feedback_(feedback_msg); + } - // TODO(sloretz) `set_cancelled`, `set_succeeded`, `set_aborted` + // 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. + * + * \param[in] result_msg the final result to send to clients. + */ + void + set_aborted(typename ActionT::Result::SharedPtr result_msg) + { + _set_aborted(); + result_msg->status = action_msgs::msg::GoalStatus::STATUS_ABORTED; + on_terminal_state_(uuid_, result_msg); + } - // TODO(sloretz) examples has this attribute as 'goal' - /// The original request message describing the goal. - const typename ACTION::Goal goal_; + /// 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. + * + * \param[in] result_msg the final result to send to clients. + */ + void + set_succeeded(typename ActionT::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. + /** + * 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. + * + * \param[in] result_msg the final result to send to clients. + */ + void + set_canceled(typename ActionT::Result::SharedPtr result_msg) + { + _set_canceled(); + result_msg->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + 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() + { + _set_executing(); + on_executing_(uuid_); + } + + /// Get the original request message describing the goal. + const std::shared_ptr + get_goal() const + { + return goal_; + } + + /// Get the unique identifier of the goal + const GoalID & + get_goal_id() const + { + 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: - explicit ServerGoalHandle(const typename ACTION::Goal goal) - : goal_(goal) {} + /// \internal + ServerGoalHandle( + std::shared_ptr rcl_handle, + GoalID uuid, + std::shared_ptr goal, + std::function)> on_terminal_state, + std::function on_executing, + std::function)> publish_feedback + ) + : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), + on_terminal_state_(on_terminal_state), on_executing_(on_executing), + publish_feedback_(publish_feedback) + { + } + + /// The original request message describing the goal. + const std::shared_ptr goal_; + + /// A unique id for the goal request. + const GoalID uuid_; + + friend Server; + + std::function)> on_terminal_state_; + std::function on_executing_; + std::function)> publish_feedback_; }; } // namespace rclcpp_action -#include // NOLINT(build/include_order) #endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_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_ diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 3ab9f4ce19..badfdbdff3 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -20,8 +20,11 @@ #include #include +#include #include +#include +#include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action { @@ -30,6 +33,20 @@ 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); + +// 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 @@ -44,5 +61,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 < uuid.size(); ++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 bac9d21996..d06d627685 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -12,30 +12,515 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include +#include +#include +#include #include +#include +#include #include +#include +#include using rclcpp_action::ServerBase; +using rclcpp_action::GoalID; namespace rclcpp_action { class ServerBaseImpl { +public: + ServerBaseImpl( + rclcpp::Clock::SharedPtr clock, + rclcpp::Logger logger + ) + : clock_(clock), logger_(logger) + { + } + + // Lock everything except user callbacks + std::recursive_mutex reentrant_mutex_; + + std::shared_ptr action_server_; + + 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; + bool goal_expired_ = false; + + // 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 + 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 rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options +) +: pimpl_(new ServerBaseImpl( + node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) { - // TODO(sloretz) use rcl_action API when available - (void)node_base; - (void)name; - (void)type_support; + 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); + (void)ret; + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_server_t in deleter"); + } + delete ptr; + }; + + pimpl_->action_server_.reset(new rcl_action_server_t, deleter); + *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server(); + + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); + + 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) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + ret = rcl_action_server_wait_set_get_num_entities( + pimpl_->action_server_.get(), + &pimpl_->num_subscriptions_, + &pimpl_->num_guard_conditions_, + &pimpl_->num_timers_, + &pimpl_->num_clients_, + &pimpl_->num_services_); + + if (RCL_RET_OK != ret) { + 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) +{ + 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; +} + +bool +ServerBase::is_ready(rcl_wait_set_t * wait_set) +{ + 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(), + &pimpl_->goal_request_ready_, + &pimpl_->cancel_request_ready_, + &pimpl_->result_request_ready_, + &pimpl_->goal_expired_); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + return pimpl_->goal_request_ready_ || + pimpl_->cancel_request_ready_ || + pimpl_->result_request_ready_ || + pimpl_->goal_expired_; +} + +void +ServerBase::execute() +{ + if (pimpl_->goal_request_ready_) { + 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 if (pimpl_->goal_expired_) { + execute_check_expired_goals(); + } else { + throw std::runtime_error("Executing action server but nothing is ready"); + } +} + +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; + + std::lock_guard lock(pimpl_->reentrant_mutex_); + + std::shared_ptr message = create_goal_request(); + 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); + } + + pimpl_->goal_request_ready_ = false; + GoalID uuid = get_goal_id_from_goal_request(message.get()); + 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); + + 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); + } + + const auto status = response_pair.first; + + // 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; + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_goal_handle_t in deleter"); + delete ptr; + } + }; + rcl_action_goal_handle_t * rcl_handle; + 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; + + 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); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + // publish status since a goal's state has changed (was accepted or has begun execution) + publish_status(); + + // Tell user to start executing action + call_goal_accepted_callback(handle, uuid, message); + } +} + +void +ServerBase::execute_cancel_request_received() +{ + rcl_ret_t ret; + rmw_request_id_t request_header; + + // Initialize cancel request + auto request = std::make_shared(); + + 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); + } + + // Convert c++ message to C message + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + 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; + + // 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); + + 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; + // 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]; + GoalID uuid; + convert(goal_info, &uuid); + auto response_code = call_handle_cancel_callback(uuid); + if (CancelResponse::ACCEPT == response_code) { + 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); + } + } + + if (!response->goals_canceling.empty()) { + // at least one goal state changed, publish a new status message + publish_status(); + } + + 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 +ServerBase::execute_result_request_received() +{ + rcl_ret_t ret; + // Get the result request message + rmw_request_id_t request_header; + std::shared_ptr result_request = create_result_request(); + 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); + } + + std::shared_ptr result_response; + + // check if the goal exists + GoalID uuid = get_goal_id_from_result_request(result_request.get()); + rcl_action_goal_info_t goal_info; + convert(uuid, &goal_info); + bool goal_exists; + 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 + 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 + pimpl_->result_requests_[uuid].push_back(request_header); + } +} + +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) { + std::lock_guard lock(pimpl_->reentrant_mutex_); + rcl_ret_t ret; + 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! + GoalID uuid; + convert(expired_goals[0], &uuid); + RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); + pimpl_->goal_results_.erase(uuid); + pimpl_->result_requests_.erase(uuid); + pimpl_->goal_handles_.erase(uuid); + } + } +} + +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 + 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); + } + + RCLCPP_SCOPE_EXIT({ + ret = rcl_action_goal_status_array_fini(&c_status_array); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message"); + } + }); + + 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 = c_status_msg.status; + // Convert C goal info to C++ goal info + 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); + } + + // 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); + } +} + +void +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; + 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"); + } + + 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); + } + } + } +} + +void +ServerBase::notify_goal_terminal_state() +{ + 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); + } +} + +void +ServerBase::publish_feedback(std::shared_ptr feedback_msg) +{ + 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"); + } +} diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index eae7f88981..14a6279352 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -12,4 +12,140 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include +#include + +#include + +namespace rclcpp_action +{ +ServerGoalHandleBase::~ServerGoalHandleBase() +{ +} + +bool +ServerGoalHandleBase::is_canceling() 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) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_CANCELING == state; +} + +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) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_EXECUTING == state; +} + +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); + } +} + +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); + } +} + +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); + } +} + +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 diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp new file mode 100644 index 0000000000..cc2779bd18 --- /dev/null +++ b/rclcpp_action/src/types.cpp @@ -0,0 +1,48 @@ +// 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(); +} + +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 diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index b6dea88077..3fde3e71ad 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -20,10 +20,13 @@ #include #include +#include #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server.hpp" +using Fibonacci = test_msgs::action::Fibonacci; +using GoalID = rclcpp_action::GoalID; class TestServer : public ::testing::Test { @@ -32,15 +35,609 @@ class TestServer : public ::testing::Test { rclcpp::init(0, nullptr); } + + std::shared_ptr + send_goal_request(rclcpp::Node::SharedPtr node, GoalID 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; + } + + void + send_cancel_request(rclcpp::Node::SharedPtr node, GoalID 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.goal_id.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) { - 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, "fibonacci", + [](const GoalID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; +} + +TEST_F(TestServer, handle_goal_called) +{ + auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); + GoalID received_uuid; + + auto handle_goal = [&received_uuid]( + const GoalID & uuid, std::shared_ptr) + { + received_uuid = uuid; + return rclcpp_action::GoalResponse::REJECT; + }; - using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server(node.get(), "fibonacci", - [](rcl_action_goal_info_t &, test_msgs::action::Fibonacci::Goal *) {}, + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server(node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, [](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/send_goal"); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + + auto request = std::make_shared(); + + 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); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + ASSERT_EQ(uuid, received_uuid); +} + +TEST_F(TestServer, handle_accepted_called) +{ + auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + handle_accepted); + (void)as; + + auto request = send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + ASSERT_TRUE(received_handle->is_active()); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_EQ(*request, *(received_handle->get_goal())); +} + +TEST_F(TestServer, handle_cancel_called) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + send_cancel_request(node, uuid); + EXPECT_TRUE(received_handle->is_canceling()); +} + +TEST_F(TestServer, publish_status_accepted) +{ + auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + 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, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (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); + + // 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_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.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); + } else { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_EXECUTING, status); + } + } +} + +TEST_F(TestServer, publish_status_canceling) +{ + auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); + const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (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); + + // 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_CANCELING, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_canceled) +{ + auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (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(std::make_shared()); + + // 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.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_succeeded) +{ + auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + 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, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (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(std::make_shared()); + + // 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.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_aborted) +{ + auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + 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, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (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(std::make_shared()); + + // 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_ABORTED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_feedback) +{ + auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); + const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + 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, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to feedback messages + using FeedbackT = 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); + ASSERT_EQ(uuid, msg->uuid); +} + +TEST_F(TestServer, get_result) +{ + auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); + const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + 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, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (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); +} + +TEST_F(TestServer, deferred_execution) +{ + auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); + const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalID &, 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, "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()); }