Warehouse sim running - but not yet working.
This commit is contained in:
@@ -51,6 +51,20 @@ RUN apt-get update && apt-get install -y \
|
|||||||
ros-${ROS_DISTRO}-xacro \
|
ros-${ROS_DISTRO}-xacro \
|
||||||
ros-${ROS_DISTRO}-robot-state-publisher
|
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
|
# Node.js for frontend development
|
||||||
RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \
|
RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \
|
||||||
&& apt-get install -y nodejs
|
&& apt-get install -y nodejs
|
||||||
|
|||||||
+83
-6
@@ -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:
|
Launch arguments can be appended when running the container manually:
|
||||||
|
|
||||||
@@ -208,18 +268,35 @@ docker run --rm \
|
|||||||
|
|
||||||
```
|
```
|
||||||
robot/
|
robot/
|
||||||
├── Dockerfile # Two-stage build: colcon compile → clean runtime image
|
├── Dockerfile # Two-stage build: colcon compile → clean runtime image
|
||||||
├── src/
|
├── src/
|
||||||
│ └── raspbot_v2/
|
│ └── raspbot_v2/
|
||||||
│ ├── package.xml
|
│ ├── package.xml
|
||||||
│ ├── setup.py
|
│ ├── setup.py
|
||||||
│ ├── launch/
|
│ ├── 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/
|
│ └── raspbot_v2/
|
||||||
│ ├── motor_controller_node.py
|
│ ├── motor_controller_node.py
|
||||||
│ ├── camera_orientation_node.py
|
│ ├── camera_orientation_node.py
|
||||||
│ └── ultrasonic_node.py
|
│ ├── ultrasonic_node.py
|
||||||
└── raspbot_v2_interface/ # Vendored Yahboom hardware library
|
│ └── led_node.py
|
||||||
|
└── raspbot_v2_interface/ # Vendored Yahboom hardware library
|
||||||
└── Raspbot_Lib/
|
└── Raspbot_Lib/
|
||||||
└── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B)
|
└── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B)
|
||||||
```
|
```
|
||||||
|
|||||||
@@ -36,3 +36,20 @@ pan_tilt_controller:
|
|||||||
goal: 0.05 # rad — acceptable position error at goal
|
goal: 0.05 # rad — acceptable position error at goal
|
||||||
tilt:
|
tilt:
|
||||||
goal: 0.05 # rad
|
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
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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])
|
||||||
@@ -48,7 +48,10 @@ def generate_launch_description():
|
|||||||
package='robot_state_publisher',
|
package='robot_state_publisher',
|
||||||
executable='robot_state_publisher',
|
executable='robot_state_publisher',
|
||||||
name='robot_state_publisher',
|
name='robot_state_publisher',
|
||||||
parameters=[{'robot_description': robot_description}],
|
parameters=[{
|
||||||
|
'robot_description': robot_description,
|
||||||
|
'use_sim_time': False,
|
||||||
|
}],
|
||||||
output='screen',
|
output='screen',
|
||||||
),
|
),
|
||||||
|
|
||||||
|
|||||||
@@ -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,
|
||||||
|
])
|
||||||
@@ -11,9 +11,10 @@ setup(
|
|||||||
data_files=[
|
data_files=[
|
||||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||||
('share/' + package_name, ['package.xml']),
|
('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 + '/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'],
|
install_requires=['setuptools'],
|
||||||
zip_safe=True,
|
zip_safe=True,
|
||||||
|
|||||||
@@ -17,7 +17,7 @@
|
|||||||
═══════════════════════════════════════════════════════════════════════ -->
|
═══════════════════════════════════════════════════════════════════════ -->
|
||||||
|
|
||||||
<gazebo>
|
<gazebo>
|
||||||
<plugin filename="gz-sim-ros2-control-system"
|
<plugin filename="gz_ros2_control-system"
|
||||||
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||||
<robot_param>robot_description</robot_param>
|
<robot_param>robot_description</robot_param>
|
||||||
<robot_param_node>robot_state_publisher</robot_param_node>
|
<robot_param_node>robot_state_publisher</robot_param_node>
|
||||||
|
|||||||
@@ -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>
|
||||||
Reference in New Issue
Block a user