rclcpp_action  master
C++ ROS Action Client Library
server.hpp
Go to the documentation of this file.
1 // Copyright 2018 Open Source Robotics Foundation, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef RCLCPP_ACTION__SERVER_HPP_
16 #define RCLCPP_ACTION__SERVER_HPP_
17 
18 #include <rcl_action/action_server.h>
19 #include <rosidl_runtime_c/action_type_support_struct.h>
20 #include <rosidl_typesupport_cpp/action_type_support.hpp>
24 #include <rclcpp/waitable.hpp>
25 
26 #include <functional>
27 #include <memory>
28 #include <mutex>
29 #include <string>
30 #include <unordered_map>
31 #include <utility>
32 
35 #include "rclcpp_action/types.hpp"
36 
37 namespace rclcpp_action
38 {
39 // Forward declaration
40 class ServerBaseImpl;
41 
43 enum class GoalResponse : int8_t
44 {
46  REJECT = 1,
50  ACCEPT_AND_DEFER = 3,
51 };
52 
54 enum class CancelResponse : int8_t
55 {
57  REJECT = 1,
59  ACCEPT = 2,
60 };
61 
64 
71 {
72 public:
74  virtual ~ServerBase();
75 
76  // -------------
77  // Waitables API
78 
82  size_t
84 
88  size_t
89  get_number_of_ready_timers() override;
90 
94  size_t
95  get_number_of_ready_clients() override;
96 
100  size_t
101  get_number_of_ready_services() override;
102 
106  size_t
108 
112  bool
113  add_to_wait_set(rcl_wait_set_t * wait_set) override;
114 
118  bool
119  is_ready(rcl_wait_set_t *) override;
120 
124  void
125  execute() override;
126 
127  // End Waitables API
128  // -----------------
129 
130 protected:
132  ServerBase(
133  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
134  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
135  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
136  const std::string & name,
137  const rosidl_action_type_support_t * type_support,
138  const rcl_action_server_options_t & options);
139 
140  // -----------------------------------------------------
141  // API for communication between ServerBase and Server<>
142 
143  // ServerBase will call this function when a goal request is received.
144  // The subclass should convert to the real type and call a user's callback.
147  virtual
150 
151  // ServerBase will determine which goal ids are being cancelled, and then call this function for
152  // each goal id.
153  // The subclass should look up a goal handle and call the user's callback.
156  virtual
158  call_handle_cancel_callback(const GoalUUID & uuid) = 0;
159 
163  virtual
164  GoalUUID
165  get_goal_id_from_goal_request(void * message) = 0;
166 
170  virtual
172  create_goal_request() = 0;
173 
177  virtual
178  void
181  GoalUUID uuid, std::shared_ptr<void> goal_request_message) = 0;
182 
186  virtual
187  GoalUUID
188  get_goal_id_from_result_request(void * message) = 0;
189 
193  virtual
195  create_result_request() = 0;
196 
200  virtual
202  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0;
203 
206  void
207  publish_status();
208 
211  void
213 
216  void
217  publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg);
218 
221  void
223 
224  // End API for communication between ServerBase and Server<>
225  // ---------------------------------------------------------
226 
227 private:
231  void
232  execute_goal_request_received();
233 
237  void
238  execute_cancel_request_received();
239 
243  void
244  execute_result_request_received();
245 
249  void
250  execute_check_expired_goals();
251 
255 };
256 
258 
267 template<typename ActionT>
268 class Server : public ServerBase, public std::enable_shared_from_this<Server<ActionT>>
269 {
270 public:
272 
273 
280 
282 
308  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
309  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
310  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
311  const std::string & name,
312  const rcl_action_server_options_t & options,
313  GoalCallback handle_goal,
314  CancelCallback handle_cancel,
315  AcceptedCallback handle_accepted
316  )
317  : ServerBase(
318  node_base,
319  node_clock,
320  node_logging,
321  name,
322  rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
323  options),
324  handle_goal_(handle_goal),
325  handle_cancel_(handle_cancel),
326  handle_accepted_(handle_accepted)
327  {
328  }
329 
330  virtual ~Server() = default;
331 
332 protected:
333  // -----------------------------------------------------
334  // API for communication between ServerBase and Server<>
335 
339  {
340  auto request = std::static_pointer_cast<
341  typename ActionT::Impl::SendGoalService::Request>(message);
342  auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
343  GoalResponse user_response = handle_goal_(uuid, goal);
344 
345  auto ros_response = std::make_shared<typename ActionT::Impl::SendGoalService::Response>();
346  ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
347  GoalResponse::ACCEPT_AND_DEFER == user_response;
348  return std::make_pair(user_response, ros_response);
349  }
350 
353  call_handle_cancel_callback(const GoalUUID & uuid) override
354  {
355  std::lock_guard<std::mutex> lock(goal_handles_mutex_);
357  auto element = goal_handles_.find(uuid);
358  if (element != goal_handles_.end()) {
359  std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
360  if (goal_handle) {
361  resp = handle_cancel_(goal_handle);
362  if (CancelResponse::ACCEPT == resp) {
363  goal_handle->_cancel_goal();
364  }
365  }
366  }
367  return resp;
368  }
369 
371  void
374  GoalUUID uuid, std::shared_ptr<void> goal_request_message) override
375  {
377  std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
378 
379  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
380  [weak_this](const GoalUUID & uuid, std::shared_ptr<void> result_message)
381  {
382  std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
383  if (!shared_this) {
384  return;
385  }
386  // Send result message to anyone that asked
387  shared_this->publish_result(uuid, result_message);
388  // Publish a status message any time a goal handle changes state
389  shared_this->publish_status();
390  // notify base so it can recalculate the expired goal timer
391  shared_this->notify_goal_terminal_state();
392  // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires)
393  std::lock_guard<std::mutex> lock(shared_this->goal_handles_mutex_);
394  shared_this->goal_handles_.erase(uuid);
395  };
396 
397  std::function<void(const GoalUUID &)> on_executing =
398  [weak_this](const GoalUUID & uuid)
399  {
400  std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
401  if (!shared_this) {
402  return;
403  }
404  (void)uuid;
405  // Publish a status message any time a goal handle changes state
406  shared_this->publish_status();
407  };
408 
411  {
412  std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
413  if (!shared_this) {
414  return;
415  }
416  shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
417  };
418 
419  auto request = std::static_pointer_cast<
420  const typename ActionT::Impl::SendGoalService::Request>(goal_request_message);
421  auto goal = std::shared_ptr<const typename ActionT::Goal>(request, &request->goal);
422  goal_handle.reset(
424  rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback));
425  {
426  std::lock_guard<std::mutex> lock(goal_handles_mutex_);
427  goal_handles_[uuid] = goal_handle;
428  }
429  handle_accepted_(goal_handle);
430  }
431 
433  GoalUUID
434  get_goal_id_from_goal_request(void * message) override
435  {
436  return
437  static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
438  }
439 
443  {
444  return std::shared_ptr<void>(new typename ActionT::Impl::SendGoalService::Request());
445  }
446 
448  GoalUUID
449  get_goal_id_from_result_request(void * message) override
450  {
451  return
452  static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
453  }
454 
458  {
459  return std::shared_ptr<void>(new typename ActionT::Impl::GetResultService::Request());
460  }
461 
464  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
465  {
466  auto result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
467  result->status = status;
468  return std::static_pointer_cast<void>(result);
469  }
470 
471  // End API for communication between ServerBase and Server<>
472  // ---------------------------------------------------------
473 
474 private:
475  GoalCallback handle_goal_;
476  CancelCallback handle_cancel_;
477  AcceptedCallback handle_accepted_;
478 
479  using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
483  std::mutex goal_handles_mutex_;
484 };
485 } // namespace rclcpp_action
486 #endif // RCLCPP_ACTION__SERVER_HPP_
node_logging_interface.hpp
rclcpp_action::ServerBase::publish_feedback
RCLCPP_ACTION_PUBLIC void publish_feedback(std::shared_ptr< void > feedback_msg)
std::weak_ptr::lock
T lock(T... args)
rclcpp_action::ServerBase::create_goal_request
virtual RCLCPP_ACTION_PUBLIC std::shared_ptr< void > create_goal_request()=0
rclcpp_action::GoalResponse
GoalResponse
A response returned by an action server callback when a goal is requested.
Definition: server.hpp:43
std::string
std::shared_ptr< void >
rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE
@ ACCEPT_AND_EXECUTE
The server accepts the goal, and is going to begin execution immediately.
std::pair
rclcpp_action::ServerBase::notify_goal_terminal_state
RCLCPP_ACTION_PUBLIC void notify_goal_terminal_state()
rclcpp_action::ServerBase::get_number_of_ready_timers
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_timers() override
rclcpp_action::ServerBase::call_handle_cancel_callback
virtual RCLCPP_ACTION_PUBLIC CancelResponse call_handle_cancel_callback(const GoalUUID &uuid)=0
node_clock_interface.hpp
types.hpp
rclcpp_action::Server::~Server
virtual ~Server()=default
rclcpp_action::ServerBase::publish_status
RCLCPP_ACTION_PUBLIC void publish_status()
std::lock_guard
rclcpp_action::CancelResponse::REJECT
@ REJECT
The server will not try to cancel the goal.
node_base_interface.hpp
rclcpp_action::Server::create_result_request
std::shared_ptr< void > create_result_request() override
Definition: server.hpp:457
rclcpp_action::ServerBase::call_handle_goal_callback
virtual RCLCPP_ACTION_PUBLIC std::pair< GoalResponse, std::shared_ptr< void > > call_handle_goal_callback(GoalUUID &, std::shared_ptr< void > request)=0
std::function< GoalResponse(const GoalUUID &, std::shared_ptr< const typename ActionT::Goal >)>
rclcpp_action::ServerBase::execute
RCLCPP_ACTION_PUBLIC void execute() override
rclcpp_action::Server::create_goal_request
std::shared_ptr< void > create_goal_request() override
Definition: server.hpp:442
rclcpp_action::ServerBase::is_ready
RCLCPP_ACTION_PUBLIC bool is_ready(rcl_wait_set_t *) override
rclcpp::Waitable
rclcpp_action
Definition: client.hpp:44
rclcpp_action::Server::get_goal_id_from_goal_request
GoalUUID get_goal_id_from_goal_request(void *message) override
Definition: server.hpp:434
std::shared_ptr::reset
T reset(T... args)
rclcpp_action::Server::create_result_response
std::shared_ptr< void > create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
Definition: server.hpp:464
rclcpp_action::ServerBase::create_result_request
virtual RCLCPP_ACTION_PUBLIC std::shared_ptr< void > create_result_request()=0
rclcpp_action::ServerBase::get_goal_id_from_goal_request
virtual RCLCPP_ACTION_PUBLIC GoalUUID get_goal_id_from_goal_request(void *message)=0
rclcpp_action::ServerBase::get_number_of_ready_guard_conditions
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_guard_conditions() override
std::enable_shared_from_this< Server< ActionT > >::shared_from_this
T shared_from_this(T... args)
rclcpp_action::Server::Server
Server(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string &name, const rcl_action_server_options_t &options, GoalCallback handle_goal, CancelCallback handle_cancel, AcceptedCallback handle_accepted)
Construct an action server.
Definition: server.hpp:307
rclcpp_action::GoalResponse::REJECT
@ REJECT
The goal is rejected and will not be executed.
waitable.hpp
rclcpp_action::ServerBase::~ServerBase
virtual RCLCPP_ACTION_PUBLIC ~ServerBase()
std::static_pointer_cast
T static_pointer_cast(T... args)
std::array
std::enable_shared_from_this
rclcpp_action::Server::CancelCallback
std::function< CancelResponse(std::shared_ptr< ServerGoalHandle< ActionT > >)> CancelCallback
Signature of a callback that accepts or rejects requests to cancel a goal.
Definition: server.hpp:277
rcl_wait_set_t
rclcpp_action::ServerBase::call_goal_accepted_callback
virtual RCLCPP_ACTION_PUBLIC void call_goal_accepted_callback(std::shared_ptr< rcl_action_goal_handle_t > rcl_goal_handle, GoalUUID uuid, std::shared_ptr< void > goal_request_message)=0
rclcpp_action::ServerBase::create_result_response
virtual RCLCPP_ACTION_PUBLIC std::shared_ptr< void > create_result_response(decltype(action_msgs::msg::GoalStatus::status) status)=0
std::weak_ptr
rclcpp_action::Server
Action Server.
Definition: server.hpp:268
rclcpp_action::ServerBase::get_number_of_ready_clients
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_clients() override
rclcpp_action::Server::call_handle_cancel_callback
CancelResponse call_handle_cancel_callback(const GoalUUID &uuid) override
Definition: server.hpp:353
rclcpp_action::Server::GoalCallback
std::function< GoalResponse(const GoalUUID &, std::shared_ptr< const typename ActionT::Goal >)> GoalCallback
Signature of a callback that accepts or rejects goal requests.
Definition: server.hpp:275
rclcpp_action::CancelResponse::ACCEPT
@ ACCEPT
The server has agreed to try to cancel the goal.
rclcpp_action::GoalResponse::ACCEPT_AND_DEFER
@ ACCEPT_AND_DEFER
The server accepts the goal, and is going to execute it later.
server_goal_handle.hpp
visibility_control.hpp
rclcpp_action::ServerBase::get_number_of_ready_subscriptions
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_subscriptions() override
rclcpp_action::CancelResponse
CancelResponse
A response returned by an action server callback when a goal has been asked to be canceled.
Definition: server.hpp:54
RCLCPP_ACTION_PUBLIC
#define RCLCPP_ACTION_PUBLIC
Definition: visibility_control.hpp:50
rclcpp_action::ServerBase::get_goal_id_from_result_request
virtual RCLCPP_ACTION_PUBLIC GoalUUID get_goal_id_from_result_request(void *message)=0
rclcpp_action::ServerBase::get_number_of_ready_services
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_services() override
std::mutex
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
std::make_pair
T make_pair(T... args)
rclcpp_action::Server::call_goal_accepted_callback
void call_goal_accepted_callback(std::shared_ptr< rcl_action_goal_handle_t > rcl_goal_handle, GoalUUID uuid, std::shared_ptr< void > goal_request_message) override
Definition: server.hpp:372
rclcpp_action::Server::get_goal_id_from_result_request
GoalUUID get_goal_id_from_result_request(void *message) override
Definition: server.hpp:449
std::unique_ptr< ServerBaseImpl >
rclcpp_action::ServerBase
Definition: server.hpp:70
std::unordered_map
rclcpp_action::Server::call_handle_goal_callback
std::pair< GoalResponse, std::shared_ptr< void > > call_handle_goal_callback(GoalUUID &uuid, std::shared_ptr< void > message) override
Definition: server.hpp:338
rclcpp_action::ServerBase::add_to_wait_set
RCLCPP_ACTION_PUBLIC bool add_to_wait_set(rcl_wait_set_t *wait_set) override
rclcpp_action::ServerBase::ServerBase
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)
rclcpp_action::ServerBase::publish_result
RCLCPP_ACTION_PUBLIC void publish_result(const GoalUUID &uuid, std::shared_ptr< void > result_msg)
rcl_action_server_options_t
rclcpp_action::Server::AcceptedCallback
std::function< void(std::shared_ptr< ServerGoalHandle< ActionT > >)> AcceptedCallback
Signature of a callback that is used to notify when the goal has been accepted.
Definition: server.hpp:279
rclcpp_action::ServerGoalHandle
Class to interact with goals on a server.
Definition: server_goal_handle.hpp:135