MoveIt Studio Behavior  2.9.2
Core Behaviors for the MoveIt Studio Application
moveit_studio::behaviors Namespace Reference

Namespaces

 affordance_transforms
 
 wrench_measurement
 

Classes

class  AveragePoseStamped
 When called multiple times in a loop this behavior will calculate the average of a Pose Stamped ROS message. 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. More...
 
class  ActivateControllers
 Activate controllers, whose names are set by the "controller_names" parameter. More...
 
class  AddPoseStampedToVector
 Adds a Cartesian pose to a sequence of Cartesian poses and stores the sequence to the blackboard. More...
 
class  AppendOrientationConstraint
 Appends an orientation constraint (based on the values specified in the input port's yaml file) to an existing constraints message. More...
 
class  BiasedCoinFlip
 Simulates flipping a biased coin with the specified probability of success provided via the input port. More...
 
class  BreakpointSubscriber
 A behavior that subscribes to a topic that can be used for pausing an objective during execution to allow introspection. This behavior will listen on the configured topic for a True/False message which will cause it to continue or abort from a breakpoint that is included in an objective. More...
 
class  CalculatePoseOffset
 Calculates the offset transform from source_pose to destination_pose. This can be used to measure the distance between two poses and returns the result relative to the source_pose. More...
 
class  CallTriggerService
 Call a service that accepts a std_srvs/srv/Trigger message. The name of the service is set through the "service_name" parameter. More...
 
class  CreateBehaviorTreeFromTaskPlan
 Create and execute a Behavior Tree from a task plan. action_mappings_file is a path to a YAML file that provides mappings between actions in the PDDL domain and Subtrees that can actually be executed. The resulting Behavior Tree will be saved in the Objectives directory as <behavior_tree_name>.xml. More...
 
class  CreateCollisionObject
 
class  CreateJointState
 Create a sensor_msgs::msg::JointState and writes it to the Blackboard. More...
 
class  CreateStampedPose
 Create a geometry_msgs::msg::PoseStamped and writes it to the Blackboard. More...
 
class  EditWaypoint
 Use the /edit_waypoints service to save the robot's current state as a new named waypoint or erase an existing waypoint. The name of the waypoint to save or delete is set through the "waypoint_name" behavior parameter. The operation to perform on the waypoint is set through the "waypoint_operation" behavior parameter, which must be set to either "save" or "erase". More...
 
class  ExecuteFollowJointTrajectory
 Accepts a JointTrajectory message via an input data port, and executes it by sending a goal to the specified FollowJointTrajectory action server. More...
 
struct  ObjectWithDistance
 Associates a GraspableObject with a distance metric. More...
 
class  GetClosestObjectToPose
 Given a collection of GraspableObjects, find the one that's closest to the provided pose. More...
 
class  GetCurrentPlanningScene
 Get the current planning scene state from the MoveIt PlanningSceneMonitor by sending a /get_planning_scene service request. More...
 
class  GetDrawerAxisFromSelection
 Given an input PoseStamped representing a grasp pose selected on a drawer handle, output three PoseStampeds that define a screw motion to open the drawer. More...
 
class  GetHingeAxisFromSurfaceSelection
 Arrange input poses into the order expected by the affordance template behavior. More...
 
class  GetLatestTransform
 Gets the latest transform from the robot model root to a frame specified as an input parameter to this behavior. More...
 
class  GetPlanUsingTAMP
 Perform Task and Motion Planning (TAMP) to generate a plan, given a PDDL problem. The PDDL domain is assumed to exist in the domain expert (for this Behavior to get a plan from a task planner, this Behavior will retrieve the domain from the domain expert). More...
 
class  InitializeMotionConstraints
 Creates a shared pointer to a new moveit_msgs::msg::Constraints and writes it to the Blackboard. More...
 
class  IsConstraintSatisfied
 Check if the robot's current state satisfies a visibility kinematic constraint relative to an object. More...
 
class  IsForceWithinThreshold
 Checks for wrench messages from a topic and report failure if it exceed a threshold value. More...
 
class  IsUserAvailable
 Checks for the presence of a user interface by checking if the /trajectory_bridge ROS node exists. More...
 
class  LoadJointTrajectoryFromYaml
 Creates and outputs a ROS JointTrajectory message from a given input yaml file. More...
 
class  LoadObjectiveParameters
 Loads the configuration parameters for a given objective. The configuration file name is given as an input port parameters to this behavior. The parameters are loaded once per objective execution. To reload the parameter from the file, just execute the objective again. More...
 
class  LoadPosesFromYaml
 Loads a vector of Poses from a file, converts it to a vector of ROS geometry_msgs/Pose messages, and writes it to an output data port. More...
 
class  LogMessage
 Logs a user specified message via the LoggerROS class. ROS 2 log severity of message specified by log_level. More...
 
class  ModifyObjectInPlanningScene
 Uses the service provided by the ApplyPlanningScene MoveGroup capability plugin to add a collision object representing a GraspableObject to the planning scene. More...
 
class  MoveGripperAction
 Actuate a gripper through its driver node's GripperCommand action. Given the name of the action topic and a target gripper position, move the gripper to the specified position. More...
 
class  MoveToJointState
 Given a JointState message and a planning group, move the group to the given joint values. More...
 
class  MoveToPose
 Given a stamped pose message, move to that pose. More...
 
class  MoveToWaypoint
 Given a named waypoint, move to that waypoint. More...
 
class  PlanCartesianPath
 Given a Cartesian-space path, plan a joint-space trajectory to move the robot tip along the path. More...
 
class  ResetPlanningSceneObjects
 Uses the service provided by the ApplyPlanningScene MoveGroup capability plugin to remove all objects which were added to the planning scene, including objects that are attached to the robot. More...
 
class  RetrieveWaypoint
 Given a named waypoint, sends a service request to the Agent WaypointManager to retrieve the joint state associated with that waypoint. More...
 
class  SaveCurrentState
 Use the /get_planning_scene service from move_group to save the robot's current state. More...
 
class  SaveJointTrajectoryToYaml
 Saves data from a ROS JointTrajectory message to a yaml file. More...
 
class  StopwatchBegin
 Saves the current epoch time as a timepoint to a data port. More...
 
class  StopwatchEnd
 Measure the difference between an input timepoint and the current timepoint, and emit a log message which states the time elapsed. More...
 
class  TransformPose
 Transforms a stamped pose given an input translation and orientation. More...
 
class  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. More...
 
class  TransformPoseFromYaml
 Transforms a stamped pose given an input yaml file that contains the translation and orientation that should be applied to the input. NOTE: The pose_parameters port is normally provided by a LoadObjectiveParameter Behavior. More...
 
class  UpdateAdmittanceController
 Updates the parameters of an existing admittance controller. More...
 
class  ValidateTrajectory
 Checks if a joint trajectory is valid, given a PlanningScene. More...
 
class  WaitForDuration
 Wait for a specified duration before succeeding. More...
 
class  WriteCalibratedPoseToYAML
 Write pose (x,y,z, roll, pitch, yaw) to YAML file. This behavior is meant to be called after the Calibrate Pose Action. TODO: Make this behavior more generic. Note: The behavior saves the calibrated_pose into the ~/.config/moveit_studio/calibration folder. More...
 
class  WritePoseToYAML
 Write contents of a geometry_msgs::msg::Pose (x,y,z,qx,qy,qz.qw) to YAML file. Note: The behavior saves the pose into the ~/.config/moveit_studio/robot_config/objectives folder. More...
 
class  GetIntFromUser
 Given a parameter name, send a service request to the Objective Server to retrieve user input value of type int for the parameter named parameter_name. More...
 
class  GetJointStateFromUser
 Given a parameter name, send a service request to the Objective Server to retrieve user input value of type sensor_msgs::msg::JointState for the parameter named parameter_name. More...
 
class  GetParameterValueFromUser
 This is a template class to get parameter values which are stored in a map in the Objective Server node. The map contains parameter overrides which are specified when creating the DoObjectiveSequence goal. Given a parameter name, send a service request to the Objective Server to retrieve user input value for the parameter named parameter_name. More...
 
class  GetPoseFromUser
 Given a parameter name, send a service request to the Objective Server to retrieve user input value of type geometry_msgs::msg::PoseStamped for the parameter named parameter_name. More...
 
class  GetStringFromUser
 Given a parameter name, send a service request to the Objective Server to retrieve user input value of type string for the parameter named parameter_name. More...
 
class  ExecuteMTCTask
 Takes an MTC Solution message 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. More...
 
class  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. More...
 
class  PlanMTCTask
 Takes a shared pointer to an existing MTC Task object via an input data port, plans the Task, and sets the solution with the lowest overall cost as an output data port. A service client sends over all the solutions to the MTC Solution Manager node which can be used for debugging. More...
 
class  PushToSolutionQueue
 Push a new MTC solution to the solution queue. More...
 
class  SplitMTCSolution
 Given an MTC Solution message and an index, create two new MTC Solution messages by splitting the subtrajectories of the input Solution at the specified index. Outputs the new Solutions onto output data ports. More...
 
class  WaitAndPopSolutionQueue
 Pops the MTC solution queue to get the next solution to be processed. More...
 
class  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. More...
 
class  SetupMTCAffordanceTemplate
 A MTC-based behavior to execute motion based on an affordance template (AT). More...
 
class  SetupMTCApproachGrasp
 Given an existing MTC Task object and a target object, appends MTC stages to describe a motion plan to approach the object. More...
 
class  SetupMTCCartesianMoveToJointState
 Given an existing MTC Task object and a joint state, appends MTC stages to describe a cartesian motion plan to that joint state. More...
 
class  SetupMTCCartesianSequence
 Given an existing MTC Task object and a sequence of target poses, appends MTC stages to plan a sequence of cartesian motions between the poses. More...
 
class  SetupMTCCurrentState
 Given an existing MTC Task object, appends an MTC CurrentState Stage to the Task. More...
 
class  SetupMTCFixedJointState
 Given an existing MTC Task object, appends an MTC FixedState Stage to the Task. More...
 
class  SetupMTCFromSolution
 Given an existing MTC Task object, appends an MTC Stage to the Task that initializes it with the final planning scene of a given solution. More...
 
class  SetupMTCGenerateCuboidGrasps
 Given an existing MTC Task object and a target object, appends MTC stages to generate cuboid grasp poses. More...
 
class  SetupMTCGenerateVacuumGrasps
 Given an existing MTC Task object and a target object, appends MTC stages to generate vacuum grasp poses on its planar surfaces. More...
 
class  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. More...
 
class  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. More...
 
class  SetupMTCMoveToJointState
 Given an existing MTC Task object and a joint state, appends MTC stages to describe a freespace motion plan to that joint state. More...
 
class  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. More...
 
class  SetupMTCMoveToPose
 Given an existing MTC Task object and a target pose, appends MTC stages to describe a freespace motion plan to that target pose. More...
 
class  SetupMTCOpenLeverHandleDoor
 Configures MTC stages to open a lever-handled door by turning the handle and pushing the door away from the robot. More...
 
class  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. More...
 
class  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. More...
 
class  SetupMTCUpdateGroupCollisionRule
 Given an MTC Task, append an MTC Stage 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. More...
 
class  CoreBehaviorsLoader
 
class  TeleoperateBase
 This is a special Behavior to enable manual teleoperation using MoveIt Servo through the Objective Server. More...
 
class  TeleoperateJointJog
 Specialization of TeleoperateBase for joint jog commands. More...
 
class  TeleoperateTwist
 Specialization of TeleoperateBase for Cartesian twist commands. More...
 
class  CalibratePoseAction
 Calls a robot_calibration_msgs::action::CalibratePose action server and outputs the results on the calibrated_poses port. NOTE: For now this behavior only supports sending a single frame for calibration. More...
 
class  CheckCuboidSimilarity
 Check if two GraspableObjects are similar within some tolerance. More...
 
class  ClearSnapshot
 Sends a request to clear the existing Octomap and Pointcloud snapshots. More...
 
class  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. More...
 
class  DetectApriltags
 Detects AprilTag markers from an image. More...
 
class  FindMaskedObjects
 Finds objects by segmenting a point cloud using a set of 2D mask images. More...
 
class  FindSingularCuboids
 Finds well-singulated cuboids supported by a surface within a point cloud. More...
 
class  GetAffordancePoses
 Returns a vector of affordance poses relative to an input GraspableObject. More...
 
class  GetCameraInfo
 Capture camera information. The name of the topic containing the camera information is set through the "topic_name" parameter, and the resulting information is available on the "message_out" output port. More...
 
class  GetDetectionPose
 Gets the stamped pose of an object detection given a label or ID, if one exists. More...
 
class  GetDoorHandlePose
 This class represents a behavior that can calculate the pose of a door lever handle along with its height and length. The convention is that the Z-axis of the pose represents the axis of rotation of the handle. The X-axis points along the handle and points toward the door hinge. More...
 
class  GetImage
 Capture an image. The name of the topic containing the image is set through the "topic_name" parameter, and the resulting image is available on the "message_out" output port. More...
 
class  GetMasks2DAction
 Calls an action server that uses moveit_studio_vision_msgs::action::GetMasks2D interface and outputs the results on the masks2d port. More...
 
class  GetMasks3DFromMasks2D
 Backprojects a number of image masks onto a point cloud with a camera model. More...
 
class  GetPointCloud
 Capture a point cloud. The name of the topic containing the point cloud is set through the "topic_name" parameter, and the resulting point cloud is available on the "message_out" output port. More...
 
class  GetPointCloudFromMask3D
 Gets the fragment of a point cloud for a 3D mask. More...
 
class  GetSynchronizedCameraTopics
 Get different camera topics time-synchronized and populate them in output ports. More...
 
class  LoadImageFromFile
 Loads an image from a file and writes it to an output data port. More...
 
class  LoadPointCloudFromFile
 Loads a point cloud from a .pcd file and write it to an output data port. More...
 
class  PublishPointCloud
 Publish a point cloud. More...
 
class  SaveImageToFile
 Save an image to disk as a .png file. The filename will follow the syntax of file_prefix_YYYYMMDD_HHMMSS.png. More...
 
class  SavePointCloudToFile
 Save a point cloud .pcd file to disk. The filename will follow the syntax of file_prefix_YYYYMMDD_HHMMSS.pcd. More...
 
class  SendPointCloudToUI
 Crop and filter a point cloud using MoveIt's sensor configuration, then publish it to a topic monitored by the UI. More...
 
class  UpdatePlanningSceneService
 Uses the service advertised by the PointCloudServiceOctomapUpdater plugin to apply a point cloud to the planning scene collision octomap and wait until the update has been applied. More...
 

Typedefs

using SetStringArray = moveit_studio_agent_msgs::srv::SetStringArray
 
using Trigger = std_srvs::srv::Trigger
 
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory
 
using GetPlanningScene = moveit_msgs::srv::GetPlanningScene
 
using ApplyPlanningScene = moveit_msgs::srv::ApplyPlanningScene
 
using GripperCommand = control_msgs::action::GripperCommand
 
using ExecuteTaskSolution = moveit_task_constructor_msgs::action::ExecuteTaskSolution
 
using RetrieveBehaviorParameter = moveit_studio_agent_msgs::srv::RetrieveBehaviorParameter
 
using StoreMtcIntrospectionData = moveit_studio_agent_msgs::srv::StoreMtcIntrospectionData
 
using CalibratePose = robot_calibration_msgs::action::CalibratePose
 
using GetMasks2D = moveit_studio_vision_msgs::action::GetMasks2D
 
using SendPointCloud2 = moveit_studio_agent_msgs::srv::SendPointCloud2
 

Functions

std::string generateBehaviorTreeXmlPrefix (const std::string &behavior_tree_name)
 
std::string generateBehaviorTreeXmlSuffix ()
 
std::vector< std::string > actionStringToWordVector (std::string action_str)
 
fp::Result< std::vector< ObjectWithDistance > > calculateClosestObjectToPose (const tf2_ros::Buffer &buffer, const std::vector< moveit_studio_vision_msgs::msg::GraspableObject > &objects, const geometry_msgs::msg::PoseStamped &pose)
 Get the closest object to the provided pose. More...
 
fp::Result< std::vector< moveit_task_constructor_msgs::msg::Solution > > splitSolutionAtIndex (const moveit_task_constructor_msgs::msg::Solution solution_in, const std::size_t index)
 Split an MTC Solution message in two by dividing its vector of subtrajectories at the specified index. More...
 
fp::Result< bool > isObjectSimilar (const tf2_ros::Buffer &buffer, const moveit_studio_vision_msgs::msg::GraspableObject &input, const moveit_studio_vision_msgs::msg::GraspableObject &reference, const std::string &base_frame, const double distance_threshold, const double orientation_threshold)
 Helper function to check if two GraspableObjects are similar within some tolerance. More...
 
fp::Result< std::vector< ObjectWithDistance > > calculateClosestObjectToPose (const tf2_ros::Buffer &buffer, const std::vector< GraspableObject > &objects, const geometry_msgs::msg::PoseStamped &pose)
 
std::string getYAMLFileName ()
 
fp::Result< bool > isObjectSimilar (const tf2_ros::Buffer &buffer, const GraspableObject &input, const GraspableObject &reference, const std::string &base_frame, const double distance_threshold, const double orientation_threshold)
 

Variables

constexpr auto kClearOctomapServiceName = "clear_octomap"
 Constant service name used by the MoveIt ClearOctomap MoveGroup capability. More...
 

Typedef Documentation

◆ ApplyPlanningScene

using moveit_studio::behaviors::ApplyPlanningScene = typedef moveit_msgs::srv::ApplyPlanningScene

◆ CalibratePose

using moveit_studio::behaviors::CalibratePose = typedef robot_calibration_msgs::action::CalibratePose

◆ ExecuteTaskSolution

typedef moveit_task_constructor_msgs::action::ExecuteTaskSolution moveit_studio::behaviors::ExecuteTaskSolution

◆ FollowJointTrajectory

using moveit_studio::behaviors::FollowJointTrajectory = typedef control_msgs::action::FollowJointTrajectory

◆ GetMasks2D

using moveit_studio::behaviors::GetMasks2D = typedef moveit_studio_vision_msgs::action::GetMasks2D

◆ GetPlanningScene

using moveit_studio::behaviors::GetPlanningScene = typedef moveit_msgs::srv::GetPlanningScene

◆ GripperCommand

using moveit_studio::behaviors::GripperCommand = typedef control_msgs::action::GripperCommand

◆ RetrieveBehaviorParameter

typedef moveit_studio_agent_msgs::srv::RetrieveBehaviorParameter moveit_studio::behaviors::RetrieveBehaviorParameter

◆ SendPointCloud2

using moveit_studio::behaviors::SendPointCloud2 = typedef moveit_studio_agent_msgs::srv::SendPointCloud2

◆ SetStringArray

using moveit_studio::behaviors::SetStringArray = typedef moveit_studio_agent_msgs::srv::SetStringArray

◆ StoreMtcIntrospectionData

using moveit_studio::behaviors::StoreMtcIntrospectionData = typedef moveit_studio_agent_msgs::srv::StoreMtcIntrospectionData

◆ Trigger

using moveit_studio::behaviors::Trigger = typedef std_srvs::srv::Trigger

Function Documentation

◆ actionStringToWordVector()

std::vector<std::string> moveit_studio::behaviors::actionStringToWordVector ( std::string  action_str)
inline

◆ calculateClosestObjectToPose() [1/2]

fp::Result<std::vector<ObjectWithDistance> > moveit_studio::behaviors::calculateClosestObjectToPose ( const tf2_ros::Buffer &  buffer,
const std::vector< GraspableObject > &  objects,
const geometry_msgs::msg::PoseStamped &  pose 
)

◆ calculateClosestObjectToPose() [2/2]

fp::Result<std::vector<ObjectWithDistance> > moveit_studio::behaviors::calculateClosestObjectToPose ( const tf2_ros::Buffer &  buffer,
const std::vector< moveit_studio_vision_msgs::msg::GraspableObject > &  objects,
const geometry_msgs::msg::PoseStamped &  pose 
)

Get the closest object to the provided pose.

Parameters
objectsCollection of objects to evaluate.
posePose to use for distance comparison.
Returns
If successful, returns a vector containing the input objects sorted in ascending order by distance to the input pose. Returns a failure state if no qualifying object was found.

◆ generateBehaviorTreeXmlPrefix()

std::string moveit_studio::behaviors::generateBehaviorTreeXmlPrefix ( const std::string &  behavior_tree_name)
inline

◆ generateBehaviorTreeXmlSuffix()

std::string moveit_studio::behaviors::generateBehaviorTreeXmlSuffix ( )
inline

◆ getYAMLFileName()

std::string moveit_studio::behaviors::getYAMLFileName ( )

◆ isObjectSimilar() [1/2]

fp::Result<bool> moveit_studio::behaviors::isObjectSimilar ( const tf2_ros::Buffer &  buffer,
const GraspableObject &  input,
const GraspableObject &  reference,
const std::string &  base_frame,
const double  distance_threshold,
const double  orientation_threshold 
)

◆ isObjectSimilar() [2/2]

fp::Result<bool> moveit_studio::behaviors::isObjectSimilar ( const tf2_ros::Buffer &  buffer,
const moveit_studio_vision_msgs::msg::GraspableObject &  input,
const moveit_studio_vision_msgs::msg::GraspableObject &  reference,
const std::string &  base_frame,
const double  distance_threshold,
const double  orientation_threshold 
)

Helper function to check if two GraspableObjects are similar within some tolerance.

Parameters
bufferTF buffer, used to look up object poses relative to base_frame at the timestamp in the object header.
inputObject being compared. Must represent a single cuboid.
referenceObject being compared against. Must represent a single cuboid.
base_frameCommon fixed frame of reference between the two objects.
distance_thresholdObjects are considered dissimilar if their centroids are more than this distance away from each other.
orientation_thresholdObjects are considered dissimilar if the orientations of their centroids differ by this magnitude in radians.
Returns
If object similarity could be calculated without errors, returns true if the objects are similar and false if they are dissimilar. Returns an error message if the objects could not be compared successfully.

◆ splitSolutionAtIndex()

fp::Result< std::vector< moveit_task_constructor_msgs::msg::Solution > > moveit_studio::behaviors::splitSolutionAtIndex ( const moveit_task_constructor_msgs::msg::Solution  solution_in,
const std::size_t  index 
)

Split an MTC Solution message in two by dividing its vector of subtrajectories at the specified index.

The subtrajectory at the index will go into the second output MTC Solution. The task IDs of the new solutions will be set to match the task ID of the input solution.

The start_scene field of each new solution will be set to a new empty moveit_msgs::msg::PlanningScene message, since the MTC ExecuteSolution MoveGroup capability actually does not use this initial scene state when executing the solution. This is an implementation compromise – while a more thorough approach would be to calculate a new start_scene for the second half of the split soltuion by applying each scene diff from the subtrajectories in the first half of the split solution to the start_scene from the original solution, we would need to create an instance of a MoveIt PlanningScene object to do that, which introduces a lot of extra overhead like retrieving the robot model and SRDF.

The sub_solution field of each new solution will be set to an empty vector of moveit_task_constructor_msgs::msg::SubSolution messages. This is another implementation compromise – since MTC apparently does not need the subsolutions to execute each of the overall solution's subtrajectories, and there isn't a clear way to associate these subsolutions with a subset of the subtrajectories, we skip the step of copying subsolutions in to the halves of the split solution.

Parameters
solution_inInput MTC Solution message
indexThe index where the subtrajectories will be split.
Returns
An fp::Result wrapping a vector of MTC solutions. If there is no error, the vector will contain two MTC Solution messages, where the first one contains the portion of the input Solution that was before the split index and the second one contains the remainder of the solution. Returns an InvalidArgument error if the index would be out of range for the vector of subtrajectories.

Variable Documentation

◆ kClearOctomapServiceName

constexpr auto moveit_studio::behaviors::kClearOctomapServiceName = "clear_octomap"
constexpr

Constant service name used by the MoveIt ClearOctomap MoveGroup capability.