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>
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> |
◆ GetMoveAlongArcSubframes()
moveit_studio::behaviors::GetMoveAlongArcSubframes::GetMoveAlongArcSubframes |
( |
const std::string & |
name, |
|
|
const BT::NodeConfiguration & |
config |
|
) |
| |
◆ 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: