Bugfix: Enable "Follow ..." feature when the tablet is attached (#55)

* Forward stretch tool info to web app

* Added tablet and wrist yaw as backup links to follow

* Only subscribe to the necessary TF

* Add comment

launch/web_interface.launch.py

@@ -422,7 +422,12 @@ def generate_launch_description():
         executable="configure_video_streams.py",
         output="screen",
         arguments=[LaunchConfiguration("params"), str(stretch_has_beta_teleop_kit)],
-        parameters=[{"has_beta_teleop_kit": stretch_has_beta_teleop_kit}],
+        parameters=[
+            {
+                "has_beta_teleop_kit": stretch_has_beta_teleop_kit,
+                "stretch_tool": stretch_tool,
+            }
+        ],
     )
     ld.add_action(configure_video_streams_node)
 

nodes/configure_video_streams.py

@@ -33,7 +33,10 @@ def __init__(self, params_file, has_beta_teleop_kit):
         with open(params_file, "r") as params:
             self.image_params = yaml.safe_load(params)
 
+        # These parameters are not used by this node. Rather, they are used by
+        # the web app to determine the robot's configuration.
         self.declare_parameter("has_beta_teleop_kit", rclpy.Parameter.Type.BOOL)
+        self.declare_parameter("stretch_tool", rclpy.Parameter.Type.STRING)
 
         self.tf_buffer = tf2_ros.Buffer(cache_time=Duration(seconds=12))
         self.tf2_listener = tf2_ros.TransformListener(self.tf_buffer, self)

src/pages/operator/tsx/MobileOperator.tsx

@@ -11,6 +11,7 @@ import { className, MoveBaseState, RemoteStream } from "shared/util";
 import {
   buttonFunctionProvider,
   hasBetaTeleopKit,
+  stretchTool,
   movementRecorderFunctionProvider,
   underMapFunctionProvider,
   underVideoFunctionProvider,
@@ -115,6 +116,7 @@ export const MobileOperator = (props: {
     buttonStateMap: buttonStateMap.current,
     hideLabels: false,
     hasBetaTeleopKit: hasBetaTeleopKit,
+    stretchTool: stretchTool,
   };
 
   function updateScreens() {

src/pages/operator/tsx/Operator.tsx

@@ -14,6 +14,7 @@ import {
   buttonFunctionProvider,
   underMapFunctionProvider,
   hasBetaTeleopKit,
+  stretchTool,
 } from ".";
 import {
   ButtonPadButton,
@@ -219,6 +220,7 @@ export const Operator = (props: {
     buttonStateMap: buttonStateMap.current,
     hideLabels: !layout.current.displayLabels,
     hasBetaTeleopKit: hasBetaTeleopKit,
+    stretchTool: stretchTool,
   };
 
   /** Properties for the global options area of the sidebar */

src/pages/operator/tsx/index.tsx

@@ -39,6 +39,7 @@ let remoteRobot: RemoteRobot;
 let connection: WebRTCConnection;
 let root: Root;
 export let hasBetaTeleopKit: boolean;
+export let stretchTool: string;
 export let occupancyGrid: ROSOccupancyGrid | undefined = undefined;
 export let storageHandler: StorageHandler;
 
@@ -153,6 +154,9 @@ function handleWebRTCMessage(message: WebRTCMessage | WebRTCMessage[]) {
     case "hasBetaTeleopKit":
       hasBetaTeleopKit = message.value;
       break;
+    case "stretchTool":
+      stretchTool = message.value;
+      break;
     case "occupancyGrid":
       if (!occupancyGrid) {
         occupancyGrid = message.message;
@@ -207,6 +211,7 @@ function configureRemoteRobot() {
   });
   occupancyGrid = undefined;
   remoteRobot.getHasBetaTeleopKit("getHasBetaTeleopKit");
+  remoteRobot.getStretchTool("getStretchTool");
   FunctionProvider.addRemoteRobot(remoteRobot);
   mapFunctionProvider = new MapFunctionProvider();
   remoteRobot.sensors.setFunctionProviderCallback(

src/pages/operator/tsx/layout_components/CameraView.tsx

@@ -28,6 +28,7 @@ import { PredictiveDisplay } from "./PredictiveDisplay";
 import {
   buttonFunctionProvider,
   hasBetaTeleopKit,
+  stretchTool,
   underVideoFunctionProvider,
 } from "..";
 import {
@@ -720,7 +721,7 @@ const UnderAdjustableOverheadButtons = (props: {
             UnderVideoButton.FollowGripper,
           ).onCheck!(props.definition.followGripper);
         }}
-        label="Follow Gripper"
+        label={getFollowGripperLabel()}
       />
       <CheckToggleButton
         checked={props.definition.predictiveDisplay || false}
@@ -772,7 +773,7 @@ const UnderRealsenseButtons = (props: {
             UnderVideoButton.FollowGripper,
           ).onCheck!(props.definition.followGripper);
         }}
-        label="Follow Gripper"
+        label={getFollowGripperLabel()}
       />
       <CheckToggleButton
         checked={props.definition.depthSensing || false}
@@ -875,3 +876,19 @@ const CameraPerspectiveButton = (props: {
   ).onClick;
   return <button onClick={onClick}>{props.perspective}</button>;
 };
+
+function getFollowGripperLabel() {
+  if (stretchTool === "eoa_wrist_dw3_tool_tablet_12in") {
+    return "Follow Tablet";
+  } else if (
+    [
+      "eoa_wrist_dw3_tool_sg3",
+      "tool_stretch_dex_wrist",
+      "tool_stretch_gripper",
+    ].includes(stretchTool)
+  ) {
+    return "Follow Gripper";
+  } else {
+    return "Follow Wrist";
+  }
+}

src/pages/operator/tsx/layout_components/CustomizableComponent.tsx

@@ -33,6 +33,8 @@ export type SharedState = {
   hideLabels?: boolean;
   /** Whether or not the beta teleop cameras are being used */
   hasBetaTeleopKit: boolean;
+  /** What tool is attached to the stretch gripper. */
+  stretchTool: string;
 };
 
 /** Properties for any of the customizable components: tabs, video streams, or

src/pages/robot/tsx/index.tsx

@@ -24,7 +24,10 @@ import {
 } from "shared/util";
 import { AllVideoStreamComponent, VideoStream } from "./videostreams";
 import ROSLIB from "roslib";
-import { HasBetaTeleopKitMessage } from "../../../shared/util";
+import {
+  HasBetaTeleopKitMessage,
+  StretchToolMessage,
+} from "../../../shared/util";
 
 export const robot = new Robot({
   jointStateCallback: forwardJointStates,
@@ -34,6 +37,7 @@ export const robot = new Robot({
   amclPoseCallback: forwardAMCLPose,
   isRunStoppedCallback: forwardIsRunStopped,
   hasBetaTeleopKitCallback: forwardHasBetaTeleopKit,
+  stretchToolCallback: forwardStretchTool,
 });
 
 export let connection: WebRTCConnection;
@@ -144,6 +148,15 @@ function forwardHasBetaTeleopKit(value: boolean) {
   } as HasBetaTeleopKitMessage);
 }
 
+function forwardStretchTool(value: string) {
+  if (!connection) throw "WebRTC connection undefined!";
+
+  connection.sendData({
+    type: "stretchTool",
+    value: value,
+  } as StretchToolMessage);
+}
+
 function forwardJointStates(
   robotPose: RobotPose,
   jointValues: ValidJointStateDict,
@@ -260,6 +273,8 @@ function handleMessage(message: WebRTCMessage) {
       break;
     case "getHasBetaTeleopKit":
       robot.getHasBetaTeleopKit();
+    case "getStretchTool":
+      robot.getStretchTool();
   }
 }
 

src/pages/robot/tsx/robot.tsx

@@ -41,6 +41,8 @@ export class Robot extends React.Component {
   private robotFrameTfClient?: ROSLIB.TFClient;
   private mapFrameTfClient?: ROSLIB.TFClient;
   private linkGripperFingerLeftTF?: ROSLIB.Transform;
+  private linkTabletTF?: ROSLIB.Transform;
+  private linkWristYawTF?: ROSLIB.Transform;
   private linkHeadTiltTF?: ROSLIB.Transform;
   private jointStateCallback: (
     robotPose: RobotPose,
@@ -53,9 +55,11 @@ export class Robot extends React.Component {
   private amclPoseCallback: (pose: ROSLIB.Transform) => void;
   private isRunStoppedCallback: (isRunStopped: boolean) => void;
   private hasBetaTeleopKitCallback: (value: boolean) => void;
+  private stretchToolCallback: (value: string) => void;
   private lookAtGripperInterval?: number; // ReturnType<typeof setInterval>
   private subscriptions: ROSLIB.Topic[] = [];
   private hasBetaTeleopKitParam: ROSLIB.Param;
+  private stretchToolParam: ROSLIB.Param;
 
   constructor(props: {
     jointStateCallback: (
@@ -69,6 +73,7 @@ export class Robot extends React.Component {
     amclPoseCallback: (pose: ROSLIB.Transform) => void;
     isRunStoppedCallback: (isRunStopped: boolean) => void;
     hasBetaTeleopKitCallback: (value: boolean) => void;
+    stretchToolCallback: (value: string) => void;
   }) {
     super(props);
     this.jointStateCallback = props.jointStateCallback;
@@ -78,6 +83,7 @@ export class Robot extends React.Component {
     this.amclPoseCallback = props.amclPoseCallback;
     this.isRunStoppedCallback = props.isRunStoppedCallback;
     this.hasBetaTeleopKitCallback = props.hasBetaTeleopKitCallback;
+    this.stretchToolCallback = props.stretchToolCallback;
   }
 
   async connect(): Promise<void> {
@@ -119,7 +125,6 @@ export class Robot extends React.Component {
     this.createRunStopService();
     this.createRobotFrameTFClient();
     this.createMapFrameTFClient();
-    this.subscribeToGripperFingerTF();
     this.subscribeToHeadTiltTF();
     this.subscribeToMapTF();
 
@@ -213,6 +218,33 @@ export class Robot extends React.Component {
     });
   }
 
+  getStretchTool() {
+    // NOTE: This information can also come from the /tool topic.
+    // However, we only need it once, so opt for a parameter.
+    this.stretchToolParam = new ROSLIB.Param({
+      ros: this.ros,
+      name: "/configure_video_streams:stretch_tool",
+    });
+
+    this.stretchToolParam.get((value: string) => {
+      console.log("stretch tool: ", value);
+      if (value === "eoa_wrist_dw3_tool_tablet_12in") {
+        this.subscribeToTabletTF();
+      } else if (
+        [
+          "eoa_wrist_dw3_tool_sg3",
+          "tool_stretch_dex_wrist",
+          "tool_stretch_gripper",
+        ].includes(value)
+      ) {
+        this.subscribeToGripperFingerTF();
+      } else {
+        this.subscribeToWristYawTF();
+      }
+      if (this.stretchToolCallback) this.stretchToolCallback(value);
+    });
+  }
+
   getOccupancyGrid() {
     let getMapService = new ROSLIB.Service({
       ros: this.ros,
@@ -370,6 +402,18 @@ export class Robot extends React.Component {
     );
   }
 
+  subscribeToTabletTF() {
+    this.robotFrameTfClient?.subscribe("link_DW3_tablet_12in", (transform) => {
+      this.linkTabletTF = transform;
+    });
+  }
+
+  subscribeToWristYawTF() {
+    this.robotFrameTfClient?.subscribe("link_wrist_yaw", (transform) => {
+      this.linkWristYawTF = transform;
+    });
+  }
+
   subscribeToHeadTiltTF() {
     this.robotFrameTfClient?.subscribe("link_head_tilt", (transform) => {
       this.linkHeadTiltTF = transform;
@@ -626,7 +670,12 @@ export class Robot extends React.Component {
       let panOffset = 0;
       let tiltOffset = 0;
       let lookIfReadyAndRepeat = () => {
-        if (this.linkGripperFingerLeftTF && this.linkHeadTiltTF) {
+        if (
+          (this.linkGripperFingerLeftTF ||
+            this.linkTabletTF ||
+            this.linkWristYawTF) &&
+          this.linkHeadTiltTF
+        ) {
           this.lookAtGripper(panOffset, tiltOffset);
         }
         this.lookAtGripperInterval = window.setTimeout(
@@ -643,19 +692,17 @@ export class Robot extends React.Component {
   }
 
   lookAtGripper(panOffset: number, tiltOffset: number) {
-    if (!this.linkGripperFingerLeftTF)
-      throw "linkGripperFingerLeftTF is undefined";
+    // If there is a gripper, follow its TF frame. Else, if there is a tablet, follow its TF frame.
+    // Else, follow the quick connect TF frame.
+    let transform: ROSLIB.Transform | undefined =
+      this.linkGripperFingerLeftTF || this.linkTabletTF || this.linkWristYawTF;
+    if (!transform)
+      throw "linkGripperFingerLeftTF, linkTabletTF, and linkWristYawTF are all undefined";
     if (!this.linkHeadTiltTF) throw "linkHeadTiltTF is undefined";
     let posDifference = {
-      x:
-        this.linkGripperFingerLeftTF.translation.x -
-        this.linkHeadTiltTF.translation.x,
-      y:
-        this.linkGripperFingerLeftTF.translation.y -
-        this.linkHeadTiltTF.translation.y,
-      z:
-        this.linkGripperFingerLeftTF.translation.z -
-        this.linkHeadTiltTF.translation.z,
+      x: transform.translation.x - this.linkHeadTiltTF.translation.x,
+      y: transform.translation.y - this.linkHeadTiltTF.translation.y,
+      z: transform.translation.z - this.linkHeadTiltTF.translation.z,
     };
 
     // Normalize posDifference

src/shared/commands.tsx

@@ -16,7 +16,8 @@ export type cmd =
   | StopMoveBaseCommand
   | PlaybackPosesCommand
   | GetBatteryVoltageCommand
-  | GetHasBetaTeleopKit;
+  | GetHasBetaTeleopKit
+  | GetStretchTool;
 
 export interface VelocityCommand {
   stop: () => void;
@@ -75,6 +76,10 @@ export interface GetHasBetaTeleopKit {
   type: "getHasBetaTeleopKit";
 }
 
+export interface GetStretchTool {
+  type: "getStretchTool";
+}
+
 export interface MoveBaseCommand {
   type: "moveBase";
   pose: ROSPose;

src/shared/remoterobot.tsx

@@ -21,7 +21,7 @@ import {
   ROSPose,
   waitUntil,
 } from "shared/util";
-import { GetHasBetaTeleopKit } from "./commands";
+import { GetHasBetaTeleopKit, GetStretchTool } from "./commands";
 
 export type robotMessageChannel = (message: cmd) => void;
 
@@ -180,6 +180,13 @@ export class RemoteRobot extends React.Component<{}, any> {
     this.robotChannel(cmd);
   }
 
+  getStretchTool(type: "getStretchTool") {
+    let cmd: GetStretchTool = {
+      type: type,
+    };
+    this.robotChannel(cmd);
+  }
+
   setMapPose(pose: ROSLIB.Transform) {
     this.mapPose = pose;
   }

src/shared/util.tsx

@@ -88,6 +88,7 @@ export type WebRTCMessage =
   | MoveBaseStateMessage
   | IsRunStoppedMessage
   | HasBetaTeleopKitMessage
+  | StretchToolMessage
   | cmd;
 
 interface StopTrajectoryMessage {
@@ -117,6 +118,11 @@ export interface HasBetaTeleopKitMessage {
   value: boolean;
 }
 
+export interface StretchToolMessage {
+  type: "stretchTool";
+  value: string;
+}
+
 export interface FollowJointTrajectoryActionResultMessage {
   type: "goalStatus";
   message: FollowJointTrajectoryActionResult;