MoveIt Pro Behavior
Core Behaviors for MoveIt Pro
moveit_studio::behaviors::GetMoveAlongArcSubframes Class Referencefinal

Given a PoseStamped for a grasp pose, and a vector of two PoseStampeds for the axis of the arc, calculates the subframes needed for motion along an arc. The arc radius is the distance between the grasp point and the arc (hinge) axis. More...

#include <get_move_along_arc_subframes.hpp>

Inheritance diagram for moveit_studio::behaviors::GetMoveAlongArcSubframes:
Collaboration diagram for moveit_studio::behaviors::GetMoveAlongArcSubframes:

Public Member Functions

 GetMoveAlongArcSubframes (const std::string &name, const BT::NodeConfiguration &config)
 
BT::NodeStatus tick () override
 Override of tick() function. Retrieves poses from input ports, uses calculateMoveAlongArcSubframes() to potentially reorder the poses, and then sets the output ports to the result. More...
 

Static Public Member Functions

static BT::PortsList providedPorts ()
 
static BT::KeyValueVector metadata ()
 

Detailed Description

Given a PoseStamped for a grasp pose, and a vector of two PoseStampeds for the axis of the arc, calculates the subframes needed for motion along an arc. The arc radius is the distance between the grasp point and the arc (hinge) axis.

See the calculateMoveAlongArcSubframes function documentation for an explanation of the transform math used here.

Data Port Name Port Type Object Type
target_grasp_pose Input geometry_msgs::msg::PoseStamped
hinge_axis_poses Input std::vector<geometry_msgs::msg::PoseStamped>
move_along_arc_subframes Output std::vector<moveit_studio_vision_msgs::msg::ObjectSubframe>

Constructor & Destructor Documentation

◆ GetMoveAlongArcSubframes()

moveit_studio::behaviors::GetMoveAlongArcSubframes::GetMoveAlongArcSubframes ( const std::string &  name,
const BT::NodeConfiguration &  config 
)

Member Function Documentation

◆ metadata()

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

◆ providedPorts()

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

◆ tick()

BT::NodeStatus moveit_studio::behaviors::GetMoveAlongArcSubframes::tick ( )
override

Override of tick() function. Retrieves poses from input ports, uses calculateMoveAlongArcSubframes() to potentially reorder the poses, and then sets the output ports to the result.

Returns
FAILURE if calculateMoveAlongArcSubframes returns false. SUCCESS if the calculation of the door hinge poses succeeds and output ports are set.

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