Lidar-odometry localization + dedicated mote_simulation package

.gitignore

@@ -9,3 +9,4 @@ log/
 
 #llm
 .claude/
+__pycache__/

CLAUDE.md

@@ -28,6 +28,8 @@ pixi run rviz           # RViz2 with mote config
 # never affects the robot/Pi env). The sim/sim-test tasks auto-select the sim
 # environment (defined only there), so no `-e sim` is needed for them.
 pixi run sim            # Headless Gazebo sim: world + robot + controllers
+#   Trailing args pass through to the launch, so pick a world with:
+#     pixi run sim world:=office_world.sdf   (default: mote_world.sdf)
 pixi run sim-test       # ~20 s headless smoke test (local pre-PR gate, needs a GPU)
 # Ad-hoc (non-task) commands still need the env named:
 #   pixi run -e sim -- ros2 launch mote_bringup slam_launch.py use_sim_time:=true
@@ -55,7 +57,7 @@ The URDF reads it via `xacro.load_yaml('$(find mote_description)/config/robot.ya
 
 ## Architecture
 
-Mote is a differential-drive robot built on **ROS 2 Jazzy**, managed entirely through pixi (no system ROS install required). Three first-party packages:
+Mote is a differential-drive robot built on **ROS 2 Jazzy**, managed entirely through pixi (no system ROS install required). Four first-party packages:
 
 ### `mote_hardware` (C++)
 A `ros2_control` `SystemInterface` plugin (`MoteHardware`) that drives two Feetech STS3215 servos via the SCServo SDK over a serial bus. Key implementation details:
@@ -77,7 +79,6 @@ Launch files, config, udev rules, and systemd services.
 - `mote_launch.py` — main bringup: robot_state_publisher, ros2_control_node, controller spawners, sllidar, laser_filter, v4l2_camera, and `localization_launch.py`. Reads `robot.yaml` for wheel geometry (injected into DiffDriveController params) and sensor config.
 - `localization_launch.py` — kinematic_icp LIDAR odometry (publishes `odom`→`base`; the map→odom corrector is slam_toolbox when mapping or AMCL when navigating). Despite the name, it does *not* run AMCL — AMCL lives in `nav2_launch.py`.
 - `slam_launch.py` — slam_toolbox (accepts `use_sim_time:=true` for the sim)
-- `sim_launch.py` — Gazebo sim (sim environment only): headless gz server with `worlds/mote_world.sdf`, robot spawn, ros_gz bridge (/clock, /scan), controllers, laser_filter. The URDF is processed with `use_sim:=true`, which swaps `MoteHardware` for `gz_ros2_control` and adds a simulated lidar (specs from `robot.yaml` `lidar.sim`). Without that flag the xacro output is unchanged. Controller params are merged into one temp file (gz_ros2_control loads a single `<parameters>` file referenced in the URDF).
 - `nav2_launch.py` — Nav2 stack
 - `rviz_launch.py` — RViz2 (dev environment only)
 
@@ -88,6 +89,12 @@ Launch files, config, udev rules, and systemd services.
 - `slam_toolbox_params.yaml` — SLAM toolbox parameters
 - `mote.rviz` — RViz2 display config
 
+### `mote_simulation` (Python/ament)
+Workstation-only Gazebo simulation, kept separate from `mote_bringup` so it can be excluded from the robot sync (`pixi run sync` skips `mote_simulation/`). Built only in the `sim` pixi environment. Contains:
+- `launch/sim_launch.py` — Gazebo sim: headless gz server, robot spawn, ros_gz bridge (/clock, /scan), controllers, laser_filter, and the shared `localization_launch.py`. Takes a `world:=` arg (file in `mote_simulation/worlds/`, default `mote_world.sdf` — the simple smoke-test room; `office_world.sdf` is a larger hospital-ward layout for stress-testing localisation). The URDF is processed with `use_sim:=true`, which swaps `MoteHardware` for `gz_ros2_control` and adds a simulated lidar (specs from `robot.yaml` `lidar.sim`). Without that flag the xacro output is unchanged. Controller params are merged into one temp file (gz_ros2_control loads a single `<parameters>` file referenced in the URDF). It pulls `controllers.yaml`, `laser_filters.yaml`, and `localization_launch.py` from `mote_bringup`'s share so the sim and the real robot can't drift apart.
+- `worlds/` — `mote_world.sdf` (smoke-test room) and `office_world.sdf` (hospital-ward stress layout).
+- `test/sim_smoke/` — `run_sim_smoke.sh` + `verify_sim.py`, the `pixi run sim-test` gate.
+
 ### Third-party submodules (`third_party/`)
 - `sllidar_ros2` — SLAMTEC RPLIDAR C1 ROS 2 driver
 - `kinematic_icp` — kinematic-ICP LIDAR odometry (reads raw wheel odom TF)

README.md

@@ -66,6 +66,7 @@ way to package everything up without worrying about ecosystem concerns.
 | [`mote_bringup`](mote_bringup/)         | Launch files for bringing up the robot                    |
 | [`mote_description`](mote_description/) | URDF robot model and TF tree                              |
 | [`mote_hardware`](mote_hardware/)       | ros2_control hardware interface for the Feetech servo bus |
+| [`mote_simulation`](mote_simulation/)   | Gazebo sim, worlds, and smoke test (workstation only)     |
 
 I'm trying to keep all dependencies from
 [Robostack](https://robostack.github.io/index.html) or `conda-forge`. Anything
@@ -187,9 +188,9 @@ robot, and asserts odometry integrates the motion, the lidar publishes sane
 scans, and slam_toolbox produces a map. It needs a working render backend
 (a GPU or fast software GL), so it's a local pre-PR gate rather than a
 hosted-CI job — see the comment in
-[`run_sim_smoke.sh`](mote_bringup/test/sim_smoke/run_sim_smoke.sh).
+[`run_sim_smoke.sh`](mote_simulation/test/sim_smoke/run_sim_smoke.sh).
 
-The world (`mote_bringup/worlds/mote_world.sdf`) is a simple walled room with
+The world (`mote_simulation/worlds/mote_world.sdf`) is a simple walled room with
 a few obstacles. The simulated lidar uses RPLIDAR C1 datasheet values from
 [`robot.yaml`](mote_description/config/robot.yaml).
 

mote_bringup/config/controllers.yaml

@@ -28,7 +28,8 @@ diff_drive_controller:
 
     odom_frame_id: odom
     base_frame_id: base_footprint
-    enable_odom_tf: true        # kinematic-icp reads this raw wheel odom TF
+    enable_odom_tf: false       # odom->base is owned by kinematic_icp; wheel
+                                # odom reaches it via the odom_tf_relay leaf
 
     # Covariance for a 2D robot — z/roll/pitch are constrained
     pose_covariance_diagonal:  [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]

mote_bringup/launch/localization_launch.py

@@ -1,26 +1,48 @@
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
+    use_sim_time = LaunchConfiguration("use_sim_time")
+
+    # Wheel odom relayed as an inverted leaf (base_footprint -> odom_wheel) so
+    # kinematic_icp can own the odom->base edge while still reading the wheel
+    # odometry as its motion prior.
+    odom_relay = Node(
+        package="mote_bringup",
+        executable="odom_tf_relay",
+        parameters=[{"use_sim_time": use_sim_time, "child_frame": "odom_wheel"}],
+        remappings=[("odom_in", "/diff_drive_controller/odom")],
+    )
+
+    # Lidar odometry (wheel prior fused in) publishes odom->base, which slam and
+    # nav consume instead of raw wheel odom.
     kinematic_icp = Node(
         package="kinematic_icp",
         executable="kinematic_icp_online_node",
         name="online_node",
         namespace="kinematic_icp",
         output="screen",
-        parameters=[{
-            "lidar_topic": "/scan_filtered",
-            "use_2d_lidar": True,
-            "lidar_odom_frame": "odom_lidar",
-            "wheel_odom_frame": "odom",
-            "base_frame": "base_footprint",
-            "publish_odom_tf": True,
-            "invert_odom_tf": True,
-            "tf_timeout": 0.05,
-        }],
-        remappings=[
-            ("lidar_odometry", "lidar_odometry"),
+        parameters=[
+            {
+                "lidar_topic": "/scan_filtered",
+                "use_2d_lidar": True,
+                "lidar_odom_frame": "odom",
+                "wheel_odom_frame": "odom_wheel",
+                "base_frame": "base_footprint",
+                "publish_odom_tf": True,
+                "invert_odom_tf": False,
+                "tf_timeout": 0.05,
+                "use_sim_time": use_sim_time,
+            }
         ],
     )
-    return LaunchDescription([kinematic_icp])
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+            odom_relay,
+            kinematic_icp,
+        ]
+    )

mote_bringup/mote_bringup/odom_tf_relay.py

@@ -0,0 +1,70 @@
+"""Republish wheel odometry as an inverted TF leaf so a lidar-odometry node can
+own the odom->base edge while still receiving the wheel odometry as a prior.
+
+diff_drive publishes the wheel pose (base in odom) on a topic; this node
+broadcasts its inverse as base_frame -> child_frame (a leaf). kinematic_icp,
+configured with wheel_odom_frame = child_frame, reads that leaf as its motion
+prior and is then free to publish the real odom -> base transform itself.
+"""
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import TransformStamped
+from tf2_ros import TransformBroadcaster
+
+
+def _rotate(q, v):
+    """Rotate vector v by quaternion q = (x, y, z, w)."""
+    x, y, z, w = q
+    ux = 2.0 * (y * v[2] - z * v[1])
+    uy = 2.0 * (z * v[0] - x * v[2])
+    uz = 2.0 * (x * v[1] - y * v[0])
+    return (
+        v[0] + w * ux + (y * uz - z * uy),
+        v[1] + w * uy + (z * ux - x * uz),
+        v[2] + w * uz + (x * uy - y * ux),
+    )
+
+
+class OdomTfRelay(Node):
+    def __init__(self):
+        super().__init__("odom_tf_relay")
+        self.child_frame = self.declare_parameter("child_frame", "odom_wheel").value
+        self.br = TransformBroadcaster(self)
+        self.create_subscription(Odometry, "odom_in", self._cb, 10)
+
+    def _cb(self, msg):
+        p = msg.pose.pose.position
+        q = msg.pose.pose.orientation
+        q_inv = (-q.x, -q.y, -q.z, q.w)
+        ti = _rotate(q_inv, (p.x, p.y, p.z))
+
+        t = TransformStamped()
+        t.header.stamp = msg.header.stamp
+        t.header.frame_id = msg.child_frame_id  # base_footprint
+        t.child_frame_id = self.child_frame  # odom_wheel (leaf)
+        t.transform.translation.x = -ti[0]
+        t.transform.translation.y = -ti[1]
+        t.transform.translation.z = -ti[2]
+        t.transform.rotation.x = q_inv[0]
+        t.transform.rotation.y = q_inv[1]
+        t.transform.rotation.z = q_inv[2]
+        t.transform.rotation.w = q_inv[3]
+        self.br.sendTransform(t)
+
+
+def main():
+    rclpy.init()
+    node = OdomTfRelay()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

mote_bringup/setup.py

@@ -14,7 +14,6 @@
         ("share/" + package_name, ["package.xml"]),
         (os.path.join("share", package_name, "launch"), glob("launch/*")),
         (os.path.join("share", package_name, "config"), glob("config/*")),
-        (os.path.join("share", package_name, "worlds"), glob("worlds/*")),
     ],
     install_requires=["setuptools"],
     zip_safe=True,
@@ -23,6 +22,8 @@
     description="Launch files for the mote",
     license="Apache-2.0",
     entry_points={
-        "console_scripts": [],
+        "console_scripts": [
+            "odom_tf_relay = mote_bringup.odom_tf_relay:main",
+        ],
     },
 )

mote_bringup/launch/sim_launch.py → mote_simulation/launch/sim_launch.py

@@ -12,16 +12,23 @@
 import yaml
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, RegisterEventHandler
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
 from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 
 
 def generate_launch_description():
     description_share = get_package_share_directory("mote_description")
     bringup_share = get_package_share_directory("mote_bringup")
+    sim_share = get_package_share_directory("mote_simulation")
 
     with open(os.path.join(description_share, "config", "robot.yaml")) as f:
         cfg = yaml.safe_load(f)
@@ -33,11 +40,13 @@ def generate_launch_description():
     with open(os.path.join(bringup_share, "config", "controllers.yaml")) as f:
         controller_params = yaml.safe_load(f)
     controller_params["controller_manager"]["ros__parameters"]["use_sim_time"] = True
-    controller_params["diff_drive_controller"]["ros__parameters"].update({
-        "wheel_separation": cfg["wheel_separation"],
-        "wheel_radius": cfg["wheel_radius"],
-        "use_sim_time": True,
-    })
+    controller_params["diff_drive_controller"]["ros__parameters"].update(
+        {
+            "wheel_separation": cfg["wheel_separation"],
+            "wheel_radius": cfg["wheel_radius"],
+            "use_sim_time": True,
+        }
+    )
     sim_controllers_file = tempfile.NamedTemporaryFile(
         mode="w", prefix="mote_sim_controllers_", suffix=".yaml", delete=False
     )
@@ -54,14 +63,22 @@ def generate_launch_description():
         "use_sim_time": True,
     }
 
-    world = os.path.join(bringup_share, "worlds", "mote_world.sdf")
+    # The world is selectable so the same launch can drive the simple
+    # smoke-test room (default) or a larger stress-test layout, e.g.
+    #   ros2 launch mote_simulation sim_launch.py world:=office_world.sdf
+    world = PathJoinSubstitution([sim_share, "worlds", LaunchConfiguration("world")])
     # gz only searches its own plugin dirs; libgz_ros2_control-system.so lives
     # in the conda env's lib dir
     gz_env = dict(os.environ)
-    gz_env["GZ_SIM_SYSTEM_PLUGIN_PATH"] = os.pathsep.join(filter(None, [
-        os.path.join(os.environ.get("CONDA_PREFIX", ""), "lib"),
-        os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""),
-    ]))
+    gz_env["GZ_SIM_SYSTEM_PLUGIN_PATH"] = os.pathsep.join(
+        filter(
+            None,
+            [
+                os.path.join(os.environ.get("CONDA_PREFIX", ""), "lib"),
+                os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""),
+            ],
+        )
+    )
     gz_server = ExecuteProcess(
         cmd=["gz", "sim", "-r", "-s", "-v", "1", world],
         env=gz_env,
@@ -118,16 +135,33 @@ def generate_launch_description():
         ],
     )
 
-    return LaunchDescription([
-        gz_server,
-        robot_state_publisher,
-        spawn_robot,
-        bridge,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=spawn_robot,
-                on_exit=[joint_state_broadcaster_spawner, diff_drive_spawner],
-            )
+    # Lidar odometry (kinematic_icp) + wheel-odom relay — the same localization
+    # stack the real robot runs, so the two can't drift out of sync.
+    localization = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(bringup_share, "launch", "localization_launch.py")
         ),
-        laser_filter,
-    ])
+        launch_arguments={"use_sim_time": "true"}.items(),
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "world",
+                default_value="mote_world.sdf",
+                description="World file in mote_simulation/worlds to load",
+            ),
+            gz_server,
+            robot_state_publisher,
+            spawn_robot,
+            bridge,
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=spawn_robot,
+                    on_exit=[joint_state_broadcaster_spawner, diff_drive_spawner],
+                )
+            ),
+            laser_filter,
+            localization,
+        ]
+    )

mote_simulation/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mote_simulation</name>
+  <version>0.0.0</version>
+  <description>Gazebo simulation bringup, worlds, and smoke test for the mote (workstation only)</description>
+  <maintainer email="michael@clach.dev">Michael Johnson</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>mote_bringup</exec_depend>
+  <exec_depend>mote_description</exec_depend>
+  <exec_depend>ros_gz_sim</exec_depend>
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>gz_ros2_control</exec_depend>
+  <exec_depend>laser_filters</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

mote_simulation/resource/mote_simulation

@@ -0,0 +1 @@
+mote_simulation

mote_simulation/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/mote_simulation
+[install]
+install_scripts=$base/lib/mote_simulation

mote_simulation/setup.py

@@ -0,0 +1,27 @@
+import os
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "mote_simulation"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*")),
+        (os.path.join("share", package_name, "worlds"), glob("worlds/*")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Michael Johnson",
+    maintainer_email="michael@clach.dev",
+    description="Gazebo simulation bringup, worlds, and smoke test for the mote",
+    license="Apache-2.0",
+    entry_points={
+        "console_scripts": [],
+    },
+)