303 lines
13 KiB
Markdown
303 lines
13 KiB
Markdown
# 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
|
||
|
||
```bash
|
||
# 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°:
|
||
|
||
```bash
|
||
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:
|
||
|
||
```bash
|
||
# 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
|
||
|
||
```bash
|
||
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):
|
||
|
||
```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:
|
||
|
||
```bash
|
||
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)
|
||
```
|