diff --git a/src/picknik_ur_base_config/objectives/standard_tree_nodes_model.xml b/src/picknik_ur_base_config/objectives/standard_tree_nodes_model.xml index d9661e94..40b524d1 100644 --- a/src/picknik_ur_base_config/objectives/standard_tree_nodes_model.xml +++ b/src/picknik_ur_base_config/objectives/standard_tree_nodes_model.xml @@ -35,10 +35,10 @@

The ParallelNode executes all its children concurrently, but not in separate threads!

Even if this may look similar to ReactiveSequence, this Control Node is the only one that can have multiple children RUNNING at the same time.

The Node is completed either when the THRESHOLD_SUCCESS or THRESHOLD_FAILURE number is reached (both configured using ports).

-

If any of the threaholds are reached, and other children are still running, they will be halted.

+

If any of the thresholds are reached, and other children are still running, they will be halted.

- Number of children which need to succeed to trigger a SUCCESS. - Number of children which need to fail to trigger a FAILURE. + Number of children which need to succeed to trigger a SUCCESS. + Number of children which need to fail to trigger a FAILURE. @@ -81,16 +81,15 @@

If the 2nd or 3d child is RUNNING and the statement changes, the RUNNING child will be stopped before starting the sibling.

- + -

Executes its child node only if the string value of a given input port is equal to the expected value.

-

If this precondition is met, the child node will be run and this node will return the status returned from the child.

+

Executes its child node only if a condition is met.

+

If the precondition is met, the child node will be run and this node will return the status returned from the child.

If the precondition is not met, this node will return the BT::NodeStatus value specified in "return_on_mismatch".

- First string value to compare. - Second string value to compare. - Status to return if value_A and value_B have different values. Can be RUNNING, SUCCESS, or FAILURE. -
+ If condition + Status to return if condition not met. Can be RUNNING, SUCCESS, or FAILURE. +

Tick the child once and return SUCCESS if the child failed or FAILURE if the child succeeded.

@@ -157,6 +156,14 @@ Value represented as a string. convertFromString must be implemented. Name of the blackboard entry where the value should be written. + + +

Introduced in BT.CPP 4 to integrate scripting language within XML

+

Allows users to read from and write to variables in the blackboard

+

Can perform operation like assignment, comparison, etc.

+
+ Code that can be parsed. +
diff --git a/src/picknik_ur_base_config/objectives/tree_nodes_model.xml b/src/picknik_ur_base_config/objectives/tree_nodes_model.xml index bcb5f73c..8934e5f7 100644 --- a/src/picknik_ur_base_config/objectives/tree_nodes_model.xml +++ b/src/picknik_ur_base_config/objectives/tree_nodes_model.xml @@ -1,14 +1,52 @@ + - - + +

- 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. + Provides functions for realtime tracking of AprilTag in published image topics. +

+

+ Parameters for tag detector are ingested parsed YAML configuration. Raw images are processed from the specified camera stream topic, + and detections are published as a vector of TransformStamped to the specified output topic. +

+

+ NOTE: This behavior will never terminate on its own. You must wrap it in a sequence that will handle halting the node.

- List of controller names to use for executing an MTC trajectory. - MoveIt Task Constructor task. + Parameters for the apriltag bt node + Target camera stream topic. + Output transform stamped topic. +
+ + +

+ Activates a controller whose name is given by the "controller_name" input port. +

+
+ The controller to activate. +
+ + +

+ 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. +

+
+ Topic the breakpoint listens to. Can be used to continue executing or halting the program to the breakpoint. +
+ + + +

+ Check if an input object is similar to another object. Succeeds if the objects are similar within the provided criteria and fails if they are not similar. +

+
+ Cuboid object to evaluate. + Cuboid object to use as a reference for comparison. + Fixed frame to use when comparing object poses. + Threshold for magnitude difference in centroid position. + Threshold for magnitude of difference in centroid orientation.
@@ -18,6 +56,16 @@

+ + + +

+ Uses 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". +

+
+ Name of the waypoint to edit. + Waypoint operation type. +
@@ -27,6 +75,44 @@ MoveIt Task Constructor plan solution. + + + +

+ Given a collection of CollisionObjects, find the one closest to the provided pose. +

+
+ Vector of moveit_msgs/CollisionObjects. + The geometry_msgs/PoseStamped used as the point of reference. + Search for objects within this distance in meters relative to the pose. + The object that is closest to the pose. +
+ + + +

+ Analyze a point cloud to find well-singulated cuboids which supported by a surface. +

+
+ Behavior parameters stored in a common configuration file for the objective. + Point cloud used as input for finding cuboids. + Vector of detected cuboids, represented as moveit_msgs/CollisionObject messages. +
+ + + +

+ 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. +

+
+ Pose of the door handle pivot point. + Pose of the tip of the door handle. + Behavior parameters stored in a common configuration file for the objective. + Point cloud used as input for finding the door handle. + Length of the door handle in meters. + Pose of the door handle. + The door handle height. +
@@ -66,6 +152,71 @@ Pose of the axis of the screw motion to open a door. Pose of the origin of the screw motion to open a door. + + + +

+ Gets the latest transform from the robot model root to a frame specified as an input parameter to this behavior. +

+
+ Behavior parameters stored in a common configuration file for the objective. + Latest transform between the target and source frames. +
+ + + +

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

+

+ Optionally takes a string uuid which can be used to identify the requester, and makes it available on an output port. The parameter is "optional", if it unset then no output will be forwarded. Note: no validation is done on the value of the UUID, so any string is provided (including the empty string) it will be set on the output port. +

+
+ Point cloud topic the behavior subscribes to. + Optional identifier for the incoming request, can be used for mapping responses + Captured point cloud in sensor_msgs::msg::PointCloud2 format. + Captured point cloud in sensor_msgs::msg::PointCloud2 format. +
+ + + +

+ Saves a single image to a file as soon as it receives a message on the topic. Filename will follow the syntax of IMAGE_TOPIC_image_raw_YYYYMMDD_HHMMSS.png +

+
+ The topic this behaviour subscribes to. + The full path to save the image in. +
+ + + +

+ 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 pointcloud_YYYYMMDD_HHMMSS.pcd +

+
+ This port expects a sensor_msgs::PointCloud2 + The full path to save the point cloud in. +
+ + + +

+ 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. +

+
+ List of controller names to use for executing an MTC trajectory. + MoveIt Task Constructor task. +
+ + + +

+ Check if the robot's current state satisfies a kinematic visibility constraint. +

+
+ The constraint is satisfied if the camera has an unobstructed view of this object. + Behavior parameters stored in a common configuration file for the objective. +
@@ -75,6 +226,14 @@ Behavior parameters stored in a common configuration file for the objective. + + + +

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

+
+

@@ -95,6 +254,16 @@ Frame ID to set for the loaded point cloud. Output point cloud message. + + + +

+ Add a collision object to the planning scene. +

+
+ The object to add to the planning scene, represented as a moveit_msgs/CollisionObject. + Name of the service advertised by the MoveIt2 ApplyPlanningScene MoveGroup capability. +
@@ -126,6 +295,25 @@ MoveIt Task Constructor task. MoveIt Task Constructor plan solution. + + + +

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

+
+ Point cloud in sensor_msgs::msg::PointCloud2 format. + Topic the point cloud is published to. +
+ + + +

+ Removes all objects which were added to the planning scene during runtime. +

+
+ Name of the service advertised by the MoveIt2 ApplyPlanningScene MoveGroup capability. +
@@ -136,6 +324,30 @@ + + + +

+ Use the "/get_planning_scene" service to save the robot's current state. +

+
+ Current robot state. +
+ + + +

+ Given a point cloud, filter it using MoveIt's settings for that sensor, convert it to ASCII PCD format, and publish it on a topic. +

+

+ 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. +

+
+ Point cloud in sensor_msgs::msg::PointCloud2 format. + The name of the sensor the point cloud was generated from + Optional identifier for the request to be published to the pcd_topic + Topic the pcd formatted point cloud is published to. +
@@ -175,6 +387,17 @@ Pose of the origin of the screw motion to open the drawer. MoveIt Task Constructor task. + + + +

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

+
+ Target joint state. + Name of the MoveIt planning group. + MoveIt Task Constructor task. +
@@ -258,6 +481,17 @@ Behavior parameters stored in a common configuration file for the objective. MoveIt Task Constructor task. + + + +

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

+
+ Behavior parameters stored in a common configuration file for the objective. + Cuboid object to grasp, represented as a moveit_msgs/CollisionObject. + MoveIt Task Constructor task. +
@@ -269,6 +503,16 @@ End-effector grasping pose. MoveIt Task Constructor task. + + + +

+ 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. +

+
+ Behavior parameters stored in a common configuration file for the objective. + MoveIt Task Constructor task. +
@@ -293,6 +537,43 @@ Name of the controller to use for servoing. + + + +

+ Updates parameters for an existing admittance controller. +

+
+ Configuration file name. +
+ + + +

+ DEPRECATED: Use UpdatePlanningSceneService and SendPointCloudToUI instead. +

+

+ Given a point cloud from the specified sensor, will sanitize the point cloud according to that sensor's params and publish the result to the relevant topic to update the occupancy map. It will also convert the resulting pointcloud to ASCII PCD format and publish it to the specified pcd_topic. +

+

+ 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 it 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 is provided (including the empty string) it will be set on the output port. +

+
+ Point cloud in sensor_msgs::msg::PointCloud2 format. + The name of the sensor the point cloud was generated from + Optional identifier for the request to be published to the pcd_topic + Topic the pcd formatted point cloud is published to. +
+ + + +

+ Updates the planning scene's collision octree using the provided point cloud, and waits until the octree has finished updating. +

+
+ Point cloud in sensor_msgs::msg::PointCloud2 format. + Name of the service advertised by the PointCloudServiceOctomapUpdater MoveIt plugin. +

@@ -305,7 +586,7 @@

- 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. + Takes a shared pointer to an MTC Solution object via an input data port, and publishes the lowest-cost trajectory in that Solution on the "/preview_solution" topic. Creates a SetBool service server on the "/execute_behavior_solution" topic and waits to receive a request containing data: true before succeeding.

MoveIt Task Constructor plan solution.