Merge pull request #78 from PickNikRobotics/7.0-sync

7.0 sync

Author: jeremydbronson

src/lab_sim/objectives/add_waypoints_to_mtc_task.xml

@@ -5,12 +5,15 @@
     ID="Add Waypoints to MTC Task"
     _subtreeOnly="true"
     _description=""
+    joint_group_name="manipulator"
+    planner_interface="moveit_default"
+    mtc_task="{mtc_task}"
   >
     <Control ID="Sequence">
       <Action
         ID="RetrieveWaypoint"
         waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Above Pick Cube"
+        waypoint_name="Look at Table"
         joint_group_name="{joint_group_name}"
       />
       <Action
@@ -24,7 +27,7 @@
       <Action
         ID="RetrieveWaypoint"
         waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Above Place Cube"
+        waypoint_name="Workspace Right"
         joint_group_name="{joint_group_name}"
       />
       <Action
@@ -40,10 +43,11 @@
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
       <input_port name="joint_group_name" default="manipulator" />
-      <inout_port name="mtc_task" default="{mtc_task}" />
       <input_port name="planner_interface" default="moveit_default" />
+      <inout_port name="mtc_task" default="{mtc_task}" />
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="false" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>

src/lab_sim/objectives/apriltag_pick_object.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
   <BehaviorTree
@@ -21,11 +22,13 @@
         ID="GetCameraInfo"
         topic_name="/wrist_camera/camera_info"
         message_out="{camera_info}"
+        timeout_sec="5.000000"
       />
       <Action
         ID="GetImage"
         topic_name="/wrist_camera/color"
         message_out="{image}"
+        timeout_sec="5.000000"
       />
       <SubTree
         ID="Get AprilTag Pose from Image"
@@ -47,7 +50,11 @@
         target_pose="{view_pose}"
         move_to_pose_solution="{move_to_pose_solution}"
       />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{move_to_pose_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
       <SubTree
         ID="Get AprilTag Pose from Image"
         image="{image}"
@@ -66,26 +73,36 @@
       <Action
         ID="LoadObjectiveParameters"
         config_file_name="pick_object_config.yaml"
+        parameters="{parameters}"
       />
       <Action
         ID="InitializeMTCTask"
         task_id="pick_object"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
         task="{pick_object_task}"
       />
-      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" />
+      <Action
+        ID="SetupMTCCurrentState"
+        task="{pick_object_task}"
+        skip_collision_check="false"
+      />
       <Action
         ID="SetupMTCPickObject"
         grasp_pose="{offset_pose}"
         task="{pick_object_task}"
         parameters="{parameters}"
+        monitored_stage="current state"
       />
       <Action
         ID="PlanMTCTask"
         solution="{pick_object_solution}"
         task="{pick_object_task}"
       />
-      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{pick_object_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>

src/lab_sim/objectives/create_pose_vector.xml

@@ -18,17 +18,6 @@
         vector="{target_poses}"
         input="{stamped_pose}"
       />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0.75;0.5;0.9"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
@@ -40,28 +29,6 @@
         vector="{target_poses}"
         input="{stamped_pose}"
       />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0.25;0.5;0.7"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0;0.5;0.6"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
@@ -73,28 +40,6 @@
         vector="{target_poses}"
         input="{stamped_pose}"
       />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.5;0.5;0.8"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.75;0.5;0.9"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"