subscribe to odom

Author: hello-vinitha

src/pages/operator/tsx/index.tsx

@@ -208,6 +208,9 @@ function handleWebRTCMessage(message: WebRTCMessage | WebRTCMessage[]) {
         case "batteryVoltage":
             remoteRobot.sensors.setBatteryVoltage(message.message);
             break;
+        case "odom":
+            remoteRobot.sensors.setOdom(message.message);
+            break;
         default:
             throw Error(`unhandled WebRTC message type ${message.type}`);
     }

src/pages/robot/tsx/index.tsx

@@ -21,6 +21,8 @@ import {
     ActionStateMessage,
     ROSBatteryState,
     BatteryVoltageMessage,
+    ROSOdometry,
+    OdomMessage,
     delay,
 } from "shared/util";
 import { AllVideoStreamComponent, VideoStream } from "./videostreams";
@@ -33,6 +35,7 @@ export const robot = new Robot({
     jointStateCallback: forwardJointStates,
     batteryStateCallback: forwardBatteryState,
     occupancyGridCallback: forwardOccupancyGrid,
+    odomCallback: forwardOdom,
     moveBaseResultCallback: (goalState: ActionState) =>
         forwardActionState(goalState, "moveBaseState"),
     playbackPosesResultCallback: (goalState: ActionState) =>
@@ -183,6 +186,15 @@ function forwardBatteryState(batteryState: ROSBatteryState) {
     } as BatteryVoltageMessage);
 }
 
+function forwardOdom(odom: ROSOdometry) {
+    if (!connection) throw "WebRTC connection undefined";
+
+    connection.sendData({
+        type: "odom",
+        message: odom,
+    } as OdomMessage);
+}
+
 function forwardOccupancyGrid(occupancyGrid: ROSOccupancyGrid) {
     if (!connection) throw "WebRTC connection undefined";
 

src/pages/robot/tsx/robot.tsx

@@ -23,6 +23,7 @@ import {
     StretchTool,
     getStretchTool,
     DiagnosticArray,
+    ROSOdometry,
 } from "shared/util";
 import {
     rosJointStatetoRobotPose,
@@ -88,6 +89,7 @@ export class Robot extends React.Component {
     ) => void;
     private batteryStateCallback: (batteryState: ROSBatteryState) => void;
     private occupancyGridCallback: (occupancyGrid: ROSOccupancyGrid) => void;
+    private odomCallback: (odom: ROSOdometry) => void;
     private moveBaseResultCallback: (goalState: ActionState) => void;
     private playbackPosesResultCallback: (goalState: ActionState) => void;
     private amclPoseCallback: (pose: Transform) => void;
@@ -109,6 +111,7 @@ export class Robot extends React.Component {
         ) => void;
         batteryStateCallback: (batteryState: ROSBatteryState) => void;
         occupancyGridCallback: (occupancyGrid: ROSOccupancyGrid) => void;
+        odomCallback: (odom: ROSOdometry) => void;
         moveBaseResultCallback: (goalState: ActionState) => void;
         playbackPosesResultCallback: (goalState: ActionState) => void;
         amclPoseCallback: (pose: Transform) => void;
@@ -121,6 +124,7 @@ export class Robot extends React.Component {
         this.jointStateCallback = props.jointStateCallback;
         this.batteryStateCallback = props.batteryStateCallback;
         this.occupancyGridCallback = props.occupancyGridCallback;
+        this.odomCallback = props.odomCallback;
         this.moveBaseResultCallback = props.moveBaseResultCallback;
         this.playbackPosesResultCallback = props.playbackPosesResultCallback;
         this.amclPoseCallback = props.amclPoseCallback;
@@ -254,6 +258,7 @@ export class Robot extends React.Component {
         this.subscribeToJointState();
         this.subscribeToJointLimits();
         this.subscribeToBatteryState();
+        this.subscribeToOdom();
         this.subscribeToMode();
         this.subscribetoJointStateDiagnostics();
         this.subscribeToActionResult(
@@ -355,6 +360,19 @@ export class Robot extends React.Component {
         });
     }
 
+    subscribeToOdom() {
+        const odomTopic: Topic<ROSOdometry> = new Topic({
+            ros: this.ros,
+            name: "/wheel_odom",
+            messageType: "nav_msgs/msg/Odometry",
+        });
+        this.subscriptions.push(odomTopic);
+
+        odomTopic.subscribe((msg: ROSOdometry) => {
+            if (this.odomCallback) this.odomCallback(msg);
+        });
+    }
+
     subscribeToMode() {
         const modeTopic: Topic = new Topic({
             ros: this.ros,

src/shared/remoterobot.tsx

@@ -22,6 +22,7 @@ import {
     ValidJoints,
     ROSPose,
     waitUntil,
+    ROSOdometry,
 } from "shared/util";
 export type robotMessageChannel = (message: cmd) => void;
 
@@ -240,6 +241,7 @@ class RobotSensors extends React.Component {
     private mode: string | undefined = undefined;
     private isHomed: boolean | undefined = undefined;
     private runStopEnabled: boolean = false;
+    private odom: ROSOdometry | undefined = undefined;
     private functionProviderCallback?: (
         inJointLimits: ValidJointStateDict,
         inCollision: ValidJointStateDict
@@ -386,6 +388,10 @@ class RobotSensors extends React.Component {
             this.runStopFunctionProviderCallback(this.runStopEnabled);
     }
 
+    setOdom(odom: ROSOdometry) {
+        this.odom = odom;
+    }
+    
     /**
      * Records a callback from the function provider. The callback is called
      * whenever the battery voltage changes.

src/shared/util.tsx

@@ -102,13 +102,13 @@ export type WebRTCMessage =
     | MapPoseMessage
     | StopTrajectoryMessage
     | StopMoveBaseMessage
-
     | BatteryVoltageMessage
     | ModeMessage
     | IsHomedMessage
     | IsRunStoppedMessage
     | StretchToolMessage
     | ActionStateMessage
+    | OdomMessage
     | cmd;
 
 interface StopTrajectoryMessage {
@@ -179,6 +179,11 @@ export interface BatteryVoltageMessage {
     message: number;
 }
 
+export interface OdomMessage {
+    type: "odom";
+    message: ROSOdometry;
+}
+
 export interface ROSPoint extends Message {
     x: number;
     y: number;
@@ -211,6 +216,22 @@ export interface ROSOccupancyGrid {
     data: number[];
 }
 
+export interface ROSOdometry extends Message {
+    header: string;
+    child_frame_id: string;
+    pose: {
+        pose: ROSPose;
+        covariance: number[];
+    };
+    twist: {
+        twist: {
+            linear: { x: number; y: number; z: number };
+            angular: { x: number; y: number; z: number };
+        };
+        covariance: number[];
+    };
+}
+
 export const JOINT_LIMITS: { [key in ValidJoints]?: [number, number] } = {
     arm_joint: [0.001, 0.518],
     wrist_roll_joint: [-2.95, 2.94],