Adds odometry output to dora node

Signed-off-by: Ari Lowy <arilow@ekumenlabs.com>

Author: arilow

andino_dora/graphs/dataflow.yml

@@ -26,8 +26,10 @@ nodes:
     path: ../../target/debug/dora_diff_drive_controller
     inputs:
       cmd_vel: dora_teleop_keyboard/cmd_vel
+      wheel_joint_positions: dora_andino_mujoco_sim/wheel_joint_positions
     outputs:
       - joints_speed_cmd # [left, right]
+      - odom # [x, y, theta, linear_vel, angular_vel, timestamp]
     env:
       WHEEL_RADIUS: 0.0315 # [m]
       WHEEL_SEPARATION: 0.137 # [m]

andino_dora/graphs/gemini_navigation.yml

@@ -25,9 +25,11 @@ nodes:
     build: cargo build -p dora_diff_drive_controller
     path: ../../target/debug/dora_diff_drive_controller
     inputs:
-      cmd_vel: dora_gemini_diff_drive_navigation/cmd_vel
+      cmd_vel: dora_teleop_keyboard/cmd_vel
+      wheel_joint_positions: dora_andino_mujoco_sim/wheel_joint_positions
     outputs:
       - joints_speed_cmd # [left, right]
+      - odom # [x, y, theta, linear_vel, angular_vel, timestamp]
     env:
       WHEEL_RADIUS: 0.0315 # [m]
       WHEEL_SEPARATION: 0.137 # [m]

andino_dora_sim/graphs/mujoco_sim.yml

@@ -26,9 +26,9 @@ nodes:
     inputs:
       cmd_vel: dora_teleop_keyboard/cmd_vel
       wheel_joint_positions: dora_andino_mujoco_sim/wheel_joint_positions
-      # wheel_joint_velocities: dora_andino_mujoco_sim/wheel_joint_velocities
     outputs:
       - joints_speed_cmd # [left, right]
+      - odom # [x, y, theta, linear_vel, angular_vel, timestamp]
     env:
       WHEEL_RADIUS: 0.0315 # [m]
       WHEEL_SEPARATION: 0.137 # [m]

andino_dora_sim/graphs/mujoco_sim_gemini_navigation.yml

@@ -24,9 +24,11 @@ nodes:
     build: cargo build -p dora_diff_drive_controller
     path: ../../target/debug/dora_diff_drive_controller
     inputs:
-      cmd_vel: dora_gemini_diff_drive_navigation/cmd_vel
+      cmd_vel: dora_teleop_keyboard/cmd_vel
+      wheel_joint_positions: dora_andino_mujoco_sim/wheel_joint_positions
     outputs:
       - joints_speed_cmd # [left, right]
+      - odom # [x, y, theta, linear_vel, angular_vel, timestamp]
     env:
       WHEEL_RADIUS: 0.0315 # [m]
       WHEEL_SEPARATION: 0.137 # [m]

dora_node_hub/dora_diff_drive_controller/src/dora_node.rs

@@ -2,6 +2,7 @@ use dora_node_api::{DoraNode, Event, arrow::array::Float64Array, dora_core::conf
 
 pub fn main() -> eyre::Result<()> {
     let output_joints_speed = DataId::from("joints_speed_cmd".to_owned());
+    let output_odom = DataId::from("odom".to_owned());
 
     let (mut node, mut events) = DoraNode::init_from_env()?;
 
@@ -68,11 +69,22 @@ pub fn main() -> eyre::Result<()> {
                             );
                             continue;
                         }
+                        let timestamp = values.value(2);
                         diff_drive_odometry.update(
                             values.value(0), // left wheel position
                             values.value(1), // right wheel position
-                            values.value(2), // timestamp
+                            timestamp,       // timestamp
                         );
+
+                        let odom_array = Float64Array::from(vec![
+                            diff_drive_odometry.current_pose.x,
+                            diff_drive_odometry.current_pose.y,
+                            diff_drive_odometry.current_pose.heading,
+                            diff_drive_odometry.linear,
+                            diff_drive_odometry.angular,
+                            timestamp,
+                        ]);
+                        node.send_output(output_odom.clone(), metadata.parameters, odom_array)?;
                         // Process wheel joint positions if needed
                     }
                     _ => {

dora_node_hub/dora_diff_drive_controller/src/odometry.rs

@@ -10,18 +10,18 @@ fn normalize_angle(angle: f64) -> f64 {
 }
 /// Odometry for a diff drive mobile robot.
 pub struct DiffDriveOdometry {
+    /// Current pose:
+    pub current_pose: Pose2D,
+
+    /// Current velocity:
+    pub linear: f64, // [m/s]
+    pub angular: f64, // [rad/s]
+
     /// The distance between the wheels.
     wheel_separation: f64, // [m]
     /// The radius of the wheels.
     wheel_radius: f64, // [m]
 
-    /// Current pose:
-    current_pose: Pose2D,
-
-    /// Current velocity:
-    linear: f64, // [m/s]
-    angular: f64, // [rad/s]
-
     /// Previous data for odometry calculations.
     previous_time: f64, // [s]
     previous_left_wheel_position: f64,  // [rad]

dora_node_hub/dora_gemini_diff_drive_navigation/README.md

@@ -81,9 +81,11 @@ uv run pytest . # Test
     build: cargo build -p dora_diff_drive_controller
     path: ../../target/debug/dora_diff_drive_controller
     inputs:
-      cmd_vel: dora_gemini_diff_drive_navigation/cmd_vel
+      cmd_vel: dora_teleop_keyboard/cmd_vel
+      wheel_joint_positions: dora_andino_mujoco_sim/wheel_joint_positions
     outputs:
       - joints_speed_cmd # [left, right]
+      - odom # [x, y, theta, linear_vel, angular_vel, timestamp]
     env:
       WHEEL_RADIUS: 0.0315 # [m]
       WHEEL_SEPARATION: 0.137 # [m]