diff --git a/.gitignore b/.gitignore index b0311b5..88a028a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,224 @@ .venv /.idea/ -.vscode/ \ No newline at end of file +.vscode/ + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[codz] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py.cover +*.lcov +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +# Pipfile.lock + +# UV +# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# uv.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +# poetry.lock +# poetry.toml + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +# pdm recommends including project-wide configuration in pdm.toml, but excluding .pdm-python. +# https://pdm-project.org/en/latest/usage/project/#working-with-version-control +# pdm.lock +# pdm.toml +.pdm-python +.pdm-build/ + +# pixi +# Similar to Pipfile.lock, it is generally recommended to include pixi.lock in version control. +# pixi.lock +# Pixi creates a virtual environment in the .pixi directory, just like venv module creates one +# in the .venv directory. It is recommended not to include this directory in version control. +.pixi/* +!.pixi/config.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule* +celerybeat.pid + +# Redis +*.rdb +*.aof +*.pid + +# RabbitMQ +mnesia/ +rabbitmq/ +rabbitmq-data/ + +# ActiveMQ +activemq-data/ + +# SageMath parsed files +*.sage.py + +# Environments +.env +.envrc +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +# .idea/ + +# Abstra +# Abstra is an AI-powered process automation framework. +# Ignore directories containing user credentials, local state, and settings. +# Learn more at https://abstra.io/docs +.abstra/ + +# Visual Studio Code +# Visual Studio Code specific template is maintained in a separate VisualStudioCode.gitignore +# that can be found at https://github.com/github/gitignore/blob/main/Global/VisualStudioCode.gitignore +# and can be added to the global gitignore or merged into this file. However, if you prefer, +# you could uncomment the following to ignore the entire vscode folder +# .vscode/ +# Temporary file for partial code execution +tempCodeRunnerFile.py + +# Ruff stuff: +.ruff_cache/ + +# PyPI configuration file +.pypirc + +# Marimo +marimo/_static/ +marimo/_lsp/ +__marimo__/ + +# Streamlit +.streamlit/secrets.toml \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..426eba3 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,50 @@ +FROM ros:humble-ros-base + +SHELL ["/bin/bash", "-c"] + +ENV ROS_DISTRO=humble \ + ROS_WS=/ws + +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + python3-colcon-common-extensions \ + python3-yaml \ + python3-scipy \ + python3-pymap3d \ + ros-humble-mavros \ + ros-humble-mavros-extras \ + ros-humble-mavros-msgs \ + ros-humble-foxglove-bridge \ + && rm -rf /var/lib/apt/lists/* + +RUN if [ -x /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh ]; then \ + /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh; \ + fi + +WORKDIR ${ROS_WS} +RUN mkdir -p ${ROS_WS}/src + +ARG PX4_MSGS_BRANCH=main +RUN git clone -b ${PX4_MSGS_BRANCH} https://github.com/PX4/px4_msgs.git ${ROS_WS}/src/px4_msgs + +ARG MICRO_XRCE_AGENT_VERSION=v2.4.3 +RUN git clone -b ${MICRO_XRCE_AGENT_VERSION} https://github.com/eProsima/Micro-XRCE-DDS-Agent.git /tmp/Micro-XRCE-DDS-Agent \ + && mkdir -p /tmp/Micro-XRCE-DDS-Agent/build \ + && cd /tmp/Micro-XRCE-DDS-Agent/build \ + && cmake .. \ + && make -j"$(nproc)" \ + && make install \ + && ldconfig /usr/local/lib/ \ + && rm -rf /tmp/Micro-XRCE-DDS-Agent + +COPY . ${ROS_WS}/src/mavinsight + +RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ + && colcon build --symlink-install + +COPY docker/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh + +ENTRYPOINT ["/ros_entrypoint.sh"] diff --git a/Dockerfile.px4 b/Dockerfile.px4 new file mode 100644 index 0000000..0e24ec5 --- /dev/null +++ b/Dockerfile.px4 @@ -0,0 +1,6 @@ +FROM px4io/px4-sitl:latest + +COPY docker/px4_entrypoint.sh /opt/px4/bin/px4-entrypoint-mavinsight.sh +RUN chmod +x /opt/px4/bin/px4-entrypoint-mavinsight.sh + +ENTRYPOINT ["/opt/px4/bin/px4-entrypoint-mavinsight.sh"] diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 0000000..a1b5d60 --- /dev/null +++ b/docker-compose.yml @@ -0,0 +1,41 @@ +services: + px4: + build: + context: . + dockerfile: Dockerfile.px4 + image: mavinsight-px4-sitl + stdin_open: true + tty: true + init: true + environment: + - ROS_DOMAIN_ID=0 + - PX4_UXRCE_DDS_PORT=8888 + ports: + # Foxglove websocket bridge. + - "8765:8765/tcp" + # Do not publish 14550/udp by default. QGroundControl binds that host + # port for UDP AutoConnect, and Docker publishing it causes: + # "Link UDP Link (AutoConnect): The bound address is already in use". + + ros2: + build: + context: . + dockerfile: Dockerfile + init: true + depends_on: + - px4 + network_mode: "service:px4" + environment: + - ROS_DISTRO=humble + - ROS_DOMAIN_ID=0 + - ROS_WS=/ws + - SKIP_COLCON_BUILD=0 + - START_UXRCE_AGENT=1 + - UXRCE_DDS_PORT=8888 + - START_MAVROS=1 + - MAVROS_NAMESPACE=uas4 + - MAVROS_FCU_URL=udp://:14540@127.0.0.1:14580 + - FOXGLOVE_PORT=8765 + - MAVINSIGHT_LAUNCH=launch_all_vehicles_no_tba.launch.py + volumes: + - .:/ws/src/mavinsight:rw diff --git a/docker/px4_entrypoint.sh b/docker/px4_entrypoint.sh new file mode 100644 index 0000000..b6bebeb --- /dev/null +++ b/docker/px4_entrypoint.sh @@ -0,0 +1,22 @@ +#!/bin/sh +set -e + +# Detect install prefix. SIH uses /opt/px4, Gazebo uses /opt/px4-gazebo. +if [ -d /opt/px4-gazebo ]; then + PX4_PREFIX=/opt/px4-gazebo +else + PX4_PREFIX=/opt/px4 +fi + +# On Docker Desktop, host.docker.internal lets PX4 send MAVLink packets to +# QGroundControl on the host without publishing the QGC UDP port from Docker. +DOCKER_HOST_IP=$(getent ahostsv4 host.docker.internal 2>/dev/null | awk '/STREAM/ {print $1; exit}') + +if [ -n "$DOCKER_HOST_IP" ]; then + sed -i "s/mavlink start -x -u/mavlink start -x -t $DOCKER_HOST_IP -u/g" \ + "$PX4_PREFIX/etc/init.d-posix/px4-rc.mavlink" +fi + +# The ROS container shares this container's network namespace, so the +# MicroXRCEAgent is reachable from PX4 on localhost. +exec "$PX4_PREFIX/bin/px4" "$@" diff --git a/docker/ros_entrypoint.sh b/docker/ros_entrypoint.sh new file mode 100644 index 0000000..02825bf --- /dev/null +++ b/docker/ros_entrypoint.sh @@ -0,0 +1,64 @@ +#!/usr/bin/env bash +set -euo pipefail + +: "${ROS_DISTRO:=humble}" +: "${ROS_WS:=/ws}" +: "${MAVINSIGHT_LAUNCH:=launch_all_vehicles.launch.py}" +: "${START_UXRCE_AGENT:=1}" +: "${UXRCE_DDS_TRANSPORT:=udp4}" +: "${UXRCE_DDS_PORT:=8888}" +: "${FOXGLOVE_PORT:=8765}" +: "${START_MAVROS:=1}" +: "${MAVROS_NAMESPACE:=uas4}" +: "${MAVROS_FCU_URL:=udp://:14540@127.0.0.1:14580}" +: "${MAVROS_GCS_URL:=}" + +# Avoid nounset failures from generated setup scripts. +set +u +export PYTHONPATH="${PYTHONPATH:-/opt/ros/humble/lib/python3.10/site-packages}" +export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:-}" +source "/opt/ros/${ROS_DISTRO}/setup.bash" +set -u + +if [ "${SKIP_COLCON_BUILD:-0}" != "1" ]; then + cd "${ROS_WS}" + colcon build --symlink-install +fi + +set +u +source "${ROS_WS}/install/setup.bash" +set -u + +pids=() + +shutdown() { + if [ "${#pids[@]}" -gt 0 ]; then + kill "${pids[@]}" 2>/dev/null || true + wait "${pids[@]}" 2>/dev/null || true + fi +} + +if [ "${START_UXRCE_AGENT}" != "0" ]; then + MicroXRCEAgent "${UXRCE_DDS_TRANSPORT}" -p "${UXRCE_DDS_PORT}" & + pids+=("$!") +fi + +if [ "${START_MAVROS}" != "0" ]; then + mavros_args=( + "namespace:=${MAVROS_NAMESPACE}" + "fcu_url:=${MAVROS_FCU_URL}" + ) + if [ -n "${MAVROS_GCS_URL}" ]; then + mavros_args+=("gcs_url:=${MAVROS_GCS_URL}") + fi + ros2 launch mavros px4.launch "${mavros_args[@]}" & + pids+=("$!") +fi + +ros2 launch mavinsight "${MAVINSIGHT_LAUNCH}" & +pids+=("$!") +ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:="${FOXGLOVE_PORT}" & +pids+=("$!") + +trap shutdown SIGINT SIGTERM EXIT +wait -n "${pids[@]}" diff --git a/launch/launch_all_vehicles_no_tba.launch.py b/launch/launch_all_vehicles_no_tba.launch.py new file mode 100644 index 0000000..6784859 --- /dev/null +++ b/launch/launch_all_vehicles_no_tba.launch.py @@ -0,0 +1,121 @@ +# python imports +from pathlib import Path +import yaml + +# ROS imports +from ament_index_python import get_package_share_directory +from launch import LaunchDescription, logging +from launch_ros.actions import Node + +package_name = "mavinsight" +namespace = "viz" +LOGGER = logging.get_logger("vehicle_launch_logger") +# Variant of launch_all_vehicles that excludes the TBA viz config. +initial_paths_overrides = ["chimera_d_4_no_tba.yaml", "c2_c130_crash.yaml"] + +def generate_launch_description(): + ld = LaunchDescription() + + share_dir = Path(get_package_share_directory(package_name)) + shared_resources = share_dir / "package_resources" + + global_config = shared_resources / "global_node_config.yaml" + + # TODO change behavior for empty initial paths override + initial_paths = [(shared_resources) / p for p in initial_paths_overrides] + + LOGGER.info(f"Initial paths: {[p.name for p in initial_paths]}") + + nodes = build_nodes(initial_paths, global_config) + for node in nodes: + ld.add_action(node) + + return ld + +def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: + LOGGER.debug("Starting build") + # initialize set of processed paths and output list + processed = set() + node_list = [] + + while paths: + # capture and error check next path + config_path = paths.pop() + LOGGER.info(f"Starting processing on {config_path.as_posix()}") + assert isinstance(config_path, Path), f"Unrecognized build_nodes input type." + if config_path.suffix != ".yaml": + LOGGER.error(f"Non-yaml config file detected: {config_path.as_posix()}. GraphMember configs must be yaml-encoded.\nSkipping...") + continue + if config_path in processed: + LOGGER.error(f"Potential circular path detected in config files.\nConfig file: {config_path.as_posix()} is contained by a sub-member.\nSkipping...") + continue + LOGGER.debug(f"non-circular path") + # path is checkable, add to processed list + processed.add(config_path) + + # resolve filename to absolute path in either sensor config or vehicle config + try: + abs_path = resolve_config_file(config_path) + except FileExistsError: + LOGGER.error(f"Duplicate filenames in Vehicle + Sensor dirs for file: {config_path.as_posix()}.\nSkipping...") + continue + if abs_path is None: + LOGGER.error(f"Cannot find file: {config_path.as_posix()} in any MAVInsight config folder.\nSkipping...") + continue + LOGGER.debug(f"abs path acquired") + + # open file and confirm yaml encoding + with open(abs_path.as_posix(), "r", encoding="utf-8") as f: + config = yaml.safe_load(f) + if type(config) is not dict: + LOGGER.error(f"Error parsing file: {abs_path.as_posix()} as yaml. GraphMember configs must be yaml-encoded.\nSkipping...") + continue + LOGGER.debug(f"file opened successfully") + + # parse yaml down to the param layer (remove the layers of nesting above params) + while len(config.keys()) == 1: + config = config[next(iter(config))] + LOGGER.debug(f"Base yaml acquired") + + # select the correct executable for this config file + try: + ex = config['executable'] + except KeyError as e: + LOGGER.error(f"Config file: {abs_path.as_posix()} contains no executable param.\nSkipping...") + continue + LOGGER.debug(f"File type identified") + + # create Node action for launch description + node_list.append(Node( + package=package_name, + executable=ex, + name=abs_path.stem, + namespace=namespace, + parameters=[global_config.as_posix(), abs_path.as_posix()], + output="screen", + )) + + # add sub-members to list of nodes to be built + sensors = config.get('sensors', []) + if len(sensors) > 0: + LOGGER.info(f"Adding new sensor files: {sensors}") + for sens in config.get("sensors", []): + paths.append(Path(sens)) + + vizs = config.get('viz', []) + if len(vizs) > 0: + LOGGER.info(f"Adding new visualization files: {vizs}") + for viz in config.get("viz", []): + paths.append(Path(viz)) + + return node_list + +def resolve_config_file(path: Path) -> Path | None: + if path.is_absolute(): + return path + + package_configs = Path(get_package_share_directory(package_name)) / "package_resources" + resolved_path = package_configs / path + if not resolved_path.is_file(): + raise FileNotFoundError(f"Could not find configs for: {path} in mavinsight configs folder.") + return resolved_path diff --git a/vehicles/chimera_d_4_no_tba.yaml b/vehicles/chimera_d_4_no_tba.yaml new file mode 100644 index 0000000..9950a4e --- /dev/null +++ b/vehicles/chimera_d_4_no_tba.yaml @@ -0,0 +1,20 @@ +/**/*: + chimera_d_4_no_tba: + ros__parameters: + coord_frame: enu_flu + display_name: Chimera D4 + ekf_origin_fix_topic: /uas4/ekf_origin/fix + ekf_origin_frame: uas4_ekf_origin + executable: vehicle + frame_name: d4_base_link + home_fix_topic: "/uas4/home_position/fix" + home_frame_name: "uas4_home_position" + home_position_topic: "/uas4/home_position/home" + namespace: "/uas4/" + location_topic: "/uas4/local_position/pose" + altitude_topic: "/uas4/altitude" + parent_frame: uas4_ekf_origin + platform: quad + velocity_topic: "uas4/local_position/velocity_local" + sensors: + - two_axis_gimbal.yaml