Migrate to a new physical robot

Uses the mecanum controller properly across physical and virtual There is a timing issue with i2c which is why the control update is limited to 10hz The sonar and LED's arent yet working, this will come soon.
2026-05-27 13:25:20 +00:00
parent c1319a6357
commit ec554bcf2c
96 changed files with 2874 additions and 3730 deletions
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(raspbot_v2_bringup)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
@@ -0,0 +1,80 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node, PushRosNamespace
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    enable_camera = LaunchConfiguration('enable_camera')
+    enable_lidar = LaunchConfiguration('enable_lidar')
+    enable_sonar = LaunchConfiguration('enable_sonar')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    use_hardware = LaunchConfiguration('use_hardware')
+
+    declared_args = [
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='ROS namespace for all robot nodes (e.g. "robot1").'
+        ),
+
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation time.'
+        ),
+
+        DeclareLaunchArgument(
+            'use_hardware', default_value='false',
+            description='Use hardware nodes.'
+        ),
+
+        DeclareLaunchArgument(
+            'enable_camera', default_value='true',
+            description='Enable the camera node.'
+        ),
+        
+        DeclareLaunchArgument(
+            'enable_lidar', default_value='true',
+            description='Enable the LiDAR node.'
+        ),
+        
+        DeclareLaunchArgument(
+            'enable_sonar', default_value='true',
+            description='Enable the sonar node.'
+        )
+    ]
+    
+    controller_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare('raspbot_v2_control'), 'launch', 'controller_manager.launch.py'
+            ])
+        ]),
+        launch_arguments={
+            'namespace': namespace,
+            'use_sim_time': LaunchConfiguration('use_sim_time')
+        }.items()
+    )
+    
+    hardware_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare('raspbot_v2_hardware'), 'launch', 'hardware.launch.py'
+            ]),
+        ]),
+        condition=IfCondition(use_hardware),
+        launch_arguments={
+            'namespace': namespace,
+        }.items(),
+    )
+            
+        
+    return LaunchDescription([
+        *declared_args,
+        PushRosNamespace(namespace),
+        controller_launch,
+        hardware_launch,
+    ])
@@ -0,0 +1,18 @@
+<?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>raspbot_v2_bringup</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.20)
+project(raspbot_v2_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
@@ -0,0 +1,46 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz — hardware-limited by STM32 I2C clock-stretch (~22 ms/write × 4 motors)
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    mecanum_drive_controller:
+      type: mecanum_drive_controller/MecanumDriveController
+
+    pan_tilt_controller:
+      type: forward_command_controller/ForwardCommandController
+
+# ── Mecanum drive controller ─────────────────────────────────────────────────
+mecanum_drive_controller:
+  ros__parameters:
+    front_left_wheel_command_joint_name:  joint_wheel_front_left
+    front_right_wheel_command_joint_name: joint_wheel_front_right
+    rear_left_wheel_command_joint_name:   joint_wheel_rear_left
+    rear_right_wheel_command_joint_name:  joint_wheel_rear_right
+
+    # Geometry (metres) — derived from raspbot_v2_params.urdf.xacro:
+    #   wx = base_length/2 - wheel_radius = 0.09 - 0.03 = 0.06 m  (front-rear)
+    #   wy = base_width/2  + wheel_width/2 = 0.045 + 0.015 = 0.06 m (left-right)
+    #   sum_of_robot_center_projection = wx + wy = 0.12
+    kinematics:
+      wheels_radius: 0.03
+      sum_of_robot_center_projection_on_X_Y_axis: 0.12
+
+    # Odometry
+    enable_odom_tf: true
+    odom_frame_id: odom
+    base_frame_id: base_footprint
+
+    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3]
+
+# ── Pan / tilt position controller ───────────────────────────────────────────
+# Accepts std_msgs/Float64MultiArray — index 0 = pan (rad), index 1 = tilt (rad)
+# Topic: /pan_tilt_controller/commands
+pan_tilt_controller:
+  ros__parameters:
+    joints:
+      - pan
+      - tilt
+    interface_name: position
@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    use_sim_time: true
@@ -0,0 +1,119 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler,IncludeLaunchDescription
+from launch.conditions import IfCondition,UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+
+from launch_ros.actions import Node, PushRosNamespace
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    use_sim_time  = LaunchConfiguration('use_sim_time')
+    use_hardware  = LaunchConfiguration('use_hardware')
+    namespace     = LaunchConfiguration('namespace')
+    enable_camera = LaunchConfiguration('enable_camera')
+
+    declared_args = [
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='ROS namespace for all robot nodes (e.g. "robot1").'
+        ),
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation time.'
+        ),
+        DeclareLaunchArgument(
+            'use_hardware', default_value='false',
+            description='Load the real I2C hardware interface plugin (set true on physical robot).'
+        ),
+        DeclareLaunchArgument(
+            'enable_camera', default_value='false',
+            description='Enable the camera node.'
+        ),
+    ]
+
+    
+    robot_controllers = PathJoinSubstitution([
+        FindPackageShare('raspbot_v2_control'), 'config', 'controllers.yaml'
+    ])
+    
+    robot_description_content = Command([
+        PathJoinSubstitution([FindExecutable(name='xacro')]),
+        ' ',
+        PathJoinSubstitution([
+            FindPackageShare('raspbot_v2_description'), 'urdf', 'raspbot_v2.urdf.xacro'
+        ]),
+        ' use_sim_time:=', use_sim_time,
+        ' use_hardware:=', use_hardware,
+        ' controller_config:=', robot_controllers,
+    ])
+    robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
+        
+    control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, robot_controllers],
+        output='both',
+        remappings=[
+            # ("~/robot_description", "robot_description"),
+            ("mecanum_drive_controller/reference", "/cmd_vel"),
+            ("mecanum_drive_controller/odometry", "/odom"),
+            ("mecanum_drive_controller/tf_odometry", "/tf"),
+        ],
+        condition=UnlessCondition(use_sim_time),
+    )
+    
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+    
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager',
+                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
+                   '--controller-manager-timeout', '30',
+        ],
+    )
+
+    robot_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['mecanum_drive_controller', '--controller-manager',
+                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
+                   '--controller-manager-timeout', '30',
+        ]
+    )
+    
+    # Delay starting the robot controller until the joint state broadcaster has finished starting up.
+    delay_robot_controller_after_joint_state_broadcaster = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_controller_spawner],
+        ),
+    )
+    
+    pan_tilt_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['pan_tilt_controller', '--controller-manager',
+                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
+                   '--controller-manager-timeout', '30',
+        ],
+        condition=IfCondition(enable_camera),
+    )
+    
+    return LaunchDescription([
+        *declared_args,
+        PushRosNamespace(namespace),
+        control_node,
+        robot_state_publisher_node,
+        joint_state_broadcaster_spawner,
+        delay_robot_controller_after_joint_state_broadcaster,
+        pan_tilt_controller_spawner,
+    ])
@@ -0,0 +1,18 @@
+<?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>raspbot_v2_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(raspbot_v2_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(urdf REQUIRED)
+
+install(
+  DIRECTORY meshes urdf launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
@@ -0,0 +1,36 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node,PushRosNamespace
+from launch.conditions import IfCondition
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command,LaunchConfiguration,PythonExpression
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    use_rviz = LaunchConfiguration('use_rviz', default='true')
+    
+    
+    urdf_file = os.path.join(
+        get_package_share_directory('raspbot_v2_description'),
+        'urdf', 'raspbot_v2.urdf.xacro')
+
+    robot_description_content = Command([
+        'xacro ', urdf_file,
+    ])
+    
+    return LaunchDescription([
+        PushRosNamespace(namespace),
+        Node(
+            package="joint_state_publisher",
+            executable="joint_state_publisher_gui",
+            name="joint_state_publisher",
+        ),
+        
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            name="robot_state_publisher",
+            parameters=[{"robot_description": robot_description_content}],
+        ),  
+    ])
@@ -0,0 +1,18 @@
+<?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>raspbot_v2_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,176 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
+    xmlns:gz="http://gazebosim.org/schema"
+>
+    <!-- ── base_footprint ───────────────────────────────────────────────────── -->
+    <!-- Virtual ground-plane reference link.  Gz Sim requires at least a
+       minimal inertial so it survives the URDF→SDF conversion. -->
+    <link name="base_footprint">
+        <inertial>
+            <mass value="0.001" />
+            <inertia ixx="1e-6" ixy="0" ixz="0"
+                iyy="1e-6" iyz="0"
+                izz="1e-6" />
+        </inertial>
+    </link>
+
+    <joint name="base_joint" type="fixed">
+        <parent link="base_footprint" />
+        <child link="base_link" />
+        <origin xyz="0 0 ${chassis_z_from_ground}" rpy="0 0 0" />
+    </joint>
+
+    <!-- ── base_link ────────────────────────────────────────────────────────── -->
+    <link name="base_link">
+        <visual>
+            <geometry>
+                <box size="${base_length} ${base_width} ${base_height}" />
+            </geometry>
+            <material name="blue" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="${base_length} ${base_width} ${base_height}" />
+            </geometry>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="${base_mass}" />
+            <!-- Solid-box inertia tensor -->
+            <inertia
+                ixx="${base_mass/12*(base_width**2  + base_height**2)}"
+                ixy="0" ixz="0"
+                iyy="${base_mass/12*(base_length**2 + base_height**2)}"
+                iyz="0"
+                izz="${base_mass/12*(base_length**2 + base_width**2)}" />
+        </inertial>
+    </link>
+
+    <!-- ── wheel macro ──────────────────────────────────────────────────────── -->
+    <!-- fdir1 y-sign: for O-type mecanum the traction diagonal is -(x*y),
+       where x=±1 (front/rear) and y=±1 (left/right in URDF frame). -->
+    <xacro:macro name="wheel" params="name x y">
+        <link name="wheel_${name}">
+            <visual>
+                <geometry>
+                    <cylinder radius="${wheel_radius}" length="${wheel_width}" />
+                </geometry>
+                <material name="arrow" />
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+            </visual>
+            <collision>
+                <geometry>
+                    <cylinder radius="${wheel_radius}" length="${wheel_width}" />
+                </geometry>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+            </collision>
+            <inertial>
+                <mass value="${wheel_mass}" />
+                <!-- Cylinder spinning around y-axis (joint axis="0 1 0"):
+             Iyy = m·r²/2  (spin axis)
+             Ixx = Izz = m·(3r²+h²)/12  (transverse axes) -->
+                <inertia
+                    ixx="${wheel_mass*(3*wheel_radius**2 + wheel_width**2)/12}"
+                    ixy="0" ixz="0"
+                    iyy="${wheel_mass*wheel_radius**2/2}"
+                    iyz="0"
+                    izz="${wheel_mass*(3*wheel_radius**2 + wheel_width**2)/12}" />
+            </inertial>
+        </link>
+        <joint name="joint_wheel_${name}" type="continuous">
+            <parent link="base_link" />
+            <child link="wheel_${name}" />
+            <origin xyz="${wx*x} ${wy*y} ${wz}" rpy="0 0 0" />
+            <axis xyz="0 1 0" />
+            <dynamics damping="0.1" friction="0.0" />
+        </joint>
+        <!-- Anisotropic friction: mu = traction direction (high grip, perpendicular to
+         roller), mu2 = roller direction (low resistance, allows roller to spin).
+         fdir1 is the traction direction in the chassis frame.  For O-type mecanum
+         the y-sign of fdir1 is -(x*y): front-left/rear-right get -sin, and
+         front-right/rear-left get +sin, matching the two roller diagonals.
+         gz:expressed_in keeps fdir1 fixed in the chassis frame as the hub spins.
+         In gz-sim9 (Harmonic) mu/mu2/fdir1 must be nested under
+         collision/surface/friction/ode — the shorthand directly under <gazebo
+         reference> is silently ignored. -->
+        <gazebo reference="wheel_${name}">
+            <collision>
+                <surface>
+                    <friction>
+                        <ode>
+                            <mu>1.0</mu>
+                            <mu2>${wheel_roller_friction}</mu2>
+                            <fdir1 gz:expressed_in="base_footprint">${cos(roller_angle)} ${-x*y*sin(roller_angle)} 0</fdir1>
+                        </ode>
+                    </friction>
+                    <contact>
+                        <ode>
+                            <kp>100000.0</kp>
+                            <kd>1.0</kd>
+                        </ode>
+                    </contact>
+                </surface>
+            </collision>
+        </gazebo>
+    </xacro:macro>
+
+    <xacro:wheel name="front_left"  x="1"  y="1"  />
+    <xacro:wheel name="front_right" x="1"  y="-1" />
+    <xacro:wheel name="rear_left"   x="-1" y="1"  />
+    <xacro:wheel name="rear_right"  x="-1" y="-1" />
+
+    <!-- ── ros2_control: drive wheels ──────────────────────────────────────── -->
+    <ros2_control name="mecanum_drive" type="system">
+        <hardware>
+            <xacro:if value="$(arg use_sim_time)">
+                <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+            </xacro:if>
+            <xacro:unless value="$(arg use_sim_time)">
+                <xacro:if value="$(arg use_hardware)">
+                    <plugin>raspbot_v2_hardware_interface/RaspbotHardwareInterface</plugin>
+                </xacro:if>
+                <xacro:unless value="$(arg use_hardware)">
+                    <plugin>mock_components/GenericSystem</plugin>
+                </xacro:unless>
+            </xacro:unless>
+        </hardware>
+        <joint name="joint_wheel_front_left">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="joint_wheel_front_right">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="joint_wheel_rear_left">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="joint_wheel_rear_right">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+    </ros2_control>
+
+    <!-- ── Gazebo: gz_ros2_control plugin ───────────────────────────────────── -->
+    <!-- Replaces the native MecanumDrive + JointStatePublisher plugins.
+         The plugin hosts the controller_manager and loads controllers.yaml. -->
+    <gazebo>
+        <plugin filename="gz_ros2_control-system"
+            name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+            <parameters>$(find raspbot_v2_control)/config/controllers.yaml</parameters>
+            <parameters>$(find raspbot_v2_control)/config/sim_overrides.yaml</parameters>
+            <ros>
+                <remapping>mecanum_drive_controller/reference:=/cmd_vel</remapping>
+                <remapping>mecanum_drive_controller/odometry:=/odom</remapping>
+                <remapping>mecanum_drive_controller/tf_odometry:=/tf</remapping>
+            </ros>
+        </plugin>
+    </gazebo>
+
+</robot>    
@@ -0,0 +1,87 @@
+<?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>
@@ -0,0 +1,42 @@
+<?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.
+       ───────────────────────────────────────────────────────────────────── -->
+  <xacro:property name="c_width" default="0.085" />
+  <xacro:property name="c_depth" default="0.08" />
+  <xacro:property name="c_height" default="0.05" />
+
+
+  <link name="cpu_housing">
+    <visual>
+      <geometry>
+        <box size="${c_width} ${c_depth} ${c_height}" />
+      </geometry>
+      <material name="green" />
+    </visual>
+    <!-- To simpllify physics for now, just model the base of the robot
+    <collision>
+      <geometry>
+        <box size="0.05 0.06 0.03" />
+      </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="cpu_housing_joint" type="fixed">
+    <parent link="base_link" />
+    <child link="cpu_housing" />
+    <!-- Front-centre of the chassis, mid-height -->
+    <origin xyz="-0.02 0.0 ${(base_height + c_height)/2}" rpy="0 0 0" />
+  </joint>
+</robot>
@@ -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>
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
+    <material name="red">
+        <color rgba="1 0 0 1" />
+    </material>
+
+    <material name="green">
+        <color rgba="0 1 0 1" />
+    </material>
+
+    <material name="blue">
+        <color rgba="0 0 1 1" />
+    </material>
+
+    <material name="arrow">
+        <color rgba="0 1 0 1" />
+    </material>
+
+    <material name="white">
+        <color rgba="1 1 1 1" />
+    </material>
+
+    <material name="light_grey">
+        <color rgba="0.8 0.8 0.8 1" />
+    </material>
+
+    <material name="dark_grey">
+        <color rgba="0.2 0.2 0.2 1" />
+    </material>
+</robot>    
@@ -0,0 +1,141 @@
+<?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.
+       ───────────────────────────────────────────────────────────────────── -->
+
+    <xacro:include filename="raspbot_v2_params.urdf.xacro" />
+
+
+    <!-- Mount bracket -->
+    <link name="camera_base">
+        <visual>
+            <geometry>
+                <box size="0.03 0.04 0.03" />
+            </geometry>
+            <material name="dark_grey" />
+        </visual>
+        <!-- To simpllify physics, just model the base of the robot
+        <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="ultrasonic" />
+        <child link="camera_base" />
+        <!-- Adjust xyz to match the physical servo mount position -->
+        <origin xyz="0 0.0 0.02" 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>
+            <xacro:if value="$(arg use_sim_time)">
+                <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+            </xacro:if>
+            <xacro:unless value="$(arg use_sim_time)">
+                <plugin>mock_components/GenericSystem</plugin>
+            </xacro:unless>
+        </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>
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
+  xmlns:gz="http://gazebosim.org/schema"
+  name="raspbot_v2">
+  <xacro:include filename="raspbot_v2_params.urdf.xacro" />
+  <xacro:include filename="materials.xacro" />
+
+  <xacro:include filename="base.xacro" />
+  <xacro:include filename="sonar.xacro" />
+  <xacro:include filename="pan_tilt.xacro" />
+  <xacro:include filename="camera.xacro" />
+  <xacro:include filename="cpu_housing.xacro" />
+  <xacro:include filename="lidar.xacro" />
+
+</robot>
@@ -0,0 +1,122 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
+  <!-- ================================================================
+       Top-level parameters
+       All key dimensions are declared as xacro:arg so they can be
+       overridden from a launch file or command line:
+
+         xacro mecanum_robot.urdf.xacro base_length:=0.6 wheel_radius:=0.1
+
+       Derived quantities (wheel offsets, inertia values) are computed
+       automatically from these arguments.
+       ================================================================ -->
+
+  <xacro:arg name="use_sim_time" default="false" />
+  <xacro:property name="use_sim_time" value="$(arg use_sim_time)" />
+  <!-- Set true when running on real hardware (selects the I2C hardware interface plugin).
+       Ignored when use_sim_time is true. Default false keeps mock_components for RViz-only use. -->
+  <xacro:arg name="use_hardware" default="false" />
+
+  <!-- Base chassis dimensions (metres) -->
+  <xacro:arg name="base_length" default="0.18" />  <!-- X: front-to-back  -->
+  <xacro:arg name="base_width" default="0.09" />  <!-- Y: side-to-side   -->
+  <xacro:arg name="base_height" default="0.03" />  <!-- Z: vertical depth -->
+  <xacro:arg name="base_mass" default="05" />   <!-- kg                -->
+
+  <!-- Wheel dimensions (metres) -->
+  <xacro:arg name="wheel_radius" default="0.03" />
+  <xacro:arg name="wheel_width" default="0.03" />
+  <xacro:arg name="wheel_mass" default="0.1" />  <!-- kg, each wheel -->
+
+  <!-- Roller friction (mu2): near-zero lets rollers spin freely; increase to
+       resist post-stop sliding.  0.0 = no resistance, 1.0 = full grip. -->
+  <xacro:arg name="wheel_roller_friction" default="0.5" />
+
+  <!-- Roller angle (radians): angle of the traction direction relative to the
+       wheel's forward axis.  Standard mecanum wheels use pi/4 (45 degrees).
+       Smaller values lean the force more forward; larger values more lateral. -->
+  <xacro:arg name="roller_angle" default="0.7854" />
+
+  <!-- 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) -->
+
+  <!-- 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-->
+  <xacro:arg name="camera_fov" default="1.047" />  <!-- rad (~60°) -->
+  <xacro:arg name="camera_width" default="640" />
+  <xacro:arg name="camera_height" default="480" />
+
+
+  <!-- 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) -->
+
+  <!-- Pull args into properties so they can be used in math expressions -->
+  <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_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_roller_friction" value="$(arg wheel_roller_friction)" />
+  <xacro:property name="roller_angle" value="$(arg roller_angle)" />
+
+  <xacro:property name="sonar_min_range" value="$(arg sonar_min_range)" />
+  <xacro:property name="sonar_max_range" value="$(arg sonar_max_range)" />
+
+  <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:include filename="$(find raspbot_v2)/urdf/gazebo.xacro"/> -->
+
+  <!-- Derived geometry:
+       base_footprint sits on the ground plane (z = 0).
+       base_link origin is placed at height = wheel_radius + base_height/2
+       so the bottom of the chassis clears the wheel axle line and the
+       wheel centres end up at exactly wheel_radius above the ground.
+
+       Wheel x offset: pull the axle slightly inward from the chassis edge
+       so the wheel hub looks natural.
+       Wheel y offset: wheel centre is half-width outside the chassis side.
+  -->
+  <xacro:property name="chassis_z_from_ground"
+    value="${wheel_radius}" />
+
+  <!-- Wheel x offset from base_link origin (along the robot's forward axis) -->
+  <xacro:property name="wx" value="${base_length/2 - wheel_radius}" />
+
+  <!-- Wheel y offset: place wheel flush outside chassis side wall -->
+  <xacro:property name="wy" value="${base_width/2 + wheel_width/2}" />
+
+  <!-- Wheel z offset relative to base_link origin.
+       base_link is at (wheel_radius + base_height/2) above the ground.
+       The wheel centre on the raspbot is aligned with the origin of base_link 
+       so the value is 0 -->
+  <xacro:property name="wz" value="0" />
+</robot>
@@ -0,0 +1,112 @@
+<?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.
+       ───────────────────────────────────────────────────────────────────── -->
+  <xacro:property name="u_base_length" default="0.05" /> <!-- x-->
+  <xacro:property name="u_base_width" default="0.06" /> <!-- y -->
+  <xacro:property name="u_base_height" default="0.03" /> <!-- z -->
+  <xacro:property name="u_sensor_radius" default="0.008" />
+  <xacro:property name="u_sensor_length" default="0.006" />
+  <xacro:property name="u_sensor_separation" default="0.026" />
+
+
+  <link name="ultrasonic">
+    <visual>
+      <geometry>
+        <box size="${u_base_length} ${u_base_width} ${u_base_height}" />
+      </geometry>
+      <material name="green" />
+    </visual>
+    <visual>
+      <geometry>
+        <cylinder radius="${u_sensor_radius}" length="${u_sensor_length}" />
+      </geometry>
+      <material name="white" />
+      <origin
+        xyz="${u_base_length/2 + u_sensor_length/2} ${u_sensor_separation/2} 0"
+        rpy="0 ${pi/2} 0" />
+    </visual>
+    <visual>
+      <geometry>
+        <cylinder radius="${u_sensor_radius}" length="${u_sensor_length}" />
+      </geometry>
+      <material name="white" />
+      <origin
+        xyz="${u_base_length/2 + u_sensor_length/2} -${u_sensor_separation/2} 0"
+        rpy="0 ${pi/2} 0" />
+    </visual>
+
+    <!-- To simpllify physics for now, just model the base of the robot
+    <collision>
+      <geometry>
+        <box size="0.05 0.06 0.03" />
+      </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.05 + 0.017)/2} 0.0 ${base_height}" rpy="0 0 0" />
+  </joint>
+
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Gazebo sonar sensor  (Gazebo Harmonic / gz-sim)
+
+       gz-sim has no native sonar sensor type.  The HC-SR04 is approximated
+       as a narrow-cone gpu_lidar — a 5×5 ray grid spanning ±7.5° (≈15° FOV)
+       in both axes, which matches the HC-SR04's beam pattern.
+
+       The sensor publishes a gz LaserScan on /ultrasonic/scan.  Bridge this
+       to ROS 2 via ros_gz_bridge, then convert to sensor_msgs/Range with a
+       small relay node (or just consume the LaserScan directly):
+         /ultrasonic/scan  (gz LaserScan) → /ultrasonic/scan  (ROS 2)
+       ───────────────────────────────────────────────────────────────────── -->
+  <gazebo reference="ultrasonic">
+    <sensor name="sonar" type="gpu_lidar">
+      <always_on>1</always_on>
+      <update_rate>10</update_rate>
+      <visualize>true</visualize>
+      <topic>/ultrasonic/scan</topic>
+      <gz_frame_id>ultrasonic</gz_frame_id>
+      <lidar>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.1309</min_angle>  <!-- −7.5° -->
+            <max_angle> 0.1309</max_angle>  <!-- +7.5° -->
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <min_angle>-0.1309</min_angle>
+            <max_angle> 0.1309</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>${sonar_min_range}</min>
+          <max>${sonar_max_range}</max>
+          <resolution>0.01</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.005</stddev>
+        </noise>
+      </lidar>
+    </sensor>
+  </gazebo>
+
+</robot>
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
@@ -0,0 +1,55 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    namespace     = LaunchConfiguration('namespace')
+    enable_camera = LaunchConfiguration('enable_camera')
+
+    declared_args = [
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='ROS namespace for all robot nodes (e.g. "robot1").'
+        ),
+        DeclareLaunchArgument(
+            'enable_camera', default_value='true',
+            description='Enable the camera orientation controller node.'
+        ),
+    ]
+
+    # ros2_control stack (controller_manager + spawners) with real hardware plugin
+    control_stack = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('raspbot_v2_control'),
+                'launch', 'controller_manager.launch.py'
+            )
+        ),
+        launch_arguments={
+            'use_sim_time': 'false',
+            'use_hardware': 'true',
+            'namespace': namespace,
+            'enable_camera': enable_camera,
+        }.items(),
+    )
+
+    camera_orientation_controller = Node(
+        package='raspbot_v2_hardware',
+        executable='camera_orientation_controller',
+        name='camera_orientation_controller',
+        output='screen',
+        condition=IfCondition(enable_camera),
+    )
+
+    return LaunchDescription([
+        *declared_args,
+        control_stack,
+        camera_orientation_controller,
+    ])
@@ -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>raspbot_v2_hardware</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
+  <license>MIT</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_mypy</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>ament_xmllint</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
@@ -0,0 +1,205 @@
+#!/usr/bin/env python3
+"""
+Camera orientation controller node.
+
+Subscribes to joint commands and drives the pan/tilt servo pair on the
+Raspbot via Ctrl_Servo.  Publishes current joint state for downstream
+consumers (e.g. tf2, rviz2).
+
+Standard interfaces
+-------------------
+Command  : /joint_command  (sensor_msgs/JointState)
+           Joint names: "pan"  – horizontal rotation (yaw)
+           Joint names: "tilt" – vertical rotation (pitch)
+           Positions in **radians** (ROS convention).
+
+State    : /joint_states   (sensor_msgs/JointState)
+           Same joint names; position in radians, reflecting the last
+           commanded angle.
+
+Parameters
+----------
+pan_servo_id        int   default 1     – Raspbot servo channel for pan
+tilt_servo_id       int   default 2     – Raspbot servo channel for tilt
+pan_min_deg         float default -60.0 – ROS lower limit for pan (°)
+pan_max_deg         float default  90.0 – ROS upper limit for pan (°)
+pan_servo_center_deg float default 90.0 – hardware servo angle that equals ROS 0°
+tilt_min_deg        float default -90.0 – ROS lower limit for tilt (°)
+tilt_max_deg        float default  45.0 – ROS upper limit for tilt (°)
+tilt_servo_center_deg float default 60.0 – hardware servo angle that equals ROS 0°
+pan_center_deg      float default  0.0  – startup position in ROS degrees
+tilt_center_deg     float default -15.0 – startup position in ROS degrees
+state_rate_hz       float default 10.0  – joint-state publish rate
+"""
+
+import math
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+
+# Servo IDs used by Ctrl_Servo
+_PAN_SERVO_ID = 1
+_TILT_SERVO_ID = 2
+
+
+def _deg_to_rad(deg: float) -> float:
+    return deg * math.pi / 180.0
+
+
+def _rad_to_deg(rad: float) -> float:
+    return rad * 180.0 / math.pi
+
+
+class CameraOrientationNode(Node):
+
+    def __init__(self):
+        super().__init__('camera_orientation')
+
+        # --- Parameters ---
+        self.declare_parameter('pan_servo_id',          _PAN_SERVO_ID)
+        self.declare_parameter('tilt_servo_id',         _TILT_SERVO_ID)
+        self.declare_parameter('pan_min_deg',           -60.0)
+        self.declare_parameter('pan_max_deg',            90.0)
+        self.declare_parameter('pan_servo_center_deg',   90.0)
+        self.declare_parameter('tilt_min_deg',          -90.0)
+        self.declare_parameter('tilt_max_deg',           45.0)
+        self.declare_parameter('tilt_servo_center_deg',  60.0)
+        self.declare_parameter('pan_center_deg',          0.0)
+        self.declare_parameter('tilt_center_deg',        -15.0)
+        self.declare_parameter('state_rate_hz',          10.0)
+
+        self._pan_id           = self.get_parameter('pan_servo_id').value
+        self._tilt_id          = self.get_parameter('tilt_servo_id').value
+        self._pan_min          = self.get_parameter('pan_min_deg').value
+        self._pan_max          = self.get_parameter('pan_max_deg').value
+        self._pan_servo_center = self.get_parameter('pan_servo_center_deg').value
+        self._tilt_min         = self.get_parameter('tilt_min_deg').value
+        self._tilt_max         = self.get_parameter('tilt_max_deg').value
+        self._tilt_servo_center = self.get_parameter('tilt_servo_center_deg').value
+
+        pan_center  = self.get_parameter('pan_center_deg').value
+        tilt_center = self.get_parameter('tilt_center_deg').value
+        rate_hz     = self.get_parameter('state_rate_hz').value
+
+        # --- Hardware ---
+        # Import here so the node can be unit-tested without hardware present
+        from raspbot_v2_hardware.raspbot_lib import Raspbot
+        self._bot = Raspbot()
+
+        # --- State (degrees, internal representation) ---
+        self._pan_deg  = pan_center
+        self._tilt_deg = tilt_center
+
+        # Drive servos to the startup centre position
+        self._apply(self._pan_deg, self._tilt_deg)
+
+        # --- Subscriber ---
+        self._cmd_sub = self.create_subscription(
+            JointState,
+            'joint_command',
+            self._joint_command_cb,
+            10,
+        )
+
+        # --- Publisher ---
+        self._state_pub = self.create_publisher(JointState, 'joint_states', 10)
+        self._state_timer = self.create_timer(1.0 / rate_hz, self._publish_state)
+
+        self.get_logger().info(
+            f'Camera orientation node started '
+            f'(pan servo {self._pan_id}, tilt servo {self._tilt_id}). '
+            f'Centred at pan={pan_center}° tilt={tilt_center}°.'
+        )
+
+    # ------------------------------------------------------------------
+    # Subscriber callback
+    # ------------------------------------------------------------------
+
+    def _joint_command_cb(self, msg: JointState):
+        """
+        Accept a JointState command.  Only joints named "pan" or "tilt"
+        are acted on; other names are silently ignored so this node can
+        coexist on a shared joint_command topic.
+
+        Positions are expected in radians (ROS convention).
+        """
+        if len(msg.name) != len(msg.position):
+            self.get_logger().warn(
+                'joint_command message has mismatched name/position lengths; ignoring.'
+            )
+            return
+
+        pan_deg  = self._pan_deg
+        tilt_deg = self._tilt_deg
+
+        for name, pos_rad in zip(msg.name, msg.position):
+            pos_deg = _rad_to_deg(pos_rad)
+            if name == 'pan':
+                pan_deg = self._clamp(pos_deg, self._pan_min, self._pan_max)
+            elif name == 'tilt':
+                tilt_deg = self._clamp(pos_deg, self._tilt_min, self._tilt_max)
+
+        self._apply(pan_deg, tilt_deg)
+
+    # ------------------------------------------------------------------
+    # Helpers
+    # ------------------------------------------------------------------
+
+    def _apply(self, pan_ros_deg: float, tilt_ros_deg: float):
+        """Convert ROS-convention degrees to servo degrees and drive hardware."""
+        pan_servo  = int(round(pan_ros_deg  + self._pan_servo_center))
+        tilt_servo = int(round(tilt_ros_deg + self._tilt_servo_center))
+        try:
+            self._bot.Ctrl_Servo(self._pan_id,  pan_servo)
+            self._bot.Ctrl_Servo(self._tilt_id, tilt_servo)
+            self._pan_deg  = pan_ros_deg
+            self._tilt_deg = tilt_ros_deg
+        except Exception as exc:
+            self.get_logger().error(f'Servo write failed: {exc}')
+
+    @staticmethod
+    def _clamp(value: float, lo: float, hi: float) -> float:
+        return max(lo, min(hi, value))
+
+    # ------------------------------------------------------------------
+    # State publisher
+    # ------------------------------------------------------------------
+
+    def _publish_state(self):
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.name     = ['pan', 'tilt']
+        msg.position = [_deg_to_rad(self._pan_deg), _deg_to_rad(self._tilt_deg)]
+        self._state_pub.publish(msg)
+
+    # ------------------------------------------------------------------
+    # Shutdown
+    # ------------------------------------------------------------------
+
+    def destroy_node(self):
+        self.get_logger().info('Parking camera servos at centre...')
+        pan_center  = self.get_parameter('pan_center_deg').value
+        tilt_center = self.get_parameter('tilt_center_deg').value
+        try:
+            self._apply(pan_center, tilt_center)
+        except Exception:
+            pass
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CameraOrientationNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+"""
+LED bar controller node.
+
+Controls the 14-LED WS2812 bar on the Raspbot via two topics:
+
+Topics
+------
+/led/color  (std_msgs/ColorRGBA)
+    Set all LEDs to a solid RGB colour.  r/g/b are 0.0–1.0; a=0.0 turns
+    the bar off.  Publishing this topic also cancels any running effect.
+
+/led/effect (std_msgs/String)
+    Start a named light effect (runs until a new command arrives):
+        river | breathing | gradient | random_running | starlight | off
+
+Parameters
+----------
+effect_speed  float  default 0.05  – delay between effect frames (s)
+effect_color  int    default 0     – colour index for breathing effect
+                                     0=red 1=green 2=blue 3=yellow
+                                     4=purple 5=cyan 6=white
+"""
+
+import threading
+from typing import TYPE_CHECKING
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import ColorRGBA, String
+
+if TYPE_CHECKING:
+    from raspbot_v2_hardware.raspbot_lib import LightShow
+
+VALID_EFFECTS = {'river', 'breathing', 'gradient', 'random_running', 'starlight', 'off'}
+
+
+class LedNode(Node):
+
+    def __init__(self):
+        super().__init__('led_controller')
+
+        self.declare_parameter('effect_speed', 0.05)
+        self.declare_parameter('effect_color', 0)
+
+        from raspbot_v2_hardware.raspbot_lib import Raspbot, LightShow
+        self._Raspbot = Raspbot
+        self._LightShow = LightShow
+        self._bot = Raspbot()
+
+        self._light_show: 'LightShow | None' = None
+        self._effect_thread: threading.Thread | None = None
+
+        self.create_subscription(ColorRGBA, 'led/color',  self._color_cb,  10)
+        self.create_subscription(String,    'led/effect', self._effect_cb, 10)
+
+        self._bot.Ctrl_WQ2812_ALL(0, 0)
+        self.get_logger().info('LED controller started (14 LEDs, bar off).')
+
+    # ------------------------------------------------------------------
+    # Callbacks
+    # ------------------------------------------------------------------
+
+    def _color_cb(self, msg: ColorRGBA) -> None:
+        self._stop_effect()
+        if msg.a == 0.0:
+            self._bot.Ctrl_WQ2812_ALL(0, 0)
+        else:
+            r = max(0, min(255, int(msg.r * 255)))
+            g = max(0, min(255, int(msg.g * 255)))
+            b = max(0, min(255, int(msg.b * 255)))
+            self._bot.Ctrl_WQ2812_brightness_ALL(r, g, b)
+
+    def _effect_cb(self, msg: String) -> None:
+        effect = msg.data.strip()
+        if effect not in VALID_EFFECTS:
+            self.get_logger().warn(
+                f"Unknown effect '{effect}'. Valid: {', '.join(sorted(VALID_EFFECTS))}"
+            )
+            return
+
+        self._stop_effect()
+
+        if effect == 'off':
+            self._bot.Ctrl_WQ2812_ALL(0, 0)
+            return
+
+        speed = self.get_parameter('effect_speed').value
+        color = self.get_parameter('effect_color').value
+
+        ls = self._LightShow()
+        self._light_show = ls
+
+        def _run():
+            ls.execute_effect(effect, ls.MAX_TIME, speed, color)
+
+        self._effect_thread = threading.Thread(target=_run, daemon=True, name=f'led_{effect}')
+        self._effect_thread.start()
+        self.get_logger().info(f"LED effect '{effect}' started.")
+
+    # ------------------------------------------------------------------
+    # Helpers
+    # ------------------------------------------------------------------
+
+    def _stop_effect(self) -> None:
+        if self._light_show is not None:
+            self._light_show.stop()
+        if self._effect_thread is not None and self._effect_thread.is_alive():
+            self._effect_thread.join(timeout=2.0)
+        self._light_show = None
+        self._effect_thread = None
+
+    # ------------------------------------------------------------------
+    # Shutdown
+    # ------------------------------------------------------------------
+
+    def destroy_node(self) -> None:
+        self.get_logger().info('Shutting down LED controller — turning off bar.')
+        self._stop_effect()
+        try:
+            self._bot.Ctrl_WQ2812_ALL(0, 0)
+        except Exception:
+            pass
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LedNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,109 @@
+from .raspbot_lib import Raspbot
+
+# Motor ID assignments (from Raspbot_Lib comments)
+_MOTOR_FL = 0  # L1 — front left
+_MOTOR_RL = 1  # L2 — rear left
+_MOTOR_FR = 2  # R1 — front right
+_MOTOR_RR = 3  # R2 — rear right
+
+_DRIVER_MAX = 255
+
+
+class MotorController:
+    """
+    Thin adapter between the ROS 2 motor controller node and raspbot_lib.
+
+    Speed convention: the node passes values in the range
+    [-max_speed, max_speed] (default max_speed = 1.0).  This class scales
+    them linearly to the driver's 0-255 range, so max_speed should match
+    whatever normalised unit you use (1.0 → full throttle).
+    """
+
+    def __init__(self, max_speed: float = 1.0):
+        self._max_speed = max_speed
+        self._bot: Raspbot | None = None
+        # Shadow register — Raspbot has no read-back, so we track state.
+        self._speeds = [0.0, 0.0, 0.0, 0.0]  # [FL, FR, RL, RR]
+
+    # ------------------------------------------------------------------
+    # Lifecycle
+    # ------------------------------------------------------------------
+
+    def initialize(self) -> None:
+        """Create the I2C connection and ensure all motors are stopped."""
+        self._bot = Raspbot()
+        self._stop_all_motors()
+
+    def shutdown(self) -> None:
+        """Stop motors and release the driver handle."""
+        if self._bot is not None:
+            self._stop_all_motors()
+            self._bot = None
+
+    # ------------------------------------------------------------------
+    # Motor control
+    # ------------------------------------------------------------------
+
+    def set_speed(
+        self,
+        front_left: float,
+        front_right: float,
+        rear_left: float,
+        rear_right: float,
+    ) -> None:
+        """
+        Set all four wheel speeds simultaneously.
+
+        Parameters
+        ----------
+        front_left, front_right, rear_left, rear_right:
+            Desired speeds in the range [-max_speed, max_speed].
+            Positive = forward, negative = backward.
+        """
+        self._require_initialized()
+        mapping = [
+            (_MOTOR_FL, front_left),
+            (_MOTOR_RL, rear_left),
+            (_MOTOR_FR, front_right),
+            (_MOTOR_RR, rear_right),
+        ]
+        for motor_id, speed in mapping:
+            self._bot.Ctrl_Muto(motor_id, self._to_driver_units(speed))
+
+        self._speeds = [front_left, front_right, rear_left, rear_right]
+
+    def stop_all(self) -> None:
+        """Immediately stop all motors."""
+        self._require_initialized()
+        self._stop_all_motors()
+        self._speeds = [0.0, 0.0, 0.0, 0.0]
+
+    def get_speeds(self) -> list[float]:
+        """
+        Return the last commanded wheel speeds as [FL, FR, RL, RR].
+
+        The Raspbot driver provides no encoder/tachometer read-back, so this
+        returns the most recently written values.
+        """
+        return list(self._speeds)
+
+    # ------------------------------------------------------------------
+    # Helpers
+    # ------------------------------------------------------------------
+
+    def _to_driver_units(self, speed: float) -> int:
+        """Scale a speed value to the driver's integer range [-255, 255]."""
+        if self._max_speed == 0:
+            return 0
+        scaled = (speed / self._max_speed) * _DRIVER_MAX
+        return int(max(-_DRIVER_MAX, min(_DRIVER_MAX, scaled)))
+
+    def _stop_all_motors(self) -> None:
+        for motor_id in (_MOTOR_FL, _MOTOR_RL, _MOTOR_FR, _MOTOR_RR):
+            self._bot.Ctrl_Muto(motor_id, 0)
+
+    def _require_initialized(self) -> None:
+        if self._bot is None:
+            raise RuntimeError(
+                "MotorController has not been initialized. Call initialize() first."
+            )
@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float32MultiArray
+
+from .motor_controller import MotorController
+
+
+class MotorControllerNode(Node):
+    def __init__(self):
+        super().__init__('motor_controller')
+
+        # --- Parameters ---
+        self.declare_parameter('wheel_base', 0.3)      # meters between left/right wheels
+        self.declare_parameter('max_speed', 1.0)       # max motor speed (library units)
+
+        self.wheel_base = self.get_parameter('wheel_base').value
+        self.max_speed  = self.get_parameter('max_speed').value
+
+        # --- Initialize your motor library ---
+        self.motors = MotorController()
+        self.motors.initialize()
+
+        # --- Subscribers ---
+        # cmd_vel is the standard ROS2 topic for velocity commands
+        self.cmd_vel_sub = self.create_subscription(
+            Twist,
+            'cmd_vel',
+            self.cmd_vel_callback,
+            10  # QoS queue depth
+        )
+
+        # Optional: direct per-wheel speed control [FL, FR, RL, RR]
+        self.wheel_speeds_sub = self.create_subscription(
+            Float32MultiArray,
+            'wheel_speeds',
+            self.wheel_speeds_callback,
+            10
+        )
+
+        # --- Publishers ---
+        # Publish current wheel speeds for feedback
+        self.speed_pub = self.create_publisher(Float32MultiArray, 'current_wheel_speeds', 10)
+
+        # --- Timer for telemetry ---
+        self.telemetry_timer = self.create_timer(0.1, self.publish_telemetry)  # 10 Hz
+
+        self.get_logger().info('Motor controller node started')
+
+    def cmd_vel_callback(self, msg: Twist):
+        """
+        Convert a Twist message (linear.x, angular.z) into
+        differential-drive wheel speeds for a 4-wheeled robot.
+        """
+        linear  = msg.linear.x   # m/s forward
+        angular = msg.angular.z  # rad/s rotation
+
+        # Differential drive kinematics
+        left_speed  = linear - (angular * self.wheel_base / 2.0)
+        right_speed = linear + (angular * self.wheel_base / 2.0)
+
+        # Clamp to max speed
+        left_speed  = max(-self.max_speed, min(self.max_speed, left_speed))
+        right_speed = max(-self.max_speed, min(self.max_speed, right_speed))
+
+        # Send to motors via your library (FL=FR=left, RL=RR=right)
+        try:
+            self.motors.set_speed(front_left=left_speed,  front_right=right_speed,
+                                  rear_left=left_speed,   rear_right=right_speed)
+        except Exception as e:
+            self.get_logger().error(f'Motor error: {e}')
+
+    def wheel_speeds_callback(self, msg: Float32MultiArray):
+        """Direct per-wheel control: [FL, FR, RL, RR]"""
+        if len(msg.data) != 4:
+            self.get_logger().warn('wheel_speeds expects exactly 4 values [FL, FR, RL, RR]')
+            return
+        try:
+            self.motors.set_speed(front_left=msg.data[0],  front_right=msg.data[1],
+                                  rear_left=msg.data[2],   rear_right=msg.data[3])
+        except Exception as e:
+            self.get_logger().error(f'Motor error: {e}')
+
+    def publish_telemetry(self):
+        """Read and publish current wheel speeds from your library."""
+        try:
+            speeds = self.motors.get_speeds()  # Expected: [FL, FR, RL, RR]
+            msg = Float32MultiArray()
+            msg.data = [float(s) for s in speeds]
+            self.speed_pub.publish(msg)
+        except Exception as e:
+            self.get_logger().warn(f'Telemetry read failed: {e}')
+
+    def destroy_node(self):
+        """Clean shutdown — stop motors before exiting."""
+        self.get_logger().info('Shutting down motors...')
+        try:
+            self.motors.stop_all()
+            self.motors.shutdown()
+        except Exception:
+            pass
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MotorControllerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,307 @@
+#!/usr/bin/env python3
+# coding: utf-8
+#
+# Source: Yahboom RASPBOT-V2 hardware library
+# Origin: https://www.yahboom.net/study/RASPBOT-V2
+# Copyright: Yahboom Technology Co., Ltd.
+# No explicit open-source license provided by the upstream source.
+# Included here unmodified for hardware compatibility.
+#
+import smbus
+import time
+import random
+import math
+
+PI5Car_I2CADDR = 0x2B
+
+
+class Raspbot():
+
+    def get_i2c_device(self, address, i2c_bus):
+        self._addr = address
+        if i2c_bus is None:
+            return smbus.SMBus(1)
+        else:
+            return smbus.SMBus(i2c_bus)
+
+    def __init__(self):
+        self._device = self.get_i2c_device(PI5Car_I2CADDR, 1)
+
+    def write_u8(self, reg, data):
+        try:
+            self._device.write_byte_data(self._addr, reg, data)
+        except Exception:
+            print('write_u8 I2C error')
+
+    def write_reg(self, reg):
+        try:
+            self._device.write_byte(self._addr, reg)
+        except Exception:
+            print('write_u8 I2C error')
+
+    def write_array(self, reg, data):
+        try:
+            self._device.write_i2c_block_data(self._addr, reg, data)
+        except Exception:
+            print('write_array I2C error')
+
+    def read_data_byte(self):
+        try:
+            buf = self._device.write_byte(self._addr)
+            return buf
+        except Exception:
+            print('read_u8 I2C error')
+
+    def read_data_array(self, reg, len):
+        try:
+            buf = self._device.read_i2c_block_data(self._addr, reg, len)
+            return buf
+        except Exception:
+            print('read_u8 I2C error')
+
+    def Ctrl_Car(self, motor_id, motor_dir, motor_speed):
+        try:
+            if (motor_dir != 1) and (motor_dir != 0):
+                motor_dir = 0
+            if motor_speed > 255:
+                motor_speed = 255
+            elif motor_speed < 0:
+                motor_speed = 0
+            self.write_array(0x01, [motor_id, motor_dir, motor_speed])
+        except Exception:
+            print('Ctrl_Car I2C error')
+
+    def Ctrl_Muto(self, motor_id, motor_speed):
+        try:
+            if motor_speed > 255:
+                motor_speed = 255
+            if motor_speed < -255:
+                motor_speed = -255
+            motor_dir = 1 if motor_speed < 0 else 0
+            self.write_array(0x01, [motor_id, motor_dir, abs(motor_speed)])
+        except Exception:
+            print('Ctrl_Car I2C error')
+
+    def Ctrl_Servo(self, id, angle):
+        try:
+            if angle < 0:
+                angle = 0
+            elif angle > 180:
+                angle = 180
+            if id == 2 and angle > 110:
+                angle = 110
+            self.write_array(0x02, [id, angle])
+        except Exception:
+            print('Ctrl_Servo I2C error')
+
+    def Ctrl_WQ2812_ALL(self, state, color):
+        try:
+            state = max(0, min(1, state))
+            self.write_array(0x03, [state, color])
+        except Exception:
+            print('Ctrl_WQ2812 I2C error')
+
+    def Ctrl_WQ2812_Alone(self, number, state, color):
+        try:
+            state = max(0, min(1, state))
+            self.write_array(0x04, [number, state, color])
+        except Exception:
+            print('Ctrl_WQ2812_Alone I2C error')
+
+    def Ctrl_WQ2812_brightness_ALL(self, R, G, B):
+        try:
+            self.write_array(0x08, [min(R, 255), min(G, 255), min(B, 255)])
+        except Exception:
+            print('Ctrl_WQ2812 I2C error')
+
+    def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B):
+        try:
+            self.write_array(0x09, [number, min(R, 255), min(G, 255), min(B, 255)])
+        except Exception:
+            print('Ctrl_WQ2812_Alone I2C error')
+
+    def Ctrl_IR_Switch(self, state):
+        try:
+            state = max(0, min(1, state))
+            self.write_array(0x05, [state])
+        except Exception:
+            print('Ctrl_IR_Switch I2C error')
+
+    def Ctrl_BEEP_Switch(self, state):
+        try:
+            state = max(0, min(1, state))
+            self.write_array(0x06, [state])
+        except Exception:
+            print('Ctrl_BEEP_Switch I2C error')
+
+    def Ctrl_Ulatist_Switch(self, state):
+        try:
+            state = max(0, min(1, state))
+            self.write_array(0x07, [state])
+        except Exception:
+            print('Ctrl_getDis_Switch I2C error')
+
+    def Read_Ultrasonic(self):
+        try:
+            diss_H = self.read_data_array(0x1b, 1)[0]
+            diss_L = self.read_data_array(0x1a, 1)[0]
+            return (diss_H << 8) | diss_L
+        except Exception:
+            print('Read_Ultrasonic I2C error')
+            return None
+
+
+class LightShow:
+
+    def __init__(self):
+        self.num_lights = 14
+        self.last_val = 0
+        self.MAX_TIME = 999999
+        self.bot = Raspbot()
+        self.running = True
+
+    def execute_effect(self, effect_name, effect_duration, speed, current_color):
+        try:
+            if effect_name == 'river':
+                self.run_river_light(effect_duration, speed)
+            elif effect_name == 'breathing':
+                self.breathing_light(effect_duration, speed, current_color)
+            elif effect_name == 'gradient':
+                self.gradient_light(effect_duration, speed)
+            elif effect_name == 'random_running':
+                self.random_running_light(effect_duration, speed)
+            elif effect_name == 'starlight':
+                self.starlight_shimmer(effect_duration, speed)
+            else:
+                print('Unknown effect name.')
+        except KeyboardInterrupt:
+            self.turn_off_all_lights()
+            self.running = False
+
+    def turn_off_all_lights(self):
+        self.bot.Ctrl_WQ2812_ALL(0, 0)
+
+    def run_river_light(self, effect_duration, speed):
+        colors = [0, 1, 2, 3, 4, 5, 6]
+        color_index = 0
+        end_time = time.time()
+        while self.running and time.time() - end_time < effect_duration:
+            for i in range(self.num_lights - 2):
+                self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index])
+                self.bot.Ctrl_WQ2812_Alone(i + 1, 1, colors[color_index])
+                self.bot.Ctrl_WQ2812_Alone(i + 2, 1, colors[color_index])
+                time.sleep(speed)
+                self.bot.Ctrl_WQ2812_ALL(0, 0)
+                time.sleep(speed)
+            color_index = (color_index + 1) % len(colors)
+        self.turn_off_all_lights()
+
+    def breathing_light(self, effect_duration, speed, current_color):
+        breath_direction = 0
+        breath_count = 0
+        end_time = time.time()
+        while self.running and time.time() - end_time < effect_duration:
+            color_map = {
+                0: (breath_count, 0, 0),
+                1: (0, breath_count, 0),
+                2: (0, 0, breath_count),
+                3: (breath_count, breath_count, 0),
+                4: (breath_count, 0, breath_count),
+                5: (0, breath_count, breath_count),
+                6: (breath_count, breath_count, breath_count),
+            }
+            r, g, b = color_map.get(current_color, (0, 0, 0))
+            self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b)
+            time.sleep(speed)
+            if breath_direction == 0:
+                breath_count = min(breath_count + 2, 255)
+                if breath_count >= 255:
+                    breath_direction = 1
+            else:
+                breath_count = max(breath_count - 2, 0)
+                if breath_count <= 0:
+                    breath_direction = 0
+        self.turn_off_all_lights()
+
+    def random_running_light(self, effect_duration, speed):
+        end_time = time.time()
+        while self.running and time.time() - end_time < effect_duration:
+            for i in range(self.num_lights):
+                self.bot.Ctrl_WQ2812_Alone(i, 1, random.randint(0, 6))
+            time.sleep(speed)
+        self.turn_off_all_lights()
+
+    def starlight_shimmer(self, effect_duration, speed):
+        colors = [0, 1, 2, 3, 4, 5, 6]
+        end_time = time.time()
+        while self.running and time.time() - end_time < effect_duration:
+            for color in colors:
+                start_time = time.time()
+                while time.time() - start_time < 1:
+                    for i in range(self.num_lights):
+                        self.bot.Ctrl_WQ2812_Alone(i, 0, 0)
+                    lights_on = random.sample(
+                        range(self.num_lights),
+                        k=random.randint(1, 7),
+                    )
+                    for i in lights_on:
+                        self.bot.Ctrl_WQ2812_Alone(i, 1, color)
+                    time.sleep(speed)
+                for i in range(self.num_lights):
+                    self.bot.Ctrl_WQ2812_Alone(i, 0, 0)
+        self.turn_off_all_lights()
+
+    def gradient_light(self, effect_duration, speed):
+        grad_color = 0
+        grad_index = 0
+        gt_red = gt_green = gt_blue = 0
+        end_time = time.time()
+        while self.running and time.time() - end_time < effect_duration:
+            if grad_color % 2 == 0:
+                gt_red = random.randint(0, 255)
+                gt_green = self.rgb_remix(random.randint(0, 255))
+                gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, random.randint(0, 255))
+                grad_color += 1
+
+            if grad_color == 1:
+                if grad_index < 14:
+                    self.bot.Ctrl_WQ2812_brightness_Alone(
+                        (grad_index % 14) + 1, gt_red, gt_green, gt_blue
+                    )
+                    grad_index += 1
+                if grad_index >= 14:
+                    grad_color = 2
+                    grad_index = 0
+
+            elif grad_color == 3:
+                if grad_index < 14:
+                    self.bot.Ctrl_WQ2812_brightness_Alone(
+                        (14 - grad_index) % 14, gt_red, gt_green, gt_blue
+                    )
+                    grad_index += 1
+                if grad_index >= 14:
+                    grad_color = 0
+                    grad_index = 0
+
+            time.sleep(speed)
+        self.turn_off_all_lights()
+
+    def rgb_remix(self, val):
+        if abs(val - self.last_val) < val % 30:
+            val = (val + self.last_val) % 255
+        self.last_val = val % 255
+        return self.last_val
+
+    def rgb_remix_u8(self, r, g, b):
+        if r > 50 and g > 50 and b > 50:
+            index = random.randint(0, 2)
+            if index == 0:
+                r = 0
+            elif index == 1:
+                g = 0
+            else:
+                b = 0
+        return r, g, b
+
+    def stop(self):
+        self.running = False
@@ -0,0 +1,167 @@
+#!/usr/bin/env python3
+"""
+Ultrasonic range sensor node.
+
+Publishes distance readings from the Raspbot's HC-SR04-style sensor as a
+standard sensor_msgs/Range message.  The sensor is only powered on while at
+least one subscriber is active, and automatically powered off when the last
+subscriber disconnects.
+
+Standard interface
+------------------
+Published : /ultrasonic/range  (sensor_msgs/Range)
+            radiation_type = ULTRASOUND
+            range in metres; +inf when target is beyond max_range,
+            -inf when closer than min_range (REP-117 convention).
+
+Parameters
+----------
+publish_rate_hz  float  default 10.0   – sensor poll and publish rate
+frame_id         str    default 'ultrasonic' – header frame_id
+min_range_m      float  default 0.02   – minimum valid range (m)
+max_range_m      float  default 4.0    – maximum valid range (m)
+field_of_view    float  default 0.2618 – sensor cone width (rad, ~15°)
+warmup_s         float  default 1.0    – seconds to wait after powering the
+                                         sensor on before trusting readings
+"""
+
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Range
+
+
+class UltrasonicNode(Node):
+
+    def __init__(self):
+        super().__init__('ultrasonic')
+
+        # --- Parameters ---
+        self.declare_parameter('publish_rate_hz', 10.0)
+        self.declare_parameter('frame_id',        'ultrasonic')
+        self.declare_parameter('min_range_m',     0.02)
+        self.declare_parameter('max_range_m',     4.0)
+        self.declare_parameter('field_of_view',   0.2618)  # ~15 degrees
+        self.declare_parameter('warmup_s',        1.0)
+
+        self._frame_id    = self.get_parameter('frame_id').value
+        self._min_range   = self.get_parameter('min_range_m').value
+        self._max_range   = self.get_parameter('max_range_m').value
+        self._fov         = self.get_parameter('field_of_view').value
+        self._warmup_s    = self.get_parameter('warmup_s').value
+        rate_hz           = self.get_parameter('publish_rate_hz').value
+
+        # --- Hardware ---
+        from raspbot_v2_hardware.raspbot_lib import Raspbot
+        self._bot = Raspbot()
+
+        # --- Sensor state ---
+        self._sensor_on    = False
+        self._enabled_at   = 0.0   # monotonic time when sensor was switched on
+
+        # --- Publisher ---
+        self._pub = self.create_publisher(Range, 'ultrasonic/range', 10)
+
+        # --- Timer ---
+        self._timer = self.create_timer(1.0 / rate_hz, self._tick)
+
+        self.get_logger().info(
+            f'Ultrasonic node started ({rate_hz} Hz, '
+            f'range {self._min_range}–{self._max_range} m). '
+            f'Sensor will activate when subscribers connect.'
+        )
+
+    # ------------------------------------------------------------------
+    # Timer callback
+    # ------------------------------------------------------------------
+
+    def _tick(self):
+        has_subscribers = self._pub.get_subscription_count() > 0
+
+        # Power the sensor on/off based on subscriber presence
+        if has_subscribers and not self._sensor_on:
+            self._set_sensor(True)
+            return  # skip this tick — let the sensor warm up
+
+        if not has_subscribers and self._sensor_on:
+            self._set_sensor(False)
+            return
+
+        if not self._sensor_on:
+            return
+
+        # Still warming up — do not publish unreliable readings
+        if time.monotonic() - self._enabled_at < self._warmup_s:
+            return
+
+        # Read and publish
+        raw_mm = self._bot.Read_Ultrasonic()
+        if raw_mm is None:
+            self.get_logger().warn('Ultrasonic read returned None', throttle_duration_sec=5.0)
+            return
+
+        self._pub.publish(self._build_msg(raw_mm))
+
+    # ------------------------------------------------------------------
+    # Helpers
+    # ------------------------------------------------------------------
+
+    def _set_sensor(self, on: bool):
+        try:
+            self._bot.Ctrl_Ulatist_Switch(1 if on else 0)
+            self._sensor_on  = on
+            self._enabled_at = time.monotonic()
+            self.get_logger().info(f'Ultrasonic sensor {"enabled" if on else "disabled"}')
+        except Exception as exc:
+            self.get_logger().error(f'Failed to set sensor state: {exc}')
+
+    def _build_msg(self, raw_mm: int) -> Range:
+        msg = Range()
+        msg.header.stamp    = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._frame_id
+        msg.radiation_type  = Range.ULTRASOUND
+        msg.field_of_view   = self._fov
+        msg.min_range       = self._min_range
+        msg.max_range       = self._max_range
+
+        distance_m = raw_mm / 1000.0
+
+        # REP-117: report +inf / -inf for out-of-range readings
+        if distance_m > self._max_range:
+            msg.range = math.inf
+        elif distance_m < self._min_range:
+            msg.range = -math.inf
+        else:
+            msg.range = distance_m
+
+        return msg
+
+    # ------------------------------------------------------------------
+    # Shutdown
+    # ------------------------------------------------------------------
+
+    def destroy_node(self):
+        if self._sensor_on:
+            try:
+                self._bot.Ctrl_Ulatist_Switch(0)
+            except Exception:
+                pass
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = UltrasonicNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/raspbot_v2_hardware
+[install]
+install_scripts=$base/lib/raspbot_v2_hardware
@@ -0,0 +1,34 @@
+from setuptools import find_packages, setup
+
+package_name = 'raspbot_v2_hardware'
+
+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']),
+        ('share/' + package_name + '/launch', ['launch/hardware.launch.py']),
+    ],
+    package_data={'': ['py.typed']},
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Matt Spencer',
+    maintainer_email='matthew@thespencers.me.uk',
+    description='TODO: Package description',
+    license='MIT',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'camera_orientation_controller = raspbot_v2_hardware.camera_orientation_node:main',
+            'led_controller                = raspbot_v2_hardware.led_node:main',
+            'ultrasonic_controller         = raspbot_v2_hardware.ultrasonic_node:main',
+        ],
+    },
+)
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright() -> None:
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8() -> None:
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)
@@ -0,0 +1,23 @@
+# Copyright 2025 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_mypy.main import main
+import pytest
+
+
+@pytest.mark.mypy
+@pytest.mark.linter
+def test_mypy() -> None:
+    rc = main(argv=[])
+    assert rc == 0, 'Found type errors!'
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257() -> None:
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_xmllint.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.xmllint
+def test_xmllint() -> None:
+    rc = main(argv=[])
+    assert rc == 0, 'Found code style errors / warnings'
@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.8)
+project(raspbot_v2_hardware_interface)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/raspbot_hardware_interface.cpp
+)
+
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  hardware_interface::hardware_interface
+  hardware_interface::mock_components
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+)
+
+pluginlib_export_plugin_description_file(
+  hardware_interface raspbot_v2_hardware_interface_plugin.xml
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+install(
+  FILES raspbot_v2_hardware_interface_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(hardware_interface pluginlib rclcpp)
+
+ament_package()
@@ -0,0 +1,74 @@
+#pragma once
+
+#include <array>
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+
+namespace raspbot_v2_hardware_interface
+{
+
+class RaspbotHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+  CallbackReturn on_init(
+    const hardware_interface::HardwareComponentInterfaceParams & params) override;
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
+
+  hardware_interface::return_type read(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  hardware_interface::return_type write(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  int i2c_fd_{-1};
+  int i2c_bus_{1};
+  double max_wheel_vel_{10.0};  // rad/s that maps to driver value 255
+
+  // Accessed only from the RT control-loop thread — no lock needed.
+  std::array<double, 4> wheel_positions_{};
+  std::array<double, 4> wheel_velocities_{};
+  std::array<double, 4> wheel_commands_{};
+
+  // The Yahboom STM32 motor controller clock-stretches the I2C bus for ~80 ms
+  // per bulk write while it applies PWM.  A blocking write() in the RT control
+  // loop therefore overruns the 20 ms budget at 50 Hz.
+  //
+  // This background thread decouples I2C from the RT loop: write() deposits
+  // the latest commands under a brief mutex hold and returns immediately; the
+  // thread drains the buffer at whatever rate the hardware allows (~12 Hz).
+  std::thread i2c_thread_;
+  std::mutex cmd_mutex_;
+  std::condition_variable cmd_cv_;
+  std::array<double, 4> desired_commands_{};  // protected by cmd_mutex_
+  bool cmd_pending_{false};                   // protected by cmd_mutex_
+  bool i2c_thread_running_{false};            // protected by cmd_mutex_
+
+  void i2c_write_loop();
+  void stop_i2c_thread();
+
+  // Joint order: [FL, FR, RL, RR]
+  static constexpr int MOTOR_FL = 0;
+  static constexpr int MOTOR_RL = 1;
+  static constexpr int MOTOR_FR = 2;
+  static constexpr int MOTOR_RR = 3;
+  static constexpr int I2C_DEVICE_ADDR = 0x2B;
+
+  bool ctrl_muto(int motor_id, int speed);
+  int to_motor_speed(double rad_s) const;
+  void stop_all_motors();
+};
+
+}  // namespace raspbot_v2_hardware_interface
@@ -0,0 +1,19 @@
+<?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>raspbot_v2_hardware_interface</name>
+  <version>0.0.1</version>
+  <description>ros2_control hardware interface plugin for Raspbot V2 mecanum drive</description>
+  <maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,12 @@
+<library path="raspbot_v2_hardware_interface">
+  <class
+    name="raspbot_v2_hardware_interface/RaspbotHardwareInterface"
+    type="raspbot_v2_hardware_interface::RaspbotHardwareInterface"
+    base_class_type="hardware_interface::SystemInterface">
+    <description>
+      ros2_control hardware interface for the Raspbot V2 mecanum drive.
+      Translates joint velocity commands from MecanumDriveController into
+      I2C motor commands via the Yahboom MCU (register 0x01, address 0x2B).
+    </description>
+  </class>
+</library>
@@ -0,0 +1,166 @@
+#include "raspbot_v2_hardware_interface/raspbot_hardware_interface.hpp"
+
+#include <fcntl.h>
+#include <linux/i2c-dev.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include <algorithm>
+#include <cerrno>
+#include <cmath>
+#include <cstring>
+#include <string>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace raspbot_v2_hardware_interface
+{
+
+hardware_interface::CallbackReturn RaspbotHardwareInterface::on_init(
+  const hardware_interface::HardwareComponentInterfaceParams & params)
+{
+  if (SystemInterface::on_init(params) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+
+  if (info_.hardware_parameters.count("max_wheel_velocity_rad_s")) {
+    max_wheel_vel_ = std::stod(info_.hardware_parameters.at("max_wheel_velocity_rad_s"));
+  }
+  if (info_.hardware_parameters.count("i2c_bus")) {
+    i2c_bus_ = std::stoi(info_.hardware_parameters.at("i2c_bus"));
+  }
+
+  wheel_positions_.fill(0.0);
+  wheel_velocities_.fill(0.0);
+  wheel_commands_.fill(0.0);
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RaspbotHardwareInterface::on_configure(
+  const rclcpp_lifecycle::State &)
+{
+  std::string dev = "/dev/i2c-" + std::to_string(i2c_bus_);
+  i2c_fd_ = ::open(dev.c_str(), O_RDWR);
+  if (i2c_fd_ < 0) {
+    RCLCPP_ERROR(get_logger(), "Failed to open %s: %s", dev.c_str(), strerror(errno));
+    return CallbackReturn::ERROR;
+  }
+  if (::ioctl(i2c_fd_, I2C_SLAVE, I2C_DEVICE_ADDR) < 0) {
+    RCLCPP_ERROR(
+      get_logger(), "Failed to set I2C slave 0x%02X: %s", I2C_DEVICE_ADDR, strerror(errno));
+    ::close(i2c_fd_);
+    i2c_fd_ = -1;
+    return CallbackReturn::ERROR;
+  }
+
+  RCLCPP_INFO(
+    get_logger(), "I2C opened on %s, slave 0x%02X, max vel %.1f rad/s",
+    dev.c_str(), I2C_DEVICE_ADDR, max_wheel_vel_);
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RaspbotHardwareInterface::on_activate(
+  const rclcpp_lifecycle::State &)
+{
+  stop_all_motors();
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RaspbotHardwareInterface::on_deactivate(
+  const rclcpp_lifecycle::State &)
+{
+  stop_all_motors();
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RaspbotHardwareInterface::on_cleanup(
+  const rclcpp_lifecycle::State &)
+{
+  if (i2c_fd_ >= 0) {
+    ::close(i2c_fd_);
+    i2c_fd_ = -1;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type RaspbotHardwareInterface::read(
+  const rclcpp::Time &, const rclcpp::Duration & period)
+{
+  // No encoder read-back: mirror the last command as velocity and integrate position.
+  const double dt = period.seconds();
+  for (size_t i = 0; i < 4; ++i) {
+    wheel_velocities_[i] = wheel_commands_[i];
+    wheel_positions_[i] += wheel_velocities_[i] * dt;
+  }
+
+  set_state("joint_wheel_front_left/position",  wheel_positions_[0]);
+  set_state("joint_wheel_front_right/position", wheel_positions_[1]);
+  set_state("joint_wheel_rear_left/position",   wheel_positions_[2]);
+  set_state("joint_wheel_rear_right/position",  wheel_positions_[3]);
+
+  set_state("joint_wheel_front_left/velocity",  wheel_velocities_[0]);
+  set_state("joint_wheel_front_right/velocity", wheel_velocities_[1]);
+  set_state("joint_wheel_rear_left/velocity",   wheel_velocities_[2]);
+  set_state("joint_wheel_rear_right/velocity",  wheel_velocities_[3]);
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type RaspbotHardwareInterface::write(
+  const rclcpp::Time &, const rclcpp::Duration &)
+{
+  wheel_commands_[0] = get_command<double>("joint_wheel_front_left/velocity");
+  wheel_commands_[1] = get_command<double>("joint_wheel_front_right/velocity");
+  wheel_commands_[2] = get_command<double>("joint_wheel_rear_left/velocity");
+  wheel_commands_[3] = get_command<double>("joint_wheel_rear_right/velocity");
+
+  // Sanitise NaN (controller not yet active)
+  for (auto & v : wheel_commands_) {
+    if (std::isnan(v)) {v = 0.0;}
+  }
+
+  ctrl_muto(MOTOR_FL, to_motor_speed(wheel_commands_[0]));
+  ctrl_muto(MOTOR_FR, to_motor_speed(wheel_commands_[1]));
+  ctrl_muto(MOTOR_RL, to_motor_speed(wheel_commands_[2]));
+  ctrl_muto(MOTOR_RR, to_motor_speed(wheel_commands_[3]));
+
+  return hardware_interface::return_type::OK;
+}
+
+// ---------------------------------------------------------------------------
+// Private helpers
+// ---------------------------------------------------------------------------
+
+bool RaspbotHardwareInterface::ctrl_muto(int motor_id, int speed)
+{
+  const int clamped = std::clamp(speed, -255, 255);
+  const uint8_t dir = (clamped < 0) ? 1 : 0;
+  const uint8_t abs_speed = static_cast<uint8_t>(std::abs(clamped));
+  // Register 0x01: [motor_id, direction, speed] — mirrors Raspbot_Lib Ctrl_Muto
+  uint8_t buf[] = {0x01, static_cast<uint8_t>(motor_id), dir, abs_speed};
+  return ::write(i2c_fd_, buf, sizeof(buf)) == static_cast<ssize_t>(sizeof(buf));
+}
+
+int RaspbotHardwareInterface::to_motor_speed(double rad_s) const
+{
+  if (max_wheel_vel_ <= 0.0) {return 0;}
+  const double scaled = (rad_s / max_wheel_vel_) * 255.0;
+  return static_cast<int>(std::clamp(scaled, -255.0, 255.0));
+}
+
+void RaspbotHardwareInterface::stop_all_motors()
+{
+  for (int id : {MOTOR_FL, MOTOR_RL, MOTOR_FR, MOTOR_RR}) {
+    ctrl_muto(id, 0);
+  }
+  wheel_commands_.fill(0.0);
+}
+
+}  // namespace raspbot_v2_hardware_interface
+
+PLUGINLIB_EXPORT_CLASS(
+  raspbot_v2_hardware_interface::RaspbotHardwareInterface,
+  hardware_interface::SystemInterface)
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.20)
+project(raspbot_v2_sim)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY launch rviz worlds
+  DESTINATION share/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
@@ -0,0 +1,103 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    rviz = LaunchConfiguration('rviz')
+    
+    declared_args = [
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='ROS namespace for all robot nodes (e.g. "robot1").'
+        ),
+        DeclareLaunchArgument(
+            'rviz', default_value='false',
+            description='If true, launch RViz.'
+        ),
+    ]
+
+
+    pkg_share = get_package_share_directory('raspbot_v2_sim')
+    rviz_config_dir = os.path.join(pkg_share, 'rviz', 'raspbot_v2.rviz')
+    world_file = os.path.join(pkg_share, 'worlds', 'warehouse_world')
+    
+    bringup_robot = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare('raspbot_v2_bringup'), 'launch', 'raspbot_v2_bringup.launch.py'
+            ])
+        ]),
+        launch_arguments={
+            'namespace': namespace,
+            'use_sim_time': 'true',
+        }.items()
+    )
+    
+    gazebo_node = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'
+            ])
+        ]),
+        launch_arguments={
+            'gz_args':         ['-r ', world_file],
+            'on_exit_shutdown': 'true',
+        }.items(),
+    )
+
+    
+    # Spawn the robot entity into Gazebo after the world has loaded.
+    # This triggers gz_ros2_control::GazeboSimROS2ControlPlugin, which hosts
+    # the controller_manager.  Without this the /controller_manager service
+    # never appears, even though Gazebo is running.
+    spawn_robot = TimerAction(
+        period=3.0,
+        actions=[Node(
+            package='ros_gz_sim',
+            executable='create',
+            parameters=[{
+                'topic': '/robot_description',
+                'name':  'raspbot_v2',
+                'x': 0.0, 'y': 0.0, 'z': 0.05,
+            }],
+            output='screen',
+        )],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        arguments=['-d', rviz_config_dir],
+        output='screen',
+        condition=IfCondition(rviz),
+    )
+
+
+    clock_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
+            '/camera/image@sensor_msgs/msg/Image[gz.msgs.Image',
+            '/camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo',
+            '/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+        ],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        *declared_args,
+        gazebo_node,
+        clock_bridge,
+        bringup_robot,
+        spawn_robot,
+        rviz_node,
+    ])
@@ -0,0 +1,49 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    
+    declared_args = [
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='ROS namespace for all robot nodes (e.g. "robot1").'
+        ),
+    ]
+
+
+    rviz_config_dir = os.path.join(
+        get_package_share_directory('raspbot_v2_sim'),
+        'rviz', 'raspbot_v2.rviz')
+    
+    bringup_robot = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare('raspbot_v2_bringup'), 'launch', 'raspbot_v2_bringup.launch.py'
+            ])
+        ]),
+        launch_arguments={
+            'namespace': namespace,
+            'use_sim_time': 'false',
+        }.items()
+    )
+
+    return LaunchDescription([
+        *declared_args,
+        bringup_robot,
+        Node(
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            arguments=['-d', rviz_config_dir],
+            output='screen',
+        )
+
+    ])
@@ -0,0 +1,18 @@
+<?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>raspbot_v2_sim</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,275 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 555
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        wheel_front_left:
+          Value: true
+        wheel_front_right:
+          Value: true
+        wheel_rear_left:
+          Value: true
+        wheel_rear_right:
+          Value: true
+      Marker Scale: 0.20000000298023224
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_footprint:
+          base_link:
+            wheel_front_left:
+              {}
+            wheel_front_right:
+              {}
+            wheel_rear_left:
+              {}
+            wheel_rear_right:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_front_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_front_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_rear_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_rear_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Camera
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/image
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 0.6006462574005127
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 66
+  Y: 60
@@ -0,0 +1,86 @@
+<?xml version="1.0" ?>
+<!--
+  Minimal Gazebo Harmonic (gz-sim) world for the Raspbot V2 simulation.
+  Contains only a ground plane, a sun, and the required system plugins.
+  Add walls, furniture, or other models as needed for realistic testing.
+-->
+<sdf version="1.9">
+  <world name="empty">
+
+    <!-- ── Physics ─────────────────────────────────────────────────────── -->
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <!-- ── Required gz-sim system plugins ───────────────────────────────── -->
+    <plugin filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics"/>
+    <plugin filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands"/>
+    <plugin filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster"/>
+
+    <!-- Sensors system — required for lidar, camera, and IMU sensors -->
+    <plugin filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <!-- Contact sensor system — required for collision detection -->
+    <plugin filename="gz-sim-contact-system"
+            name="gz::sim::systems::Contact"/>
+
+    <!-- ── Lighting ─────────────────────────────────────────────────────── -->
+    <light name="sun" type="directional">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <direction>-0.5 0.1 -0.9</direction>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+    </light>
+
+    <!-- ── Ground plane ─────────────────────────────────────────────────── -->
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>
@@ -0,0 +1,89 @@
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="warehouse">
+
+    <!-- ── Physics ─────────────────────────────────────────────────────── -->
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <!-- ── Required gz-sim system plugins ───────────────────────────────── -->
+    <plugin filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics"/>
+    <plugin filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands"/>
+    <plugin filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster"/>
+
+    <!-- Sensors system — required for lidar, camera, and IMU sensors -->
+    <plugin filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <!-- Contact sensor system — required for collision detection -->
+    <plugin filename="gz-sim-contact-system"
+            name="gz::sim::systems::Contact"/>
+
+    <!-- ── Lighting ─────────────────────────────────────────────────────── -->
+    <light name="sun" type="directional">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <direction>-0.5 0.1 -0.9</direction>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+    </light>
+
+    <!-- ── Ground plane ─────────────────────────────────────────────────── -->
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- ── Small Warehouse (OpenRobotics / Gazebo Fuel) ─────────────────── -->
+    <include>
+      <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Warehouse</uri>
+      <name>warehouse</name>
+      <!-- Offset so the mesh floor (native z≈0.0986) sits at world z=0 -->
+      <pose>0 0 -0.0986 0 0 0</pose>
+    </include>
+
+  </world>
+</sdf>