13 KiB
13 KiB
Robot Controller
ROS 2 nodes for the Yahboom Raspbot V2 — differential-drive motor control, pan/tilt camera orientation, and ultrasonic range sensing.
All three nodes share the same I²C bus. The Linux kernel serialises individual transactions, so they run as separate processes without additional locking.
Motor controller
┌───────────────────────────────────┐
│ MotorControllerNode │
│ │
/cmd_vel ──────────>│ Twist → differential kinematics │
(geometry_msgs/Twist)│ left = linear − (angular × wb/2)│
│ right = linear + (angular × wb/2)│
│ │
/wheel_speeds ──────>│ Direct per-wheel override │
(Float32MultiArray │ [FL, FR, RL, RR] │
4 × float32) │ │
│ ▼ │
│ raspbot_v2_interface │
│ I²C bus 1, addr 0x2B │
│ ▼ │
│ /dev/i2c-1 ─────────> Motors │
│ │
/current_wheel_speeds│<─ telemetry @ 10 Hz │
(Float32MultiArray) │ [FL, FR, RL, RR] │
└───────────────────────────────────┘
Topics
| Topic | Direction | Type | Description |
|---|---|---|---|
/cmd_vel |
Subscribed | geometry_msgs/Twist |
Velocity command — linear.x (m/s) and angular.z (rad/s) |
/wheel_speeds |
Subscribed | std_msgs/Float32MultiArray |
Direct per-wheel speed override [FL, FR, RL, RR] in library units (0–255) |
/current_wheel_speeds |
Published | std_msgs/Float32MultiArray |
Current wheel speeds read from hardware, published at 10 Hz |
Parameters
| Parameter | Default | Description |
|---|---|---|
wheel_base |
0.3 |
Distance between left and right wheels in metres |
max_speed |
1.0 |
Maximum motor speed in library units |
Sending velocity commands
# Drive forward at 0.2 m/s
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.02}, angular: {z: 0.0}}"
# Turn on the spot
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0}, angular: {z: 0.5}}"
# Stop
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0}, angular: {z: 0.0}}"
Camera orientation controller
┌──────────────────────────────────────┐
│ CameraOrientationNode │
│ │
/joint_command ────────>│ JointState (names: pan, tilt) │
(sensor_msgs/ │ position in radians │
JointState) │ │
│ pan → servo 1 (0°–180°) │
│ tilt → servo 2 (0°–110°) │
│ │
│ ▼ │
│ raspbot_v2_interface │
│ I²C bus 1, addr 0x2B │
│ ▼ │
│ /dev/i2c-1 ──────> Pan/tilt servos │
│ │
/joint_states <────────│ current angles @ 10 Hz │
(sensor_msgs/ │ position in radians │
JointState) │ │
└──────────────────────────────────────┘
Topics
| Topic | Direction | Type | Description |
|---|---|---|---|
/joint_command |
Subscribed | sensor_msgs/JointState |
Commanded pan/tilt angles. Joint names "pan" and "tilt", positions in radians. Unknown joint names are ignored. |
/joint_states |
Published | sensor_msgs/JointState |
Current angles reflected from the last command, published at 10 Hz |
Parameters
| Parameter | Default | Description |
|---|---|---|
pan_servo_id |
1 |
Raspbot servo channel for pan |
tilt_servo_id |
2 |
Raspbot servo channel for tilt |
pan_min_deg |
0.0 |
Pan lower limit (degrees) |
pan_max_deg |
180.0 |
Pan upper limit (degrees) |
tilt_min_deg |
0.0 |
Tilt lower limit (degrees) |
tilt_max_deg |
110.0 |
Tilt upper limit (degrees) — hardware cap |
pan_center_deg |
90.0 |
Startup and shutdown park position for pan |
tilt_center_deg |
60.0 |
Startup and shutdown park position for tilt |
state_rate_hz |
10.0 |
~/joint_states publish rate |
Hardware interface
The node drives the pan and tilt servos over I²C bus 1 (device address 0x2B). The same /dev/i2c-1 device used by the motor controller is sufficient — no additional device node is required.
Commanding the camera
Pan to centre (90°) and tilt to 30°:
ros2 topic pub --once /joint_command sensor_msgs/msg/JointState \
"{name: ['pan', 'tilt'], position: [1.5708, 0.5236]}"
A single axis can be commanded by omitting the other joint name:
# Pan only
ros2 topic pub --once /joint_command sensor_msgs/msg/JointState \
"{name: ['pan'], position: [0.0]}"
Ultrasonic range sensor
┌──────────────────────────────────────┐
│ UltrasonicNode │
│ │
│ Sensor off when no subscribers │
│ Sensor on when subscribers > 0 │
│ 1 s warm-up after power-on │
│ │
│ ▼ │
│ raspbot_v2_interface │
│ I²C bus 1, addr 0x2B │
│ ▼ │
│ /dev/i2c-1 ──────> HC-SR04 sensor │
│ │
/ultrasonic/range <────│ Range @ configurable rate │
(sensor_msgs/Range) │ radiation_type = ULTRASOUND │
│ range in metres (REP-117) │
└──────────────────────────────────────┘
Topics
| Topic | Direction | Type | Description |
|---|---|---|---|
/ultrasonic/range |
Published | sensor_msgs/Range |
Distance in metres. +inf when beyond max range, -inf when closer than min range (REP-117). Only published while subscribers are connected. |
Parameters
| Parameter | Default | Description |
|---|---|---|
publish_rate_hz |
10.0 |
Sensor poll and publish rate |
frame_id |
'ultrasonic' |
header.frame_id on published messages |
min_range_m |
0.02 |
Minimum valid range in metres |
max_range_m |
4.0 |
Maximum valid range in metres |
field_of_view |
0.2618 |
Sensor cone width in radians (~15°) |
warmup_s |
1.0 |
Seconds to wait after powering the sensor on before publishing |
Verifying range readings
ros2 topic echo /ultrasonic/range
The sensor activates automatically when a subscriber connects and deactivates when it disconnects.
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):
cd /home/ws
colcon build --packages-select raspbot_v2 --symlink-install
source install/setup.bash
Launching
# 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:
docker run --rm \
--network=host \
--device /dev/i2c-1 \
--env ROS_DOMAIN_ID=0 \
raspbot_v2:latest \
ros2 launch raspbot_v2 robot.launch.py \
wheel_base:=0.25 max_speed:=0.8 tilt_center_deg:=45.0
| Argument | Default | Description |
|---|---|---|
wheel_base |
0.3 |
Distance between left and right wheels (m) |
max_speed |
1.0 |
Maximum motor speed in library units |
pan_center_deg |
90.0 |
Pan angle at startup and shutdown (degrees) |
tilt_center_deg |
60.0 |
Tilt angle at startup and shutdown (degrees) |
ultrasonic_rate_hz |
10.0 |
Ultrasonic sensor publish rate (Hz) |
Project layout
robot/
├── Dockerfile # Two-stage build: colcon compile → clean runtime image
├── src/
│ └── raspbot_v2/
│ ├── package.xml
│ ├── setup.py
│ ├── launch/
│ │ ├── 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
│ └── led_node.py
└── raspbot_v2_interface/ # Vendored Yahboom hardware library
└── Raspbot_Lib/
└── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B)