AveragePoseStamped Class
This Behavior calculates the running average of incoming Pose Stamped ROS messages. More...
Declaration
Included Headers
Base class
| class | SharedResourcesNode<BehaviorTreeNodeT> |
|
The SharedResourcesNode class provides a BehaviorContext object when constructing a BehaviorTree.Cpp node. More... | |
Public Constructors Index
| AveragePoseStamped (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< moveit_pro::behaviors::BehaviorContext > &shared_resources) | |
|
Constructor for the AveragePoseStamped behavior. More... | |
Public Member Functions Index
| 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 |
Private Member Functions Index
| void | publishTopicsAndSetOutputs (const tf2::Transform &avg_transform, const geometry_msgs::msg::PoseStamped &pose_update) |
|
Publishes the TF Frame of the average pose and sets the avg_pose output port. More... | |
| tl::expected< geometry_msgs::msg::PoseStamped, std::string > | poseSampleInReferenceFrame (const geometry_msgs::msg::PoseStamped &pose_sample) const |
|
Transforms a pose sample into the frame given by the "reference_frame" port, at the sample's timestamp. More... | |
Private Member Attributes Index
| boost::circular_buffer< geometry_msgs::msg::PoseStamped > | pose_samples_ |
|
Holds the number of pose samples to average. More... | |
| tf2::Quaternion | quat_avg_ |
|
The average quaternion calculated from pose_samples_. More... | |
| tf2::Vector3 | pos_avg_ |
|
The average position calculated from pose_samples_. More... | |
Public Static Functions Index
| static BT::PortsList | providedPorts () |
|
Implementation of the required providedPorts() function for the AveragePoseStamped Behavior. More... | |
| static BT::KeyValueVector | metadata () |
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 |
| reference_frame | Input | std::string |
| 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. If the optional "reference_frame" port is set, each pose sample is transformed into that frame at the sample's timestamp before averaging, and the average pose is returned in that frame. Set it to a fixed frame whenever the samples arrive in a moving frame, such as a camera frame during visual servoing. If "reference_frame" is empty, all samples must share the same frame ID, and that frame must be fixed.
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.
Definition at line 51 of file average_pose_stamped.hpp.
Public Constructors
AveragePoseStamped()
|
Constructor for the AveragePoseStamped behavior.
- Parameters
-
name The 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_resources A 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.
config This 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.
Declaration at line 66 of file average_pose_stamped.hpp, definition at line 72 of file average_pose_stamped.cpp.
Public Member Functions
onHalted()
|
Declaration at line 97 of file average_pose_stamped.hpp, definition at line 288 of file average_pose_stamped.cpp.
onRunning()
|
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.
Declaration at line 95 of file average_pose_stamped.hpp, definition at line 197 of file average_pose_stamped.cpp.
onStart()
|
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
Declaration at line 86 of file average_pose_stamped.hpp, definition at line 134 of file average_pose_stamped.cpp.
Private Member Functions
poseSampleInReferenceFrame()
|
Transforms a pose sample into the frame given by the "reference_frame" port, at the sample's timestamp.
- Parameters
-
[in] pose_sample The pose sample to transform.
- Returns
The transformed sample, the unchanged sample when the port is empty or unset, or an error message when the transform fails.
Declaration at line 116 of file average_pose_stamped.hpp, definition at line 123 of file average_pose_stamped.cpp.
publishTopicsAndSetOutputs()
|
Publishes the TF Frame of the average pose and sets the avg_pose output port.
- Parameters
-
[in] avg_transform The average pose
[in] pose_update The current sampled pose
Declaration at line 106 of file average_pose_stamped.hpp, definition at line 105 of file average_pose_stamped.cpp.
Private Member Attributes
pos_avg_
|
The average position calculated from pose_samples_.
Definition at line 131 of file average_pose_stamped.hpp.
pose_samples_
|
Holds the number of pose samples to average.
Definition at line 121 of file average_pose_stamped.hpp.
quat_avg_
|
The average quaternion calculated from pose_samples_.
Definition at line 126 of file average_pose_stamped.hpp.
Public Static Functions
metadata()
| static |
Declaration at line 75 of file average_pose_stamped.hpp, definition at line 100 of file average_pose_stamped.cpp.
providedPorts()
| static |
Implementation of the required providedPorts() function for the AveragePoseStamped Behavior.
- Returns
the ports listed above.
Declaration at line 73 of file average_pose_stamped.hpp, definition at line 78 of file average_pose_stamped.cpp.
The documentation for this class was generated from the following files:
Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.