AutoAPMS
Resilient Robot Mission Management
Loading...
Searching...
No Matches
RosActionNode< ActionT > Class Template Reference

Generic behavior tree node wrapper for a ROS 2 action client. More...

#include <auto_apms_behavior_tree_core/node/ros_action_node.hpp>

Public Member Functions

 RosActionNode (const std::string &instance_name, const Config &config, Context context)
 Constructor.
 
virtual void onHalt ()
 Callback invoked when the node is halted by the behavior tree.
 
virtual bool setGoal (Goal &goal)
 Set the goal message to be sent to the ROS 2 action.
 
virtual BT::NodeStatus onResultReceived (const WrappedResult &result)
 Callback invoked after the result that is sent by the action server when the goal terminated was received.
 
virtual BT::NodeStatus onFeedback (const Feedback &feedback)
 Callback invoked after action feedback was received.
 
virtual BT::NodeStatus onFailure (ActionNodeErrorCode error)
 Callback invoked when one of the errors in ActionNodeErrorCode occur.
 
void cancelGoal ()
 Synchronous method that sends a request to the server to cancel the current action.
 
bool createClient (const std::string &action_name)
 Create the client of the ROS 2 action.
 
std::string getActionName () const
 Get the name of the action this node connects with.
 

Static Public Member Functions

static BT::PortsList providedBasicPorts (BT::PortsList addition)
 Derived nodes implementing the static method RosActionNode::providedPorts may call this method to also include the default port for ROS 2 behavior tree nodes.
 
static BT::PortsList providedPorts ()
 If a behavior tree requires input/output data ports, the developer must define this method accordingly.
 

Detailed Description

template<class ActionT>
class auto_apms_behavior_tree::core::RosActionNode< ActionT >

Generic behavior tree node wrapper for a ROS 2 action client.

When ticked, this node sends an action goal and awaits the execution result asynchronously. When halted, the node blocks until both the action was cancelled and the result was received. Inheriting classes must reimplement the virtual methods as described below.

By default, the name of the action will be determined as follows:

  1. If a value is passed using the input port named port, use that.
  2. Otherwise, use the value from NodeRegistrationOptions::port passed on construction as part of RosNodeContext.

It is possible to customize which port is used to determine the action name and also extend the input's value with a prefix or suffix. This is achieved by including the special pattern (input:<port_name>) in NodeRegistrationOptions::port and replacing <port_name> with the desired input port name.

Example: Given the user implements an input port BT::InputPort<std::string>("my_port"), one may create a client for the action "foo/bar" by defining NodeRegistrationOptions::port as (input:my_port)/bar and providing the string "foo" to the port with name my_port.

Additionally, the following characteristics depend on NodeRegistrationOptions:

  • wait_timeout: Period [s] (measured since tree construction) after the action is considered unreachable.
  • request_timeout: Period [s] (measured since sending a goal request) after the node aborts waiting for a server response.
  • allow_unreachable: Flag whether to tolerate if the action is unreachable when trying to create the client. If set to true, a warning is logged. Otherwise, an exception is raised.
  • logger_level: Minimum severity level enabled for logging using the ROS 2 Logger API.
Template Parameters
ActionTType of the ROS 2 action.

Definition at line 97 of file ros_action_node.hpp.

Constructor & Destructor Documentation

◆ RosActionNode()

template<class ActionT>
RosActionNode ( const std::string & instance_name,
const Config & config,
Context context )
inlineexplicit

Constructor.

Derived nodes are automatically created by TreeBuilder::instantiate when included inside a node manifest associated with the behavior tree resource.

Parameters
instance_nameName given to this specific node instance.
configStructure of internal data determined at runtime by BT::BehaviorTreeFactory.
contextAdditional parameters specific to ROS 2 determined at runtime by TreeBuilder.

Definition at line 271 of file ros_action_node.hpp.

Member Function Documentation

◆ providedBasicPorts()

template<class ActionT>
static BT::PortsList providedBasicPorts ( BT::PortsList addition)
inlinestatic

Derived nodes implementing the static method RosActionNode::providedPorts may call this method to also include the default port for ROS 2 behavior tree nodes.

Parameters
additionAdditional ports to add to the ports list.
Returns
List of ports containing the default port along with node-specific ports.

Definition at line 142 of file ros_action_node.hpp.

◆ providedPorts()

template<class ActionT>
static BT::PortsList providedPorts ( )
inlinestatic

If a behavior tree requires input/output data ports, the developer must define this method accordingly.

Returns
List of ports used by this node.

Definition at line 153 of file ros_action_node.hpp.

◆ onHalt()

template<class ActionT>
void onHalt ( )
inlinevirtual

Callback invoked when the node is halted by the behavior tree.

Afterwards, the ROS 2 action will be cancelled.

By default, this callback does nothing.

Definition at line 287 of file ros_action_node.hpp.

◆ setGoal()

template<class ActionT>
bool setGoal ( Goal & goal)
inlinevirtual

Set the goal message to be sent to the ROS 2 action.

The node may deny to send a goal by returning false. Otherwise, this method should return true.

By default, this callback always returns true.

Parameters
goalReference to the goal message.
Returns
false if the request should not be sent. In that case, onFailure(INVALID_GOAL) will be called.

Definition at line 292 of file ros_action_node.hpp.

◆ onResultReceived()

template<class ActionT>
BT::NodeStatus onResultReceived ( const WrappedResult & result)
inlinevirtual

Callback invoked after the result that is sent by the action server when the goal terminated was received.

Based on the result message, the node may return BT::NodeStatus::SUCCESS or BT::NodeStatus::FAILURE.

By default, this callback returns BT::NodeStatus::SUCCESS if the action finished or was canceled successfully. Otherwise it returns BT::NodeStatus::FAILURE.

Parameters
resultReference to the result message.
Returns
Final return status of the node.

Definition at line 298 of file ros_action_node.hpp.

◆ onFeedback()

template<class ActionT>
BT::NodeStatus onFeedback ( const Feedback & feedback)
inlinevirtual

Callback invoked after action feedback was received.

The node may cancel the current action by returning BT::NodeStatus::SUCCESS or BT::NodeStatus::FAILURE. Otherwise, this should return BT::NodeStatus::RUNNING to indicate that the action should continue.

By default, this callback always returns BT::NodeStatus::RUNNING.

Parameters
feedbackReceived feedback message.
Returns
Final return status of the node.

Definition at line 324 of file ros_action_node.hpp.

◆ onFailure()

template<class ActionT>
BT::NodeStatus onFailure ( ActionNodeErrorCode error)
inlinevirtual

Callback invoked when one of the errors in ActionNodeErrorCode occur.

Based on the error code, the node may return BT::NodeStatus::SUCCESS or BT::NodeStatus::FAILURE.

By default, this callback throws auto_apms_behavior_tree::exceptions::RosNodeError and interrupts the tree.

Parameters
errorCode for the error that has occurred.
Returns
Final return status of the node.

Definition at line 330 of file ros_action_node.hpp.

◆ cancelGoal()

template<class T>
void cancelGoal ( )
inline

Synchronous method that sends a request to the server to cancel the current action.

This method returns as soon as the action is successfully cancelled or an error occurred in the process.

Definition at line 339 of file ros_action_node.hpp.

◆ createClient()

template<class ActionT>
bool createClient ( const std::string & action_name)
inline

Create the client of the ROS 2 action.

Parameters
action_nameName of the action.
Returns
true if the client was created successfully, false otherwise.

Definition at line 614 of file ros_action_node.hpp.

◆ getActionName()

template<class ActionT>
std::string getActionName ( ) const
inline

Get the name of the action this node connects with.

Returns
String representing the action name.

Definition at line 674 of file ros_action_node.hpp.


The documentation for this class was generated from the following file: