-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
bt_action_node.hpp
489 lines (421 loc) · 16.9 KB
/
bt_action_node.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
// Copyright (c) 2018 Intel Corporation
//
// 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 NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#include <memory>
#include <string>
#include <chrono>
#include "behaviortree_cpp/action_node.h"
#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behavior_tree/bt_utils.hpp"
namespace nav2_behavior_tree
{
using namespace std::chrono_literals; // NOLINT
/**
* @brief Abstract class representing an action based BT node
* @tparam ActionT Type of action
*/
template<class ActionT>
class BtActionNode : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::BtActionNode constructor
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
BtActionNode(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true)
{
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
// Get the required items from the blackboard
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
// timeout should be less than bt_loop_duration to be able to finish the current tick
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
// Initialize the input and output messages
goal_ = typename ActionT::Goal();
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
std::string remapped_action_name;
if (getInput("server_name", remapped_action_name)) {
action_name_ = remapped_action_name;
}
createActionClient(action_name_);
// Give the derive class a chance to do any initialization
RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
}
BtActionNode() = delete;
virtual ~BtActionNode()
{
}
/**
* @brief Create instance of an action client
* @param action_name Action name to create client for
*/
void createActionClient(const std::string & action_name)
{
// Now that we have the ROS node to use, create the action client for this BT action
action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
action_name.c_str(),
wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string("Action server ") + action_name +
std::string(" not available"));
}
}
/**
* @brief Any subclass of BtActionNode that accepts parameters must provide a
* providedPorts method and call providedBasicPorts in it.
* @param addition Additional ports to add to BT port list
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedBasicPorts(BT::PortsList addition)
{
BT::PortsList basic = {
BT::InputPort<std::string>("server_name", "Action server name"),
BT::InputPort<std::chrono::milliseconds>("server_timeout")
};
basic.insert(addition.begin(), addition.end());
return basic;
}
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts()
{
return providedBasicPorts({});
}
// Derived classes can override any of the following methods to hook into the
// processing for the action: on_tick, on_wait_for_result, and on_success
/**
* @brief Function to perform some user-defined operation on tick
* Could do dynamic checks, such as getting updates to values on the blackboard
*/
virtual void on_tick()
{
}
/**
* @brief Function to perform some user-defined operation after a timeout
* waiting for a result that hasn't been received yet. Also provides access to
* the latest feedback message from the action server. Feedback will be nullptr
* in subsequent calls to this function if no new feedback is received while waiting for a result.
* @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
*/
virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>/*feedback*/)
{
}
/**
* @brief Function to perform some user-defined operation upon successful
* completion of the action. Could put a value on the blackboard.
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
*/
virtual BT::NodeStatus on_success()
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief Function to perform some user-defined operation when the action is aborted.
* @return BT::NodeStatus Returns FAILURE by default, user may override return another value
*/
virtual BT::NodeStatus on_aborted()
{
return BT::NodeStatus::FAILURE;
}
/**
* @brief Function to perform some user-defined operation when the action is cancelled.
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
*/
virtual BT::NodeStatus on_cancelled()
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick() override
{
// first step to be done only at the beginning of the Action
if (!BT::isStatusActive(status())) {
// setting the status to RUNNING to notify the BT Loggers (if any)
setStatus(BT::NodeStatus::RUNNING);
// reset the flag to send the goal or not, allowing the user the option to set it in on_tick
should_send_goal_ = true;
// user defined callback, may modify "should_send_goal_".
on_tick();
if (!should_send_goal_) {
return BT::NodeStatus::FAILURE;
}
send_new_goal();
}
try {
// if new goal was sent and action server has not yet responded
// check the future goal handle
if (future_goal_handle_) {
auto elapsed =
(node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
// return RUNNING if there is still some time before timeout happens
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
// if server has taken more time than the specified timeout value return FAILURE
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str());
future_goal_handle_.reset();
return BT::NodeStatus::FAILURE;
}
}
// The following code corresponds to the "RUNNING" loop
if (rclcpp::ok() && !goal_result_available_) {
// user defined callback. May modify the value of "goal_updated_"
on_wait_for_result(feedback_);
// reset feedback to avoid stale information
feedback_.reset();
auto goal_status = goal_handle_->get_status();
if (goal_updated_ &&
(goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
{
goal_updated_ = false;
send_new_goal();
auto elapsed =
(node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str());
future_goal_handle_.reset();
return BT::NodeStatus::FAILURE;
}
}
callback_group_executor_.spin_some();
// check if, after invoking spin_some(), we finally received the result
if (!goal_result_available_) {
// Yield this Action, returning RUNNING
return BT::NodeStatus::RUNNING;
}
}
} catch (const std::runtime_error & e) {
if (e.what() == std::string("send_goal failed") ||
e.what() == std::string("Goal was rejected by the action server"))
{
// Action related failure that should not fail the tree, but the node
return BT::NodeStatus::FAILURE;
} else {
// Internal exception to propagate to the tree
throw e;
}
}
BT::NodeStatus status;
switch (result_.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
status = on_success();
break;
case rclcpp_action::ResultCode::ABORTED:
status = on_aborted();
break;
case rclcpp_action::ResultCode::CANCELED:
status = on_cancelled();
break;
default:
throw std::logic_error("BtActionNode::Tick: invalid status value");
}
goal_handle_.reset();
return status;
}
/**
* @brief The other (optional) override required by a BT action. In this case, we
* make sure to cancel the ROS2 action if it is still running.
*/
void halt() override
{
if (should_cancel_goal()) {
auto future_result = action_client_->async_get_result(goal_handle_);
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(
node_->get_logger(),
"Failed to cancel action server for %s", action_name_.c_str());
}
if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(
node_->get_logger(),
"Failed to get result for %s in node halt!", action_name_.c_str());
}
on_cancelled();
}
// this is probably redundant, since the parent node
// is supposed to call it, but we keep it, just in case
resetStatus();
}
protected:
/**
* @brief Function to check if current goal should be cancelled
* @return bool True if current goal should be cancelled, false otherwise
*/
bool should_cancel_goal()
{
// Shut the node down if it is currently running
if (status() != BT::NodeStatus::RUNNING) {
return false;
}
// No need to cancel the goal if goal handle is invalid
if (!goal_handle_) {
return false;
}
callback_group_executor_.spin_some();
auto status = goal_handle_->get_status();
// Check if the goal is still executing
return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
}
/**
* @brief Function to send new goal to action server
*/
void send_new_goal()
{
goal_result_available_ = false;
auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
send_goal_options.result_callback =
[this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
if (future_goal_handle_) {
RCLCPP_DEBUG(
node_->get_logger(),
"Goal result for %s available, but it hasn't received the goal response yet. "
"It's probably a goal result for the last goal request", action_name_.c_str());
return;
}
// TODO(#1652): a work around until rcl_action interface is updated
// if goal ids are not matched, the older goal call this callback so ignore the result
// if matched, it must be processed (including aborted)
if (this->goal_handle_->get_goal_id() == result.goal_id) {
goal_result_available_ = true;
result_ = result;
emitWakeUpSignal();
}
};
send_goal_options.feedback_callback =
[this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
const std::shared_ptr<const typename ActionT::Feedback> feedback) {
feedback_ = feedback;
emitWakeUpSignal();
};
future_goal_handle_ = std::make_shared<
std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
action_client_->async_send_goal(goal_, send_goal_options));
time_goal_sent_ = node_->now();
}
/**
* @brief Function to check if the action server acknowledged a new goal
* @param elapsed Duration since the last goal was sent and future goal handle has not completed.
* After waiting for the future to complete, this value is incremented with the timeout value.
* @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
*/
bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
{
auto remaining = server_timeout_ - elapsed;
// server has already timed out, no need to sleep
if (remaining <= std::chrono::milliseconds(0)) {
future_goal_handle_.reset();
return false;
}
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
auto result =
callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
elapsed += timeout;
if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
future_goal_handle_.reset();
throw std::runtime_error("send_goal failed");
}
if (result == rclcpp::FutureReturnCode::SUCCESS) {
goal_handle_ = future_goal_handle_->get();
future_goal_handle_.reset();
if (!goal_handle_) {
throw std::runtime_error("Goal was rejected by the action server");
}
return true;
}
return false;
}
/**
* @brief Function to increment recovery count on blackboard if this node wraps a recovery
*/
void increment_recovery_count()
{
int recovery_count = 0;
[[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count); // NOLINT
recovery_count += 1;
config().blackboard->set("number_recoveries", recovery_count); // NOLINT
}
std::string action_name_;
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
// All ROS2 actions have a goal and a result
typename ActionT::Goal goal_;
bool goal_updated_{false};
bool goal_result_available_{false};
typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
// To handle feedback from action server
std::shared_ptr<const typename ActionT::Feedback> feedback_;
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
// The timeout value while waiting for response from a server when a
// new action goal is sent or canceled
std::chrono::milliseconds server_timeout_;
// The timeout value for BT loop execution
std::chrono::milliseconds max_timeout_;
// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
// To track the action server acknowledgement when a new goal is sent
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
future_goal_handle_;
rclcpp::Time time_goal_sent_;
// Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
bool should_send_goal_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_