m5p3nc3r/ros-raspbot-v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add image and streaming from USB camera

Browse Source 
Plus a little freshen up of the readme's
This commit is contained in:
Matt Spencer
2026-05-07 16:38:36 +00:00
parent 59a019ed7b
commit ef78f19e72
18 changed files with 1080 additions and 327 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.devcontainer/Dockerfile
+16 -1
View File
@@ -8,7 +8,7 @@ RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi
# Create the user
RUN groupadd --gid $USER_GID $USERNAME \
&& useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
&& useradd --uid $USER_UID --gid $USER_GID -m $USERNAME -s /bin/bash \
#
# [Optional] Add sudo support. Omit if you don't need to install software after connecting.
&& apt-get update \
@@ -57,6 +57,21 @@ RUN python3 -m venv /opt/webui-venv \
"uvicorn[standard]" \
numpy
# Docker CLI for accessing the host daemon via the mounted socket
# docker-compose-plugin is only in Docker's official repo, not Ubuntu's.
# .docker_gid is written by initializeCommand on the host before the build,
# ensuring the in-container docker group GID matches the host socket GID.
COPY .docker_gid /tmp/.docker_gid
RUN apt-get update && apt-get install -y ca-certificates curl gnupg \
&& install -m 0755 -d /etc/apt/keyrings \
&& curl -fsSL https://download.docker.com/linux/ubuntu/gpg | gpg --dearmor -o /etc/apt/keyrings/docker.gpg \
&& chmod a+r /etc/apt/keyrings/docker.gpg \
&& echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu $(. /etc/os-release && echo "$VERSION_CODENAME") stable" \
> /etc/apt/sources.list.d/docker.list \
&& apt-get update && apt-get install -y docker-ce-cli docker-compose-plugin \
&& groupadd -g "$(cat /tmp/.docker_gid)" docker \
&& usermod -aG docker $USERNAME
ENV SHELL /bin/bash
# ********************************************************
.devcontainer/devcontainer.json
+3 -1
View File
@@ -2,6 +2,7 @@
"name": "ROS 2 Development Container",
"privileged": true,
"remoteUser": "matt",
"initializeCommand": "stat -c %g /var/run/docker.sock > .devcontainer/.docker_gid",
"build": {
"dockerfile": "Dockerfile",
"args": {
@@ -38,7 +39,8 @@
"mounts": [
"source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached",
"source=/dev/dri,target=/dev/dri,type=bind,consistency=cached",
"source=${localEnv:HOME}/.gitconfig,target=/home/matt/.gitconfig,type=bind,consistency=cached,readonly"
"source=${localEnv:HOME}/.gitconfig,target=/home/matt/.gitconfig,type=bind,consistency=cached,readonly",
"source=/var/run/docker.sock,target=/var/run/docker.sock,type=bind"
],
"postCreateCommand": "sudo rosdep update && sudo rosdep install --from-paths lidar/src robot/src --ignore-src -y && sudo chown -R $(whoami) /home/ws/ && npm --prefix /home/ws/webui/frontend install"
}
.gitignore
+3
View File
@@ -14,3 +14,6 @@ __pycache__/
*.py[cod]
*.egg-info/
dist/
# Generated by devcontainer initializeCommand — machine-specific
.devcontainer/.docker_gid
README.md
+39 -325
View File
@@ -1,189 +1,21 @@
# raspbot_v2
ROS 2 package for the Yahboom Raspbot V2 platform — differential-drive motor control and pan/tilt camera orientation.
ROS 2 robot platform based on the Yahboom Raspbot V2. Multiple services run as Docker containers, coordinated by Docker Compose.
---
## Architecture
## Sub-projects
Both nodes share the same I²C bus. The Linux kernel serialises individual transactions, so they can 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 |
---
### 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.
---
### 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 will activate automatically when this command runs and deactivate when it is stopped.
---
### RPLIDAR A1
Runs in a separate container built from [lidar/Dockerfile](lidar/Dockerfile).
```
┌──────────────────────────────────────┐
│ sllidar_ros2 (rplidar_node) │
│ │
│ serial 115200 baud │
│ angle_compensate = true │
│ scan_mode = Sensitivity │
│ ▼ │
│ /dev/ttyUSB0 ──────> RPLIDAR A1 │
│ │
/scan <───────│ LaserScan @ ~10 Hz │
(sensor_msgs/ │ 360° scan, range 0.15–12 m │
LaserScan) │ │
└──────────────────────────────────────┘
```
#### Topics
| Topic | Direction | Type | Description |
|---|---|---|---|
| `/scan` | Published | `sensor_msgs/LaserScan` | 360° laser scan in the `laser` frame |
#### Configuration
The LIDAR container is configured via environment variables in `.env` or `docker-compose.yml`. See the [Launching](#launching) section for details.
#### Verifying LIDAR data
```bash
ros2 topic echo /scan
```
| Directory | Description |
|---|---|
| [robot/](robot/README.md) | Differential-drive motor control, pan/tilt camera, and ultrasonic range sensor |
| [lidar/](lidar/README.md) | RPLIDAR A1 laser scanner |
| [oled/](oled/README.md) | OLED display dashboard |
| [wifi/](wifi/README.md) | Wi-Fi hotspot fallback manager |
| [camera_publisher/](camera_publisher/README.md) | V4L2 camera → ROS 2 topic publisher |
| [webrtc_streamer/](webrtc_streamer/README.md) | WebRTC browser stream server |
| [webui/](webui/README.md) | Browser-based robot controller |
| [ansible/](ansible/README.md) | Provisioning playbook for the Raspberry Pi |
---
@@ -193,7 +25,7 @@ ros2 topic echo /scan
Use the [Raspberry Pi Imager](https://www.raspberrypi.com/software/) to write Raspberry Pi OS (64-bit, Lite recommended) to a microSD card.
Before writing, open the imager's **Advanced options** (⚙) and configure:
Before writing, open **Advanced options** (⚙) and configure:
| Setting | Value |
|---|---|
@@ -202,7 +34,7 @@ Before writing, open the imager's **Advanced options** (⚙) and configure:
| Username / Password | Your preferred credentials |
| Wi-Fi | Your network SSID and password (if not using Ethernet) |
Write the image, insert the card, and power on the Pi. Once it has booted and is reachable on the network (test with `ping raspbot-v2.local`), proceed to the next step.
Write the image, insert the card, and power on the Pi. Once it is reachable on the network (test with `ping raspbot-v2.local`), proceed to the next step.
### 2. Provision with Ansible
@@ -214,16 +46,14 @@ The [ansible/](ansible/) directory contains a playbook that handles the remainin
### Prerequisites
- Docker (with BuildKit enabled)
- For cross-compilation from an amd64 host, QEMU user-space emulation must be registered with the kernel. If you haven't done this before, run once:
- Docker with BuildKit enabled
- For cross-compilation from an amd64 host, register QEMU user-space emulation once:
```bash
docker run --rm --privileged tonistiigi/binfmt --install arm64
```
### Build with Docker Compose (recommended)
Both images are defined in `docker-compose.yml`. Build them together:
### Build all images
```bash
docker compose build
@@ -236,51 +66,36 @@ docker compose build robot
docker compose build lidar
```
The builds are split into two stages each:
1. **builder** — compiles the ROS package(s) with `colcon`; the lidar builder also clones `sllidar_ros2` from GitHub
2. **runtime** — copies only the install overlay into a clean `ros:kilted-ros-core` base; no build tools in the final image
### Build images individually
```bash
# Robot controller
docker build --platform linux/arm64 -f robot/Dockerfile -t raspbot_v2:latest .
# LIDAR
docker build --platform linux/arm64 -f lidar/Dockerfile -t raspbot_v2_lidar:latest .
```
---
## Deploying
Pipe both images directly to the target over SSH — no intermediate file or registry needed:
Pipe images directly to the target over SSH — no intermediate file or registry needed:
```bash
docker save raspbot_v2:latest raspbot_v2_lidar:latest \
| ssh matt@raspbot-v2.local docker load
| ssh <user>@raspbot-v2.local docker load
```
Then copy the compose file to the target:
Then copy the compose file and any `.env` to the target:
```bash
scp docker-compose.yml matt@raspbot-v2.local:~/
scp docker-compose.yml <user>@raspbot-v2.local:~/
```
Replace `matt` with the username configured in [ansible/inventory.ini](ansible/inventory.ini).
Replace `<user>` with the username configured in [ansible/inventory.ini](ansible/inventory.ini).
---
## Launching
### Start everything with Docker Compose (recommended)
### Start everything
```bash
docker compose up
```
This starts both the robot controller and LIDAR containers. Logs from both are interleaved in the terminal, each line prefixed with the service name. To run in the background:
To run in the background:
```bash
docker compose up -d
@@ -290,12 +105,13 @@ docker compose down # stop and remove containers
### Environment variables
Create a `.env` file in the same directory as `docker-compose.yml` to override defaults:
Create a `.env` file alongside `docker-compose.yml` to override defaults:
```bash
ROS_DOMAIN_ID=0
LIDAR_PORT=/dev/ttyUSB0
LIDAR_FRAME_ID=laser
WIFI_SSID=MyNetwork
```
| Variable | Default | Description |
@@ -303,101 +119,9 @@ LIDAR_FRAME_ID=laser
| `ROS_DOMAIN_ID` | `0` | ROS 2 domain — must match on all nodes |
| `LIDAR_PORT` | `/dev/ttyUSB0` | Host device node for the RPLIDAR |
| `LIDAR_FRAME_ID` | `laser` | `frame_id` in published `LaserScan` messages |
### Run containers individually
```bash
# Robot controller
docker run --rm \
--network=host \
--device /dev/i2c-1 \
--env ROS_DOMAIN_ID=0 \
raspbot_v2:latest
# LIDAR
docker run --rm \
--network=host \
--device /dev/ttyUSB0 \
--env ROS_DOMAIN_ID=0 \
raspbot_v2_lidar:latest
```
### Overriding robot launch parameters
Launch arguments can be appended after the image name:
```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
```
Available launch arguments:
| 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) |
### LIDAR device permissions
The RPLIDAR connects as a USB serial device. If the user running Docker is not in the `dialout` group, add them and log back in:
```bash
sudo usermod -aG dialout $USER
```
### Sending velocity commands from the host
With the container running, publish from another terminal (requires ROS 2 on the host or a second container on the same network):
```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}}"
```
### Commanding the camera from the host
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]}"
```
### Verifying telemetry
```bash
# Wheel speeds
ros2 topic echo /current_wheel_speeds
# Camera orientation
ros2 topic echo /joint_states
```
| `WIFI_SSID` | _(empty)_ | Target SSID; if unset the Wi-Fi container creates a hotspot immediately |
| `HOTSPOT_SSID` | `raspbot-hotspot` | Fallback hotspot SSID |
| `HOTSPOT_PASSWORD` | `raspbot1234` | Fallback hotspot passphrase |
---
@@ -405,24 +129,14 @@ ros2 topic echo /joint_states
```
.
├── docker-compose.yml # Launches robot and lidar containers together
├── docker-entrypoint.sh # Sources ROS overlays before exec (shared by both images)
├── robot/
│ ├── Dockerfile # Robot controller image (two-stage)
│ ├── src/
│ │ └── raspbot_v2/
│ │ ├── package.xml # ROS package manifest
│ │ ├── setup.py # ament_python build definition
│ │ ├── launch/
│ │ │ └── robot.launch.py # Starts all robot nodes together
│ │ └── raspbot_v2/
│ │ ├── __init__.py
│ │ ├── motor_controller_node.py # Differential-drive motor control
│ │ ├── camera_orientation_node.py # Pan/tilt servo control
│ │ └── ultrasonic_node.py # HC-SR04 range sensor
│ └── raspbot_v2_interface/ # Vendored Yahboom hardware library
│ └── Raspbot_Lib/
│ └── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B)
└── lidar/
└── Dockerfile # RPLIDAR A1 image (two-stage, clones sllidar_ros2)
├── docker-compose.yml
├── docker-entrypoint.sh
├── robot/ # Motor controller, pan/tilt, ultrasonic
├── lidar/ # RPLIDAR A1
├── oled/ # OLED display dashboard
├── wifi/ # Wi-Fi hotspot fallback
├── camera_publisher/ # V4L2 camera → ROS 2 topic
├── webrtc_streamer/ # WebRTC browser stream
├── webui/ # Browser-based controller UI
└── ansible/ # Raspberry Pi provisioning
```
camera_publisher/Dockerfile
+22
View File
@@ -0,0 +1,22 @@
FROM ros:kilted-ros-core
RUN apt-get update && apt-get install -y --no-install-recommends \
gstreamer1.0-plugins-base \
gstreamer1.0-plugins-good \
python3-gi \
gir1.2-gstreamer-1.0 \
&& rm -rf /var/lib/apt/lists/*
WORKDIR /app
COPY camera_publisher.py .
COPY entrypoint.sh .
RUN chmod +x entrypoint.sh
ENV VIDEO_DEVICE=/dev/video0
ENV WIDTH=640
ENV HEIGHT=480
ENV FPS=30
ENV CAMERA_FORMAT=raw
ENV CAMERA_NAMESPACE=/camera
ENTRYPOINT ["./entrypoint.sh"]
camera_publisher/README.md
+43
View File
@@ -0,0 +1,43 @@
# Camera Publisher
ROS 2 node that captures frames from a V4L2 USB camera using GStreamer and publishes them as `sensor_msgs/Image` messages.
---
## Architecture
```
USB Camera → v4l2src (GStreamer) → sensor_msgs/Image → /camera/image_raw
```
No OpenCV or cv_bridge required. Frames are captured via `v4l2src` and passed directly into the ROS 2 message.
---
## Published topics
| Topic | Type | Description |
|---|---|---|
| `/<CAMERA_NAMESPACE>/image_raw` | `sensor_msgs/Image` | Raw camera frames |
---
## Environment variables
| Variable | Default | Description |
|---|---|---|
| `VIDEO_DEVICE` | `/dev/video0` | V4L2 device path |
| `WIDTH` | `640` | Capture width in pixels |
| `HEIGHT` | `480` | Capture height in pixels |
| `FPS` | `30` | Capture frame rate |
| `CAMERA_FORMAT` | `raw` | Capture format (`raw` or `mjpeg`) |
| `CAMERA_NAMESPACE` | `/camera` | ROS 2 namespace (topic becomes `/<ns>/image_raw`) |
---
## Finding your camera device
```bash
v4l2-ctl --list-devices
v4l2-ctl -d /dev/video0 --list-formats-ext
```
camera_publisher/camera_publisher.py
+124
View File
@@ -0,0 +1,124 @@
#!/usr/bin/env python3
"""
Minimal ROS2 camera publisher using GStreamer v4l2src.
Publishes sensor_msgs/Image to /<CAMERA_NAMESPACE>/image_raw.
No OpenCV or cv_bridge required.
"""
import os
import threading
import gi
gi.require_version('Gst', '1.0')
from gi.repository import Gst, GLib
import rclpy
from rclpy.executors import SingleThreadedExecutor
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image
DEVICE = os.environ.get('VIDEO_DEVICE', '/dev/video0')
WIDTH = int(os.environ.get('WIDTH', '640'))
HEIGHT = int(os.environ.get('HEIGHT', '480'))
FPS = int(os.environ.get('FPS', '30'))
FORMAT = os.environ.get('CAMERA_FORMAT', 'raw').lower()
NAMESPACE = os.environ.get('CAMERA_NAMESPACE', '/camera')
TOPIC = f'{NAMESPACE}/image_raw'
class CameraPublisher(Node):
def __init__(self):
# DDS must initialise before GStreamer — same constraint as webrtc_streamer.
super().__init__('camera_publisher')
qos = QoSProfile(
# reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1,
)
self.pub = self.create_publisher(Image, TOPIC, qos)
self.get_logger().info(f'Publishing to {TOPIC}')
Gst.init(None)
glib_loop = GLib.MainLoop()
threading.Thread(target=glib_loop.run, daemon=True).start()
if FORMAT == 'mjpeg':
src = (
f'v4l2src device={DEVICE} ! '
f'image/jpeg,width={WIDTH},height={HEIGHT},framerate={FPS}/1 ! '
'jpegdec ! '
)
else:
src = (
f'v4l2src device={DEVICE} ! '
f'video/x-raw,width={WIDTH},height={HEIGHT},framerate={FPS}/1 ! '
)
desc = src + 'videoconvert ! video/x-raw,format=BGR ! appsink name=sink emit-signals=true sync=false max-buffers=1 drop=true'
self.get_logger().info(f'Pipeline: {desc}')
self._pipeline = Gst.parse_launch(desc)
sink = self._pipeline.get_by_name('sink')
sink.connect('new-sample', self._on_frame)
self._bus = self._pipeline.get_bus()
self._bus.add_signal_watch()
self._bus.connect('message::error', self._on_error)
self._bus.connect('message::warning', self._on_warning)
self._pipeline.set_state(Gst.State.PLAYING)
self._frame_count = 0
def _on_error(self, bus, msg):
err, debug = msg.parse_error()
self.get_logger().error(f'GStreamer error: {err.message}')
if debug:
self.get_logger().error(f'GStreamer debug: {debug}')
def _on_warning(self, bus, msg):
self.get_logger().warning(f'GStreamer warning: {msg.parse_warning()[0].message}')
def _on_frame(self, sink):
sample = sink.emit('pull-sample')
buf = sample.get_buffer()
caps = sample.get_caps().get_structure(0)
w = caps.get_value('width')
h = caps.get_value('height')
msg = Image()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'camera'
msg.height = h
msg.width = w
msg.encoding = 'bgr8'
msg.step = w * 3
msg.is_bigendian = False
msg.data = buf.extract_dup(0, buf.get_size())
self.pub.publish(msg)
self._frame_count += 1
if self._frame_count % 100 == 0:
self.get_logger().info(f'Frames published: {self._frame_count}')
return Gst.FlowReturn.OK
def main():
rclpy.init()
node = CameraPublisher()
executor = SingleThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
finally:
node._pipeline.set_state(Gst.State.NULL)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
camera_publisher/entrypoint.sh
+3
View File
@@ -0,0 +1,3 @@
#!/bin/sh
. /opt/ros/kilted/setup.sh
exec python3 /app/camera_publisher.py
docker-compose.yml
+40
View File
@@ -99,3 +99,43 @@ services:
environment:
- ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0}
restart: unless-stopped
camera_publisher:
build:
context: ./camera_publisher
platforms:
- linux/amd64
- linux/arm64
image: ${REGISTRY:-camera-publisher}:${TAG:-latest}
restart: unless-stopped
network_mode: host
ipc: host
devices:
- /dev/video0:/dev/video0
- /dev/video1:/dev/video1
- /dev/media0:/dev/media0
environment:
VIDEO_DEVICE: /dev/video0
WIDTH: 640
HEIGHT: 480
FPS: 30
CAMERA_FORMAT: mjpeg
CAMERA_NAMESPACE: /camera
FASTDDS_BUILTIN_TRANSPORTS: LARGE_DATA
webrtc_streamer:
build:
context: ./webrtc_streamer
platforms:
- linux/amd64
- linux/arm64
image: ${REGISTRY:-webrtc-streamer}:${TAG:-latest}
restart: unless-stopped
network_mode: host
ipc: host
environment:
IMAGE_TOPIC: /camera/image_raw
PORT: 8443
BITRATE: 2000000
FASTDDS_BUILTIN_TRANSPORTS: LARGE_DATA
lidar/README.md
+20
View File
@@ -5,6 +5,26 @@ official `sllidar_ros2` driver.
---
## Architecture
```
┌──────────────────────────────────────┐
│ sllidar_ros2 (rplidar_node) │
│ │
│ serial 115200 baud │
│ angle_compensate = true │
│ scan_mode = Sensitivity │
│ ▼ │
│ /dev/ttyUSB0 ──────> RPLIDAR A1 │
│ │
/scan <───────│ LaserScan @ ~10 Hz │
(sensor_msgs/ │ 360° scan, range 0.15–12 m │
LaserScan) │ │
└──────────────────────────────────────┘
```
---
## How the build works
The driver source lives in `lidar/sllidar_ros2/` as a **git subtree** of this
oled/README.md
+42
View File
@@ -0,0 +1,42 @@
# OLED Display
ROS 2 node that renders a live dashboard on the I²C OLED display.
---
## Dashboard contents
- Robot hostname and IP address
- Ultrasonic range reading
- Camera pan / tilt angles
- CPU and memory usage
---
## Subscribed topics
| Topic | Type | Description |
|---|---|---|
| `/ultrasonic/range` | `sensor_msgs/Range` | Distance reading from the HC-SR04 sensor |
| `/joint_states` | `sensor_msgs/JointState` | Current pan/tilt angles |
---
## Parameters
| Parameter | Default | Description |
|---|---|---|
| `driver` | `ssd1306` | OLED driver chip: `ssd1306` or `sh1106` |
| `i2c_port` | `1` | I²C bus number |
| `i2c_address` | `0x3C` | I²C device address |
| `width` | `128` | Display width in pixels |
| `height` | `64` | Display height in pixels |
| `rotate` | `0` | Display rotation: `0`, `1`, `2`, or `3` (0°/90°/180°/270°) |
| `refresh_hz` | `2.0` | Display update rate |
| `data_timeout_s` | `5.0` | Seconds before a stale reading is shown as `---` |
---
## Hardware
The display connects over **I²C bus 1** (`/dev/i2c-1`), the same bus used by the motor controller and servo driver. Default address is `0x3C`.
robot/README.md
+225
View File
@@ -0,0 +1,225 @@
# 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.
---
## 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 # Starts all three nodes together
│ └── raspbot_v2/
│ ├── motor_controller_node.py
│ ├── camera_orientation_node.py
│ └── ultrasonic_node.py
└── raspbot_v2_interface/ # Vendored Yahboom hardware library
└── Raspbot_Lib/
└── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B)
```
webrtc_streamer/Dockerfile
+65
View File
@@ -0,0 +1,65 @@
# ── Stage 1: full install — source for cherry-picked plugins ────────────
FROM ros:kilted-ros-core AS gst-builder
RUN apt-get update && apt-get install -y --no-install-recommends \
gstreamer1.0-plugins-base \
gstreamer1.0-plugins-good \
gstreamer1.0-plugins-bad \
gstreamer1.0-nice \
&& rm -rf /var/lib/apt/lists/*
# Copy only the 8 plugin .so files the pipeline actually uses.
# Each plugin bundle (plugins-base/good/bad/nice) contains dozens more
# that we never touch; leaving them out keeps the runtime image lean.
RUN set -e && ARCH=$(uname -m)-linux-gnu && \
mkdir /gst-select && \
for p in \
libgstapp.so \
libgstvideoconvertscale.so \
libgstvpx.so \
libgstrtp.so \
libgstrtpmanager.so \
libgstwebrtc.so \
libgstdtls.so \
libgstsrtp.so \
libgstnice.so \
; do cp /usr/lib/${ARCH}/gstreamer-1.0/${p} /gst-select/; done
# ── Stage 2: lean runtime ───────────────────────────────────────────────
FROM ros:kilted-ros-core
RUN apt-get update && apt-get install -y --no-install-recommends \
# GStreamer core runtime and base utility libs (libgstvideo etc.) — no plugins
libgstreamer1.0-0 \
libgstreamer-plugins-base1.0-0 \
# Python GI bindings + WebRTC/SDP typelibs; pulls libgstreamer-plugins-bad1.0-0
python3-gi \
gir1.2-gst-plugins-bad-1.0 \
# Shared libs the cherry-picked plugins link against
libvpx9 \
libnice10 \
libsrtp2-1 \
liborc-0.4-0t64 \
# App deps
python3-numpy \
python3-websockets \
&& rm -rf /var/lib/apt/lists/*
# Install cherry-picked plugins and refresh the linker cache
COPY --from=gst-builder /gst-select/ /tmp/gst-select/
RUN ARCH=$(uname -m)-linux-gnu && \
cp /tmp/gst-select/*.so /usr/lib/${ARCH}/gstreamer-1.0/ && \
rm -rf /tmp/gst-select && \
ldconfig
WORKDIR /app
COPY webrtc_streamer.py .
COPY entrypoint.sh .
RUN chmod +x entrypoint.sh
ENV IMAGE_TOPIC=/camera/image_raw
ENV PORT=8443
ENV BITRATE=2000000
EXPOSE 8443
ENTRYPOINT ["./entrypoint.sh"]
webrtc_streamer/README.md
+40
View File
@@ -0,0 +1,40 @@
# WebRTC Streamer
ROS 2 node that subscribes to a camera image topic, encodes frames as VP8 via GStreamer, and streams them to browsers over WebRTC.
A single encode pass is shared across all connected clients.
---
## Architecture
```
/camera/image_raw → webrtc_streamer → WebSocket :8443 → Browser
(ROS 2 topic) (VP8 encode) (signaling)
```
---
## Subscribed topics
| Topic | Type | Description |
|---|---|---|
| `IMAGE_TOPIC` | `sensor_msgs/Image` | Camera frames to encode and stream |
---
## Environment variables
| Variable | Default | Description |
|---|---|---|
| `IMAGE_TOPIC` | `/camera/image_raw` | ROS 2 topic to subscribe to |
| `PORT` | `8443` | WebSocket signaling port |
| `BITRATE` | `2000000` | VP8 target bitrate in bps |
---
## Connecting a browser
Open `client.html` (located in this directory) in any browser that can reach the host. Enter the server hostname or IP and click **Connect**.
The client connects to `ws://<host>:8443` for WebRTC signaling. No TURN server is required for LAN use.
webrtc_streamer/client.html
+63
View File
@@ -0,0 +1,63 @@
<!DOCTYPE html>
<html>
<head><title>WebRTC Camera</title></head>
<body>
<form id="connectForm">
<input id="host" type="text" placeholder="raspbot-v2.local" style="width:220px" />
<button type="submit">Connect</button>
</form>
<video id="video" autoplay playsinline muted style="width:640px;display:none"></video>
<script>
const form = document.getElementById("connectForm");
const video = document.getElementById("video");
// Persist the last-used host across page reloads
const hostInput = document.getElementById("host");
hostInput.value = localStorage.getItem("webrtcHost") || location.hostname || "";
let pc, ws;
function disconnect() {
if (ws) ws.close();
if (pc) pc.close();
video.style.display = "none";
}
form.addEventListener("submit", e => {
e.preventDefault();
const host = hostInput.value.trim();
if (!host) return;
localStorage.setItem("webrtcHost", host);
disconnect();
video.style.display = "";
pc = new RTCPeerConnection({});
ws = new WebSocket(`ws://${host}:8443`);
pc.ontrack = e => video.srcObject = e.streams[0];
pc.onicecandidate = e => {
if (e.candidate)
ws.send(JSON.stringify({ type: "ice", mlineindex: e.candidate.sdpMLineIndex, candidate: e.candidate.candidate }));
};
ws.onmessage = async ({ data }) => {
const msg = JSON.parse(data);
console.log(data);
if (msg.type === "offer") {
await pc.setRemoteDescription({ type: "offer", sdp: msg.sdp });
const answer = await pc.createAnswer();
await pc.setLocalDescription(answer);
ws.send(JSON.stringify({ type: "answer", sdp: answer.sdp }));
} else if (msg.type === "ice") {
await pc.addIceCandidate({ sdpMLineIndex: msg.mlineindex, candidate: msg.candidate });
}
};
ws.onerror = () => { alert(`Could not connect to ws://${host}:8443`); video.style.display = "none"; };
ws.onclose = () => video.style.display = "none";
});
</script>
</body>
</html>
webrtc_streamer/entrypoint.sh
+3
View File
@@ -0,0 +1,3 @@
#!/bin/sh
. /opt/ros/kilted/setup.sh
exec python3 webrtc_streamer.py
webrtc_streamer/webrtc_streamer.py
+293
View File
@@ -0,0 +1,293 @@
#!/usr/bin/env python3
"""
ROS2 node: /camera/image_raw → WebRTC stream (multi-client).
A single encode pipeline feeds all connected browsers simultaneously.
Each browser gets its own webrtcbin branch spliced onto a shared tee.
appsrc → videoconvert → vp8enc → tee ─┬→ queue → rtpvp8pay → webrtcbin (client 1)
└→ queue → rtpvp8pay → webrtcbin (client 2)
Signaling via WebSocket on PORT (default 8443). Point client.html at this host.
Initialisation order note: the ROS node (DDS) must be created before Gst.init()
is called. GLib's GObject type-system conflicts with DDS if GStreamer initialises
first. WebRTCStreamer.__init__ enforces this by calling super().__init__() before
touching GStreamer.
"""
import asyncio
import itertools
import json
import os
import threading
import numpy as np
import rclpy
from rclpy.executors import SingleThreadedExecutor
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image
import gi
gi.require_version('Gst', '1.0')
gi.require_version('GstWebRTC', '1.0')
gi.require_version('GstSdp', '1.0')
from gi.repository import Gst, GstWebRTC, GstSdp, GLib
import websockets
IMAGE_TOPIC = os.environ.get('IMAGE_TOPIC', '/camera/image_raw')
PORT = int(os.environ.get('PORT', '8443'))
BITRATE = int(os.environ.get('BITRATE', '2000000'))
_client_ids = itertools.count()
# ------------------------------------------------------------------ #
# GStreamer pipeline #
# ------------------------------------------------------------------ #
class StreamManager:
"""Shared encode pipeline. Clients attach/detach webrtcbin branches dynamically."""
def __init__(self, logger):
self._log = logger
desc = (
"appsrc name=src format=time is-live=true do-timestamp=true block=false ! "
"videoconvert ! "
f"vp8enc deadline=1 target-bitrate={BITRATE} keyframe-max-dist=5 cpu-used=8 ! "
"tee name=t allow-not-linked=true"
)
self.pipeline = Gst.parse_launch(desc)
self.appsrc = self.pipeline.get_by_name('src')
self.tee = self.pipeline.get_by_name('t')
self._clients = {}
self._lock = threading.Lock()
self._caps_set = False
self._bus = self.pipeline.get_bus()
self._bus.add_signal_watch()
self._bus.connect('message::error', self._on_error)
self._bus.connect('message::warning', self._on_warning)
def _on_error(self, bus, msg):
err, dbg = msg.parse_error()
self._log.error(f'GStreamer error: {err.message} — {dbg}')
def _on_warning(self, bus, msg):
warn, dbg = msg.parse_warning()
self._log.warning(f'GStreamer warning: {warn.message} — {dbg}')
def start(self):
self.pipeline.set_state(Gst.State.PLAYING)
def stop(self):
self.pipeline.set_state(Gst.State.NULL)
def push_frame(self, img):
h, w = img.shape[:2]
if not self._caps_set:
self.appsrc.set_property(
'caps',
Gst.Caps.from_string(
f"video/x-raw,format=BGR,width={w},height={h},framerate=0/1"
)
)
self._caps_set = True
buf = Gst.Buffer.new_wrapped(bytes(img))
self.appsrc.emit('push-buffer', buf)
def add_client(self, client_id, on_offer, on_ice):
"""Splice a new queue → rtpvp8pay → webrtcbin branch onto the tee."""
queue = Gst.ElementFactory.make('queue', f'queue_{client_id}')
pay = Gst.ElementFactory.make('rtpvp8pay', f'pay_{client_id}')
capsfilter = Gst.ElementFactory.make('capsfilter', f'caps_{client_id}')
webrtcbin = Gst.ElementFactory.make('webrtcbin', f'webrtc_{client_id}')
capsfilter.set_property('caps', Gst.Caps.from_string(
'application/x-rtp,media=video,encoding-name=VP8,payload=96'
))
webrtcbin.set_property('bundle-policy', GstWebRTC.WebRTCBundlePolicy.MAX_BUNDLE)
webrtcbin.connect('on-negotiation-needed', lambda el: on_offer(el))
webrtcbin.connect('on-ice-candidate', lambda el, idx, cand: on_ice(idx, cand))
with self._lock:
for el in (queue, pay, capsfilter, webrtcbin):
self.pipeline.add(el)
queue.link(pay)
pay.link(capsfilter)
# GStreamer 1.22+: explicit sink pad request required.
# capsfilter.link(webrtcbin) silently succeeds but never triggers
# on-negotiation-needed because no transceiver is registered.
webrtc_sink = webrtcbin.request_pad_simple('sink_%u')
capsfilter.get_static_pad('src').link(webrtc_sink)
tee_src = self.tee.request_pad_simple('src_%u')
tee_src.link(queue.get_static_pad('sink'))
for el in (queue, pay, capsfilter, webrtcbin):
el.sync_state_with_parent()
self._clients[client_id] = {
'elements': (queue, pay, capsfilter, webrtcbin),
'tee_src': tee_src,
'webrtc_sink': webrtc_sink,
}
self._log.info(f'Client {client_id} added ({len(self._clients)} connected)')
return webrtcbin
def remove_client(self, client_id):
"""Unlink and discard a client's pipeline branch."""
with self._lock:
if client_id not in self._clients:
return
info = self._clients.pop(client_id)
tee_src = info['tee_src']
webrtc_sink = info['webrtc_sink']
queue, pay, capsfilter, webrtcbin = info['elements']
tee_src.unlink(queue.get_static_pad('sink'))
self.tee.release_request_pad(tee_src)
capsfilter.get_static_pad('src').unlink(webrtc_sink)
webrtcbin.release_request_pad(webrtc_sink)
for el in (webrtcbin, capsfilter, pay, queue):
el.set_state(Gst.State.NULL)
self.pipeline.remove(el)
self._log.info(f'Client {client_id} removed ({len(self._clients)} connected)')
# ------------------------------------------------------------------ #
# ROS2 node — owns the GStreamer pipeline #
# ------------------------------------------------------------------ #
class WebRTCStreamer(Node):
def __init__(self):
# DDS must be initialised before GStreamer — see module docstring.
super().__init__('webrtc_streamer')
Gst.init(None)
self._glib_loop = GLib.MainLoop()
threading.Thread(target=self._glib_loop.run, daemon=True).start()
self.manager = StreamManager(self.get_logger())
self.manager.start()
self._frame_count = 0
qos = QoSProfile(
# reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1,
)
self.sub = self.create_subscription(Image, IMAGE_TOPIC, self._on_image, qos)
self.get_logger().info(f'Subscribed to {IMAGE_TOPIC}')
self.get_logger().info(f'WebRTC signaling on ws://0.0.0.0:{PORT}')
def _on_image(self, msg):
# Decode the raw sensor_msgs/Image without cv_bridge / OpenCV.
# v4l2_camera publishes bgr8 or rgb8; videoconvert handles anything else.
img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, -1)
if msg.encoding == 'rgb8':
img = img[:, :, ::-1] # RGB → BGR (view; bytes() below makes it contiguous)
self._frame_count += 1
if self._frame_count % 100 == 0:
self.get_logger().info(f'Frames: {self._frame_count}')
self.manager.push_frame(img)
# ------------------------------------------------------------------ #
# WebRTC signaling #
# ------------------------------------------------------------------ #
class WebRTCClient:
def __init__(self, client_id, websocket, manager, loop):
self.client_id = client_id
self.ws = websocket
self.loop = loop
self.webrtcbin = manager.add_client(
client_id,
on_offer=self._on_negotiation_needed,
on_ice=self._send_ice,
)
def _on_negotiation_needed(self, element):
promise = Gst.Promise.new_with_change_func(self._on_offer_created, element)
element.emit('create-offer', None, promise)
def _on_offer_created(self, promise, element):
reply = promise.get_reply()
offer = reply.get_value('offer')
element.emit('set-local-description', offer, Gst.Promise.new())
msg = json.dumps({'type': 'offer', 'sdp': offer.sdp.as_text()})
asyncio.run_coroutine_threadsafe(self.ws.send(msg), self.loop)
def _send_ice(self, mline_index, candidate):
msg = json.dumps({'type': 'ice', 'mlineindex': mline_index, 'candidate': candidate})
asyncio.run_coroutine_threadsafe(self.ws.send(msg), self.loop)
def handle_answer(self, sdp_text):
_, sdp = GstSdp.SDPMessage.new_from_text(sdp_text)
answer = GstWebRTC.WebRTCSessionDescription.new(GstWebRTC.WebRTCSDPType.ANSWER, sdp)
self.webrtcbin.emit('set-remote-description', answer, Gst.Promise.new())
def handle_ice(self, mline_index, candidate):
self.webrtcbin.emit('add-ice-candidate', mline_index, candidate)
def stop(self, manager):
manager.remove_client(self.client_id)
async def handler(websocket, manager):
client_id = next(_client_ids)
client = WebRTCClient(client_id, websocket, manager, asyncio.get_running_loop())
try:
async for raw in websocket:
msg = json.loads(raw)
if msg['type'] == 'answer':
client.handle_answer(msg['sdp'])
elif msg['type'] == 'ice':
client.handle_ice(msg['mlineindex'], msg['candidate'])
except websockets.exceptions.ConnectionClosed:
pass
finally:
client.stop(manager)
# ------------------------------------------------------------------ #
# Entry point #
# ------------------------------------------------------------------ #
async def run_signaling(manager):
async with websockets.serve(
lambda ws, *_: handler(ws, manager), '0.0.0.0', PORT
):
await asyncio.Future()
def main():
rclpy.init()
# WebRTCStreamer.__init__ creates the ROS node first (DDS), then GStreamer.
node = WebRTCStreamer()
executor = SingleThreadedExecutor()
executor.add_node(node)
def spin():
while rclpy.ok():
try:
executor.spin_once(timeout_sec=0.1)
except AttributeError:
pass # known rclpy/Kilted executor bug; non-fatal
threading.Thread(target=spin, daemon=True).start()
try:
asyncio.run(run_signaling(node.manager))
finally:
node.manager.stop()
executor.shutdown()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
wifi/README.md
+36
View File
@@ -0,0 +1,36 @@
# Wi-Fi Manager
Monitors the robot's Wi-Fi connection and falls back to a hotspot if the target SSID is unavailable.
If the configured SSID is not seen within `WIFI_TIMEOUT` seconds of startup, or if the connection is later lost, the manager creates a NetworkManager Wi-Fi hotspot so the robot remains reachable. When the target SSID becomes visible again the hotspot is torn down and the robot reconnects.
Communicates with the host NetworkManager via `nmcli` (the host D-Bus socket is bind-mounted into the container).
---
## Environment variables
| Variable | Default | Description |
|---|---|---|
| `WIFI_SSID` | _(empty)_ | Target SSID to connect to. If empty, the hotspot starts immediately. |
| `HOTSPOT_SSID` | `raspbot-hotspot` | SSID of the fallback hotspot |
| `HOTSPOT_PASSWORD` | `raspbot1234` | Passphrase for the fallback hotspot |
| `HOTSPOT_BAND` | `bg` | Wi-Fi band: `bg` (2.4 GHz) or `a` (5 GHz) |
| `WIFI_IFACE` | _(auto-detect)_ | Wi-Fi interface name. Auto-detected from NetworkManager if empty. |
| `WIFI_TIMEOUT` | `30` | Seconds to wait for the target SSID before creating the hotspot |
| `POLL_INTERVAL` | `15` | Seconds between connection checks |
---
## Docker requirements
The container needs `NET_ADMIN` capability and access to the host D-Bus socket:
```yaml
cap_add:
- NET_ADMIN
volumes:
- /run/dbus/system_bus_socket:/run/dbus/system_bus_socket
```
Both are already configured in `docker-compose.yml`.