From 63b2fd231c5dd6d65accfad456a132459999b780 Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Tue, 7 Jan 2025 13:19:00 +0100
Subject: [PATCH] Fix JointDiagnostic Objective

---
 src/lab_sim/objectives/joint_diagnostic.xml   |  12 +-
 .../objectives/request_teleoperation.xml      | 161 ++++++++++++++++++
 2 files changed, 167 insertions(+), 6 deletions(-)
 create mode 100644 src/lab_sim/objectives/request_teleoperation.xml

diff --git a/src/lab_sim/objectives/joint_diagnostic.xml b/src/lab_sim/objectives/joint_diagnostic.xml
index aebb7879..74bb23d1 100644
--- a/src/lab_sim/objectives/joint_diagnostic.xml
+++ b/src/lab_sim/objectives/joint_diagnostic.xml
@@ -11,8 +11,8 @@
         ID="Move to Waypoint"
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+        link_padding="0.0"
       />
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
@@ -21,15 +21,15 @@
             ID="Move to Waypoint"
             waypoint_name="Wrist 2 Max"
             joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            planner_interface="moveit_default"
+            controller_names="/joint_trajectory_controller"
+            link_padding="0.0"
           />
           <SubTree
             ID="Move to Waypoint"
             waypoint_name="Wrist 2 Min"
             joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            planner_interface="moveit_default"
+            controller_names="/joint_trajectory_controller"
+            link_padding="0.0"
           />
         </Control>
       </Decorator>
diff --git a/src/lab_sim/objectives/request_teleoperation.xml b/src/lab_sim/objectives/request_teleoperation.xml
new file mode 100644
index 00000000..276fff00
--- /dev/null
+++ b/src/lab_sim/objectives/request_teleoperation.xml
@@ -0,0 +1,161 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
+  <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
+  <BehaviorTree
+    ID="Request Teleoperation"
+    _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
+    _favorite="false"
+    _hardcoded="false"
+    _subtreeOnly="true"
+  >
+    <Control ID="Sequence">
+      <Action ID="Script" code="teleop_mode := 0" />
+      <Control ID="Parallel" success_count="1" failure_count="1">
+        <Action
+          ID="DoTeleoperateAction"
+          enable_user_interaction="{enable_user_interaction}"
+          user_interaction_prompt="{user_interaction_prompt}"
+          initial_teleop_mode="{initial_teleop_mode}"
+          current_teleop_mode="{teleop_mode}"
+        />
+        <Decorator ID="KeepRunningUntilFailure">
+          <Control ID="Sequence">
+            <!--Closing and opening the gripper-->
+            <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
+              <SubTree ID="Close Gripper" />
+            </Decorator>
+            <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
+              <SubTree ID="Open Gripper" />
+            </Decorator>
+            <!--Joint sliders interpolate to joint state-->
+            <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
+              <Control ID="Sequence">
+                <Control ID="Fallback" name="root">
+                  <Control ID="Sequence">
+                    <Action
+                      ID="RetrieveJointStateParameter"
+                      timeout_sec="-1"
+                      joint_state="{target_joint_state}"
+                    />
+                    <SubTree
+                      ID="Interpolate to Joint State"
+                      _collapsed="false"
+                      target_joint_state="{target_joint_state}"
+                    />
+                    <Action
+                      ID="PublishEmpty"
+                      topic="/studio_ui/motion_ended"
+                      queue_size="1"
+                      use_best_effort="false"
+                    />
+                  </Control>
+                  <Control ID="Sequence">
+                    <Action
+                      ID="PublishEmpty"
+                      topic="/studio_ui/motion_ended"
+                      queue_size="1"
+                      use_best_effort="false"
+                    />
+                    <Action ID="AlwaysFailure" />
+                  </Control>
+                </Control>
+              </Control>
+            </Decorator>
+            <!--Interactive markers move to pose-->
+            <Decorator ID="ForceSuccess" _while="teleop_mode == 4">
+              <Control ID="Sequence">
+                <Control ID="Fallback" name="root">
+                  <Control ID="Sequence">
+                    <Action
+                      ID="RetrievePoseParameter"
+                      timeout_sec="-1"
+                      pose="{target_pose}"
+                    />
+                    <SubTree
+                      ID="Move to Pose"
+                      _collapsed="false"
+                      target_pose="{target_pose}"
+                    />
+                    <Action
+                      ID="PublishEmpty"
+                      topic="/studio_ui/motion_ended"
+                      queue_size="1"
+                      use_best_effort="false"
+                    />
+                  </Control>
+                  <Control ID="Sequence">
+                    <Action
+                      ID="PublishEmpty"
+                      topic="/studio_ui/motion_ended"
+                      queue_size="1"
+                      use_best_effort="false"
+                    />
+                    <Action ID="AlwaysFailure" />
+                  </Control>
+                </Control>
+              </Control>
+            </Decorator>
+            <!--Waypoint buttons move to joint state-->
+            <Decorator ID="ForceSuccess" _while="teleop_mode == 3">
+              <Control ID="Sequence">
+                <Control ID="Fallback" name="root">
+                  <Control ID="Sequence">
+                    <Action
+                      ID="RetrieveJointStateParameter"
+                      timeout_sec="-1"
+                      joint_state="{target_joint_state}"
+                    />
+                    <SubTree
+                      ID="Move to Joint State"
+                      _collapsed="false"
+                      target_joint_state="{target_joint_state}"
+                      link_padding="0.0"
+                    />
+                    <Action
+                      ID="PublishEmpty"
+                      topic="/studio_ui/motion_ended"
+                      queue_size="1"
+                      use_best_effort="false"
+                    />
+                  </Control>
+                  <Control ID="Sequence">
+                    <Action
+                      ID="PublishEmpty"
+                      topic="/studio_ui/motion_ended"
+                      queue_size="1"
+                      use_best_effort="false"
+                    />
+                    <Action ID="AlwaysFailure" />
+                  </Control>
+                </Control>
+              </Control>
+            </Decorator>
+            <!--Cartesian and joint jogging-->
+            <Control ID="Sequence" _while="teleop_mode == 2">
+              <Action
+                ID="TeleoperateTwist"
+                controller_name="servo_controller"
+              />
+            </Control>
+            <Control ID="Sequence" _while="teleop_mode == 1">
+              <Action
+                ID="TeleoperateJointJog"
+                controller_name="servo_controller"
+              />
+            </Control>
+          </Control>
+        </Decorator>
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Request Teleoperation">
+      <input_port name="enable_user_interaction" default="false" />
+      <input_port name="user_interaction_prompt" default="" />
+      <input_port name="initial_teleop_mode" default="3" />
+      <MetadataFields>
+        <Metadata subcategory="User Input" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>