Dualtip ml objectives

.gitmodules

@@ -16,3 +16,9 @@
 [submodule "src/dependencies/moveit_studio_ur_pstop_manager"]
 	path = src/dependencies/moveit_studio_ur_pstop_manager
 	url = [email protected]:PickNikRobotics/moveit_studio_ur_pstop_manager.git
+[submodule "src/dependencies/moveit_pro_clipseg"]
+	path = src/dependencies/moveit_pro_clipseg
+	url = https://github.com/PickNikRobotics/moveit_pro_clipseg.git
+[submodule "src/dependencies/serial"]
+	path = src/dependencies/serial
+	url = https://github.com/tylerjw/serial.git

src/dual_arm_mobile_hw/launch/agent_bridge.launch.xml

@@ -6,7 +6,6 @@
   <include
     file="$(find-pkg-share dual_arm_mobile_hw)/launch/rs_cameras.launch.xml"
   >
-    <arg name="scene_camera_serial_no" value="'135322061686'" />
     <arg name="wrist_camera_serial_no" value="'211122063503'" />
   </include>
 </launch>

src/dual_arm_mobile_hw/launch/rs_cameras.launch.xml

@@ -4,6 +4,9 @@
   <arg name="wrist_camera_serial_no" default="'0'" />
 
   <!-- Scene camera  -->
+  <!-- Commented out: Scene camera with serial 135322061686 is not connected.
+       Uncomment below and set scene_camera_serial_no to the correct serial when the camera is available. -->
+  <!--
   <include file="$(find-pkg-share dual_arm_mobile_hw)/launch/pro_rs_launch.py">
     <arg name="camera_name" value="scene_camera" />
     <arg name="serial_no" value="$(var scene_camera_serial_no)" />
@@ -14,7 +17,6 @@
     <arg name="pointcloud.ordered_pc" value="True" />
     <arg name="decimation_filter.enable" value="False" />
 
-    <!--# information about the topics the camera publishes the raw image and info-->
     <arg name="rgb_info" value="/scene_camera/color/camera_info" />
     <arg name="rgb_image" value="/scene_camera/color/image_raw" />
     <arg
@@ -34,6 +36,7 @@
       value="/scene_camera/depth/image_rect_raw"
     />
   </include>
+  -->
   <!-- Wrist camera  -->
   <include file="$(find-pkg-share dual_arm_mobile_hw)/launch/pro_rs_launch.py">
     <arg name="camera_name" value="left_wrist_mounted_camera" />

src/dual_arm_mobile_hw/objectives/getleftrightpickposefrombottlepose.xml

@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="GetLeftRightPickPoseFromBottlePose"
+>
+  <!--//////////-->
+  <BehaviorTree ID="GetLeftRightPickPoseFromBottlePose">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        stamped_pose="{table_pose}"
+        orientation_xyzw="0.5;0.5;0.5;0.5"
+        name="Hard coding the table pose for now but should be taken from qr code on the table"
+        position_xyz="0;0;0"
+      />
+      <SubTree
+        ID="GetQuaternionVectorFromPoseStamped"
+        pose_stamped_in="{table_pose}"
+        quaternion_vector="{table_quaternion_vector}"
+      />
+      <SubTree
+        ID="GetPositionVectorFromPoseStamped"
+        pose_stamped_in="{bottle_in_world_pose}"
+        position_vector="{position_vector}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        orientation_xyzw="{table_quaternion_vector}"
+        position_xyz="{position_vector}"
+        stamped_pose="{bottle_bottom_aligned_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        input_pose="{bottle_bottom_aligned_pose}"
+        output_pose="{bottle_bottom_aligned_pose}"
+        translation_xyz="0;0;0"
+        quaternion_xyzw="0;-0.7071;0;0.7071"
+      />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        orientation_xyzw="{table_quaternion_vector}"
+        position_xyz="{position_vector}"
+        stamped_pose="{bottle_bottom_aligned_left_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        input_pose="{bottle_bottom_aligned_pose}"
+        output_pose="{bottle_bottom_aligned_left_pose}"
+        translation_xyz="0;0.2;-0.1"
+        quaternion_xyzw="0;0;0;0"
+      />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        orientation_xyzw="{table_quaternion_vector}"
+        position_xyz="{position_vector}"
+        stamped_pose="{bottle_bottom_aligned_right_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        input_pose="{bottle_bottom_aligned_pose}"
+        output_pose="{bottle_bottom_aligned_right_pose}"
+        translation_xyz="0;0.1;0.16"
+        quaternion_xyzw="0;1;0;0"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="bottle_bottom_aligned_left_pose"
+        marker_size="0.100000"
+        pose="{bottle_bottom_aligned_left_pose}"
+        marker_text="bottle_bottom_aligned_left_pose"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="bottle_bottom_aligned_right_pose"
+        marker_size="0.100000"
+        pose="{bottle_bottom_aligned_right_pose}"
+        marker_text="bottle_bottom_aligned_right_pose"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="GetLeftRightPickPoseFromBottlePose">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+      <inout_port
+        name="bottle_bottom_aligned_left_pose"
+        default="{bottle_bottom_aligned_left_pose}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      >
+        left pre-grasp pose
+      </inout_port>
+      <inout_port
+        name="bottle_bottom_aligned_right_pose"
+        default="{bottle_bottom_aligned_right_pose}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      >
+        right pre-grasp pose
+      </inout_port>
+      <inout_port
+        name="bottle_in_world_pose"
+        default="{bottle_in_world_pose}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      >
+        Input for current pose of bottle in world frame
+      </inout_port>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/dual_arm_mobile_hw/objectives/getpositionvectorfromposestamped.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="GetPositionVectorFromPoseStamped">
+  <!--//////////-->
+  <BehaviorTree ID="GetPositionVectorFromPoseStamped">
+    <Control ID="Sequence" name="">
+      <Action
+        ID="UnpackPoseStampedMessage"
+        header="{header}"
+        pose_stamped="{pose_stamped_in}"
+        pose="{temp_pose_1}"
+      />
+      <Action
+        ID="UnpackPoseMessage"
+        orientation="{orientation_quat}"
+        pose="{temp_pose_1}"
+        position="{temp_position}"
+      />
+      <Action
+        ID="UnpackPointMessage"
+        point="{temp_position}"
+        x="{px}"
+        y="{py}"
+        z="{pz}"
+      />
+      <Action ID="ResetVector" vector="{position_vector}" />
+      <Action
+        ID="InsertInVector"
+        index="0"
+        output_vector="{position_vector}"
+        element="{px}"
+        input_vector="{position_vector}"
+      />
+      <Action
+        ID="InsertInVector"
+        input_vector="{position_vector}"
+        output_vector="{position_vector}"
+        element="{py}"
+        index="1"
+      />
+      <Action
+        ID="InsertInVector"
+        index="2"
+        input_vector="{position_vector}"
+        output_vector="{position_vector}"
+        element="{pz}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="GetPositionVectorFromPoseStamped">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Pose Handling" />
+      </MetadataFields>
+      <inout_port
+        name="pose_stamped_in"
+        default="{pose_stamped_in}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      />
+      <inout_port
+        name="position_vector"
+        default="{position_vector}"
+        type="std::vector&lt;double&gt;"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/dual_arm_mobile_hw/objectives/getquaternionvectorfromposestamped.xml

@@ -0,0 +1,78 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="GetQuaternionVectorFromPoseStamped"
+>
+  <!--//////////-->
+  <BehaviorTree ID="GetQuaternionVectorFromPoseStamped">
+    <Control ID="Sequence" name="">
+      <Action
+        ID="UnpackPoseStampedMessage"
+        header="{header}"
+        pose_stamped="{pose_stamped_in}"
+        pose="{temp_pose_1}"
+      />
+      <Action
+        ID="UnpackPoseMessage"
+        orientation="{orientation_quat}"
+        pose="{temp_pose_1}"
+        position="{pc_centroid_position}"
+      />
+      <Action
+        ID="UnpackQuaternionMessage"
+        quaternion="{orientation_quat}"
+        w="{qw}"
+        x="{qx}"
+        y="{qy}"
+        z="{qz}"
+      />
+      <Action ID="ResetVector" vector="{quaternion_vector}" />
+      <Action
+        ID="InsertInVector"
+        index="0"
+        input_vector="{quaternion_vector}"
+        output_vector="{quaternion_vector}"
+        element="{qx}"
+      />
+      <Action
+        ID="InsertInVector"
+        input_vector="{quaternion_vector}"
+        output_vector="{quaternion_vector}"
+        element="{qy}"
+        index="1"
+      />
+      <Action
+        ID="InsertInVector"
+        index="2"
+        input_vector="{quaternion_vector}"
+        output_vector="{quaternion_vector}"
+        element="{qz}"
+      />
+      <Action
+        ID="InsertInVector"
+        index="3"
+        input_vector="{quaternion_vector}"
+        output_vector="{quaternion_vector}"
+        element="{qw}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="GetQuaternionVectorFromPoseStamped">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Pose Handling" />
+      </MetadataFields>
+      <inout_port
+        name="pose_stamped_in"
+        default="{pose_stamped_in}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      />
+      <inout_port
+        name="quaternion_vector"
+        default="{quaternion_vector}"
+        type="std:vector&lt;double&gt;"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>