Add some SwitchUIPrimaryView Behaviors (#232)

PickNikRobotics · Mar 5, 2024 · b68b966 · b68b966
1 parent 4b544be
commit b68b966
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 19 deletions.
diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -17,19 +17,34 @@
             </Decorator>
             <!-- Joint sliders interpolate to joint state -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
-              <SubTree ID="Interpolate to Joint State"/>
+              <Control ID="Sequence">
+                <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+                <SubTree ID="Interpolate to Joint State"/>
+              </Control>
             </Decorator>
             <!-- Interactive markers move to pose -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 4">
-              <SubTree ID="Move to Pose"/>
+              <Control ID="Sequence">
+                <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+                <SubTree ID="Move to Pose"/>
+              </Control>
             </Decorator>
             <!-- Waypoint buttons move to joint state -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 3">
-              <SubTree ID="Move to Joint State"/>
+              <Control ID="Sequence">
+                <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+                <SubTree ID="Move to Joint State"/>
+              </Control>
             </Decorator>
             <!-- Cartesian and joint jogging -->
-            <Action ID="TeleoperateTwist" controller_name="servo_controller" _while="teleop_mode == 2"/>
-            <Action ID="TeleoperateJointJog" controller_name="servo_controller" _while="teleop_mode == 1"/>
+            <Control ID="Sequence" _while="teleop_mode == 2">
+              <Action ID="SwitchUIPrimaryView" primary_view_name="/wrist_mounted_camera/color/image_raw"/>
+              <Action ID="TeleoperateTwist" controller_name="servo_controller"/>
+            </Control>
+            <Control ID="Sequence" _while="teleop_mode == 1">
+              <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+              <Action ID="TeleoperateJointJog" controller_name="servo_controller"/>
+            </Control>
           </Control>
         </Decorator>
       </Control>

diff --git a/src/picknik_ur_site_config/objectives/take_snapshot.xml b/src/picknik_ur_site_config/objectives/take_snapshot.xml
@@ -1,16 +1,17 @@
 <?xml version="1.0"?>
-  <root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
-    <!-- ////////// -->
-    <BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene" _favorite="true">
-        <Control ID="Sequence">
-            <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
-            <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
-            <Control ID="Fallback" name="TryGetStringFromUser">
-                <Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}" />
-                <!-- if we fail to get the string from the user fallback to an empty value for the uuid -->
-                <Action ID="Script" code="uuid := ''"/>
-            </Control>
-            <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{uuid}"/>
-        </Control>
-    </BehaviorTree>
+<root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
+  <!-- ////////// -->
+  <BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene" _favorite="true">
+    <Control ID="Sequence">
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+      <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
+      <Control ID="Fallback" name="TryGetStringFromUser">
+        <Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}"/>
+        <!-- if we fail to get the string from the user fallback to an empty value for the uuid -->
+        <Action ID="Script" code="uuid := ''"/>
+      </Control>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{uuid}"/>
+    </Control>
+  </BehaviorTree>
 </root>