1717
1818#include < memory>
1919#include < string>
20+ #include < optional>
2021#include < rclcpp/executors.hpp>
2122#include < rclcpp/allocator/allocator_common.hpp>
2223#include " behaviortree_cpp/action_node.h"
@@ -103,6 +104,10 @@ class RosActionNode : public BT::ActionNodeBase
103104 * @brief Any subclass of RosActionNode that has ports must implement a
104105 * providedPorts method and call providedBasicPorts in it.
105106 *
107+ * The basic ports:
108+ *
109+ * - `action_name` Action server name
110+ *
106111 * @param addition Additional ports to add to BT port list
107112 * @return PortsList containing basic ports along with node-specific ports
108113 */
@@ -152,8 +157,15 @@ class RosActionNode : public BT::ActionNodeBase
152157 }
153158
154159 /* * Callback invoked when something goes wrong.
160+ * The result is provided if it is available.
155161 * It must return either SUCCESS or FAILURE.
156162 */
163+ virtual BT::NodeStatus onFailure (ActionNodeErrorCode error,
164+ const std::optional<WrappedResult>&)
165+ {
166+ return onFailure (error);
167+ }
168+
157169 virtual BT::NodeStatus onFailure (ActionNodeErrorCode /* error*/ )
158170 {
159171 return NodeStatus::FAILURE;
@@ -244,7 +256,7 @@ RosActionNode<T>::ActionClientInstance::ActionClientInstance(
244256 std::shared_ptr<rclcpp::Node> node, const std::string& action_name)
245257{
246258 callback_group =
247- node->create_callback_group (rclcpp::CallbackGroupType::MutuallyExclusive);
259+ node->create_callback_group (rclcpp::CallbackGroupType::MutuallyExclusive, false );
248260 callback_executor.add_callback_group (callback_group, node->get_node_base_interface ());
249261 action_client = rclcpp_action::create_client<T>(node, action_name, callback_group);
250262}
@@ -309,7 +321,7 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)
309321 if (it == registry.end () || it->second .expired ())
310322 {
311323 client_instance_ = std::make_shared<ActionClientInstance>(node, action_name);
312- registry.insert ({ action_client_key_, client_instance_ } );
324+ registry.insert_or_assign ( action_client_key_, client_instance_);
313325 }
314326 else
315327 {
@@ -360,8 +372,8 @@ inline NodeStatus RosActionNode<T>::tick()
360372
361373 if (!client_instance_)
362374 {
363- throw BT::RuntimeError (" RosActionNode: no client was specified neither as default or "
364- " in the ports" );
375+ throw BT::RuntimeError (" RosActionNode: no client was specified, neither as default "
376+ " nor in the ports" );
365377 }
366378
367379 auto & action_client = client_instance_->action_client ;
@@ -390,7 +402,7 @@ inline NodeStatus RosActionNode<T>::tick()
390402
391403 if (!setGoal (goal))
392404 {
393- return CheckStatus (onFailure (INVALID_GOAL));
405+ return CheckStatus (onFailure (INVALID_GOAL, {} ));
394406 }
395407
396408 typename ActionClient::SendGoalOptions goal_options;
@@ -432,7 +444,7 @@ inline NodeStatus RosActionNode<T>::tick()
432444 // Check if server is ready
433445 if (!action_client->action_server_is_ready ())
434446 {
435- return onFailure (SERVER_UNREACHABLE);
447+ return onFailure (SERVER_UNREACHABLE, {} );
436448 }
437449
438450 future_goal_handle_ = action_client->async_send_goal (goal, goal_options);
@@ -459,7 +471,7 @@ inline NodeStatus RosActionNode<T>::tick()
459471 {
460472 if ((now () - time_goal_sent_) > timeout)
461473 {
462- return CheckStatus (onFailure (SEND_GOAL_TIMEOUT));
474+ return CheckStatus (onFailure (SEND_GOAL_TIMEOUT, {} ));
463475 }
464476 else
465477 {
@@ -490,11 +502,11 @@ inline NodeStatus RosActionNode<T>::tick()
490502 {
491503 if (result_.code == rclcpp_action::ResultCode::ABORTED)
492504 {
493- return CheckStatus (onFailure (ACTION_ABORTED));
505+ return CheckStatus (onFailure (ACTION_ABORTED, result_ ));
494506 }
495507 else if (result_.code == rclcpp_action::ResultCode::CANCELED)
496508 {
497- return CheckStatus (onFailure (ACTION_CANCELLED));
509+ return CheckStatus (onFailure (ACTION_CANCELLED, result_ ));
498510 }
499511 else
500512 {
0 commit comments