add fallback to bt

src/brain/config/bt_vision_grasp.params.yaml

@@ -215,6 +215,30 @@
 
     '
 - name: s11_right_arm_pre_cam_take_photo
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
+    
+    '
+- name: s12_gripper_open
+  params: 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s13_right_arm_pre_cam_take_photo
   params: 'body_id: 2
 
     data_type: 2

src/brain/config/bt_vision_grasp.xml

@@ -1,7 +1,10 @@
 <root BTCPP_format="4" main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <Sequence name="root">
-      <SubTree ID="vision_grasp_subtree"/>
+      <Fallback name="fallback_main_action">
+        <SubTree ID="vision_grasp_subtree"/>
+        <SubTree ID="recovery_1_subtree"/>
+      </Fallback>
       <SubTree ID="putdown_subtree"/>
     </Sequence>
   </BehaviorTree>
@@ -56,4 +59,15 @@
       </RetryUntilSuccessful>
     </Sequence>
   </BehaviorTree>
+
+  <BehaviorTree ID="recovery_1_subtree">
+    <Sequence name="recovery_1_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="3">
+        <Sequence> 
+          <GripperCmd_H name="s12_gripper_open" />
+          <Arm_H name="s13_right_arm_pre_cam_take_photo" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
 </root>