Got toggle body pose overlay checkbox working

src/pages/operator/tsx/function_providers/UnderVideoFunctionProvider.tsx

@@ -35,7 +35,7 @@ export enum UnderVideoButton {
     MoveToPregraspGoalReached = "Goal Reached",
     ToggleTabletOrientation = "Toggle Tablet Orientation",
     GetTabletOrientation = "Get Tablet Orientation",
-    ShowTablet = "Show Tablet",
+    RealsenseBodyPoseEstimate = "Show Body Pose",
 }
 
 /** Array of different perspectives for the overhead camera */
@@ -60,7 +60,7 @@ export const realsenseMoveToPregraspButtons: UnderVideoButton[] = [
 
 /** Array of different options for the ShowTablet feature on the realsense camera */
 export const realsenseShowTabletButtons: UnderVideoButton[] = [
-    UnderVideoButton.ShowTablet,
+    UnderVideoButton.RealsenseBodyPoseEstimate,
 ];
 
 /** Array of different actions for the wrist */
@@ -277,10 +277,13 @@ export class UnderVideoFunctionProvider extends FunctionProvider {
                         return this.tabletOrientation;
                     },
                 };
-            case UnderVideoButton.ShowTablet:
+            case UnderVideoButton.RealsenseBodyPoseEstimate:
                 return {
-                    onClick: () =>
-                        FunctionProvider.remoteRobot?.getHumanPoseEstimate(),
+                    onCheck: (toggle: boolean) =>
+                        FunctionProvider.remoteRobot?.setToggle(
+                            "setRealsenseBodyPoseEstimate",
+                            toggle,
+                        ),
                 };
             default:
                 throw Error(

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

@@ -670,6 +670,9 @@ function executeRealsenseSettings(definition: RealsenseVideoStreamDef) {
     underVideoFunctionProvider.provideFunctions(
         UnderVideoButton.RealsenseDepthSensing,
     ).onCheck!(definition.depthSensing || false);
+    underVideoFunctionProvider.provideFunctions(
+        UnderVideoButton.RealsenseBodyPoseEstimate,
+    ).onCheck!(definition.bodyPoseAR || false);
 }
 
 /**
@@ -980,15 +983,17 @@ const UnderRealsenseButtons = (props: {
                 label="Depth Sensing"
             />
             {moveToPregraspButtons}
-            {
-                <button
-                    // className="map-cancel-btn"
-                    onPointerDown={() => {}}
-                >
-                    <span>Show Tablet</span>
-                    {/* <span className="material-icons">cancel</span> */}
-                </button>
-            }
+            <CheckToggleButton
+                checked={props.definition.bodyPoseAR || false}
+                onClick={() => {
+                    props.definition.bodyPoseAR = !props.definition.bodyPoseAR;
+                    setRerender(!rerender);
+                    underVideoFunctionProvider.provideFunctions(
+                        UnderVideoButton.RealsenseBodyPoseEstimate,
+                    ).onCheck!(props.definition.bodyPoseAR);
+                }}
+                label="Show Body Pose"
+            />
             {/* <CheckToggleButton
                 checked={props.definition.arucoMarkers || false}
                 onClick={() => {

src/pages/operator/tsx/utils/component_definitions.tsx

@@ -202,6 +202,10 @@ export type RealsenseVideoStreamDef = CameraViewDefinition & {
      * move to the pre-grasp position relative to the clicked object.
      */
     selectObjectForMoveToPregrasp?: boolean;
+    /**
+     * If the user has toggled on the human body pose estimate overlay.
+     */
+    bodyPoseAR?: boolean;
 };
 
 /**

src/pages/robot/tsx/index.tsx

@@ -270,6 +270,10 @@ function handleMessage(message: WebRTCMessage) {
         case "setExpandedGripper":
             robot.setExpandedGripper(message.toggle);
             break;
+        case "setRealsenseBodyPoseEstimate":
+            robot.setComputeBodyPose(message.toggle);
+            robot.setRealsenseShowBodyPose(message.toggle);
+            break;
         case "setRunStop":
             robot.setRunStop(message.toggle);
             break;
@@ -304,9 +308,6 @@ function handleMessage(message: WebRTCMessage) {
         case "stopTextToSpeech":
             robot.stopTextToSpeech();
             break;
-        case "getHumanPoseEstimate":
-            robot.executeEstimateHumanPoseGoal();
-            break;
     }
 }
 

src/pages/robot/tsx/robot.tsx

@@ -51,6 +51,8 @@ export class Robot extends React.Component {
     private setRealsenseDepthSensingService?: ROSLIB.Service;
     private setGripperDepthSensingService?: ROSLIB.Service;
     private setExpandedGripperService?: ROSLIB.Service;
+    private setRealsenseShowBodyPoseService?: ROSLIB.Service;
+    private setComputeBodyPoseService?: ROSLIB.Service;
     private setRunStopService?: ROSLIB.Service;
     private robotFrameTfClient?: ROSLIB.TFClient;
     private mapFrameTfClient?: ROSLIB.TFClient;
@@ -272,6 +274,8 @@ export class Robot extends React.Component {
         this.createRealsenseDepthSensingService();
         this.createGripperDepthSensingService();
         this.createExpandedGripperService();
+        this.createRealsenseShowBodyPoseService();
+        this.createComputeBodyPoseService();
         this.createRunStopService();
         this.createRobotFrameTFClient();
         this.createMapFrameTFClient();
@@ -597,6 +601,23 @@ export class Robot extends React.Component {
         });
     }
 
+    createRealsenseShowBodyPoseService() {
+        this.setRealsenseShowBodyPoseService = new ROSLIB.Service({
+            ros: this.ros,
+            name: "/realsense_body_pose_ar",
+            serviceType: "std_srvs/srv/SetBool",
+        });
+    }
+
+    createComputeBodyPoseService() {
+        this.setComputeBodyPoseService = new ROSLIB.Service({
+            ros: this.ros,
+            // TODO: Change this to the correct service name!
+            name: "/detection/toggle",
+            serviceType: "std_srvs/srv/SetBool",
+        });
+    }
+
     createRunStopService() {
         this.setRunStopService = new ROSLIB.Service({
             ros: this.ros,
@@ -712,6 +733,39 @@ export class Robot extends React.Component {
         );
     }
 
+    setRealsenseShowBodyPose(toggle: boolean) {
+        var request = new ROSLIB.ServiceRequest({ data: toggle });
+        this.setRealsenseShowBodyPoseService?.callService(
+            request,
+            (response: boolean) => {
+                response
+                    ? console.log(
+                          "Successfully set realsense depth sensing to",
+                          toggle,
+                      )
+                    : console.log(
+                          "Failed to set realsense depth sensing to",
+                          toggle,
+                      );
+            },
+        );
+    }
+
+    setComputeBodyPose(toggle: boolean) {
+        var request = new ROSLIB.ServiceRequest({ data: toggle });
+        this.setComputeBodyPoseService?.callService(
+            request,
+            (response: boolean) => {
+                response
+                    ? console.log(
+                          "Successfully set compute body pose to",
+                          toggle,
+                      )
+                    : console.log("Failed to set compute body pose to", toggle);
+            },
+        );
+    }
+
     setRunStop(toggle: boolean) {
         var request = new ROSLIB.ServiceRequest({ data: toggle });
         this.setRunStopService?.callService(request, (response: boolean) => {});

src/shared/commands.tsx

@@ -69,6 +69,7 @@ export interface ToggleCommand {
         | "setFollowGripper"
         | "setRealsenseDepthSensing"
         | "setGripperDepthSensing"
+        | "setRealsenseBodyPoseEstimate"
         | "setRunStop";
     toggle: boolean;
 }

src/shared/remoterobot.tsx

@@ -167,6 +167,7 @@ export class RemoteRobot extends React.Component<{}, any> {
             | "setFollowGripper"
             | "setRealsenseDepthSensing"
             | "setGripperDepthSensing"
+            | "setRealsenseBodyPoseEstimate"
             | "setRunStop",
         toggle: boolean,
     ) {

stretch_web_teleop_helpers/conversions.py

@@ -265,18 +265,27 @@ def project_points_to_pixels(
     -------
     npt.NDArray[np.float32]: The array of (u, v) pixel coordinates. Size: (N, 2).
     """
-    points_homogenous = np.hstack((points, np.ones((points.shape[0], 1))))
-    coords = np.matmul(proj, np.transpose(points_homogenous))  # 3 x N
+    # Project the points to the image plane
+    points_homogeneous = np.hstack((points, np.ones((points.shape[0], 1))))
+    coords = np.matmul(proj, np.transpose(points_homogeneous))  # 3 x N
+
+    # Filter our all points whose third homogeneous coordinate is 0 or nan
+    coords = coords[:, ~np.isnan(coords[2, :]) & (coords[2, :] != 0)]
+
+    # Convert the coordinates to pixel indices
     u_idx = (coords[0, :] / coords[2, :]).astype(int)
     v_idx = (coords[1, :] / coords[2, :]).astype(int)
     uv = np.vstack((u_idx, v_idx)).T  # N x 2
+
+    # Remove duplicate and out-of-bounds pixel coordinates
     uv_dedup = np.unique(uv, axis=0)
     in_bounds_idx = np.where(
         (uv_dedup[:, 0] >= 0)
         & (uv_dedup[:, 0] < width)
         & (uv_dedup[:, 1] >= 0)
         & (uv_dedup[:, 1] < height)
     )
+
     return uv_dedup[in_bounds_idx]