From c1319a6357e06ea472efeaedee73985e3d6358a3 Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Thu, 7 May 2026 22:07:02 +0000 Subject: [PATCH] Warehouse sim running - but not yet working. --- .devcontainer/Dockerfile | 14 + robot/README.md | 89 ++++- robot/src/raspbot_v2/config/controllers.yaml | 17 + robot/src/raspbot_v2/config/nav2_params.yaml | 314 ++++++++++++++++++ robot/src/raspbot_v2/config/rviz2_config.rviz | 274 +++++++++++++++ robot/src/raspbot_v2/config/slam_toolbox.yaml | 64 ++++ .../src/raspbot_v2/launch/house_sim_launch.py | 42 +++ robot/src/raspbot_v2/launch/robot.launch.py | 5 +- robot/src/raspbot_v2/launch/sim_launch.py | 259 +++++++++++++++ robot/src/raspbot_v2/setup.py | 5 +- .../urdf/raspbot_v2.ros2_control.xacro | 2 +- robot/src/raspbot_v2/worlds/empty_world.sdf | 86 +++++ 12 files changed, 1161 insertions(+), 10 deletions(-) create mode 100644 robot/src/raspbot_v2/config/nav2_params.yaml create mode 100644 robot/src/raspbot_v2/config/rviz2_config.rviz create mode 100644 robot/src/raspbot_v2/config/slam_toolbox.yaml create mode 100644 robot/src/raspbot_v2/launch/house_sim_launch.py create mode 100644 robot/src/raspbot_v2/launch/sim_launch.py create mode 100644 robot/src/raspbot_v2/worlds/empty_world.sdf diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 2661feb..9dff356 100644 --- a/.devcontainer/Dockerfile +++ b/.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 diff --git a/robot/README.md b/robot/README.md index 39eab22..f27d313 100644 --- a/robot/README.md +++ b/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: @@ -208,18 +268,35 @@ docker run --rm \ ``` robot/ -├── Dockerfile # Two-stage build: colcon compile → clean runtime image +├── Dockerfile # Two-stage build: colcon compile → clean runtime image ├── src/ │ └── raspbot_v2/ │ ├── 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 -└── raspbot_v2_interface/ # Vendored Yahboom hardware library +│ ├── 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) + └── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B) ``` diff --git a/robot/src/raspbot_v2/config/controllers.yaml b/robot/src/raspbot_v2/config/controllers.yaml index e78d744..f113830 100644 --- a/robot/src/raspbot_v2/config/controllers.yaml +++ b/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 diff --git a/robot/src/raspbot_v2/config/nav2_params.yaml b/robot/src/raspbot_v2/config/nav2_params.yaml new file mode 100644 index 0000000..9300c56 --- /dev/null +++ b/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 diff --git a/robot/src/raspbot_v2/config/rviz2_config.rviz b/robot/src/raspbot_v2/config/rviz2_config.rviz new file mode 100644 index 0000000..5fb857e --- /dev/null +++ b/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: + + - 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: + 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 diff --git a/robot/src/raspbot_v2/config/slam_toolbox.yaml b/robot/src/raspbot_v2/config/slam_toolbox.yaml new file mode 100644 index 0000000..e0806fd --- /dev/null +++ b/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 diff --git a/robot/src/raspbot_v2/launch/house_sim_launch.py b/robot/src/raspbot_v2/launch/house_sim_launch.py new file mode 100644 index 0000000..41d68d8 --- /dev/null +++ b/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]) diff --git a/robot/src/raspbot_v2/launch/robot.launch.py b/robot/src/raspbot_v2/launch/robot.launch.py index 2ee0801..cc27412 100644 --- a/robot/src/raspbot_v2/launch/robot.launch.py +++ b/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', ), diff --git a/robot/src/raspbot_v2/launch/sim_launch.py b/robot/src/raspbot_v2/launch/sim_launch.py new file mode 100644 index 0000000..fc311f3 --- /dev/null +++ b/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, + ]) diff --git a/robot/src/raspbot_v2/setup.py b/robot/src/raspbot_v2/setup.py index 0db3946..75ff63c 100644 --- a/robot/src/raspbot_v2/setup.py +++ b/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, diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro index 3db0a29..633c738 100644 --- a/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro +++ b/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro @@ -17,7 +17,7 @@ ═══════════════════════════════════════════════════════════════════════ --> - robot_description robot_state_publisher diff --git a/robot/src/raspbot_v2/worlds/empty_world.sdf b/robot/src/raspbot_v2/worlds/empty_world.sdf new file mode 100644 index 0000000..dc9f290 --- /dev/null +++ b/robot/src/raspbot_v2/worlds/empty_world.sdf @@ -0,0 +1,86 @@ + + + + + + + + 0.001 + 1.0 + + + + + + + + + + ogre2 + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + 1000 + 0.9 + 0.01 + 0.001 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + +