MoveIt Pro Behavior  5.2.0
Core Behaviors for MoveIt Pro
moveit_studio_behavior

Provides core MoveIt Pro Behavior plugins, which enable fundamental planning, execution, and perception processing capabilities.

List of Behaviors

This is a brief overview of the Behaviors built into the core Behaviors plugin. For details on each type of built in Behavior, refer to the class's documentation.

Generic Behaviors

InitializeMotionConstraints

Creates a shared pointer to a new moveit_msgs::msg::Constraints and writes it to the Blackboard.

Data Port Name Port Type Object Type
constraints_name input std::string
constraints output std::shared_ptr<moveit_msgs::msg::Constraints>

CallTriggerService

Calls a service that accepts a std_srvs::srv::Trigger message. The name of the service is set through the service_name parameter.

Data Port Name Port Type Object Type
service_name Input std::string

CreateGraspableObject

Creates a GraspableObject message (of a primitive Cuboid) given its pose given as individual points, the dimensions of the Cuboid, a frame ID, an ID for the newly created object, and whether any top, front, or side graspable faces should be generated.

Data Port Name Port Type Object Type
px Input double
py Input double
pz Input double
rx Input double
ry Input double
rz Input double
dx Input double
dy Input double
dz Input double
frame_id Input std::string
object_id Input std::string
generate_top_face Input bool
generate_front_face Input bool
generate_side_faces Input bool
cuboid_object Output moveit_studio_vision_msgs::msg::GraspableObject

ExtractGraspableObjectPose

Changes an input GraspableObject into a PoseStamped by getting its pose and its ID. This is used to create a new PoseStamped message, with the pose being the GraspableObject message's pose.

Data Port Name Port Type Object Type
graspable_object Input moveit_studio_vision_msgs::msg::GraspableObject
port Output geometry_msgs::msg::PoseStamped

AddSubframeToObject

Annotates an input GraspableObject with an input Subframe (which contains a pose and an ID). This subframe is added to the graspable_object, with the subframe pose being relative to the GraspableObject message's pose.

Data Port Name Port Type Object Type
subframe Input moveit_studio_vision_msgs::msg::ObjectSubframe
graspable_object InOut moveit_studio_vision_msgs::msg::GraspableObject

MoveGripperAction

Actuate a gripper through its driver node's GripperCommand action server. Given the name of the action topic and a target gripper position, move the gripper to the specified position.

Data Port Name Port Type Object Type
gripper_command_action_name Input std::string
position Input double

PublishEmpty

Publish a std_msgs::msg::Empty message to a topic.

Data Port Name Port Type Object Type
topic Input std::string
queue_size Input size_t
use_best_effort Input bool

PublishString

Publish a std_msgs::msg::String message to a topic.

Data Port Name Port Type Object Type
message Input std::string OR std_msgs::msg::String
topic Input std::string
queue_size Input size_t
use_best_effort Input bool

TransformPose

Transforms a stamped pose given an input translation and orientation.

The transformation occurs in the reference frame specified by the pose stamp. For example, if the input pose is expressed in the "world" frame, the translation and orientation are also applied in the "world" frame to produce the output pose.

Data Port Name Port Type Object Type
input_pose input geometry_msgs::msg::PoseStamped
position_xyz input std::vector<double>
orientation_xyzw input std::vector<double>
output_pose output geometry_msgs::msg::PoseStamped

TransformPoseFrame

Transforms the reference frame of an input stamped pose to the frame specified by a frame ID, along with the position/orientation transformation that entails.

Data Port Name Port Type Object Type
input_pose input geometry_msgs::msg::PoseStamped
target_frame_id input std::string
output_pose output geometry_msgs::msg::PoseStamped

AppendOrientationConstraint

Given an existing moveit_msgs::msg::Constraints, creates a moveit_msgs::msg::OrientationConstraint from an input yaml file and appends this orientation constraint to the existing constraints message.

Data Port Name Port Type Object Type
config_file_name Input std::string
constraints Bidirectional std::shared_ptr<moveit_msgs::msg::Constraints>

BiasedCoinFlip

Simulates flipping a biased coin with the specified probability of success provided via the input port.

Data Port Name Port Type Object Type
success_probability Input double

FollowJointTrajectory

Accepts a JointTrajectory message via an input data port, and executes it by sending a goal to the specified FollowJointTrajectory action server.

Data Port Name Port Type Object Type
follow_joint_trajectory_action_name Input std::string
joint_trajectory_msg Input trajectory_msgs::msg::JointTrajectory

ForceExceedsThreshold

Monitors wrench messages published to topic, and returns SUCCESS if the magnitude of the wrench measurements exceeds a force threshold for a minimum number of consecutive readings.

Data Port Name Port Type Object Type
wrench_topic_name Input std::string
hand_frame_name Input std::string
wrench_frame_name Input std::string
minimum_consecutive_wrench_values Input std::size_t
force_threshold Input double

LoadJointTrajectoryFromYaml

Creates and outputs a ROS JointTrajectory message from a given input yaml file.

Data Port Name Port Type Object Type
joint_trajectory_yaml_file_name input std::string
joint_trajectory_namespace input std::string
joint_trajectory_msg output trajectory_msgs::msg::JointTrajectory

SaveJointTrajectoryToYaml

Saves data from a ROS JointTrajectory message to a yaml file in the objectives directory.

Data Port Name Port Type Object Type
joint_trajectory_msg input trajectory_msgs::msg::JointTrajectory
joint_trajectory_yaml_file_name input std::string
joint_trajectory_namespace input std::string

ReadTextFileAsString

Read the contents of a text file and output the contents as a std::string.

Data Port Name Port Type Object Type
text_filename input std::string
string_file_contents output std::string

GetGraspAndTwistSubframes

Given an input PoseStamped representing a grasp pose selected on an object, get the three Subframes that define a Grasp Screw Motion (grab and twist about an axis) where the object is grabbed and rotated by grasp_rotation_z_radians amount.

Assumes that the Z- axis of the grasp pose matches the normal vector of the front face of the object.

Data Port Name Port Type Object Type
target_grasp_pose Input geometry_msgs::msg::PoseStamped
grasp_rotation_z_radians Input double
grasp_pose Output geometry_msgs::msg::PoseStamped
screw_origin_pose Output geometry_msgs::msg::PoseStamped
screw_axis_pose Output geometry_msgs::msg::PoseStamped

GetMoveAlongArcSubframes

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.

Assumes that the Z+ axis of the two poses on the hinge axis are oriented facing into the surface of the door.

The direction of the door relative to the surface normal is determined by calculating the vector cross product of the vector from the hinge origin pose to the grasp pose and the vector from the hinge origin pose to the hinge axis pose, and then calculating the dot product of the resulting vector and the Z-axis of the hinge origin pose. If the dot product is positive, the hinge origin and hinge axis poses are correctly ordered. If the dot product is negative, the hinge origin and hinge axis poses need to be reversed for positive door rotation to open the door towards the viewpoint.

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>

AveragePoseStamped

Calculates the running average of incoming Pose Stamped ROS messages. If the current sample (pose_sample) exceeds a max_distance or max_rotation 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.

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

MoveIt Task Constructor (MTC) Behaviors

InitializeMTCTask

Creates a shared pointer to a new MTC Task object, populates it with global settings (for example, the names of controllers to enable by default when executing trajectories planned by this Task), and sets it as an output data port.

Data Port Name Port Type Object Type
task_id Input std::string
controller_names Input std::string
task Output std::shared_ptr<moveit::task_constructor::Task>

PlanMTCTask

Takes a shared pointer to an existing MTC Task object via an input data port, plans the Task, and sets the resulting MTC Solution object as an output data port.

Data Port Name Port Type Object Type
task Input std::shared_ptr<moveit::task_constructor::Task>
solution Output std::shared_ptr<const moveit::task_constructor::SolutionBase>

ExecuteMTCTask

Takes a shared pointer to an MTC Solution object via an input data port, and executes the lowest-cost trajectory in that Solution using the MTC ExecuteTaskSolution MoveGroup capability's /execute_task_solution action server.

Data Port Name Port Type Object Type
solution Input std::shared_ptr<const moveit::task_constructor::SolutionBase>

SetupMTCCurrentState

Given an existing MTC Task object, appends an MTC CurrentState Stage to the Task.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>

SetupMTCApproachGrasp

Given an existing MTC Task object and a target object, appends MTC stages to describe a motion plan to approach the object.

This Behavior must be followed by another Behavior that adds MTC stages for grasp pose generation. The monitored_stage output provides the name of the MTC stage containing the initial (pre-aproach) planning scene, which can be used by the grasp generation stage(s).

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
target_object Input moveit_studio_vision_msgs::msg::GraspableObject
parameters Input YAML::Node
monitored_stage Output std::string

SetupMTCGenerateCuboidGrasps

Given an existing MTC Task object and a target object, appends MTC stages to generate cuboid grasp poses.

The monitored_stage input provides the name of the MTC stage containing the initial (pre-aproach) planning scene, which can be used by the grasp generation stage(s) in this Behavior.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
target_object Input moveit_studio_vision_msgs::msg::GraspableObject
monitored_stage Input std::string
parameters Input YAML::Node

SetupMTCGenerateVacuumGrasps

Given an existing MTC Task object and a target object, appends MTC stages to generate vacuum grasp poses on its planar surfaces.

The monitored_stage input provides the name of the MTC stage containing the initial (pre-aproach) planning scene, which can be used by the grasp generation stage(s) in this Behavior.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
target_object Input moveit_studio_vision_msgs::msg::GraspableObject
monitored_stage Input std::string
parameters Input YAML::Node

SetupMTCRetractFromGrasp

Given an existing MTC Task object and a target object, appends MTC stages to describe a motion plan to retract after grasping the object.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
target_object Input moveit_studio_vision_msgs::msg::GraspableObject
parameters Input YAML::Node

SetupMTCMoveToJointState

Given an existing MTC Task object and a joint state, appends MTC stages to describe a freespace motion plan to that joint state.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
planning_group_name Input std::string
joint_state Input sensor_msgs::msg::JointState
use_all_planners Input bool
constraints Input std::shared_ptr<moveit_msgs::msgs::Constraints>

SetupMTCMoveToNamedState

Given an existing MTC Task object and the name of a known state, appends MTC stages to describe a freespace motion plan to that state.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
planning_group_name Input std::string
goal_state_name Input std::string

SetupMTCInterpolateToJointState

Given an existing MTC Task object and a joint state, appends MTC stages to describe a joint-interpolated motion plan to that joint state.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
planning_group_name Input std::string
joint_state Input sensor_msgs::msg::JointState

SetupMTCUpdateGroupCollisionRule

Add an MTC Stage to an MTC Task that modifies the planning scene's Allowed Collision Matrix to permit or forbid collision between a planning scene object and the links of a named robot planning group while planning subsequent Stages.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
parameters Input YAML::Node
Parameter Name Parameter Type
group_name std::string
object_name std::string
allow_collision bool

SetupMTCMoveAlongFrameAxis

Given an existing MTC Task object, frame name, axis, and move distance, appends MTC stages to describe a cartesian motion plan in the frame's axis with the specified distance.

Data Port Name Port Type Object Type
parameters Input YAML::Node
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
axis Input std::string
move_distance Input double
Parameter Name Parameter Type
arm_group_name std::string
hand_frame_name std::string
velocity_acceleration_scaling_factor double

SetupMTCPickObject

Given an existing MTC Task object and a target grasp pose, appends MTC stages to describe a motion plan to approach, grasp and lift an object at that pose.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
grasp_pose Input geometry_msgs::msg::PoseStamped
parameters Input YAML::Node

SetupMTCGraspThenMoveAlongArcPull

Configures MTC stages to perform a motion that can be parameterized as a grasp followed by a pulling motion in a screw-trajectory (a circular arc). Examples in practice include opening pull doors and drawers.

The input data ports are generally calculated by separate perception processing Behaviors in a previous step of the Objective.

Given an existing MTC Task object and input parameters that configure a screw motion affordance template, perform the following steps:

  • Move to a pre-grasp pose offset from the specified grasp pose.
  • Approach the grasp pose.
  • Close the gripper.
  • Move along a screw-parameterised trajectory.
  • Open the gripper.
  • Retreat from the grasp pose.
Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
grasp_pose Input geometry_msgs::msg::PoseStamped
screw_origin_pose Input geometry_msgs::msg::PoseStamped
screw_axis_pose Input geometry_msgs::msg::PoseStamped
translation_distance Input double
rotation_distance Input double
use_circular_arc Input bool

SetupMTCGraspAndTwistThenMoveAlongArcPush

Configures MTC stages to grasp a location, rotate the end-effector about an axis to turn the grasp point (for example a handle), and then push the end-effector away from the robot base while still grasping (for example to open a door).

The input data ports are generally calculated by separate perception processing Behaviors in a previous step of the Objective.

Data Port Name Port Type Object Type
task Bidirectional std::shared_ptr<moveit::task_constructor::Task>
handle_pose Input geometry_msgs::msg::PoseStamped
handle_z_offset Input double
handle_length Input double
approach_distance Input double
push_open_distance Input double
move_distance Input double
use_circular_arc Input bool

Reachability Analysis Behaviors

GetMeshNormalPoses

Gets the normal vectors for a given mesh and returns the normal vector poses to the caller.

The mesh must be associated with a specific link name in the robot model URDF.

Data Port Name Port Type Object Type
link_name input std::string
discretization_step_size input double
mesh_normal_poses output std::vector<geometry_msgs::msg::PoseStamped>

User Input Behaviors

DoTeleoperateAction

Starts teleoperation by sending a goal to the teleoperation action server in the web UI.

This goal contains configuration options for enabling user interactions and prompts, as well as setting the initial teleoperation mode for the UI to appear in. While the initial and current teleoperation modes are specified as integer values, these values correspond to the enumerations in the moveit_studio_sdk_msgs::msg::TeleoperationMode ROS message.

Data Port Name Port Type Object Type
enable_user_interaction input bool
user_interaction_prompt input std::string
initial_teleop_mode input int
current_teleop_mode output int

IsUserAvailable

Checks for the presence of a user interface by checking if the /trajectory_bridge ROS node exists.

Data Port Name Port Type Object Type
n/a n/a n/a

RetrieveJointStateParameter

Retrieves a joint state from the MoveIt Pro parameter manager node.

If no parameter is retrieved within the provided "timeout_sec" parameter, this Behavior returns FAILURE. Leaving the timeout_sec input empty makes this Behavior wait indefinitely to retrieve the parameter.

Data Port Name Port Type Object Type
timeout_sec input double
joint_state output sensor_msgs::msg::JointState

RetrievePoseParameter

Retrieves a stamped pose from the MoveIt Pro parameter manager node.

If no parameter is retrieved within the provided "timeout_sec" parameter, this Behavior returns FAILURE. Leaving the timeout_sec input empty makes this Behavior wait indefinitely to retrieve the parameter.

Data Port Name Port Type Object Type
timeout_sec input double
pose output geometry_msgs::msg::PoseStamped

RetrieveWaypoint

Given a named waypoint, sends a service request to the Agent WaypointManager to retrieve the joint state associated with that waypoint.

Data Port Name Port Type Object Type
waypoint_name Input std::string
waypoint_joint_state Output sensor_msgs::msg::JointState

TeleoperateJointJog

This is a special Behavior to run human-in-the-loop teleoperation in joint jog mode using MoveIt Servo through the Objective Server.

When started, this Behavior will run INDEFINITELY until it is halted. This will happen either when the root node of the behavior tree is halted as the Objective is canceled, or when this Behavior's parent control node halts it. When this Behavior first transitions from IDLE to RUNNING, it starts and unpauses Servo control using the services advertised by the Servo server node. While this Behavior is RUNNING, it subscribes to JointJog command messages that originate in the user interface, and republishes these messages to the command topics advertised by the Servo server node. When this Behavior is halted, it pauses Servo control using the server's services.

Data Port Name Port Type Object Type
controller_name Input std::string

TeleoperateTwist

This is a special Behavior to run human-in-the-loop teleoperation in Cartesian twist mode using MoveIt Servo through the Objective Server.

When started, this Behavior will run INDEFINITELY until it is halted. This will happen either when the root node of the behavior tree is halted as the Objective is canceled, or when this Behavior's parent control node halts it. When this Behavior first transitions from IDLE to RUNNING, it starts and unpauses Servo control using the services advertised by the Servo server node. While this Behavior is RUNNING, it subscribes to TwistStamped command messages that originate in the user interface, and republishes these messages to the command topics advertised by the Servo server node. When this Behavior is halted, it pauses Servo control using the server's services.

Data Port Name Port Type Object Type
controller_name Input std::string

WaitForDuration

Wait for a specified duration before succeeding.

Data Port Name Port Type Object Type
delay_duration Input double

WaitForUserTrajectoryApproval

Takes a shared pointer to an MTC Solution object via an input data port, and publishes the lowest-cost trajectory in that Solution on MTC's /solution introspection topic. Creates a SetBool service server on the /execute_behavior_solution topic and waits to receive a request containing data: true before succeeding.

Data Port Name Port Type Object Type
solution Input std::shared_ptr<const moveit::task_constructor::SolutionBase>

Vision Behaviors

CheckCuboidSimilarity

Compares two input Cuboid GraspableObjects and checks whether they are similar to within a given distance threshold and orientation threshold. Returns true if they're within threshold, false if not, and returns an error message if they could not be compared.

Data Port Name Port Type Object Type
input_cuboid Input moveit_studio_vision_msgs::msg::GraspableObject
reference_cuboid Input moveit_studio_vision_msgs::msg::GraspableObject
distance_threshold Input std::string
orientation_threshold Input std::string
base_frame Input std::string

ClearSnapshot

Clears the stored PointCloud snapshot including the pointcloud collision Octomap. No inputs or outputs.

CropPointsInBox

Given a point cloud and a box-shaped region of interest, create a new point cloud which contains only the points that are inside the region of interest. The dimensions and size of the region of interest are defined relative to its centroid.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
crop_box_centroid_pose input geometry_msgs::msg::PoseStamped
crop_box_size input std::vector<double>
point_cloud_cropped output sensor_msgs::msg::PointCloud2

DetectAprilTags

Detects AprilTag markers from an image.

Data Port Name Port Type Object Type
image input sensor_msgs::msg::Image
camera_info input sensor_msgs::msg::CameraInfo
parameters input YAML::Node
detections output moveit_studio_vision_msgs::msg::ObjectDetectionArray

FindMaskedObjects

Finds GraspableObjects by segmenting a point cloud using a set of 2D mask images. The output GraspableObjects will be transformed so that their parent frame is the frame defined by the base_frame config parameter.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
camera_info input sensor_msgs::msg::CameraInfo
masks2d input std::vector<moveit_studio_vision_msgs::msg::Mask2D>
base_frame input std::string
plane_inlier_threshold input double
minimum_face_area input double
face_separation_threshold input double
detected_shapes output std::vector<moveit_msgs::msg::GraspableObject>

FindSingularCuboids

Finds well-singulated cuboid GraspableObjects supported by a surface within a point cloud. This behavior makes some assumptions about the environment:

  • The input point cloud must include a large flat surface, such as a tabletop or floor.
  • The filtering and cropping steps are expected to remove all points which are far away from this surface.
  • After filtering, all remaining clusters of points are assumed to be objects which are supported by the surface.

The output GraspableObjects will be transformed so that their parent frame is the frame defined by the base_frame config parameter.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
parameters input YAML::Node
detected_shapes output std::vector<moveit_msgs::msg::GraspableObject>

GetCameraInfo

Captures camera information and makes it available on an output port.

Data Port Name Port Type Object Type
topic_name input std::string
message_out output sensor_msgs::msg::CameraInfo

GetDetectionPose

Gets the stamped pose of an object detection given a label or ID, if one exists.

The target_id and target_label ports define the match criteria for the detection. A target_id of -1 denotes any ID is allowed. Similarly, a target_label that is an empty string denotes any label is allowed.

If multiple detections that match the criteria exist in the list, return the first one.

Data Port Name Port Type Object Type
detections input moveit_studio_vision_msgs::msg::ObjectDetectionArray
target_id input int
target_label input std::string
detection_pose output geometry_msgs::msg::PoseStamped

GetDoorHandle

Calculates the pose, length, and width of a door handle.

By convention, the z-axis of target_handle_pose is aligned with the handle's axis of rotation, and the x-axis points along the handle toward the door hinge.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
handle_poses input std::vector<geometry_msgs::msg::PoseStamped>
handle_tip_pose input geometry_msgs::msg::PoseStamped
minimum_door_handle_depth input double
target_output_frame_id input std::string
target_handle_pose output geometry_msgs::msg::PoseStamped
target_handle_z_offset output double
target_handle_length output double

GetGraspableObjectsFromMasks3D

Finds objects in point cloud segments represented by 3D masks. The output GraspableObjects will be transformed so that their parent frame is the frame defined by the base_frame config parameter.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
masks3d input std::vector<moveit_studio_vision_msgs::msg::Mask3D>
base_frame input std::string
plane_inlier_threshold input double
minimum_face_area input double
face_separation_threshold input double
graspable_objects output std::vector<moveit_msgs::msg::GraspableObject>

GetImage

Captures an image and makes it available on an output port.

Data Port Name Port Type Object Type
topic_name input std::string
message_out output sensor_msgs::msg::Image

GetMasks2dAction

Calls an action server that uses moveit_studio_vision_msgs::action::GetMasks2D interface and outputs the results on the masks2d port.

Data Port Name Port Type Object Type
image Input sensor_msgs::msg::Image
action_name Input std::string
valid_classes Input std::vector<std::string>
max_nms_iou Input double
min_relative_area Input double
max_relative_area Input double
timeout_sec Input double
masks2d Output std::vector<moveit_studio_vision_msgs::msg::Mask2D>

GetMasks3DFromMasks2D

Backprojects a number of image masks onto a point cloud with a camera model. Converts 2D masks into 3D masks.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
masks2d input std::vector<moveit_studio_vision_msgs::msg::Mask2D>
camera_info input sensor_msgs::msg::CameraInfo
masks3d output std::vector<moveit_studio_vision_msgs::msg::Mask3D>

GetPointCloud

Captures a point cloud and makes it available on an output port.

Data Port Name Port Type Object Type
topic_name input std::string
message_out output sensor_msgs::msg::PointCloud2

GetPointCloudFromMasks

Gets the fragment of a point cloud for a 3D mask.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
mask3d input moveit_studio_vision_msgs::msg::Mask3D
point_cloud_fragment output sensor_msgs::msg::PointCloud2

GetPoseFromPixelCoords

Given an ordered point cloud and a vector of normalized pixel XY coordinates, outputs a vector of stamped poses corresponding to points normal to the selected coordinates.

This assumes that the pixel XY coordinates are normalized, i.e., have values in the range [0..1] relative to the image's height and width. Additionally, the output poses always align the Z axis with the normal of the plane by using the smallest eigenvalue of a planar patch with radius neighbor_radius around each selected point. These Z axes point towards the origin of the point cloud frame.

Data Port Name Port Type Object Type
point_cloud input sensor_msgs::msg::PointCloud2
pixel_coords input std::vector<geometry_msgs::msg::PointStamped>
downsample_fraction input double
neighbor_radius input double
output_poses output std::vector<geometry_msgs::msg::PoseStamped>

GetSyncedImageAndPointCloud

Get data from time-synchronized image and point cloud topics and populate them in output ports.

Data Port Name Port Type Object Type
rgb_camera_info_topic_name Input std::string
rgb_image_topic_name Input std::string
point_cloud_topic_name Input std::string
point_cloud Output sensor_msgs::msg::PointCloud2
rgb_image Output sensor_msgs::msg::Image
rgb_camera_info Output sensor_msgs::msg::CameraInfo

GetSyncedImages

Get data from time-synchronized image topics and populate them in output ports.

Data Port Name Port Type Object Type
camera_info_1_topic_name Input std::string
image_1_topic_name Input std::string
camera_info_2_topic_name Input std::string
image_2_topic_name Input std::string
image_1 Output sensor_msgs::msg::Image
camera_info_1 Output sensor_msgs::msg::CameraInfo
image_2 Output sensor_msgs::msg::Image
camera_info_2 Output sensor_msgs::msg::CameraInfo

LoadImageFromFile

Loads an image from a file and writes it to an output data port.

Data Port Name Port Type Object Type
file_path input std::string
frame_id input std::string
image output sensor_msgs::msg::Image

LoadPointCloudFromFile

Loads a point cloud from a .pcd or .stl file, optionally recolors it, and writes it to an output data port.

The scale, num_sampled_points, and random_seed variables are used only when reading an .stl file, since these parameters control sampling the point cloud from a mesh.

Data Port Name Port Type Object Type
file_path input std::string
frame_id input std::string
scale input float
num_sampled_points input size_t
random_seed input size_t
color input std::vector<uint8_t>
point_cloud output sensor_msgs::msg::PointCloud2

PublishPointCloud

Publishes a point cloud on a ROS topic (typically used for debugging purposes).

Data Port Name Port Type Object Type
point_cloud_topic Input std::string
point_cloud Input sensor_msgs::msg::PointCloud2

SaveImageToFile

Save the contents of an image on the blackboard to a file. Filename will follow the syntax of file_prefix_YYYYMMDD_HHMMSS.png.

Data Port Name Port Type Object Type
file_path input std::string
file_prefix input std::string
image input sensor_msgs::msg::Image

SavePointCloudToFile

Save the contents of a point cloud on the blackboard to a pcd file using the pcl::PointXYZRGB point type. Filename will follow the syntax of file_prefix_YYYYMMDD_HHMMSS.pcd.

Data Port Name Port Type Object Type
file_path input std::string
file_prefix input std::string
point_cloud input sensor_msgs::msg::PointCloud2

SendPointCloudToUI

Crop and filter a point cloud using MoveIt's sensor configuration, then publish it to a topic monitored by the UI.

Will clean up the point cloud, transform it to the "world" frame (which is currently required by the MoveIt Studio web app), and publish the result to the appropriate topic for the provided sensor to update the occupancy map. Will also convert the point cloud to ASCII PCD format to send relevant data through to the UI.

The UUID parameter can be used to track the pointcloud request through to other behaviors, if required. It is an optional input port, and if unset then the published message will have an empty string in its UUID field.

NOTE: No validation is done on the value of the UUID, so any string that is provided (including the empty string) will be set on the output port.

Data Port Name Port Type Object Type
point_cloud Input sensor_msgs::msg::PointCloud2
sensor_name Input std::string
pcd_topic Input std::string
point_cloud_uuid Input std::string

TransformPointCloud

Transforms a point cloud given an input pose in the same frame as the point cloud.

The frame IDs of the input point cloud and transform pose must match, or this Behavior will fail. The output point cloud will similarly be with respect to this frame.

Data Port Name Port Type Object Type
input_cloud input sensor_msgs::msg::PointCloud2
transform_pose input geometry_msgs::msg::PoseStamped
output_cloud output sensor_msgs::msg::PointCloud2

TransformPointCloudFrame

Transforms a point cloud to a target coordinate frame.

Data Port Name Port Type Object Type
input_cloud input sensor_msgs::msg::PointCloud2
target_frame input std::string
output_cloud output sensor_msgs::msg::PointCloud2

Visualization Behaviors

PublishMarkers

Publishes a set of markers to the UI for visualization.

Data Port Name Port Type Object Type
marker_poses input std::vector<geometry_msgs::msg::PoseStamped>
marker_type input std::string
marker_scale_xyz input std::vector<double>
marker_namespace input std::string