Add odometry (#54)

arilow · francocipollone · web-flow · commit 48ea149f6d9d · 2025-08-13T17:31:35.000-03:00
Signed-off-by: Ari Lowy &lt;arilow@ekumenlabs.com&gt;
Signed-off-by: Franco Cipollone &lt;franco.c@ekumenlabs.com&gt;
Co-authored-by: Ari Lowy &lt;arilow@ekumenlabs.com&gt;
Co-authored-by: Franco Cipollone &lt;franco.c@ekumenlabs.com&gt;
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/Cargo.toml b/Cargo.toml
@@ -17,6 +17,7 @@ repository = "https://github.com/ekumenlabs/andino-rs"
 authors = ["Franco Cipollone <franco.c@ekumenlabs.com>"]
 
 [workspace.dependencies]
+approx = { version = "0.5" }
 clap = { version = "4.5" }
 crossterm = { version = "0.29" }
 env_logger = { version = "0.10" }
diff --git a/andino_dora/graphs/dataflow.yml b/andino_dora/graphs/dataflow.yml
@@ -26,15 +26,20 @@ nodes:
     path: ../../target/debug/dora_diff_drive_controller
     inputs:
       cmd_vel: dora_teleop_keyboard/cmd_vel
+      wheel_joint_positions: dora_andino_hal/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]
 
   # Node that reads the keyboard input and outputs the character pressed.
   - id: dora_keyboard
-    build: uv pip install git+https://github.com/dora-rs/dora.git@v0.3.12#egg=dora-keyboard&subdirectory=node-hub/dora-keyboard
+    # TODO(#55): Note we are here not using `uv`. It seems there is an issue with `uv` for the Raspberry Pi's architecture
+    # causing an illegal instruction error.
+    # Once this is fixed, we can switch back to using `uv` for consistency.
+    build: pip install dora-keyboard
     path: dora-keyboard
     outputs:
       - char
@@ -52,11 +57,10 @@ nodes:
       DEFAULT_ANGULAR_SPEED: 1.0 # [rad/s]
 
   # - id: dora-record
-  #   custom:
-  #     build: cargo install --git https://github.com/dora-rs/dora --tag v0.3.12 dora-record
-  #     source: dora-record
-  #     inputs:
-  #       wheel_joint_positions: dora_andino_hal/wheel_joint_positions
-  #       wheel_joint_velocities: dora_andino_hal/wheel_joint_velocities
-  #       joints_speed_cmd: diff_drive_controller/joints_speed_cmd
-        # cmd_vel: gamepad/cmd_vel
+  #   build: cargo install --git https://github.com/dora-rs/dora --tag v0.3.12 dora-record
+  #   path: dora-record
+  #   inputs:
+  #     wheel_joint_positions: dora_andino_hal/wheel_joint_positions
+  #     wheel_joint_velocities: dora_andino_hal/wheel_joint_velocities
+  #     joints_speed_cmd: dora_diff_drive_controller/joints_speed_cmd
+  #     odom: dora_diff_drive_controller/odom
diff --git a/andino_dora/graphs/gemini_navigation.yml b/andino_dora/graphs/gemini_navigation.yml
@@ -7,8 +7,8 @@ nodes:
       tick: dora/timer/millis/100
       joints_speed_cmd: dora_diff_drive_controller/joints_speed_cmd
     outputs:
-      - wheel_joint_positions # [left, right]
-      - wheel_joint_velocities # [left, right]
+      - wheel_joint_positions # [left, right, timestamp ]
+      - wheel_joint_velocities # [left, right, timestamp ]
     env:
       # Serial port name.
       SERIAL_DEVICE: /dev/ttyUSB0
@@ -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]
diff --git a/andino_dora_sim/graphs/mujoco_sim.yml b/andino_dora_sim/graphs/mujoco_sim.yml
@@ -12,8 +12,8 @@ nodes:
       camera_tick: dora/timer/millis/100 # 10 Hz
       joints_speed_cmd: dora_diff_drive_controller/joints_speed_cmd
     outputs:
-      - wheel_joint_positions # [left, right]
-      - wheel_joint_velocities # [left, right]
+      - wheel_joint_positions # [left, right, timestamp]
+      - wheel_joint_velocities # [left, right, timestamp]
       - camera_image # Encoding: bgr8
     env:
       TIMESTEP: 0.001 # [s]
@@ -25,8 +25,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]
diff --git a/andino_dora_sim/graphs/mujoco_sim_gemini_navigation.yml b/andino_dora_sim/graphs/mujoco_sim_gemini_navigation.yml
@@ -12,8 +12,8 @@ nodes:
       camera_tick: dora/timer/millis/100 # 10 Hz
       joints_speed_cmd: dora_diff_drive_controller/joints_speed_cmd
     outputs:
-      - wheel_joint_positions # [left, right]
-      - wheel_joint_velocities # [left, right]
+      - wheel_joint_positions # [left, right, timestamp]
+      - wheel_joint_velocities # [left, right, timestamp]
       - camera_image # Encoding: bgr8
     env:
       TIMESTEP: 0.001 # [s]
@@ -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]
diff --git a/dora_node_hub/dora_andino_hal/src/dora_node.rs b/dora_node_hub/dora_andino_hal/src/dora_node.rs
@@ -60,6 +60,7 @@ pub fn main() -> eyre::Result<()> {
                         let wheel_joint_positions_data = Float64Array::from(vec![
                             andino_hal_state.left_wheel_state.position,
                             andino_hal_state.right_wheel_state.position,
+                            metadata.timestamp().get_time().to_duration().as_secs_f64(),
                         ]);
                         node.send_output(
                             output_wheel_joint_positions.clone(),
@@ -70,6 +71,7 @@ pub fn main() -> eyre::Result<()> {
                         let wheel_joint_velocities_data = Float64Array::from(vec![
                             andino_hal_state.left_wheel_state.velocity,
                             andino_hal_state.right_wheel_state.velocity,
+                            metadata.timestamp().get_time().to_duration().as_secs_f64(),
                         ]);
                         node.send_output(
                             output_wheel_joint_velocities.clone(),
diff --git a/dora_node_hub/dora_andino_mujoco_sim/README.md b/dora_node_hub/dora_andino_mujoco_sim/README.md
@@ -59,8 +59,8 @@ nodes:
       model_tick: dora/timer/millis/1 # ~1000Hz
       joints_speed_cmd: dora_diff_drive_controller/joints_speed_cmd
     outputs:
-      - wheel_joint_positions # [left, right]
-      - wheel_joint_velocities # [left, right]
+      - wheel_joint_positions # [left, right, timestamp]
+      - wheel_joint_velocities # [left, right, timestamp]
     env:
       TIMESTEP: 0.001 # [s]
 ```
diff --git a/dora_node_hub/dora_andino_mujoco_sim/dora_andino_mujoco_sim/assets/andino/andino.xml b/dora_node_hub/dora_andino_mujoco_sim/dora_andino_mujoco_sim/assets/andino/andino.xml
@@ -34,17 +34,17 @@
                 <geom type="mesh" rgba="0 0 1 1" mesh="chassis_top"/>
             </body><!-- chassis_top -->
 
-            <body name="right_wheel" pos="0.0185 -0.0675 -0.017">
+            <body name="right_wheel" pos="0.0185 -0.0685 -0.017">
                 <site name="right_wheel"/>
                 <joint name="right_wheel_joint" type="hinge" axis="0 1 0" damping="0.001"  />
                 <geom class="visual" quat="0 0 -0.707107 -0.707107" type="mesh"  mesh="wheel"/>
-                <geom quat="1 1 0 0" type="cylinder" mass="0.03" size="0.03 0.010" />
+                <geom quat="1 1 0 0" type="cylinder" mass="0.03" size="0.0315 0.010" />
             </body><!-- right_wheel -->
 
-            <body name="left_wheel" pos="0.0185 0.0675 -0.017">
+            <body name="left_wheel" pos="0.0185 0.0685 -0.017">
                 <joint name="left_wheel_joint" type="hinge" axis="0 1 0" damping="0.001" />
                 <geom class="visual" quat="0.707107 0.707107 0 0" type="mesh" mesh="wheel"/>
-                <geom quat="1 1 0 0" type="cylinder" mass="0.03" size="0.03 0.010" />
+                <geom quat="1 1 0 0" type="cylinder" mass="0.03" size="0.0315 0.010" />
             </body><!-- left_wheel -->
 
             <body name="caster_base" pos="-0.076 0 -0.0155" >
diff --git a/dora_node_hub/dora_andino_mujoco_sim/dora_andino_mujoco_sim/main.py b/dora_node_hub/dora_andino_mujoco_sim/dora_andino_mujoco_sim/main.py
@@ -74,19 +74,20 @@ def main() -> None:
 
                         # Advance the simulation
                         mujoco.mj_step(mj_model, mj_data)
-
                         # Send position and velocity data or any other form of feedback from MuJoCo.
                         # Replicating same behavior as the dora_andino_hal we outputs:
                         # - wheel_joint_positions
                         # - wheel_joint_velocities
                         wheel_joint_positions = [
                             mj_data.joint("left_wheel_joint").qpos[0],
                             mj_data.joint("right_wheel_joint").qpos[0],
+                            mj_data.time,
                         ]
 
                         wheel_joint_velocities = [
                             mj_data.joint("left_wheel_joint").qvel[0],
                             mj_data.joint("right_wheel_joint").qvel[0],
+                            mj_data.time,
                         ]
                         node.send_output(
                             "wheel_joint_positions",
diff --git a/dora_node_hub/dora_diff_drive_controller/Cargo.toml b/dora_node_hub/dora_diff_drive_controller/Cargo.toml
@@ -11,3 +11,4 @@ authors = { workspace = true }
 
 eyre = { workspace = true }
 dora-node-api = { workspace = true}
+approx = { workspace = true}
diff --git a/dora_node_hub/dora_diff_drive_controller/src/dora_node.rs b/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()?;
 
@@ -19,7 +20,10 @@ pub fn main() -> eyre::Result<()> {
         wheel_radius, wheel_separation
     );
 
-    let diff_drive_controller = crate::controller::DiffDriveController::new(wheel_separation, wheel_radius);
+    let diff_drive_controller: crate::controller::DiffDriveController =
+        crate::controller::DiffDriveController::new(wheel_separation, wheel_radius);
+    let mut diff_drive_odometry: crate::odometry::DiffDriveOdometry =
+        crate::odometry::DiffDriveOdometry::new(wheel_separation, wheel_radius);
 
     while let Some(event) = events.recv() {
         match event {
@@ -51,6 +55,37 @@ pub fn main() -> eyre::Result<()> {
                         let speed_array = Float64Array::from(vec![left_wheel_speed, right_wheel_speed]);
                         node.send_output(output_joints_speed.clone(), metadata.parameters, speed_array)?;
                     }
+                    "wheel_joint_positions" => {
+                        // Receives wheel joint positions
+                        let values = if let Some(float_array) = data.as_any().downcast_ref::<Float64Array>() {
+                            float_array
+                        } else {
+                            eprintln!("wheel_joint_positions: Not a Float64Array!");
+                            continue;
+                        };
+                        if values.len() != 3 {
+                            eprintln!(
+                                "wheel_joint_positions: Not a Float64Array with 3 elements. It expects a Float64Array with 3 elements: [left_wheel_position, right_wheel_position, timestamp]"
+                            );
+                            continue;
+                        }
+                        let timestamp = values.value(2);
+                        diff_drive_odometry.update(
+                            values.value(0), // left wheel position
+                            values.value(1), // right wheel position
+                            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)?;
+                    }
                     _ => {
                         println!("Unexpected input id: {:?}", id);
                     }
diff --git a/dora_node_hub/dora_diff_drive_controller/src/lib.rs b/dora_node_hub/dora_diff_drive_controller/src/lib.rs
@@ -1,2 +1,4 @@
 pub mod controller;
 pub mod dora_node;
+pub mod odometry;
+pub mod pose_2d;
diff --git a/dora_node_hub/dora_diff_drive_controller/src/odometry.rs b/dora_node_hub/dora_diff_drive_controller/src/odometry.rs
diff --git a/dora_node_hub/dora_diff_drive_controller/src/pose_2d.rs b/dora_node_hub/dora_diff_drive_controller/src/pose_2d.rs
diff --git a/dora_node_hub/dora_gemini_diff_drive_navigation/README.md b/dora_node_hub/dora_gemini_diff_drive_navigation/README.md
