diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 4a2fc25..fc5cf9f 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -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 # ******************************************************** diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 7cf9b95..827b1df 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -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" } \ No newline at end of file diff --git a/.gitignore b/.gitignore index 03b4ab6..5578fe3 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,6 @@ __pycache__/ *.py[cod] *.egg-info/ dist/ + +# Generated by devcontainer initializeCommand — machine-specific +.devcontainer/.docker_gid diff --git a/README.md b/README.md index f6d4189..c1762c8 100644 --- a/README.md +++ b/README.md @@ -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 @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 @raspbot-v2.local:~/ ``` -Replace `matt` with the username configured in [ansible/inventory.ini](ansible/inventory.ini). +Replace `` 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 ``` diff --git a/camera_publisher/Dockerfile b/camera_publisher/Dockerfile new file mode 100644 index 0000000..c207d0a --- /dev/null +++ b/camera_publisher/Dockerfile @@ -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"] diff --git a/camera_publisher/README.md b/camera_publisher/README.md new file mode 100644 index 0000000..d79830b --- /dev/null +++ b/camera_publisher/README.md @@ -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 | +|---|---|---| +| `//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 `//image_raw`) | + +--- + +## Finding your camera device + +```bash +v4l2-ctl --list-devices +v4l2-ctl -d /dev/video0 --list-formats-ext +``` diff --git a/camera_publisher/camera_publisher.py b/camera_publisher/camera_publisher.py new file mode 100644 index 0000000..0eaf842 --- /dev/null +++ b/camera_publisher/camera_publisher.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +""" +Minimal ROS2 camera publisher using GStreamer v4l2src. +Publishes sensor_msgs/Image to //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() diff --git a/camera_publisher/entrypoint.sh b/camera_publisher/entrypoint.sh new file mode 100644 index 0000000..9bd6705 --- /dev/null +++ b/camera_publisher/entrypoint.sh @@ -0,0 +1,3 @@ +#!/bin/sh +. /opt/ros/kilted/setup.sh +exec python3 /app/camera_publisher.py diff --git a/docker-compose.yml b/docker-compose.yml index 6f2baa1..ad5e64a 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -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 + diff --git a/lidar/README.md b/lidar/README.md index 515ed8d..2042365 100644 --- a/lidar/README.md +++ b/lidar/README.md @@ -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 diff --git a/oled/README.md b/oled/README.md new file mode 100644 index 0000000..e80e7e5 --- /dev/null +++ b/oled/README.md @@ -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`. diff --git a/robot/README.md b/robot/README.md new file mode 100644 index 0000000..39eab22 --- /dev/null +++ b/robot/README.md @@ -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) +``` diff --git a/webrtc_streamer/Dockerfile b/webrtc_streamer/Dockerfile new file mode 100644 index 0000000..2783a31 --- /dev/null +++ b/webrtc_streamer/Dockerfile @@ -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"] diff --git a/webrtc_streamer/README.md b/webrtc_streamer/README.md new file mode 100644 index 0000000..8359b31 --- /dev/null +++ b/webrtc_streamer/README.md @@ -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://:8443` for WebRTC signaling. No TURN server is required for LAN use. diff --git a/webrtc_streamer/client.html b/webrtc_streamer/client.html new file mode 100644 index 0000000..1bcc7f8 --- /dev/null +++ b/webrtc_streamer/client.html @@ -0,0 +1,63 @@ + + +WebRTC Camera + +
+ + +
+ + + + diff --git a/webrtc_streamer/entrypoint.sh b/webrtc_streamer/entrypoint.sh new file mode 100644 index 0000000..2d7acce --- /dev/null +++ b/webrtc_streamer/entrypoint.sh @@ -0,0 +1,3 @@ +#!/bin/sh +. /opt/ros/kilted/setup.sh +exec python3 webrtc_streamer.py diff --git a/webrtc_streamer/webrtc_streamer.py b/webrtc_streamer/webrtc_streamer.py new file mode 100644 index 0000000..3933792 --- /dev/null +++ b/webrtc_streamer/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() diff --git a/wifi/README.md b/wifi/README.md new file mode 100644 index 0000000..f32f1c1 --- /dev/null +++ b/wifi/README.md @@ -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`.