Add image and streaming from USB camera
Plus a little freshen up of the readme's
This commit is contained in:
@@ -8,7 +8,7 @@ RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi
|
|||||||
|
|
||||||
# Create the user
|
# Create the user
|
||||||
RUN groupadd --gid $USER_GID $USERNAME \
|
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.
|
# [Optional] Add sudo support. Omit if you don't need to install software after connecting.
|
||||||
&& apt-get update \
|
&& apt-get update \
|
||||||
@@ -57,6 +57,21 @@ RUN python3 -m venv /opt/webui-venv \
|
|||||||
"uvicorn[standard]" \
|
"uvicorn[standard]" \
|
||||||
numpy
|
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
|
ENV SHELL /bin/bash
|
||||||
|
|
||||||
# ********************************************************
|
# ********************************************************
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
"name": "ROS 2 Development Container",
|
"name": "ROS 2 Development Container",
|
||||||
"privileged": true,
|
"privileged": true,
|
||||||
"remoteUser": "matt",
|
"remoteUser": "matt",
|
||||||
|
"initializeCommand": "stat -c %g /var/run/docker.sock > .devcontainer/.docker_gid",
|
||||||
"build": {
|
"build": {
|
||||||
"dockerfile": "Dockerfile",
|
"dockerfile": "Dockerfile",
|
||||||
"args": {
|
"args": {
|
||||||
@@ -38,7 +39,8 @@
|
|||||||
"mounts": [
|
"mounts": [
|
||||||
"source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached",
|
"source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached",
|
||||||
"source=/dev/dri,target=/dev/dri,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"
|
"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"
|
||||||
}
|
}
|
||||||
@@ -14,3 +14,6 @@ __pycache__/
|
|||||||
*.py[cod]
|
*.py[cod]
|
||||||
*.egg-info/
|
*.egg-info/
|
||||||
dist/
|
dist/
|
||||||
|
|
||||||
|
# Generated by devcontainer initializeCommand — machine-specific
|
||||||
|
.devcontainer/.docker_gid
|
||||||
|
|||||||
@@ -1,189 +1,21 @@
|
|||||||
# raspbot_v2
|
# 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.
|
| Directory | Description |
|
||||||
|
|---|---|
|
||||||
### Motor controller
|
| [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 |
|
||||||
│ MotorControllerNode │
|
| [camera_publisher/](camera_publisher/README.md) | V4L2 camera → ROS 2 topic publisher |
|
||||||
│ │
|
| [webrtc_streamer/](webrtc_streamer/README.md) | WebRTC browser stream server |
|
||||||
/cmd_vel ──────────>│ Twist → differential kinematics │
|
| [webui/](webui/README.md) | Browser-based robot controller |
|
||||||
(geometry_msgs/Twist)│ left = linear − (angular × wb/2)│
|
| [ansible/](ansible/README.md) | Provisioning playbook for the Raspberry Pi |
|
||||||
│ 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
|
|
||||||
```
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@@ -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.
|
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 |
|
| Setting | Value |
|
||||||
|---|---|
|
|---|---|
|
||||||
@@ -202,7 +34,7 @@ Before writing, open the imager's **Advanced options** (⚙) and configure:
|
|||||||
| Username / Password | Your preferred credentials |
|
| Username / Password | Your preferred credentials |
|
||||||
| Wi-Fi | Your network SSID and password (if not using Ethernet) |
|
| 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
|
### 2. Provision with Ansible
|
||||||
|
|
||||||
@@ -214,16 +46,14 @@ The [ansible/](ansible/) directory contains a playbook that handles the remainin
|
|||||||
|
|
||||||
### Prerequisites
|
### Prerequisites
|
||||||
|
|
||||||
- Docker (with BuildKit enabled)
|
- 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:
|
- For cross-compilation from an amd64 host, register QEMU user-space emulation once:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
docker run --rm --privileged tonistiigi/binfmt --install arm64
|
docker run --rm --privileged tonistiigi/binfmt --install arm64
|
||||||
```
|
```
|
||||||
|
|
||||||
### Build with Docker Compose (recommended)
|
### Build all images
|
||||||
|
|
||||||
Both images are defined in `docker-compose.yml`. Build them together:
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
docker compose build
|
docker compose build
|
||||||
@@ -236,51 +66,36 @@ docker compose build robot
|
|||||||
docker compose build lidar
|
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
|
## 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
|
```bash
|
||||||
docker save raspbot_v2:latest raspbot_v2_lidar:latest \
|
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
|
```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
|
## Launching
|
||||||
|
|
||||||
### Start everything with Docker Compose (recommended)
|
### Start everything
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
docker compose up
|
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
|
```bash
|
||||||
docker compose up -d
|
docker compose up -d
|
||||||
@@ -290,12 +105,13 @@ docker compose down # stop and remove containers
|
|||||||
|
|
||||||
### Environment variables
|
### 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
|
```bash
|
||||||
ROS_DOMAIN_ID=0
|
ROS_DOMAIN_ID=0
|
||||||
LIDAR_PORT=/dev/ttyUSB0
|
LIDAR_PORT=/dev/ttyUSB0
|
||||||
LIDAR_FRAME_ID=laser
|
LIDAR_FRAME_ID=laser
|
||||||
|
WIFI_SSID=MyNetwork
|
||||||
```
|
```
|
||||||
|
|
||||||
| Variable | Default | Description |
|
| Variable | Default | Description |
|
||||||
@@ -303,101 +119,9 @@ LIDAR_FRAME_ID=laser
|
|||||||
| `ROS_DOMAIN_ID` | `0` | ROS 2 domain — must match on all nodes |
|
| `ROS_DOMAIN_ID` | `0` | ROS 2 domain — must match on all nodes |
|
||||||
| `LIDAR_PORT` | `/dev/ttyUSB0` | Host device node for the RPLIDAR |
|
| `LIDAR_PORT` | `/dev/ttyUSB0` | Host device node for the RPLIDAR |
|
||||||
| `LIDAR_FRAME_ID` | `laser` | `frame_id` in published `LaserScan` messages |
|
| `LIDAR_FRAME_ID` | `laser` | `frame_id` in published `LaserScan` messages |
|
||||||
|
| `WIFI_SSID` | _(empty)_ | Target SSID; if unset the Wi-Fi container creates a hotspot immediately |
|
||||||
### Run containers individually
|
| `HOTSPOT_SSID` | `raspbot-hotspot` | Fallback hotspot SSID |
|
||||||
|
| `HOTSPOT_PASSWORD` | `raspbot1234` | Fallback hotspot passphrase |
|
||||||
```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
|
|
||||||
```
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@@ -405,24 +129,14 @@ ros2 topic echo /joint_states
|
|||||||
|
|
||||||
```
|
```
|
||||||
.
|
.
|
||||||
├── docker-compose.yml # Launches robot and lidar containers together
|
├── docker-compose.yml
|
||||||
├── docker-entrypoint.sh # Sources ROS overlays before exec (shared by both images)
|
├── docker-entrypoint.sh
|
||||||
├── robot/
|
├── robot/ # Motor controller, pan/tilt, ultrasonic
|
||||||
│ ├── Dockerfile # Robot controller image (two-stage)
|
├── lidar/ # RPLIDAR A1
|
||||||
│ ├── src/
|
├── oled/ # OLED display dashboard
|
||||||
│ │ └── raspbot_v2/
|
├── wifi/ # Wi-Fi hotspot fallback
|
||||||
│ │ ├── package.xml # ROS package manifest
|
├── camera_publisher/ # V4L2 camera → ROS 2 topic
|
||||||
│ │ ├── setup.py # ament_python build definition
|
├── webrtc_streamer/ # WebRTC browser stream
|
||||||
│ │ ├── launch/
|
├── webui/ # Browser-based controller UI
|
||||||
│ │ │ └── robot.launch.py # Starts all robot nodes together
|
└── ansible/ # Raspberry Pi provisioning
|
||||||
│ │ └── 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)
|
|
||||||
```
|
```
|
||||||
|
|||||||
@@ -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"]
|
||||||
@@ -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
|
||||||
|
```
|
||||||
@@ -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()
|
||||||
@@ -0,0 +1,3 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
. /opt/ros/kilted/setup.sh
|
||||||
|
exec python3 /app/camera_publisher.py
|
||||||
@@ -99,3 +99,43 @@ services:
|
|||||||
environment:
|
environment:
|
||||||
- ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0}
|
- ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0}
|
||||||
restart: unless-stopped
|
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
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
## How the build works
|
||||||
|
|
||||||
The driver source lives in `lidar/sllidar_ros2/` as a **git subtree** of this
|
The driver source lives in `lidar/sllidar_ros2/` as a **git subtree** of this
|
||||||
|
|||||||
@@ -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`.
|
||||||
+225
@@ -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)
|
||||||
|
```
|
||||||
@@ -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"]
|
||||||
@@ -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.
|
||||||
@@ -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>
|
||||||
@@ -0,0 +1,3 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
. /opt/ros/kilted/setup.sh
|
||||||
|
exec python3 webrtc_streamer.py
|
||||||
@@ -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()
|
||||||
@@ -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`.
|
||||||
Reference in New Issue
Block a user