rclcpp_action Server implementation (#593)
* Commiting to back up work, does not function * Can call user callback when goal request received * fini action server in destructor * rename user callback virtual functions * handle_execute test passes * Remove out of date comment * Refactor execute into three functions * Remove unused file * Add failing cancel test * Cancel test passes * Remove out of date comments * Make sure server publishes status when accepting a goal * Send status when goals transition to cancelling * Refactored sending goal request to its own function * Refactor cancel request into it's own function * Comment with remaining tests * Executing and terminal state statuses * publish feedback works * server sends result to clients that request it * Remove out of date comment * Add ServerGoalHandle::is_active() * Cleanup when goals expire * Can pass in action server options * cpplint and uncrustify fixes * Fix clang warnings * Copy rcl goal handle * Fix clang warning * Use intermediate value to avoid left shift on 32bit integer * RCLCPP_ACTION_PUBLIC everwhere * Change callback parameter from C type to C++ * Add accessors for request and uuid * Feedback must include goal id * Document Server<> and ServerBase<> * handle_execute -> handle_accepted * Test deferred execution * only publish feedback if goal is executing * Documentation for ServerGoalHandle * document msg parameters * remove unnecessary fini * notify_goal_done only takes server * Use unique_indentifier_msgs * create_server accepts group and removes waitable * uncrustify * Use weak ptr to avoid crash if goal handle lives longer than server * Handle goal callback const message * Goal handle doesn't have server pointer anymore * Lock goal_handles_ on Server<> * rcl_action_server_t protected with mutex * ServerBase results protected with mutex * protect rcl goal handle with mutex * is_cancel_request -> is_canceling * Add missing include * use GoalID and change uuid -> goal_id * Keep rcl goal handle alive until it expires on server * uncrustify * Move UUID hash * Log messages in server * ACTION -> ActionT * Cancel abandoned goal handles * Add convert() for C and C++ goal id * Remove unused variable * Constant reference * Move variable declaration down * is_ready if goal expired * map[] default constructs if it doesn't exist * Use rcl_action_get_goal_status_array() * Array -> GoalID * Use reentrant mutex for everything * comment * scope exit to fini cancel response * using GoalID
This commit is contained in:
Shane Loretz
GitHub
parent
91167393ea
commit
fe09d937b7
10 changed files with 2017 additions and 137 deletions
|
|
@ -15,7 +15,13 @@
|
|||
#ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_
|
||||
#define RCLCPP_ACTION__CREATE_SERVER_HPP_
|
||||
|
||||
#include <rcl_action/action_server.h>
|
||||
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base_interface.hpp>
|
||||
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
|
||||
#include <rclcpp/node_interfaces/node_logging_interface.hpp>
|
||||
#include <rclcpp/node_interfaces/node_waitables_interface.hpp>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
|
@ -25,19 +31,59 @@
|
|||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
template<typename ACTION>
|
||||
typename Server<ACTION>::SharedPtr
|
||||
template<typename ActionT>
|
||||
typename Server<ActionT>::SharedPtr
|
||||
create_server(
|
||||
rclcpp::Node * node,
|
||||
rclcpp::Node::SharedPtr node,
|
||||
const std::string & name,
|
||||
typename Server<ACTION>::GoalCallback handle_goal,
|
||||
typename Server<ACTION>::CancelCallback handle_cancel)
|
||||
typename Server<ActionT>::GoalCallback handle_goal,
|
||||
typename Server<ActionT>::CancelCallback handle_cancel,
|
||||
typename Server<ActionT>::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<ACTION>::make_shared(
|
||||
node->get_node_base_interface(),
|
||||
name,
|
||||
handle_goal,
|
||||
handle_cancel);
|
||||
std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
|
||||
node->get_node_waitables_interface();
|
||||
std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
|
||||
bool group_is_null = (nullptr == group.get());
|
||||
|
||||
auto deleter = [weak_node, weak_group, group_is_null](Server<ActionT> * 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<Server<ActionT>> fake_shared_ptr(ptr, [](Server<ActionT> *) {});
|
||||
|
||||
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<Server<ActionT>> action_server(new Server<ActionT>(
|
||||
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_
|
||||
|
|
|
|||
|
|
@ -15,81 +15,471 @@
|
|||
#ifndef RCLCPP_ACTION__SERVER_HPP_
|
||||
#define RCLCPP_ACTION__SERVER_HPP_
|
||||
|
||||
#include <rcl_action/action_server.h>
|
||||
#include <rosidl_generator_c/action_type_support_struct.h>
|
||||
#include <rosidl_typesupport_cpp/action_type_support.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base_interface.hpp>
|
||||
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
|
||||
#include <rclcpp/node_interfaces/node_logging_interface.hpp>
|
||||
#include <rclcpp/waitable.hpp>
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
#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
|
||||
ServerBase(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
const std::string & name,
|
||||
const rosidl_action_type_support_t * type_support);
|
||||
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual ~ServerBase();
|
||||
|
||||
// TODO(sloretz) add a link between this class and callbacks in the server class
|
||||
// -------------
|
||||
// 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 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
|
||||
std::pair<GoalResponse, std::shared_ptr<void>>
|
||||
call_handle_goal_callback(GoalID &, std::shared_ptr<void> 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<void>
|
||||
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_action_goal_handle_t> rcl_goal_handle,
|
||||
GoalID uuid, std::shared_ptr<void> 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<void>
|
||||
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<void>
|
||||
create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
void
|
||||
publish_status();
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
void
|
||||
notify_goal_terminal_state();
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
void
|
||||
publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg);
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
void
|
||||
publish_feedback(std::shared_ptr<void> 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<ServerBaseImpl> 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<typename ACTION>
|
||||
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<typename ActionT>
|
||||
class Server : public ServerBase, public std::enable_shared_from_this<Server<ActionT>>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server)
|
||||
|
||||
using GoalCallback = std::function<void (rcl_action_goal_info_t &, typename ACTION::Goal *)>;
|
||||
using CancelCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ACTION>>)>;
|
||||
/// Signature of a callback that accepts or rejects goal requests.
|
||||
using GoalCallback = std::function<GoalResponse(
|
||||
const GoalID &, std::shared_ptr<const typename ActionT::Goal>)>;
|
||||
/// Signature of a callback that accepts or rejects requests to cancel a goal.
|
||||
using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
|
||||
/// Signature of a callback that is used to notify when the goal has been accepted.
|
||||
using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
|
||||
|
||||
// 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<ACTION>()),
|
||||
rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
|
||||
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<GoalResponse, std::shared_ptr<void>>
|
||||
call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override
|
||||
{
|
||||
// TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type
|
||||
static_assert(
|
||||
std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value,
|
||||
"Assuming user fields were merged with goal request fields");
|
||||
GoalResponse user_response = handle_goal_(
|
||||
uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
|
||||
|
||||
auto ros_response = std::make_shared<typename ActionT::GoalRequestService::Response>();
|
||||
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<std::mutex> lock(goal_handles_mutex_);
|
||||
CancelResponse resp = CancelResponse::REJECT;
|
||||
auto element = goal_handles_.find(uuid);
|
||||
if (element != goal_handles_.end()) {
|
||||
std::shared_ptr<ServerGoalHandle<ActionT>> 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_action_goal_handle_t> rcl_goal_handle,
|
||||
GoalID uuid, std::shared_ptr<void> goal_request_message) override
|
||||
{
|
||||
std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
|
||||
std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
|
||||
|
||||
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state =
|
||||
[weak_this](const GoalID & uuid, std::shared_ptr<void> result_message)
|
||||
{
|
||||
std::shared_ptr<Server<ActionT>> 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<std::mutex> lock(shared_this->goal_handles_mutex_);
|
||||
shared_this->goal_handles_.erase(uuid);
|
||||
};
|
||||
|
||||
std::function<void(const GoalID &)> on_executing =
|
||||
[weak_this](const GoalID & uuid)
|
||||
{
|
||||
std::shared_ptr<Server<ActionT>> 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<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback =
|
||||
[weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg)
|
||||
{
|
||||
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
|
||||
if (!shared_this) {
|
||||
return;
|
||||
}
|
||||
shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
|
||||
};
|
||||
|
||||
goal_handle.reset(
|
||||
new ServerGoalHandle<ActionT>(
|
||||
rcl_goal_handle, uuid,
|
||||
std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
|
||||
on_terminal_state, on_executing, publish_feedback));
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<typename ActionT::GoalRequestService::Request *>(message)->uuid;
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_goal_request() override
|
||||
{
|
||||
return std::shared_ptr<void>(new typename ActionT::GoalRequestService::Request());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
GoalID
|
||||
get_goal_id_from_result_request(void * message) override
|
||||
{
|
||||
return static_cast<typename ActionT::GoalResultService::Request *>(message)->uuid;
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_result_request() override
|
||||
{
|
||||
return std::shared_ptr<void>(new typename ActionT::GoalResultService::Request());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
|
||||
{
|
||||
auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
|
||||
result->status = status;
|
||||
return std::static_pointer_cast<void>(result);
|
||||
}
|
||||
|
||||
// End API for communication between ServerBase and Server<>
|
||||
// ---------------------------------------------------------
|
||||
|
||||
private:
|
||||
GoalCallback handle_goal_;
|
||||
CancelCallback handle_cancel_;
|
||||
AcceptedCallback handle_accepted_;
|
||||
|
||||
using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
|
||||
/// A map of goal id to goal handle weak pointers.
|
||||
/// This is used to provide a goal handle to handle_cancel.
|
||||
std::unordered_map<GoalID, GoalHandleWeakPtr> goal_handles_;
|
||||
std::mutex goal_handles_mutex_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
#endif // RCLCPP_ACTION__SERVER_HPP_
|
||||
|
|
|
|||
|
|
@ -15,41 +15,258 @@
|
|||
#ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
|
||||
#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
|
||||
|
||||
#include <rcl_action/types.h>
|
||||
#include <rcl_action/goal_handle.h>
|
||||
|
||||
#include <action_msgs/msg/goal_status.hpp>
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
#include "rclcpp_action/visibility_control.hpp"
|
||||
#include "rclcpp_action/types.hpp"
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
template<typename ACTION>
|
||||
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;
|
||||
|
||||
/// Send an update about the progress of a goal.
|
||||
virtual void
|
||||
publish_feedback(const typename ACTION::Feedback * feedback_msg) = 0;
|
||||
/// Indicate if goal is pending or executing.
|
||||
/// \return false if goal has reached a terminal state.
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
bool
|
||||
is_active() const;
|
||||
|
||||
// TODO(sloretz) `set_cancelled`, `set_succeeded`, `set_aborted`
|
||||
/// Indicate if goal is executing.
|
||||
/// \return true only if the goal is in an executing state.
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
bool
|
||||
is_executing() const;
|
||||
|
||||
// TODO(sloretz) examples has this attribute as 'goal'
|
||||
/// The original request message describing the goal.
|
||||
const typename ACTION::Goal goal_;
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
~ServerGoalHandleBase();
|
||||
|
||||
protected:
|
||||
explicit ServerGoalHandle(const typename ACTION::Goal goal)
|
||||
: goal_(goal) {}
|
||||
// -------------------------------------------------------------------------
|
||||
// API for communication between ServerGoalHandleBase and ServerGoalHandle<>
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
ServerGoalHandleBase(
|
||||
std::shared_ptr<rcl_action_goal_handle_t> 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_action_goal_handle_t> rcl_handle_;
|
||||
mutable std::mutex rcl_handle_mutex_;
|
||||
};
|
||||
|
||||
// Forward declare server
|
||||
template<typename ActionT>
|
||||
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<typename ActionT>
|
||||
class ServerGoalHandle : public ServerGoalHandleBase
|
||||
{
|
||||
public:
|
||||
/// 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.
|
||||
*
|
||||
* \param[in] feedback_msg the message to publish to clients.
|
||||
*/
|
||||
void
|
||||
publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
|
||||
{
|
||||
feedback_msg->uuid = uuid_;
|
||||
publish_feedback_(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.
|
||||
*
|
||||
* \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);
|
||||
}
|
||||
|
||||
/// 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<const typename ActionT::Goal>
|
||||
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<typename ActionT::Result>();
|
||||
null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
|
||||
on_terminal_state_(uuid_, null_result);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
/// \internal
|
||||
ServerGoalHandle(
|
||||
std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
|
||||
GoalID uuid,
|
||||
std::shared_ptr<const typename ActionT::Goal> goal,
|
||||
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state,
|
||||
std::function<void(const GoalID &)> on_executing,
|
||||
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> 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<const typename ActionT::Goal> goal_;
|
||||
|
||||
/// A unique id for the goal request.
|
||||
const GoalID uuid_;
|
||||
|
||||
friend Server<ActionT>;
|
||||
|
||||
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state_;
|
||||
std::function<void(const GoalID &)> on_executing_;
|
||||
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
||||
#include <rclcpp_action/server_goal_handle_impl.hpp> // NOLINT(build/include_order)
|
||||
#endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
|
||||
|
|
|
|||
|
|
@ -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 <rcl_action/goal_handle.h>
|
||||
#include <rcl_action/types.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
template<typename ACTION>
|
||||
class Server;
|
||||
|
||||
template<typename ACTION>
|
||||
class ServerGoalHandleImpl : public ServerGoalHandle<ACTION>
|
||||
{
|
||||
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<ACTION>(goal)
|
||||
{
|
||||
}
|
||||
|
||||
private:
|
||||
friend Server<ACTION>;
|
||||
std::shared_ptr<rcl_action_server_t> rcl_server_;
|
||||
std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
||||
#endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_
|
||||
|
|
@ -20,8 +20,11 @@
|
|||
#include <action_msgs/msg/goal_status.hpp>
|
||||
#include <action_msgs/msg/goal_info.hpp>
|
||||
|
||||
#include <climits>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp_action/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
|
|
@ -30,6 +33,20 @@ using GoalID = std::array<uint8_t, UUID_SIZE>;
|
|||
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<rclcpp_action::GoalID>
|
|||
return lhs < rhs;
|
||||
}
|
||||
};
|
||||
|
||||
/// Hash a goal id so it can be used as a key in std::unordered_map
|
||||
template<>
|
||||
struct hash<rclcpp_action::GoalID>
|
||||
{
|
||||
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_
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue