Warehouse sim running - but not yet working.

.devcontainer/Dockerfile

@@ -51,6 +51,20 @@ RUN apt-get update && apt-get install -y \
     ros-${ROS_DISTRO}-xacro \
     ros-${ROS_DISTRO}-robot-state-publisher
 
+# Gazebo Harmonic + ROS 2 integration
+RUN apt-get update && apt-get install -y \
+    ros-${ROS_DISTRO}-ros-gz \
+    ros-${ROS_DISTRO}-gz-ros2-control
+
+# Navigation and SLAM
+RUN apt-get update && apt-get install -y \
+    ros-${ROS_DISTRO}-nav2-bringup \
+    ros-${ROS_DISTRO}-slam-toolbox \
+    ros-${ROS_DISTRO}-joint-state-publisher \
+    ros-${ROS_DISTRO}-joint-state-broadcaster \
+    ros-${ROS_DISTRO}-joint-trajectory-controller \
+    ros-${ROS_DISTRO}-controller-manager
+
 # Node.js for frontend development
 RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \
     && apt-get install -y nodejs

robot/README.md

@@ -180,7 +180,67 @@ The sensor activates automatically when a subscriber connects and deactivates wh
 
 ---
 
-## Launch arguments
+## Simulation (Gazebo)
+
+The simulation runs inside the **devcontainer** — not as a Docker Compose service. The devcontainer already has the display socket, GPU access, and host networking that Gazebo requires. Docker Compose is used only for deployment to the physical robot.
+
+Build the ROS workspace inside the devcontainer (once, or after source changes):
+
+```bash
+cd /home/ws
+colcon build --packages-select raspbot_v2 --symlink-install
+source install/setup.bash
+```
+
+### Launching
+
+```bash
+# Gazebo only — robot spawned, no SLAM or navigation
+ros2 launch raspbot_v2 sim_launch.py
+
+# Gazebo + SLAM (building a map) + RViz
+ros2 launch raspbot_v2 sim_launch.py use_slam:=true use_rviz:=true
+
+# Full stack — SLAM + Nav2 autonomous navigation + RViz
+ros2 launch raspbot_v2 sim_launch.py use_slam:=true use_nav2:=true use_rviz:=true
+
+# Use a custom world file
+ros2 launch raspbot_v2 sim_launch.py world:=/path/to/my_world.sdf
+```
+
+### Launch arguments
+
+| Argument | Default | Description |
+|---|---|---|
+| `world` | `worlds/empty_world.sdf` | Path to the Gazebo world SDF |
+| `use_slam` | `false` | Run slam_toolbox in async mapping mode |
+| `use_nav2` | `false` | Run the Nav2 navigation stack |
+| `use_rviz` | `true` | Open RViz2 with the pre-configured layout |
+| `wheel_base` | `0.14` | Wheel separation (m) — match the motor controller value |
+
+### Configuration files
+
+| File | Purpose |
+|---|---|
+| `config/slam_toolbox.yaml` | SLAM mode, map resolution, scan topic, frame IDs |
+| `config/nav2_params.yaml` | BT navigator, DWB local planner, costmaps, AMCL, recovery behaviours |
+| `config/controllers.yaml` | ros2_control — joint_state_broadcaster and pan_tilt_controller with PID gains |
+| `config/rviz2_config.rviz` | Pre-configured displays: Map, RobotModel, LaserScan, Camera, TF, path plans |
+| `worlds/empty_world.sdf` | Flat ground plane with lighting — add walls and furniture as needed |
+
+### Relationship to the hardware launch
+
+`sim_launch.py` and `robot.launch.py` use the same URDF and the same `raspbot_v2` package. The difference is what drives the joints:
+
+| | Simulation | Hardware |
+|---|---|---|
+| Drive | `gz-sim-diff-drive-system` plugin | `motor_controller_node` via I²C |
+| Pan/tilt | `gz_ros2_control` + `pan_tilt_controller` | `camera_orientation_node` via I²C |
+| Clock | Gazebo simulated time (`use_sim_time: true`) | System clock |
+
+---
+
+## Hardware launch arguments
 
 Launch arguments can be appended when running the container manually:
 
@@ -214,11 +274,28 @@ robot/
 │       ├── package.xml
 │       ├── setup.py
 │       ├── launch/
-│       │   └── robot.launch.py   # Starts all three nodes together
+│       │   ├── robot.launch.py        # Hardware — starts all nodes on the physical robot
+│       │   └── sim_launch.py          # Simulation — Gazebo + optional SLAM/Nav2/RViz
+│       ├── urdf/
+│       │   ├── raspbot_v2.urdf.xacro  # Top-level — includes all sub-files
+│       │   ├── robot_base.xacro       # Chassis, wheels, diff-drive plugin
+│       │   ├── pan_tilt.xacro         # Pan/tilt mount, joints, ros2_control block
+│       │   ├── camera.xacro           # Camera link, optical frame, Gazebo sensor
+│       │   ├── lidar.xacro            # Lidar link, Gazebo gpu_lidar sensor
+│       │   ├── sonar.xacro            # Ultrasonic link, Gazebo narrow-lidar approximation
+│       │   └── raspbot_v2.ros2_control.xacro  # gz_ros2_control plugin
+│       ├── config/
+│       │   ├── controllers.yaml       # ros2_control — joint_state_broadcaster + pan_tilt_controller
+│       │   ├── slam_toolbox.yaml      # SLAM mode, map resolution, scan topic, frame IDs
+│       │   ├── nav2_params.yaml       # BT navigator, DWB planner, costmaps, AMCL, recovery
+│       │   └── rviz2_config.rviz      # Pre-configured displays: Map, RobotModel, LaserScan, Camera
+│       ├── worlds/
+│       │   └── empty_world.sdf        # Flat ground plane with lighting
 │       └── raspbot_v2/
 │           ├── motor_controller_node.py
 │           ├── camera_orientation_node.py
-│           └── ultrasonic_node.py
+│           ├── ultrasonic_node.py
+│           └── led_node.py
 └── raspbot_v2_interface/              # Vendored Yahboom hardware library
     └── Raspbot_Lib/
         └── Raspbot_Lib.py             # I²C driver (smbus, bus 1, addr 0x2B)

robot/src/raspbot_v2/config/controllers.yaml

@@ -36,3 +36,20 @@ pan_tilt_controller:
         goal: 0.05         # rad — acceptable position error at goal
       tilt:
         goal: 0.05         # rad
+    # PID gains — active when command_interfaces includes velocity or effort,
+    # or when the hardware plugin uses them for closed-loop position control.
+    # Tune on the real robot: raise p until oscillation, then halve it; add d
+    # to damp, and a small i to remove steady-state error.
+    gains:
+      pan:
+        p: 10.0
+        i: 0.01
+        d: 1.0
+        i_clamp: 1.0
+        ff_velocity_scale: 1.0
+      tilt:
+        p: 10.0
+        i: 0.01
+        d: 1.0
+        i_clamp: 1.0
+        ff_velocity_scale: 1.0

robot/src/raspbot_v2/config/nav2_params.yaml

@@ -0,0 +1,314 @@
+# Nav2 parameters for the Raspbot V2
+# Small differential-drive indoor robot: 20 × 15 cm chassis, max 0.2 m/s
+#
+# Tuning starting points — adjust after observing behaviour in simulation:
+#   robot_radius      → measure the actual half-diagonal
+#   inflation_radius  → must clear the robot + a comfortable margin
+#   max_vel_x         → limited by motor speed and I²C latency
+#   DWB critic scales → path alignment vs obstacle clearance trade-off
+
+# ── AMCL (particle-filter localisation against a saved map) ─────────────────
+# Not used when slam_toolbox runs in mapping mode — slam_toolbox publishes
+# the map → odom transform directly.  Switch to this when using a pre-built
+# map with slam_toolbox in localization mode.
+amcl:
+  ros__parameters:
+    use_sim_time: true
+    alpha1: 0.002   # rotation noise from rotation
+    alpha2: 0.002   # rotation noise from translation
+    alpha3: 0.002   # translation noise from translation
+    alpha4: 0.002   # translation noise from rotation
+    alpha5: 0.002   # (strafe — 0 for differential drive)
+    base_frame_id: base_footprint
+    global_frame_id: map
+    odom_frame_id: odom
+    robot_model_type: nav2_amcl::DifferentialMotionModel
+    scan_topic: /scan
+    laser_model_type: likelihood_field
+    laser_max_range: 12.0
+    laser_min_range: 0.15
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    pf_err: 0.05
+    pf_z: 0.99
+    resample_interval: 1
+    sigma_hit: 0.2
+    z_hit: 0.5
+    z_rand: 0.5
+    z_max: 0.05
+    z_short: 0.05
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    update_min_d: 0.25      # m — minimum travel before filter update
+    update_min_a: 0.2       # rad — minimum rotation before filter update
+    transform_tolerance: 1.0
+    tf_broadcast: true
+    save_pose_rate: 0.5
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    do_beamskip: false
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+
+# ── BT Navigator ─────────────────────────────────────────────────────────────
+bt_navigator:
+  ros__parameters:
+    use_sim_time: true
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 10          # ms
+    default_server_timeout: 20    # ms
+    navigators:
+      - navigate_to_pose
+      - navigate_through_poses
+    navigate_to_pose:
+      plugin: nav2_bt_navigator::NavigateToPoseNavigator
+    navigate_through_poses:
+      plugin: nav2_bt_navigator::NavigateThroughPosesNavigator
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
+
+# ── Controller server (local planner — DWB) ──────────────────────────────────
+controller_server:
+  ros__parameters:
+    use_sim_time: true
+    controller_frequency: 20.0    # Hz
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    speed_limit_topic: speed_limit
+    progress_checker_plugins: [progress_checker]
+    goal_checker_plugins: [general_goal_checker]
+    controller_plugins: [FollowPath]
+
+    progress_checker:
+      plugin: nav2_controller::SimpleProgressChecker
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    general_goal_checker:
+      plugin: nav2_controller::SimpleGoalChecker
+      stateful: true
+      xy_goal_tolerance: 0.25    # m
+      yaw_goal_tolerance: 0.25   # rad
+
+    FollowPath:
+      plugin: dwb_core::DWBLocalPlanner
+      debug_trajectory_details: true
+
+      # Velocity limits — conservative for a small indoor robot
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.20      # m/s
+      max_vel_y: 0.0       # differential drive
+      max_vel_theta: 1.0   # rad/s
+      min_speed_xy: 0.0
+      max_speed_xy: 0.20
+      min_speed_theta: 0.0
+
+      # Acceleration limits
+      acc_lim_x: 0.5       # m/s²
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2   # rad/s²
+      decel_lim_x: -1.0
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+
+      # Trajectory sampling
+      vx_samples: 20
+      vy_samples: 1        # 1 = no lateral sampling for diff drive
+      vtheta_samples: 20
+      sim_time: 1.7        # seconds to simulate each candidate trajectory
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: true
+      stateful: true
+
+      # Critics and their weights
+      critics:
+        - RotateToGoal
+        - Oscillation
+        - BaseObstacle
+        - GoalAlign
+        - PathAlign
+        - PathDist
+        - GoalDist
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+# ── Planner server (global planner — NavFn/A*) ───────────────────────────────
+planner_server:
+  ros__parameters:
+    use_sim_time: true
+    planner_plugins: [GridBased]
+    GridBased:
+      plugin: nav2_navfn_planner::NavfnPlanner
+      tolerance: 0.5
+      use_astar: false     # false = Dijkstra; true = A* (faster but less optimal)
+      allow_unknown: true
+
+# ── Behaviour server (recovery actions) ──────────────────────────────────────
+behavior_server:
+  ros__parameters:
+    use_sim_time: true
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins:
+      - spin
+      - backup
+      - drive_on_heading
+      - wait
+      - assisted_teleop
+    spin:
+      plugin: nav2_behaviors::Spin
+    backup:
+      plugin: nav2_behaviors::BackUp
+    drive_on_heading:
+      plugin: nav2_behaviors::DriveOnHeading
+    wait:
+      plugin: nav2_behaviors::Wait
+    assisted_teleop:
+      plugin: nav2_behaviors::AssistedTeleop
+    # Conservative limits for recovery motions
+    max_rotational_vel: 1.0    # rad/s
+    min_rotational_vel: 0.4    # rad/s
+    rotational_acc_lim: 3.2    # rad/s²
+    simulate_ahead_time: 2.0   # seconds
+
+# ── Velocity smoother ────────────────────────────────────────────────────────
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: true
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: OPEN_LOOP
+    max_velocity: [0.20, 0.0, 1.0]   # [x m/s, y m/s, theta rad/s]
+    min_velocity: [-0.20, 0.0, -1.0]
+    max_accel: [0.5, 0.0, 3.2]
+    max_decel: [-1.0, 0.0, -3.2]
+    odom_topic: odom
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+
+# ── Waypoint follower ─────────────────────────────────────────────────────────
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: true
+    loop_rate: 20
+    stop_on_failure: false
+    action_server_result_timeout: 900.0
+    waypoint_task_executor_plugin: wait_at_waypoint
+    wait_at_waypoint:
+      plugin: nav2_waypoint_follower::WaitAtWaypoint
+      enabled: true
+      waypoint_pause_duration: 200   # ms
+
+# ── Local costmap ─────────────────────────────────────────────────────────────
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: true
+      update_frequency: 5.0     # Hz
+      publish_frequency: 2.0    # Hz
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3                  # m — window size
+      height: 3                 # m
+      resolution: 0.05          # m/cell
+      # Footprint of the actual chassis (in metres, relative to base_footprint)
+      # Replace robot_radius with footprint for a more accurate bounding shape.
+      robot_radius: 0.15
+      plugins:
+        - obstacle_layer
+        - inflation_layer
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /scan
+          data_type: LaserScan
+          marking: true
+          clearing: true
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          max_obstacle_height: 2.0
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        inflation_radius: 0.30    # m — must be > robot_radius
+        cost_scaling_factor: 3.0
+      always_send_full_costmap: true
+
+# ── Global costmap ────────────────────────────────────────────────────────────
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: true
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      robot_radius: 0.15
+      resolution: 0.05
+      track_unknown_space: true
+      plugins:
+        - static_layer
+        - obstacle_layer
+        - inflation_layer
+      static_layer:
+        plugin: nav2_costmap_2d::StaticLayer
+        map_subscribe_transient_local: true
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /scan
+          data_type: LaserScan
+          marking: true
+          clearing: true
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          max_obstacle_height: 2.0
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        inflation_radius: 0.30
+        cost_scaling_factor: 3.0
+
+# ── Map server / saver ────────────────────────────────────────────────────────
+map_server:
+  ros__parameters:
+    use_sim_time: true
+    yaml_filename: ""   # set at launch when loading a pre-built map
+
+map_saver:
+  ros__parameters:
+    use_sim_time: true
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: true

robot/src/raspbot_v2/config/rviz2_config.rviz

@@ -0,0 +1,274 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /RobotModel1
+        - /LaserScan1
+        - /Map1
+      Splitter Ratio: 0.5
+    Tree Height: 600
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /2D Pose Estimate1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+
+Visualization Manager:
+  Class: ""
+  Displays:
+
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Value: true
+      Alpha: 0.5
+      Cell Size: 1
+      Color: 160; 160; 164
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+
+    - Class: rviz_default_plugins/RobotModel
+      Name: RobotModel
+      Value: true
+      Alpha: 1
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Links:
+        All Links Enabled: true
+      TF Prefix: ""
+      Update Interval: 0
+      Visual Enabled: true
+      Collision Enabled: false
+
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Value: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Scale: 0.3
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+
+    - Class: rviz_default_plugins/Map
+      Name: Map
+      Value: true
+      Alpha: 0.7
+      Color Scheme: map
+      Draw Behind: false
+      Topic:
+        Depth: 1
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map_updates
+
+    - Class: rviz_default_plugins/LaserScan
+      Name: LaserScan
+      Value: true
+      Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Color: 255; 50; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Position Transformer: XYZ
+      Size (m): 0.05
+      Size (Pixels): 3
+      Style: Spheres
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+
+    - Class: rviz_default_plugins/Image
+      Name: Camera
+      Value: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /camera/image_raw
+
+    - Class: rviz_default_plugins/Path
+      Name: Global Plan
+      Value: true
+      Alpha: 1
+      Buffer Length: 1
+      Color: 0; 128; 0
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /plan
+
+    - Class: rviz_default_plugins/Path
+      Name: Local Plan
+      Value: true
+      Alpha: 0.5
+      Buffer Length: 1
+      Color: 0; 0; 255
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /local_plan
+
+    - Class: nav2_rviz_plugins/ParticleCloud
+      Name: Particle Cloud
+      Value: true
+      Color: 50; 200; 50
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /particle_cloud
+
+  Tools:
+    - 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
+
+  Views:
+    Current:
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 100
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 0
+      Y: 0
+
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 900
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000357fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f00700065007200740069006500730200000000ffffffff0000009100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500670065007400730100000000ffffffff0000000000000000fb0000000c00430061006d0065007261010000018d0000010a00000000000000000000000100000200000003a5fc0200000002fb0000000a0049006d006100670065010000014f0000016a0000000000000000fb00000010006400690073007000600061007900730100000000000003a50000005000fffffffb000000100044006900730070006c006100790073010000003b000002b40000002800ffffff000000010000010f000003a5fc0200000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000000003a5000000f500fffffffb000000120053006500610072006300680020003100000000ffffffff0000000000000000fb0000000c00430061006d006500720061000000000000000000000000000000000000010f0000035700000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1400
+  X: 50
+  Y: 50

robot/src/raspbot_v2/config/slam_toolbox.yaml

@@ -0,0 +1,64 @@
+slam_toolbox:
+  ros__parameters:
+
+    use_sim_time: true
+
+    # ── Solver ──────────────────────────────────────────────────────────────
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ── Frames ──────────────────────────────────────────────────────────────
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+
+    # ── Mode ────────────────────────────────────────────────────────────────
+    # mapping      — build a new map; publishes /map and /map_metadata
+    # localization — localise in a previously-saved map (load with map_server)
+    # lifelong     — continuous mapping with loop closure; memory-intensive
+    mode: mapping
+
+    # ── Map parameters ──────────────────────────────────────────────────────
+    resolution: 0.05          # metres per pixel — 5 cm is good for indoor navigation
+    max_laser_range: 12.0     # m — matches RPLIDAR A1 spec; trim to actual room size
+    map_update_interval: 5.0  # seconds between map publishes
+
+    # ── Timing / TF ─────────────────────────────────────────────────────────
+    transform_publish_period: 0.02   # seconds (50 Hz) — map → odom TF publish rate
+    transform_timeout: 0.2           # seconds — how long to wait for a TF lookup
+    tf_buffer_duration: 30.0         # seconds — TF history kept in memory
+    throttle_scans: 1                # process every Nth scan (1 = all)
+    minimum_time_interval: 0.5       # seconds — minimum gap between processed scans
+
+    # ── Scan matching ────────────────────────────────────────────────────────
+    use_scan_matching: true
+    use_scan_barycenter: true
+
+    # How far the robot must move/turn before adding a new node to the graph
+    minimum_travel_distance: 0.5    # m
+    minimum_travel_heading: 0.5     # rad (~29°)
+
+    # Scan buffer — larger = more context for loop closure but more memory
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0   # m
+
+    # Matching quality thresholds
+    link_match_minimum_response_fine: 0.1
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+
+    # Angular search parameters
+    fine_search_angle_offset: 0.00349     # rad (~0.2°)
+    coarse_search_angle_offset: 0.349     # rad (~20°)
+    coarse_angle_resolution: 0.0349       # rad (~2°)
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+
+    debug_logging: false
+    stack_size_to_use: 40000000    # bytes — increase if you see stack overflows

robot/src/raspbot_v2/launch/house_sim_launch.py

@@ -0,0 +1,42 @@
+"""
+house_sim_launch.py — Gazebo simulation using the Nav2 warehouse world
+
+Thin wrapper around sim_launch.py that sets the world to the warehouse
+environment from nav2_minimal_tb4_sim.  All other arguments (use_slam,
+use_nav2, use_rviz, wheel_base) are forwarded as-is.
+
+Note: the warehouse world pulls models from Gazebo Fuel on first launch.
+They are cached in ~/.gz/fuel after that and work offline.
+
+Usage:
+  ros2 launch raspbot_v2 house_sim_launch.py
+  ros2 launch raspbot_v2 house_sim_launch.py use_slam:=true use_nav2:=true use_rviz:=true
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    world = os.path.join(
+        get_package_share_directory('nav2_minimal_tb4_sim'),
+        'worlds',
+        'warehouse.sdf',
+    )
+
+    sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('raspbot_v2'),
+                'launch',
+                'sim_launch.py',
+            )
+        ),
+        launch_arguments={'world': world}.items(),
+    )
+
+    return LaunchDescription([sim])

robot/src/raspbot_v2/launch/robot.launch.py

@@ -48,7 +48,10 @@ def generate_launch_description():
             package='robot_state_publisher',
             executable='robot_state_publisher',
             name='robot_state_publisher',
-            parameters=[{'robot_description': robot_description}],
+            parameters=[{
+                'robot_description': robot_description,
+                'use_sim_time': False,
+            }],
             output='screen',
         ),
 

robot/src/raspbot_v2/launch/sim_launch.py

@@ -0,0 +1,259 @@
+"""
+sim_launch.py — Gazebo Harmonic simulation for the Raspbot V2
+
+Brings up:
+  • gz_sim         — Gazebo with the configured world
+  • robot_state_publisher — publishes robot_description and TF from URDF
+  • ros_gz_sim/create    — spawns the robot into the Gazebo world
+  • ros_gz_bridge        — bridges gz ↔ ROS 2 topics
+  • controller_manager   — loaded by gz_ros2_control plugin in URDF
+  • joint_state_broadcaster + pan_tilt_controller (spawned after gz starts)
+  • slam_toolbox   — online async SLAM (optional, --slam)
+  • nav2           — navigation stack   (optional, --nav2, requires --slam or map)
+  • rviz2          — pre-configured visualiser (optional, --rviz)
+
+Usage:
+  ros2 launch raspbot_v2 sim_launch.py
+  ros2 launch raspbot_v2 sim_launch.py use_slam:=true use_nav2:=true use_rviz:=true
+  ros2 launch raspbot_v2 sim_launch.py world:=/path/to/my_world.sdf
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    TimerAction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    pkg = get_package_share_directory('raspbot_v2')
+
+    # ── Paths ──────────────────────────────────────────────────────────────
+    urdf_file        = os.path.join(pkg, 'urdf', 'raspbot_v2.urdf.xacro')
+    controllers_yaml = os.path.join(pkg, 'config', 'controllers.yaml')
+    slam_yaml        = os.path.join(pkg, 'config', 'slam_toolbox.yaml')
+    nav2_yaml        = os.path.join(pkg, 'config', 'nav2_params.yaml')
+    rviz_config      = os.path.join(pkg, 'config', 'rviz2_config.rviz')
+    default_world    = os.path.join(pkg, 'worlds', 'empty_world.sdf')
+
+    # ── Launch arguments ───────────────────────────────────────────────────
+    world_arg    = DeclareLaunchArgument('world',    default_value=default_world,
+                                         description='Path to the Gazebo world SDF file')
+    slam_arg     = DeclareLaunchArgument('use_slam', default_value='false',
+                                         description='Launch slam_toolbox in async mapping mode')
+    nav2_arg     = DeclareLaunchArgument('use_nav2', default_value='false',
+                                         description='Launch the Nav2 navigation stack')
+    rviz_arg     = DeclareLaunchArgument('use_rviz', default_value='true',
+                                         description='Launch RViz2 with the pre-configured layout')
+    wheel_base_arg = DeclareLaunchArgument('wheel_base', default_value='0.14',
+                                           description='Wheel separation (m) — must match motor controller')
+
+    # ── Robot description (xacro → URDF string) ────────────────────────────
+    robot_description = ParameterValue(
+        Command([
+            'xacro ', urdf_file,
+            ' wheel_separation:=', LaunchConfiguration('wheel_base'),
+            ' controllers_config:=', controllers_yaml,
+        ]),
+        value_type=str,
+    )
+
+    # ── robot_state_publisher ──────────────────────────────────────────────
+    rsp = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[{
+            'robot_description': robot_description,
+            'use_sim_time': True,
+        }],
+        output='screen',
+    )
+
+    # ── Gazebo ────────────────────────────────────────────────────────────
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('ros_gz_sim'),
+                'launch', 'gz_sim.launch.py',
+            )
+        ),
+        launch_arguments={
+            'gz_args': [LaunchConfiguration('world'), ' -r'],
+        }.items(),
+    )
+
+    # ── Spawn robot model into Gazebo ─────────────────────────────────────
+    # Reads robot_description from the RSP node's topic and spawns the entity.
+    spawn_robot = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-name',  'raspbot_v2',
+            '-topic', 'robot_description',
+            '-x', '0.0', '-y', '0.0', '-z', '0.05',
+            '-R', '0.0', '-P', '0.0', '-Y', '0.0',
+        ],
+        output='screen',
+    )
+
+    # ── ros_gz_bridge — topic bridging between gz and ROS 2 ──────────────
+    # Format: /ros_topic@ros_msg_type[gz_msg_type  (gz→ROS)
+    #         /ros_topic@ros_msg_type]gz_msg_type  (ROS→gz)
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        name='ros_gz_bridge',
+        arguments=[
+            # Drive commands: ROS → gz
+            '/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
+            # Odometry: gz → ROS
+            '/odom@nav_msgs/msg/Odometry[gz.msgs.Odometry',
+            # TF from diff-drive plugin: gz → ROS
+            '/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
+            # Lidar scan: gz → ROS
+            '/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+            # Sonar (simulated as narrow lidar): gz → ROS
+            '/ultrasonic/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+            # Camera image: gz → ROS
+            '/camera/image@sensor_msgs/msg/Image[gz.msgs.Image',
+            # Camera info: gz → ROS
+            '/camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo',
+            # Wheel joint states from JointStatePublisher plugin: gz → ROS
+            '/gz/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model',
+            # Clock: gz → ROS (required for use_sim_time)
+            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
+        ],
+        remappings=[
+            # Remap camera topic to match what image_transport / webrtc_streamer expects
+            ('/camera/image', '/camera/image_raw'),
+        ],
+        parameters=[{'use_sim_time': True}],
+        output='screen',
+    )
+
+    # ── ros2_control — spawn controllers ───────────────────────────────────
+    # Controllers are started by the gz_ros2_control plugin inside Gazebo.
+    # We use TimerAction to wait for the controller_manager to be ready
+    # before spawning controllers (gz takes a few seconds to initialise).
+    spawn_jsb = TimerAction(
+        period=5.0,
+        actions=[
+            Node(
+                package='controller_manager',
+                executable='spawner',
+                arguments=[
+                    'joint_state_broadcaster',
+                    '--controller-manager', '/controller_manager',
+                ],
+                output='screen',
+            ),
+        ],
+    )
+
+    spawn_pan_tilt = TimerAction(
+        period=6.0,    # after joint_state_broadcaster is active
+        actions=[
+            Node(
+                package='controller_manager',
+                executable='spawner',
+                arguments=[
+                    'pan_tilt_controller',
+                    '--controller-manager', '/controller_manager',
+                ],
+                output='screen',
+            ),
+        ],
+    )
+
+    # ── Joint state relay — merge gz wheel states with ros2_control states ─
+    # robot_state_publisher needs all joint states on one /joint_states topic.
+    # joint_state_broadcaster publishes pan/tilt; /gz/joint_states has wheels.
+    # joint_state_publisher merges both sources automatically if configured.
+    joint_state_relay = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_merger',
+        parameters=[{
+            'use_sim_time': True,
+            'source_list': ['/joint_states', '/gz/joint_states'],
+            'rate': 50,
+        }],
+        output='screen',
+    )
+
+    # ── slam_toolbox — online async SLAM ──────────────────────────────────
+    slam = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('slam_toolbox'),
+                'launch', 'online_async_launch.py',
+            ])
+        ),
+        launch_arguments={
+            'slam_params_file': slam_yaml,
+            'use_sim_time': 'true',
+        }.items(),
+        condition=IfCondition(LaunchConfiguration('use_slam')),
+    )
+
+    # ── Nav2 navigation stack ─────────────────────────────────────────────
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('nav2_bringup'),
+                'launch', 'navigation_launch.py',
+            ])
+        ),
+        launch_arguments={
+            'params_file': nav2_yaml,
+            'use_sim_time': 'true',
+        }.items(),
+        condition=IfCondition(LaunchConfiguration('use_nav2')),
+    )
+
+    # ── RViz2 ─────────────────────────────────────────────────────────────
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_config],
+        parameters=[{'use_sim_time': True}],
+        condition=IfCondition(LaunchConfiguration('use_rviz')),
+        output='screen',
+    )
+
+    return LaunchDescription([
+        # Arguments
+        world_arg,
+        slam_arg,
+        nav2_arg,
+        rviz_arg,
+        wheel_base_arg,
+
+        # Core simulation
+        gz_sim,
+        rsp,
+        spawn_robot,
+        bridge,
+
+        # ros2_control controllers (delayed — wait for gz to start)
+        spawn_jsb,
+        spawn_pan_tilt,
+        joint_state_relay,
+
+        # Optional stacks
+        slam,
+        nav2,
+        rviz,
+    ])

robot/src/raspbot_v2/setup.py

@@ -11,9 +11,10 @@ setup(
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name + '/launch', ['launch/robot.launch.py']),
+        ('share/' + package_name + '/launch', glob.glob('launch/*.py')),
         ('share/' + package_name + '/urdf',   glob.glob('urdf/*.xacro')),
-        ('share/' + package_name + '/config', glob.glob('config/*.yaml')),
+        ('share/' + package_name + '/config', glob.glob('config/*')),
+        ('share/' + package_name + '/worlds', glob.glob('worlds/*.sdf')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro

@@ -17,7 +17,7 @@
        ═══════════════════════════════════════════════════════════════════════ -->
 
   <gazebo>
-    <plugin filename="gz-sim-ros2-control-system"
+    <plugin filename="gz_ros2_control-system"
             name="gz_ros2_control::GazeboSimROS2ControlPlugin">
       <robot_param>robot_description</robot_param>
       <robot_param_node>robot_state_publisher</robot_param_node>

robot/src/raspbot_v2/worlds/empty_world.sdf

@@ -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>