AutoAPMS
Resilient Robot Mission Management
Loading...
Searching...
No Matches
RosServiceNode< ServiceT > Class Template Reference

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

#include <auto_apms_behavior_tree_core/node/ros_service_node.hpp>

Public Member Functions

 RosServiceNode (const std::string &instance_name, const Config &config, Context context)
 Constructor.
 
virtual bool setRequest (typename Request::SharedPtr &request)
 Set the request to be sent to the ROS 2 service.
 
virtual BT::NodeStatus onResponseReceived (const typename Response::SharedPtr &response)
 Callback invoked after the service response was received.
 
virtual BT::NodeStatus onFailure (ServiceNodeErrorCode error)
 Callback invoked when one of the errors in ServiceNodeErrorCode occur.
 
bool createClient (const std::string &service_name)
 Create the client of the ROS 2 service.
 
std::string getServiceName () const
 Get the name of the service this node connects with.
 

Static Public Member Functions

static BT::PortsList providedBasicPorts (BT::PortsList addition)
 ADerived nodes implementing the static method RosServiceNode::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 ServiceT>
class auto_apms_behavior_tree::core::RosServiceNode< ServiceT >

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

When ticked, this node sends a service request and awaits the response asynchronously. Inheriting classes must reimplement the virtual methods as described below.

By default, the name of the service 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 service 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 service "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 service 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 service 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
ServiceTType of the ROS 2 service.

Definition at line 91 of file ros_service_node.hpp.

Constructor & Destructor Documentation

◆ RosServiceNode()

template<class ServiceT>
RosServiceNode ( 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 229 of file ros_service_node.hpp.

Member Function Documentation

◆ providedBasicPorts()

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

ADerived nodes implementing the static method RosServiceNode::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 134 of file ros_service_node.hpp.

◆ providedPorts()

template<class ServiceT>
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 145 of file ros_service_node.hpp.

◆ setRequest()

template<class ServiceT>
bool setRequest ( typename Request::SharedPtr & request)
inlinevirtual

Set the request to be sent to the ROS 2 service.

The node may deny to query a service call by returning false. Otherwise, this method should return true.

By default, this callback returns true.

Parameters
requestRequest message.
Returns
false if the request should not be sent. In that case, onFailure(INVALID_REQUEST) will be called.

Definition at line 356 of file ros_service_node.hpp.

◆ onResponseReceived()

template<class ServiceT>
BT::NodeStatus onResponseReceived ( const typename Response::SharedPtr & response)
inlinevirtual

Callback invoked after the service response was received.

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

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

Parameters
responseResponse message.
Returns
Final return status of the node.

Definition at line 362 of file ros_service_node.hpp.

◆ onFailure()

template<class ServiceT>
BT::NodeStatus onFailure ( ServiceNodeErrorCode error)
inlinevirtual

Callback invoked when one of the errors in ServiceNodeErrorCode 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 368 of file ros_service_node.hpp.

◆ createClient()

template<class ServiceT>
bool createClient ( const std::string & service_name)
inline

Create the client of the ROS 2 service.

Parameters
service_nameName of the service.
Returns
true if the client was created successfully, false otherwise.

Definition at line 377 of file ros_service_node.hpp.

◆ getServiceName()

template<class ServiceT>
std::string getServiceName ( ) const
inline

Get the name of the service this node connects with.

Returns
String representing the service name.

Definition at line 437 of file ros_service_node.hpp.


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