MoveIt Pro Behavior
Core Behaviors for MoveIt Pro
moveit_studio::behaviors::AveragePoseStamped Class Reference

This Behavior calculates the running average of incoming Pose Stamped ROS messages. More...

#include <average_pose_stamped.hpp>

Inheritance diagram for moveit_studio::behaviors::AveragePoseStamped:
Collaboration diagram for moveit_studio::behaviors::AveragePoseStamped:

Public Member Functions

 AveragePoseStamped (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< moveit_studio::behaviors::BehaviorContext > &shared_resources)
 Constructor for the AveragePoseStamped behavior. More...
 
BT::NodeStatus onStart () override
 Adds a Pose Stamped sample to the running average and returns it on a port. More...
 
BT::NodeStatus onRunning () override
 Averages the pose. More...
 
void onHalted () override
 
- Public Member Functions inherited from moveit_studio::behaviors::SharedResourcesNode< BT::StatefulActionNode >
 SharedResourcesNode (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< BehaviorContext > &shared_resources)
 Constructor for SharedResourcesNode. Called by BT::BehaviorTreeFactory when creating a new behavior tree containing this node. More...
 

Static Public Member Functions

static BT::PortsList providedPorts ()
 Implementation of the required providedPorts() function for the AveragePoseStamped Behavior. More...
 
static BT::KeyValueVector metadata ()
 

Additional Inherited Members

- Protected Attributes inherited from moveit_studio::behaviors::SharedResourcesNode< BT::StatefulActionNode >
std::shared_ptr< BehaviorContextshared_resources_
 

Detailed Description

This Behavior calculates the running average of incoming Pose Stamped ROS messages.

Data Port Name Port Type Object Type
run_continuously Input bool
num_samples Input double
max_distance Input double
max_rotation Input double
pose_sample Input geometry_msgs::msg::PoseStamped
avg_pose Output geometry_msgs::msg::PoseStamped

If the current sample exceeds a "max_distance" or "max_rotation" (specified as input ports) from the current average pose the behavior will return FAILURE. The Behavior will use the last "num_samples" Pose Stamped messages to calculate the average pose.

Notes:

  • This Behavior can be configured to terminate after a finite number of samples or continuously calculate the average using a rolling window of a fixed size denoted by "num_samples". By default, the Behavior terminates after a finite number of samples. Setting "run_continuously" to true enables the continuous calculation functionality. Letting the Behavior continuously calculate the average can be useful if used in conjunction with parallel nodes. Limitations:
  • If the first sample is an outlier compared to what is normally returned this behavior can fail because the average is initially set by the first pose.

Constructor & Destructor Documentation

◆ AveragePoseStamped()

moveit_studio::behaviors::AveragePoseStamped::AveragePoseStamped ( const std::string &  name,
const BT::NodeConfiguration &  config,
const std::shared_ptr< moveit_studio::behaviors::BehaviorContext > &  shared_resources 
)

Constructor for the AveragePoseStamped behavior.

Parameters
nameThe name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
shared_resourcesA shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
configThis contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.

An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor.

Member Function Documentation

◆ metadata()

BT::KeyValueVector moveit_studio::behaviors::AveragePoseStamped::metadata ( )
static

◆ onHalted()

void moveit_studio::behaviors::AveragePoseStamped::onHalted ( )
override

◆ onRunning()

BT::NodeStatus moveit_studio::behaviors::AveragePoseStamped::onRunning ( )
override

Averages the pose.

Returns
RUNNING if number of samples hasn't been reached or if run_parallel is true, FAILURE if a new pose sample exceeds the max position or rotation threshold or if average orientation calculation fails, SUCCESS if number of samples has been reached and run_parallel is false.

◆ onStart()

BT::NodeStatus moveit_studio::behaviors::AveragePoseStamped::onStart ( )
override

Adds a Pose Stamped sample to the running average and returns it on a port.

Returns
Failure when a port was set incorrectly, Success otherwise.

Initializes the position and quaternion states and sets the average pose to the first pose received

Returns
SUCCESS if number of poses to sample was less than 1 and the behavior is not set to run in parallel

◆ providedPorts()

BT::PortsList moveit_studio::behaviors::AveragePoseStamped::providedPorts ( )
static

Implementation of the required providedPorts() function for the AveragePoseStamped Behavior.

Returns
the ports listed above.

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