Initial commit

.github/workflows/ci-checks.yml

@@ -0,0 +1,38 @@
+# This workflow will install Python dependencies, run tests and lint with a variety of Python versions
+# For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions
+
+name: CI checks
+
+on:
+  push:
+    branches: [ main ]
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  linting_testing:
+
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v2
+      with:
+        submodules: true
+    - name: Set up Python 3
+      uses: actions/setup-python@v2
+      with:
+        python-version: '3.x'
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install -r requirements_dev.txt
+    - name: Generate brefv code
+      run: |
+        mkdir brefv
+        datamodel-codegen --input brefv-spec/envelope.json --input-file-type jsonschema --output brefv/envelope.py
+        datamodel-codegen --input brefv-spec/messages --input-file-type jsonschema  --reuse-model --output brefv/messages
+    - name: Run black
+      run: |
+        black --check main.py
+    - name: Run pylint
+      run: |
+        pylint  main.py

.github/workflows/release.yml

@@ -0,0 +1,36 @@
+name: Release
+
+on:
+  release:
+    types: [published]
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    steps:
+      -
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v1
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+      -
+        name: Login to GitHub Container Registry
+        uses: docker/login-action@v1 
+        with:
+          registry: ghcr.io
+          username: ${{ github.repository_owner }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+      -
+        name: Build and push
+        id: docker_build
+        uses: docker/build-push-action@v2
+        with:
+          file: Dockerfile
+          push: true
+          tags: |
+            ghcr.io/mo-rise/crowsnest-connector-lidar-ouster:latest
+            ghcr.io/mo-rise/crowsnest-connector-lidar-ouster:${{ github.event.release.tag_name }}
+      -
+        name: Image digest
+        run: echo ${{ steps.docker_build.outputs.digest }}

.gitignore

@@ -1,3 +1,6 @@
+# Brefv
+brefv/
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "brefv-spec"]
+	path = brefv-spec
+	url = https://github.com/MO-RISE/brefv.git

Dockerfile

@@ -0,0 +1,19 @@
+FROM python:3.9-slim
+
+COPY --chmod=555 ./bin/* /usr/local/bin/
+
+COPY requirements.txt requirements.txt
+
+RUN pip3 install -r requirements.txt
+
+WORKDIR /app
+
+COPY brefv-spec/ brefv-spec/
+RUN mkdir brefv && \
+    datamodel-codegen --input brefv-spec/envelope.json --input-file-type jsonschema --output brefv/envelope.py && \
+    datamodel-codegen --input brefv-spec/messages --input-file-type jsonschema  --reuse-model --output brefv/messages
+
+
+COPY main.py main.py
+
+CMD ["python3", "main.py"]

README.md

@@ -1,2 +1,58 @@
 # crowsnest-connector-lidar-ouster
 A crowsnest microservice for connecting to an Ouster Lidar
+
+### How it works
+
+For now, this microservice jsut does the basics.
+* Connects to an already configured Ouster sensor
+* Listens on the continuous stream of LidarScanPackets
+* Transform these to the NED frame (requires manual input for now and assumes a static sensor)
+* Wraps into a brefv message and outputs over MQTT
+
+The docker image also contains a command line application for configuring the sensor, run as:
+```
+docker run --rm ghcr.io/mo-rise/crowsnest-connector-lidar-ouster ouster-configure --help
+```
+
+### Typical setup (docker-compose)
+
+```yaml
+version: '3'
+services:
+
+  ouster-lidar:
+    image: ghcr.io/mo-rise/crowsnest-connector-lidar-ouster:latest
+    restart: unless-stopped
+    network_mode: "host"
+    environment:
+      - MQTT_BROKER_HOST=localhost
+      - MQTT_BROKER_PORT=1883
+      - MQTT_TOPIC_POINTCLOUD=CROWSNEST/<platform>/LIDAR/<device_id>/POINTCLOUD
+      - OUSTER_HOSTNAME=<IP of sensor>
+      - OUSTER_ATTITUDE=90,45,180
+      - POINTCLOUD_FREQUENCY=2
+```
+
+
+
+## Development setup
+To setup the development environment:
+
+    python3 -m venv venv
+    source ven/bin/activate
+
+Install everything thats needed for development:
+
+    pip install -r requirements_dev.txt
+
+To run the linters:
+
+    black main.py tests
+    pylint main.py
+
+To run the tests:
+
+    no automatic tests yet...
+
+## License
+Apache 2.0, see [LICENSE](./LICENSE)

bin/ouster-configure

@@ -0,0 +1,77 @@
+#!/usr/bin/env python3
+
+"""
+Command line utility tool for configuring an Ouster Lidar Sensor.
+
+Currently just provides the basic settings to be changed
+"""
+import argparse
+from ouster import client
+
+
+def configure_sensor(
+    hostname: str,
+    lidar_port: int,
+    imu_port: int,
+    lidar_mode: client.LidarMode,
+    operating_mode: client.OperatingMode,
+) -> client.SensorConfig:
+
+    # Configure the Ouster lidar sensor
+    config = client.SensorConfig()
+    config.udp_port_lidar = lidar_port
+    config.udp_port_imu = imu_port
+    config.lidar_mode = lidar_mode
+    config.operating_mode = operating_mode
+
+    client.set_config(hostname, config, persist=True, udp_dest_auto=True)
+
+    return client.get_config(hostname)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="ouster-configure")
+    parser.add_argument(
+        "hostname",
+        type=str,
+        help="Hostname of Ouster sensor",
+    )
+    parser.add_argument(
+        "--lidar_port",
+        type=int,
+        default=7502,
+        help="UDP port for LIDAR packets",
+    )
+    parser.add_argument(
+        "--imu_port",
+        type=int,
+        default=7503,
+        help="UDP port for IMU packets",
+    )
+    valid_lidar_modes = [val.name for val in client.LidarMode.values]
+    parser.add_argument(
+        "--lidar_mode",
+        choices=valid_lidar_modes,
+        default="1024x10",
+        # action=client.LidarMode.from_string,
+        help="Lidar mode, see sensor documentation",
+    )
+    valid_operaring_modes = [val.name for val in client.OperatingMode.values]
+    parser.add_argument(
+        "--operating_mode",
+        choices=valid_operaring_modes,
+        default="NORMAL",
+        # action=client.OperatingMode.from_string,
+        help="Operating mode, see sensor documentation",
+    )
+
+    args = parser.parse_args()
+    config = configure_sensor(
+        hostname=args.hostname,
+        lidar_port=args.lidar_port,
+        imu_port=args.imu_port,
+        lidar_mode=client.LidarMode.from_string(args.lidar_mode),
+        operating_mode=client.OperatingMode.from_string(args.operating_mode),
+    )
+
+    print(config)

main.py

@@ -0,0 +1,139 @@
+"""Main entrypoint for this application"""
+import logging
+import warnings
+import threading
+from contextlib import closing
+from functools import partial
+from datetime import datetime, timezone
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+from streamz import Stream
+from environs import Env
+from paho.mqtt.client import Client as MQTT
+from ouster import client
+
+from brefv.envelope import Envelope
+
+
+# Reading config from environment variables
+env = Env()
+
+MQTT_BROKER_HOST: str = env("MQTT_BROKER_HOST")
+MQTT_BROKER_PORT: int = env.int("MQTT_BROKER_PORT", 1883)
+MQTT_CLIENT_ID: str = env("MQTT_CLIENT_ID", None)
+MQTT_TRANSPORT: str = env("MQTT_TRANSPORT", "tcp")
+MQTT_TLS: bool = env.bool("MQTT_TLS", False)
+MQTT_USER: str = env("MQTT_USER", None)
+MQTT_PASSWORD: str = env("MQTT_PASSWORD", None)
+
+MQTT_TOPIC_POINTCLOUD: str = env("MQTT_TOPIC_POINTCLOUD")
+
+OUSTER_HOSTNAME: str = env("OUSTER_HOSTNAME")
+
+# These are a set of Euler angles (roll, pitch, yaw) taking us from the platform body
+# frame to the Sensor frame, as defined in the Sensor documentation.
+OUSTER_ATTITUDE: list = env.list(
+    "OUSTER_ATTITUDE", [0, 0, 0], subcast=float, validate=lambda x: len(x) == 3
+)
+POINTCLOUD_FREQUENCY = env.float("POINTCLOUD_FREQUENCY", default=2)
+
+LOG_LEVEL = env.log_level("LOG_LEVEL", logging.WARNING)
+
+
+# Setup logger
+logging.basicConfig(level=LOG_LEVEL)
+logging.captureWarnings(True)
+warnings.filterwarnings("once")
+LOGGER = logging.getLogger("crowsnest-connector-lidar-ouster")
+
+# Create mqtt client and configure it according to configuration
+mq = MQTT(client_id=MQTT_CLIENT_ID, transport=MQTT_TRANSPORT)
+mq.username_pw_set(MQTT_USER, MQTT_PASSWORD)
+if MQTT_TLS:
+    mq.tls_set()
+
+mq.enable_logger(LOGGER)
+
+
+def rotate_pcd(pcd: np.ndarray, attitude: list) -> np.ndarray:
+    """Rotate pcd according to the sensor attitude
+
+    Args:
+        pcd (np.ndarray): The un-rotated point cloud (in sensor frame)
+        attitude (np.ndarray): The attitude of the sensor ([roll, pitch, yaw]) in degrees
+
+    Returns:
+        np.ndarray: The rotated point cloud (in NED frame)
+    """
+    points = pcd.reshape(-1, pcd.shape[-1])
+    LOGGER.debug("Rotating %d points using attitude: %s", len(points), attitude)
+    transform = Rotation.from_euler("zyx", attitude[::-1], degrees=True)
+    return transform.apply(points)
+
+
+def to_brefv(pcd: np.ndarray) -> Envelope:
+    """From point cloud to brefv envelope"""
+
+    envelope = Envelope(
+        sent_at=datetime.now(timezone.utc).isoformat(),
+        message=pcd.tolist(),
+    )
+
+    LOGGER.debug("Assembled into brefv envelope: %s", envelope)
+
+    return envelope
+
+
+def to_mqtt(envelope: dict):
+    """Publish an envelope to a mqtt topic"""
+
+    topic = MQTT_TOPIC_POINTCLOUD
+    payload = envelope.json()
+
+    LOGGER.debug("Publishing on %s with payload: %s", topic, payload)
+    try:
+        mq.publish(
+            topic,
+            payload,
+        )
+    except Exception:  # pylint: disable=broad-except
+        LOGGER.exception("Failed publishing to broker!")
+
+
+if __name__ == "__main__":
+
+    # Build pipeline
+    LOGGER.info("Building pipeline...")
+    source = Stream()
+    source.latest().rate_limit(1 / POINTCLOUD_FREQUENCY).map(
+        partial(rotate_pcd, attitude=OUSTER_ATTITUDE)
+    ).map(to_brefv).sink(to_mqtt)
+
+    LOGGER.info("Connecting to MQTT broker...")
+    mq.connect(MQTT_BROKER_HOST, MQTT_BROKER_PORT)
+
+    # Ouster SDK runs in the foreground so we put the MQTT stuff in a separate thread
+    threading.Thread(target=mq.loop_forever, daemon=True).start()
+
+    LOGGER.info("Connecting to Ouster sensor...")
+
+    # Connect with the Ouster sensor and start processing lidar scans
+    config = client.get_config(OUSTER_HOSTNAME)
+    LOGGER.info("Sensor configuration: \n %s", config)
+
+    LOGGER.info("Processing packages!")
+
+    with closing(
+        client.Scans.stream(OUSTER_HOSTNAME, config.udp_port_lidar, complete=True)
+    ) as stream:
+
+        # Create a look-up table to cartesian projection
+        xyz_lut = client.XYZLut(stream.metadata)
+
+        for scan in stream:
+
+            # obtain destaggered xyz representation
+            xyz_destaggered = client.destagger(stream.metadata, xyz_lut(scan))
+
+            source.emit(xyz_destaggered)

requirements.txt

@@ -0,0 +1,8 @@
+paho-mqtt
+streamz
+environs
+pydantic
+datamodel-code-generator
+ouster-sdk
+numpy
+scipy