rclcpp_action  master
C++ ROS Action Client Library
server_goal_handle.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_GOAL_HANDLE_HPP_
16 #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
17 
18 #include <rcl_action/types.h>
19 #include <rcl_action/goal_handle.h>
20 
21 #include <action_msgs/msg/goal_status.hpp>
22 
23 #include <functional>
24 #include <memory>
25 #include <mutex>
26 
28 #include "rclcpp_action/types.hpp"
29 
30 namespace rclcpp_action
31 {
32 
35 
43 {
44 public:
48  bool
49  is_canceling() const;
50 
54  bool
55  is_active() const;
56 
60  bool
61  is_executing() const;
62 
64  virtual
66 
67 protected:
68  // -------------------------------------------------------------------------
69  // API for communication between ServerGoalHandleBase and ServerGoalHandle<>
70 
75  )
76  : rcl_handle_(rcl_handle)
77  {
78  }
79 
82  void
83  _abort();
84 
87  void
88  _succeed();
89 
92  void
93  _cancel_goal();
94 
97  void
98  _canceled();
99 
102  void
103  _execute();
104 
108  bool
109  try_canceling() noexcept;
110 
111  // End API for communication between ServerGoalHandleBase and ServerGoalHandle<>
112  // -----------------------------------------------------------------------------
113 
114 private:
116  mutable std::mutex rcl_handle_mutex_;
117 };
118 
119 // Forward declare server
120 template<typename ActionT>
121 class Server;
122 
124 
134 template<typename ActionT>
136 {
137 public:
139 
148  void
150  {
151  auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
152  feedback_message->goal_id.uuid = uuid_;
153  feedback_message->feedback = *feedback_msg;
154  publish_feedback_(feedback_message);
155  }
156 
158 
167  void
168  abort(typename ActionT::Result::SharedPtr result_msg)
169  {
170  _abort();
171  auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
172  response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
173  response->result = *result_msg;
174  on_terminal_state_(uuid_, response);
175  }
176 
178 
187  void
188  succeed(typename ActionT::Result::SharedPtr result_msg)
189  {
190  _succeed();
191  auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
192  response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
193  response->result = *result_msg;
194  on_terminal_state_(uuid_, response);
195  }
196 
198 
207  void
208  canceled(typename ActionT::Result::SharedPtr result_msg)
209  {
210  _canceled();
211  auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
212  response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
213  response->result = *result_msg;
214  on_terminal_state_(uuid_, response);
215  }
216 
218 
223  void
225  {
226  _execute();
227  on_executing_(uuid_);
228  }
229 
232  get_goal() const
233  {
234  return goal_;
235  }
236 
238  const GoalUUID &
239  get_goal_id() const
240  {
241  return uuid_;
242  }
243 
245  {
246  // Cancel goal if handle was allowed to destruct without reaching a terminal state
247  if (try_canceling()) {
248  auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
249  null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
250  on_terminal_state_(uuid_, null_result);
251  }
252  }
253 
254 protected:
258  GoalUUID uuid,
260  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
261  std::function<void(const GoalUUID &)> on_executing,
263  )
264  : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
265  on_terminal_state_(on_terminal_state), on_executing_(on_executing),
266  publish_feedback_(publish_feedback)
267  {
268  }
269 
272 
275 
277 
281 };
282 } // namespace rclcpp_action
283 
284 #endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
RCLCPP_ACTION_PUBLIC void _abort()
RCLCPP_ACTION_PUBLIC ServerGoalHandleBase(std::shared_ptr< rcl_action_goal_handle_t > rcl_handle)
Definition: server_goal_handle.hpp:73
RCLCPP_ACTION_PUBLIC bool is_canceling() const
void canceled(typename ActionT::Result::SharedPtr result_msg)
Indicate that a goal has been canceled.
Definition: server_goal_handle.hpp:208
void execute()
Indicate that the server is starting to execute a goal.
Definition: server_goal_handle.hpp:224
const std::shared_ptr< const typename ActionT::Goal > get_goal() const
Get the user provided message describing the goal.
Definition: server_goal_handle.hpp:232
RCLCPP_ACTION_PUBLIC bool try_canceling() noexcept
Definition: server_goal_handle.hpp:42
void succeed(typename ActionT::Result::SharedPtr result_msg)
Indicate that a goal has succeeded.
Definition: server_goal_handle.hpp:188
const GoalUUID uuid_
A unique id for the goal request.
Definition: server_goal_handle.hpp:274
Action Server.
Definition: server.hpp:268
Class to interact with goals on a server.
Definition: server_goal_handle.hpp:135
RCLCPP_ACTION_PUBLIC void _succeed()
RCLCPP_ACTION_PUBLIC void _cancel_goal()
virtual RCLCPP_ACTION_PUBLIC ~ServerGoalHandleBase()
const std::shared_ptr< const typename ActionT::Goal > goal_
The user provided message describing the goal.
Definition: server_goal_handle.hpp:271
RCLCPP_ACTION_PUBLIC void _execute()
virtual ~ServerGoalHandle()
Definition: server_goal_handle.hpp:244
std::function< void(const GoalUUID &)> on_executing_
Definition: server_goal_handle.hpp:279
ServerGoalHandle(std::shared_ptr< rcl_action_goal_handle_t > rcl_handle, GoalUUID uuid, std::shared_ptr< const typename ActionT::Goal > goal, std::function< void(const GoalUUID &, std::shared_ptr< void >)> on_terminal_state, std::function< void(const GoalUUID &)> on_executing, std::function< void(std::shared_ptr< typename ActionT::Impl::FeedbackMessage >)> publish_feedback)
Definition: server_goal_handle.hpp:256
#define RCLCPP_ACTION_PUBLIC
Definition: visibility_control.hpp:50
void abort(typename ActionT::Result::SharedPtr result_msg)
Indicate that a goal could not be reached and has been aborted.
Definition: server_goal_handle.hpp:168
const GoalUUID & get_goal_id() const
Get the unique identifier of the goal.
Definition: server_goal_handle.hpp:239
Definition: client.hpp:44
void publish_feedback(std::shared_ptr< typename ActionT::Feedback > feedback_msg)
Send an update about the progress of a goal.
Definition: server_goal_handle.hpp:149
RCLCPP_ACTION_PUBLIC void _canceled()
RCLCPP_ACTION_PUBLIC bool is_executing() const
RCLCPP_ACTION_PUBLIC bool is_active() const