From 54ed5c6a82f79fbfc2b9ce3b0a32e59e009a232e Mon Sep 17 00:00:00 2001
From: Matt Spencer <matthew@thespencers.me.uk>
Date: Thu, 7 May 2026 20:41:18 +0000
Subject: [PATCH] Refactor the URDF into multiple parts.

Makes it easier to work on an individual part in isolation.
Also change the vscode devconsole visualiser to one that works with includes
---
 .devcontainer/Dockerfile                      |   5 +
 .devcontainer/devcontainer.json               |   2 +-
 .gitignore                                    |   3 +
 robot/src/raspbot_v2/launch/robot.launch.py   |   5 +-
 robot/src/raspbot_v2/setup.py                 |   4 +-
 robot/src/raspbot_v2/urdf/camera.xacro        |  83 +++++++
 robot/src/raspbot_v2/urdf/lidar.xacro         |  77 +++++++
 robot/src/raspbot_v2/urdf/pan_tilt.xacro      | 134 ++++++++++++
 .../src/raspbot_v2/urdf/raspbot_v2.urdf.xacro | 205 ++++++++----------
 robot/src/raspbot_v2/urdf/robot_base.xacro    | 133 ++++++++++++
 robot/src/raspbot_v2/urdf/sonar.xacro         |  58 +++++
 11 files changed, 587 insertions(+), 122 deletions(-)
 create mode 100644 robot/src/raspbot_v2/urdf/camera.xacro
 create mode 100644 robot/src/raspbot_v2/urdf/lidar.xacro
 create mode 100644 robot/src/raspbot_v2/urdf/pan_tilt.xacro
 create mode 100644 robot/src/raspbot_v2/urdf/robot_base.xacro
 create mode 100644 robot/src/raspbot_v2/urdf/sonar.xacro

diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
index fc5cf9f..2661feb 100644
--- a/.devcontainer/Dockerfile
+++ b/.devcontainer/Dockerfile
@@ -46,6 +46,11 @@ RUN apt-get update && apt-get install -y \
 #     ros-${ROS_DISTRO}-ament-lint-auto \
 #     ros-${ROS_DISTRO}-ament-lint-common
 
+# URDF / xacro tooling — needed to generate flat URDF for IDE preview
+RUN apt-get update && apt-get install -y \
+    ros-${ROS_DISTRO}-xacro \
+    ros-${ROS_DISTRO}-robot-state-publisher
+
 # Node.js for frontend development
 RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \
     && apt-get install -y nodejs
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 827b1df..9da4688 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -21,7 +21,7 @@
                 "eamodio.gitlens",
                 "ms-iot.vscode-ros",
                 "anthropic.claude-code",
-                "Ranch-Hand-Robotics.urdf-editor"
+                "morningfrog.urdf-visualizer"
             ]
         }
     },
diff --git a/.gitignore b/.gitignore
index 5578fe3..d94a87b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,9 @@
 .vscode/browse.vc.db-wal
 .vscode/c_cpp_properties.json
 
+# Generated URDF preview (regenerate with the "Generate preview URDF" VS Code task)
+**/urdf/preview.urdf
+
 # colcon build artefacts
 build/
 install/
diff --git a/robot/src/raspbot_v2/launch/robot.launch.py b/robot/src/raspbot_v2/launch/robot.launch.py
index 732b562..a54f189 100644
--- a/robot/src/raspbot_v2/launch/robot.launch.py
+++ b/robot/src/raspbot_v2/launch/robot.launch.py
@@ -13,7 +13,10 @@ def generate_launch_description():
         get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro'
     )
     robot_description = ParameterValue(
-        Command(['xacro ', urdf_file]),
+        Command([
+            'xacro ', urdf_file,
+            ' wheel_separation:=', LaunchConfiguration('wheel_base'),
+        ]),
         value_type=str,
     )
 
diff --git a/robot/src/raspbot_v2/setup.py b/robot/src/raspbot_v2/setup.py
index e7c3816..61221cf 100644
--- a/robot/src/raspbot_v2/setup.py
+++ b/robot/src/raspbot_v2/setup.py
@@ -1,3 +1,5 @@
+import glob
+
 from setuptools import setup
 
 package_name = 'raspbot_v2'
@@ -10,7 +12,7 @@ setup(
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/launch', ['launch/robot.launch.py']),
-        ('share/' + package_name + '/urdf',   ['urdf/raspbot_v2.urdf.xacro']),
+        ('share/' + package_name + '/urdf',   glob.glob('urdf/*.xacro')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
diff --git a/robot/src/raspbot_v2/urdf/camera.xacro b/robot/src/raspbot_v2/urdf/camera.xacro
new file mode 100644
index 0000000..c8eca3a
--- /dev/null
+++ b/robot/src/raspbot_v2/urdf/camera.xacro
@@ -0,0 +1,83 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Camera module
+
+       Mounted on camera_tilt_link (output of the tilt joint in pan_tilt.xacro).
+
+       Two frames are defined:
+         camera_link         — physical body of the camera module
+         camera_optical_frame — REP-103 optical frame (Z forward, X right, Y down)
+                                used as the sensor origin in Gazebo and ROS image topics
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <!-- Physical camera body — visual only, small box representing the module -->
+  <link name="camera_link">
+    <visual>
+      <geometry><box size="0.025 0.06 0.02"/></geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <geometry><box size="0.025 0.06 0.02"/></geometry>
+    </collision>
+    <inertial>
+      <mass value="0.03"/>
+      <inertia ixx="0.000005" ixy="0" ixz="0"
+               iyy="0.000005" iyz="0" izz="0.000005"/>
+    </inertial>
+  </link>
+
+  <joint name="camera_joint" type="fixed">
+    <parent link="camera_tilt_link"/>
+    <child  link="camera_link"/>
+    <origin xyz="0.01 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- REP-103 optical frame: Z forward, X right, Y down.
+       The rpy="-pi/2 0 -pi/2" rotation maps the body frame (X-fwd, Y-left, Z-up)
+       to the optical frame convention expected by image_geometry and cv_bridge. -->
+  <link name="camera_optical_frame"/>
+
+  <joint name="camera_optical_joint" type="fixed">
+    <parent link="camera_link"/>
+    <child  link="camera_optical_frame"/>
+    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
+  </joint>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Gazebo camera sensor  (Gazebo Harmonic / gz-sim)
+
+       Image data is published on gz topics and bridged to ROS 2 via
+       ros_gz_bridge:
+         /camera/image  (gz) → /camera/image_raw  (ROS 2)
+         /camera/info   (gz) → /camera/camera_info (ROS 2)
+       ───────────────────────────────────────────────────────────────────── -->
+  <gazebo reference="camera_link">
+    <sensor name="camera" type="camera">
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>true</visualize>
+      <camera>
+        <horizontal_fov>${camera_fov}</horizontal_fov>
+        <image>
+          <width>${camera_width}</width>
+          <height>${camera_height}</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.05</near>
+          <far>50.0</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <!-- Publishes to gz topic /camera/image -->
+      <topic>/camera/image</topic>
+    </sensor>
+  </gazebo>
+
+</robot>
diff --git a/robot/src/raspbot_v2/urdf/lidar.xacro b/robot/src/raspbot_v2/urdf/lidar.xacro
new file mode 100644
index 0000000..69e32f9
--- /dev/null
+++ b/robot/src/raspbot_v2/urdf/lidar.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       RPLIDAR A1 — 2D laser scanner
+
+       frame_id "laser" matches what sllidar_ros2 publishes on /scan.
+       Adjust xyz to the physical mounting position on the chassis.
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <link name="laser">
+    <visual>
+      <geometry>
+        <cylinder radius="0.04" length="0.04"/>
+      </geometry>
+      <material name="light_grey"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder radius="0.04" length="0.04"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.17"/>
+      <!-- Solid cylinder -->
+      <inertia ixx="0.000087" ixy="0" ixz="0"
+               iyy="0.000087" iyz="0"
+               izz="0.000136"/>
+    </inertial>
+  </link>
+
+  <joint name="laser_joint" type="fixed">
+    <parent link="base_link"/>
+    <child  link="laser"/>
+    <!-- x/y/z: position of lidar centre relative to base_link centre.
+         x negative = rear-mounted; adjust to your physical layout. -->
+    <origin xyz="${lidar_x} ${lidar_y} ${lidar_z}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Gazebo gpu_lidar sensor  (Gazebo Harmonic / gz-sim)
+
+       Scan data is published on gz topic /scan and bridged to ROS 2 via
+       ros_gz_bridge:
+         /scan  (gz) → /scan  (ROS 2, sensor_msgs/LaserScan)
+       ───────────────────────────────────────────────────────────────────── -->
+  <gazebo reference="laser">
+    <sensor name="lidar" type="gpu_lidar">
+      <always_on>1</always_on>
+      <update_rate>10</update_rate>
+      <visualize>true</visualize>
+      <topic>/scan</topic>
+      <gz_frame_id>laser</gz_frame_id>
+      <lidar>
+        <scan>
+          <horizontal>
+            <samples>360</samples>
+            <resolution>1</resolution>
+            <min_angle>-3.14159265</min_angle>
+            <max_angle> 3.14159265</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>${lidar_min_range}</min>
+          <max>${lidar_max_range}</max>
+          <resolution>0.01</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.01</stddev>
+        </noise>
+      </lidar>
+    </sensor>
+  </gazebo>
+
+</robot>
diff --git a/robot/src/raspbot_v2/urdf/pan_tilt.xacro b/robot/src/raspbot_v2/urdf/pan_tilt.xacro
new file mode 100644
index 0000000..636f00b
--- /dev/null
+++ b/robot/src/raspbot_v2/urdf/pan_tilt.xacro
@@ -0,0 +1,134 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Camera pan / tilt mechanism
+
+       Joint names (pan, tilt) must match the names published by
+       camera_orientation_node on /joint_states so that robot_state_publisher
+       drives the TF tree correctly from live hardware.
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <!-- Mount bracket — fixed to the top-front of the chassis -->
+  <link name="camera_base">
+    <visual>
+      <geometry><box size="0.03 0.04 0.03"/></geometry>
+      <material name="dark_grey"/>
+    </visual>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0"
+               iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+  </link>
+
+  <joint name="camera_base_joint" type="fixed">
+    <parent link="base_link"/>
+    <child  link="camera_base"/>
+    <!-- Adjust xyz to match the physical servo mount position -->
+    <origin xyz="${base_length/2 - 0.02} 0.0 ${base_height/2 + 0.015}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- Pan link — rotates around Z (yaw) -->
+  <link name="camera_pan_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.000001" ixy="0" ixz="0"
+               iyy="0.000001" iyz="0" izz="0.000001"/>
+    </inertial>
+  </link>
+
+  <joint name="pan" type="revolute">
+    <parent link="camera_base"/>
+    <child  link="camera_pan_link"/>
+    <axis xyz="0 0 1"/>
+    <origin xyz="0 0 0.02" rpy="0 0 0"/>
+    <!-- Physical range: 0°–180°, centred at 90° → ROS range: ±90° -->
+    <limit lower="${pan_min_rad}" upper="${pan_max_rad}"
+           effort="1.0" velocity="1.5"/>
+    <dynamics damping="0.1" friction="0.05"/>
+  </joint>
+
+  <!-- Tilt link — rotates around Y (pitch) -->
+  <link name="camera_tilt_link">
+    <inertial>
+      <mass value="0.02"/>
+      <inertia ixx="0.000002" ixy="0" ixz="0"
+               iyy="0.000002" iyz="0" izz="0.000002"/>
+    </inertial>
+  </link>
+
+  <joint name="tilt" type="revolute">
+    <parent link="camera_pan_link"/>
+    <child  link="camera_tilt_link"/>
+    <axis xyz="0 1 0"/>
+    <origin xyz="0.02 0 0" rpy="0 0 0"/>
+    <!-- Physical range: 0°–110°, centred at 60° → ROS range: -60° to +50° -->
+    <limit lower="${tilt_min_rad}" upper="${tilt_max_rad}"
+           effort="1.0" velocity="1.5"/>
+    <dynamics damping="0.1" friction="0.05"/>
+  </joint>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Transmissions  (for ros2_control)
+
+       These describe the mapping between joint commands and actuators.
+       Required when using gz_ros2_control for Gazebo simulation.
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <transmission name="pan_transmission">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="pan">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="pan_actuator">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="tilt_transmission">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="tilt">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="tilt_actuator">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       ros2_control block
+
+       For Gazebo simulation, set the hardware plugin to:
+         gz_ros2_control/GazeboSimSystem
+       For the real robot, replace with the custom I²C servo hardware plugin.
+       ───────────────────────────────────────────────────────────────────── -->
+  <ros2_control name="pan_tilt_servos" type="system">
+    <hardware>
+      <!-- Gazebo simulation: -->
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+      <!-- Real robot (replace with actual plugin when available):
+      <plugin>raspbot_v2_hardware/ServoHardware</plugin>
+      -->
+    </hardware>
+    <joint name="pan">
+      <command_interface name="position">
+        <param name="min">${pan_min_rad}</param>
+        <param name="max">${pan_max_rad}</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="tilt">
+      <command_interface name="position">
+        <param name="min">${tilt_min_rad}</param>
+        <param name="max">${tilt_max_rad}</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+
+</robot>
diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
index 699d35a..8d3e511 100644
--- a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
+++ b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
@@ -1,132 +1,99 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
 
-  <!--
-    Physical dimensions — measure your robot and update these.
-    All values in metres.
-  -->
-  <xacro:property name="base_length" value="0.20"/>
-  <xacro:property name="base_width"  value="0.15"/>
-  <xacro:property name="base_height" value="0.06"/>
+  <!-- ═══════════════════════════════════════════════════════════════════════
+       raspbot_v2.urdf.xacro — top-level composition file
 
-  <!-- ── Materials ──────────────────────────────────────────────── -->
-  <material name="dark_grey">
-    <color rgba="0.2 0.2 0.2 1.0"/>
-  </material>
-  <material name="light_grey">
-    <color rgba="0.7 0.7 0.7 1.0"/>
-  </material>
-  <material name="blue">
-    <color rgba="0.0 0.4 0.8 1.0"/>
-  </material>
+       All physical parameters are declared here as xacro:arg so they can
+       be overridden at build time:
+         xacro raspbot_v2.urdf.xacro wheel_separation:=0.14
 
-  <!-- ── Base link ──────────────────────────────────────────────── -->
-  <link name="base_link">
-    <visual>
-      <geometry>
-        <box size="${base_length} ${base_width} ${base_height}"/>
-      </geometry>
-      <material name="dark_grey"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="${base_length} ${base_width} ${base_height}"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="1.0"/>
-      <inertia ixx="0.01" ixy="0.0" ixz="0.0"
-               iyy="0.01" iyz="0.0" izz="0.02"/>
-    </inertial>
-  </link>
+       Sub-files reference these properties directly; they are not intended
+       to be included in isolation.
+       ═══════════════════════════════════════════════════════════════════════ -->
 
-  <!-- ── Footprint (ground-projected origin) ────────────────────── -->
-  <link name="base_footprint"/>
-  <joint name="base_footprint_joint" type="fixed">
-    <parent link="base_footprint"/>
-    <child  link="base_link"/>
-    <origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
-  </joint>
+  <!-- ── Chassis ────────────────────────────────────────────────────────── -->
+  <xacro:arg name="base_length"    default="0.20"/>  <!-- m, front-to-rear -->
+  <xacro:arg name="base_width"     default="0.15"/>  <!-- m, left-to-right -->
+  <xacro:arg name="base_height"    default="0.06"/>  <!-- m -->
+  <xacro:arg name="base_mass"      default="1.0"/>   <!-- kg -->
 
-  <!-- ── Lidar ──────────────────────────────────────────────────── -->
-  <!--
-    frame_id published by sllidar_ros2 is "laser".
-    Adjust xyz to where the lidar is mounted on your robot.
-    Typical: top-centre, facing forward.
-  -->
-  <link name="laser">
-    <visual>
-      <geometry><cylinder radius="0.04" length="0.04"/></geometry>
-      <material name="light_grey"/>
-    </visual>
-  </link>
-  <joint name="laser_joint" type="fixed">
-    <parent link="base_link"/>
-    <child  link="laser"/>
-    <!-- Adjust x/y/z: x=forward, y=left, z=up from base_link centre -->
-    <origin xyz="-0.03 0.0 0.09" rpy="0 0 0"/>
-  </joint>
+  <!-- ── Wheels ─────────────────────────────────────────────────────────── -->
+  <!-- wheel_separation: centre-to-centre distance between left and right wheels.
+       Should match the wheel_base parameter in the motor controller node. -->
+  <xacro:arg name="wheel_separation" default="0.14"/> <!-- m -->
+  <xacro:arg name="wheel_radius"     default="0.033"/> <!-- m -->
+  <xacro:arg name="wheel_width"      default="0.02"/>  <!-- m -->
+  <xacro:arg name="wheel_mass"       default="0.1"/>   <!-- kg -->
+  <!-- Front-to-rear distance between wheel pairs (half-value used per side) -->
+  <xacro:arg name="wheel_base_half"  default="0.07"/>  <!-- m -->
 
-  <!-- ── Ultrasonic sensor ──────────────────────────────────────── -->
-  <!--
-    frame_id published by ultrasonic_node is "ultrasonic".
-    Typical: front face of the chassis, pointing forward.
-  -->
-  <link name="ultrasonic">
-    <visual>
-      <geometry><box size="0.02 0.04 0.02"/></geometry>
-      <material name="light_grey"/>
-    </visual>
-  </link>
-  <joint name="ultrasonic_joint" type="fixed">
-    <parent link="base_link"/>
-    <child  link="ultrasonic"/>
-    <!-- Front-centre of the chassis, roughly mid-height -->
-    <origin xyz="${base_length/2} 0.0 0.0" rpy="0 0 0"/>
-  </joint>
+  <!-- ── Pan / tilt limits (radians, ROS convention: 0 = centre) ────────── -->
+  <!-- Pan:  physical 0°–180°, centred at 90°  → ±π/2                       -->
+  <!-- Tilt: physical 0°–110°, centred at 60°  → −60° (−1.047) to +50° (+0.873) -->
+  <xacro:arg name="pan_min_rad"  default="${-pi/2}"/>
+  <xacro:arg name="pan_max_rad"  default="${ pi/2}"/>
+  <xacro:arg name="tilt_min_rad" default="${-pi/3}"/>   <!-- −60° -->
+  <xacro:arg name="tilt_max_rad" default="${5*pi/18}"/> <!-- +50° -->
 
-  <!-- ── Camera pan/tilt mount ──────────────────────────────────── -->
-  <!--
-    The camera_orientation node publishes joint_states for joints
-    named "pan" and "tilt".  robot_state_publisher will drive these
-    dynamically from that topic.
-  -->
-  <link name="camera_base">
-    <visual>
-      <geometry><box size="0.03 0.03 0.03"/></geometry>
-      <material name="dark_grey"/>
-    </visual>
-  </link>
-  <joint name="camera_base_joint" type="fixed">
-    <parent link="base_link"/>
-    <child  link="camera_base"/>
-    <!-- Top-front of robot; adjust to match servo mount position -->
-    <origin xyz="0.05 0.0 0.06" rpy="0 0 0"/>
-  </joint>
+  <!-- ── Camera ─────────────────────────────────────────────────────────── -->
+  <xacro:arg name="camera_fov"    default="1.047"/>  <!-- rad (~60°) -->
+  <xacro:arg name="camera_width"  default="640"/>
+  <xacro:arg name="camera_height" default="480"/>
 
-  <!-- Pan joint (yaw, around Z) -->
-  <link name="camera_pan_link"/>
-  <joint name="pan" type="revolute">
-    <parent link="camera_base"/>
-    <child  link="camera_pan_link"/>
-    <axis xyz="0 0 1"/>
-    <origin xyz="0 0 0.015" rpy="0 0 0"/>
-    <limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
-  </joint>
+  <!-- ── Lidar ──────────────────────────────────────────────────────────── -->
+  <!-- Mounting position relative to base_link centre -->
+  <xacro:arg name="lidar_x"         default="-0.03"/> <!-- m, slightly rearward -->
+  <xacro:arg name="lidar_y"         default="0.0"/>
+  <xacro:arg name="lidar_z"         default="0.09"/>  <!-- m above base_link centre -->
+  <xacro:arg name="lidar_min_range" default="0.15"/>  <!-- m (RPLIDAR A1 spec) -->
+  <xacro:arg name="lidar_max_range" default="12.0"/>  <!-- m (RPLIDAR A1 spec) -->
 
-  <!-- Tilt joint (pitch, around Y) -->
-  <link name="camera_link">
-    <visual>
-      <geometry><box size="0.03 0.06 0.02"/></geometry>
-      <material name="blue"/>
-    </visual>
-  </link>
-  <joint name="tilt" type="revolute">
-    <parent link="camera_pan_link"/>
-    <child  link="camera_link"/>
-    <axis xyz="0 1 0"/>
-    <origin xyz="0.02 0 0" rpy="0 0 0"/>
-    <limit lower="-1.0472" upper="0.7854" effort="1.0" velocity="1.0"/>
-  </joint>
+  <!-- ── Sonar ──────────────────────────────────────────────────────────── -->
+  <xacro:arg name="sonar_min_range" default="0.02"/>  <!-- m (HC-SR04 spec) -->
+  <xacro:arg name="sonar_max_range" default="4.0"/>   <!-- m (HC-SR04 spec) -->
+
+  <!-- ═══════════════════════════════════════════════════════════════════════
+       Resolve args → properties so sub-files can use ${name} syntax
+       ═══════════════════════════════════════════════════════════════════════ -->
+
+  <xacro:property name="base_length"    value="$(arg base_length)"/>
+  <xacro:property name="base_width"     value="$(arg base_width)"/>
+  <xacro:property name="base_height"    value="$(arg base_height)"/>
+  <xacro:property name="base_mass"      value="$(arg base_mass)"/>
+
+  <xacro:property name="wheel_separation" value="$(arg wheel_separation)"/>
+  <xacro:property name="wheel_radius"     value="$(arg wheel_radius)"/>
+  <xacro:property name="wheel_width"      value="$(arg wheel_width)"/>
+  <xacro:property name="wheel_mass"       value="$(arg wheel_mass)"/>
+  <xacro:property name="wheel_base_half"  value="$(arg wheel_base_half)"/>
+
+  <xacro:property name="pan_min_rad"  value="$(arg pan_min_rad)"/>
+  <xacro:property name="pan_max_rad"  value="$(arg pan_max_rad)"/>
+  <xacro:property name="tilt_min_rad" value="$(arg tilt_min_rad)"/>
+  <xacro:property name="tilt_max_rad" value="$(arg tilt_max_rad)"/>
+
+  <xacro:property name="camera_fov"    value="$(arg camera_fov)"/>
+  <xacro:property name="camera_width"  value="$(arg camera_width)"/>
+  <xacro:property name="camera_height" value="$(arg camera_height)"/>
+
+  <xacro:property name="lidar_x"         value="$(arg lidar_x)"/>
+  <xacro:property name="lidar_y"         value="$(arg lidar_y)"/>
+  <xacro:property name="lidar_z"         value="$(arg lidar_z)"/>
+  <xacro:property name="lidar_min_range" value="$(arg lidar_min_range)"/>
+  <xacro:property name="lidar_max_range" value="$(arg lidar_max_range)"/>
+
+  <xacro:property name="sonar_min_range" value="$(arg sonar_min_range)"/>
+  <xacro:property name="sonar_max_range" value="$(arg sonar_max_range)"/>
+
+  <!-- ═══════════════════════════════════════════════════════════════════════
+       Sub-file includes  (order matters — base must come first)
+       ═══════════════════════════════════════════════════════════════════════ -->
+
+  <xacro:include filename="robot_base.xacro"/>
+  <xacro:include filename="pan_tilt.xacro"/>
+  <xacro:include filename="camera.xacro"/>
+  <xacro:include filename="lidar.xacro"/>
+  <xacro:include filename="sonar.xacro"/>
 
 </robot>
diff --git a/robot/src/raspbot_v2/urdf/robot_base.xacro b/robot/src/raspbot_v2/urdf/robot_base.xacro
new file mode 100644
index 0000000..2022983
--- /dev/null
+++ b/robot/src/raspbot_v2/urdf/robot_base.xacro
@@ -0,0 +1,133 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Inertia helpers
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <xacro:macro name="box_inertia" params="m x y z">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia ixx="${m*(y*y+z*z)/12}" ixy="0" ixz="0"
+               iyy="${m*(x*x+z*z)/12}" iyz="0"
+               izz="${m*(x*x+y*y)/12}"/>
+    </inertial>
+  </xacro:macro>
+
+  <!-- Cylinder axis along Z -->
+  <xacro:macro name="cylinder_inertia" params="m r h">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
+               iyy="${m*(3*r*r+h*h)/12}" iyz="0"
+               izz="${m*r*r/2}"/>
+    </inertial>
+  </xacro:macro>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Materials
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <material name="dark_grey">  <color rgba="0.2 0.2 0.2 1.0"/></material>
+  <material name="light_grey"> <color rgba="0.7 0.7 0.7 1.0"/></material>
+  <material name="blue">       <color rgba="0.0 0.4 0.8 1.0"/></material>
+  <material name="black">      <color rgba="0.05 0.05 0.05 1.0"/></material>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Base chassis
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <link name="base_footprint"/>
+
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <box size="${base_length} ${base_width} ${base_height}"/>
+      </geometry>
+      <material name="dark_grey"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="${base_length} ${base_width} ${base_height}"/>
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="${base_mass}"
+                       x="${base_length}" y="${base_width}" z="${base_height}"/>
+  </link>
+
+  <!-- base_footprint is the ground-projected origin; base_link is the chassis body -->
+  <joint name="base_footprint_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child  link="base_link"/>
+    <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Wheels  (FL / FR / RL / RR)
+       Cylinders rotated 90° so their axis lies along Y (roll axis).
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <xacro:macro name="wheel" params="name x_sign y_sign">
+    <link name="wheel_${name}">
+      <visual>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+        <material name="black"/>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+      </collision>
+      <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
+    </link>
+
+    <joint name="wheel_${name}_joint" type="continuous">
+      <parent link="base_link"/>
+      <child  link="wheel_${name}"/>
+      <!-- x_sign: +1=front, -1=rear  |  y_sign: +1=left, -1=right -->
+      <origin xyz="${x_sign * wheel_base_half} ${y_sign * wheel_separation/2} ${-base_height/2 + wheel_radius}"
+              rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+    </joint>
+
+    <!-- Friction and contact properties for Gazebo -->
+    <gazebo reference="wheel_${name}">
+      <mu1>1.0</mu1>
+      <mu2>0.5</mu2>
+      <kp>1e6</kp>
+      <kd>1.0</kd>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:wheel name="front_left"  x_sign=" 1" y_sign=" 1"/>
+  <xacro:wheel name="front_right" x_sign=" 1" y_sign="-1"/>
+  <xacro:wheel name="rear_left"   x_sign="-1" y_sign=" 1"/>
+  <xacro:wheel name="rear_right"  x_sign="-1" y_sign="-1"/>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Gazebo — differential drive plugin
+       Bridges to /cmd_vel and publishes /odom.
+       ───────────────────────────────────────────────────────────────────── -->
+  <gazebo>
+    <plugin filename="gz-sim-diff-drive-system"
+            name="gz::sim::systems::DiffDrive">
+      <!-- Use averaged front/rear pairs for each side -->
+      <left_joint>wheel_front_left_joint</left_joint>
+      <left_joint>wheel_rear_left_joint</left_joint>
+      <right_joint>wheel_front_right_joint</right_joint>
+      <right_joint>wheel_rear_right_joint</right_joint>
+      <wheel_separation>${wheel_separation}</wheel_separation>
+      <wheel_radius>${wheel_radius}</wheel_radius>
+      <odom_publish_frequency>10</odom_publish_frequency>
+      <topic>cmd_vel</topic>
+      <odom_topic>odom</odom_topic>
+      <frame_id>odom</frame_id>
+      <child_frame_id>base_footprint</child_frame_id>
+    </plugin>
+  </gazebo>
+
+</robot>
diff --git a/robot/src/raspbot_v2/urdf/sonar.xacro b/robot/src/raspbot_v2/urdf/sonar.xacro
new file mode 100644
index 0000000..b6fd96e
--- /dev/null
+++ b/robot/src/raspbot_v2/urdf/sonar.xacro
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       HC-SR04 ultrasonic range sensor
+
+       frame_id "ultrasonic" matches what ultrasonic_node publishes on
+       /ultrasonic/range.  Mounted on the front face of the chassis,
+       pointing forward.
+       ───────────────────────────────────────────────────────────────────── -->
+
+  <link name="ultrasonic">
+    <visual>
+      <geometry><box size="0.02 0.04 0.02"/></geometry>
+      <material name="light_grey"/>
+    </visual>
+    <collision>
+      <geometry><box size="0.02 0.04 0.02"/></geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.000001" ixy="0" ixz="0"
+               iyy="0.000001" iyz="0" izz="0.000001"/>
+    </inertial>
+  </link>
+
+  <joint name="ultrasonic_joint" type="fixed">
+    <parent link="base_link"/>
+    <child  link="ultrasonic"/>
+    <!-- Front-centre of the chassis, mid-height -->
+    <origin xyz="${base_length/2 + 0.01} 0.0 0.0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Gazebo sonar sensor  (Gazebo Harmonic / gz-sim)
+
+       Simulated as a narrow-cone ray sensor. Data is published on gz topic
+       /ultrasonic/range and bridged to ROS 2 via ros_gz_bridge:
+         /ultrasonic/range  (gz) → /ultrasonic/range  (ROS 2, sensor_msgs/Range)
+       ───────────────────────────────────────────────────────────────────── -->
+  <gazebo reference="ultrasonic">
+    <sensor name="sonar" type="sonar">
+      <always_on>1</always_on>
+      <update_rate>10</update_rate>
+      <visualize>true</visualize>
+      <topic>/ultrasonic/range</topic>
+      <gz_frame_id>ultrasonic</gz_frame_id>
+      <sonar>
+        <!-- HC-SR04 datasheet values -->
+        <min>${sonar_min_range}</min>
+        <max>${sonar_max_range}</max>
+        <!-- Half-angle of the cone in radians (~15° full FOV → 0.13 rad half-angle) -->
+        <radius>0.13</radius>
+      </sonar>
+    </sensor>
+  </gazebo>
+
+</robot>