├── .github └── workflows │ ├── ci-format.yaml │ └── ci.yaml ├── .gitignore ├── .pre-commit-config.yaml ├── BETAFLIGHT_README.md ├── CONTRIBUTING.md ├── Docker ├── dev │ ├── Dockerfile │ └── entrypoint.sh └── nvidia_jetson │ └── Dockerfile ├── LICENSE ├── README.md ├── _images ├── betaflight-simulation.drawio └── betaflight-simulation.drawio.png ├── betaflight_configurator ├── CMakeLists.txt ├── cmake │ └── 99_betaflight_configurator_setup.sh.in └── package.xml ├── betaflight_controller ├── CMakeLists.txt ├── package.xml └── src │ ├── SocketBetaflight.cpp │ ├── SocketBetaflight.hpp │ └── main.cpp ├── betaflight_demo ├── CMakeLists.txt ├── launch │ └── betaflight_demo.launch.py ├── package.xml └── src │ └── altitude_control.cpp ├── betaflight_gazebo ├── CMakeLists.txt ├── include │ └── betaflight_gazebo │ │ ├── BetaflightPlugin.hpp │ │ ├── BetaflightSocket.hpp │ │ └── Util.hpp ├── package.xml └── src │ ├── BetaflightPlugin.cpp │ ├── BetaflightSocket.cpp │ └── Util.cpp ├── betaflight_sim ├── CMakeLists.txt ├── README.md ├── cmake │ └── 99_betaflight_setup.sh.in ├── config │ └── eeprom.bin ├── launch │ ├── quadcopter.launch.py │ └── quadcopter_joystick.launch.py ├── models │ └── iris_with_standoffs │ │ ├── meshes │ │ ├── iris.dae │ │ ├── iris_prop_ccw.dae │ │ └── iris_prop_cw.dae │ │ ├── model.config │ │ └── model.sdf ├── package.xml └── worlds │ └── empty_betaflight_world.sdf ├── build_gazebo_from_source.md ├── dependencies.repos ├── gz_aerial_plugins ├── CMakeLists.txt ├── package.xml └── src │ ├── AttitudeWidget.qml │ ├── CompassWidget.qml │ ├── DroneHmi.config │ ├── DroneHmi.cpp │ ├── DroneHmi.hpp │ ├── DroneHmi.qml │ └── DroneHmi.qrc ├── px4_sim ├── CMakeLists.txt ├── README.md ├── cmake │ └── 99_px4_setup.sh.in ├── config │ ├── multi.yaml │ └── two_isolated.yaml ├── dds_topics.yaml ├── launch │ ├── px4_sim.launch.py │ └── px4_sim_multi.launch.py └── package.xml ├── qgroundcontrol ├── CMakeLists.txt └── package.xml ├── vehicle_gateway ├── CMakeLists.txt ├── include │ └── vehicle_gateway │ │ └── vehicle_gateway.hpp └── package.xml ├── vehicle_gateway_betaflight ├── CMakeLists.txt ├── README.md ├── include │ └── vehicle_gateway_betaflight │ │ └── vehicle_gateway_betaflight.hpp ├── package.xml ├── plugins.xml └── src │ ├── vehicle_gateway_betaflight.cpp │ └── vehicle_gateway_betaflight_main.cpp ├── vehicle_gateway_demo ├── CMakeLists.txt ├── README.md ├── config │ ├── config.rviz │ ├── leader_follower_multicast_discovery.json │ ├── single_marker_tracker.yaml │ └── two_vtols.yaml ├── launch │ └── px4_demo.launch.py ├── package.xml └── src │ ├── aruco_demo.cpp │ ├── circles.cpp │ ├── follower.cpp │ └── vtol_position_control.cpp ├── vehicle_gateway_integration_test ├── CMakeLists.txt ├── package.xml └── test │ ├── CMakeLists.txt │ ├── script │ ├── arm_disarm.py │ ├── takeoff_land.py │ └── takeoff_land_multirobot.py │ ├── test_px4.py │ └── test_px4_multirobot.py ├── vehicle_gateway_models ├── CMakeLists.txt ├── configs_px4 │ ├── rc_cessna_stock │ │ ├── model.config │ │ └── model.sdf │ ├── standard_vtol_camera │ │ ├── model.config │ │ └── model.sdf │ ├── standard_vtol_stock │ │ ├── model.config │ │ └── model.sdf │ ├── x500_camera │ │ ├── model.config │ │ └── model.sdf │ └── x500_stock │ │ ├── model.config │ │ └── model.sdf ├── models │ ├── aruco_marker │ │ ├── materials │ │ │ └── aruco_4x4_50_id_1.png │ │ ├── model.config │ │ └── model.sdf │ ├── camera │ │ ├── model.config │ │ └── model.sdf │ └── standard_vtol_camera │ │ ├── model.config │ │ └── model.sdf └── package.xml ├── vehicle_gateway_multi ├── CMakeLists.txt ├── README.md ├── config │ └── zenoh_all_localhost.json5 ├── package.xml └── src │ ├── vehicle_gateway_multi_bridge.cpp │ └── vehicle_gateway_multi_bridge_client.cpp ├── vehicle_gateway_px4 ├── CMakeLists.txt ├── include │ └── vehicle_gateway_px4 │ │ └── vehicle_gateway_px4.hpp ├── launch │ └── px4.launch.xml ├── package.xml ├── plugins.xml └── src │ └── vehicle_gateway_px4.cpp ├── vehicle_gateway_python ├── CMakeLists.txt ├── examples │ ├── mc_to_fw_to_mc.py │ ├── mc_to_fw_to_offboard.py │ ├── param_tuning_attempts │ │ └── 4004_gz_standard_vtol │ ├── position_control.py │ ├── test_takeoff_land.py │ ├── velocity_control.py │ ├── vtol_body_rates.py │ └── vtol_position_control.py ├── package.xml ├── src │ └── vehicle_gateway_python │ │ ├── _vehicle_gateway_pybind11.cpp │ │ ├── destroyable.cpp │ │ ├── destroyable.hpp │ │ ├── exceptions.hpp │ │ ├── vehicle_gateway.cpp │ │ └── vehicle_gateway.hpp └── vehicle_gateway │ ├── __init__.py │ └── impl │ ├── __init__.py │ └── implementation_singleton.py ├── vehicle_gateway_python_helpers ├── package.xml ├── resource │ └── vehicle_gateway_python_helpers ├── setup.cfg ├── setup.py ├── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py └── vehicle_gateway_python_helpers │ ├── __init__.py │ └── helpers.py ├── vehicle_gateway_sim_performance ├── CMakeLists.txt ├── COLCON_IGNORE ├── include │ ├── linux_cpu_system_measurement.hpp │ └── linux_memory_system_measurement.hpp ├── package.xml ├── script │ └── show_data.bash ├── src │ ├── linux_cpu_system_measurement.cpp │ ├── linux_memory_system_measurement.cpp │ ├── main.cpp │ └── vtol_position_control.cpp └── test │ ├── CMakeLists.txt │ ├── common.py │ ├── test_px4_multirobot.py │ └── test_px4_multirobot_multiple_domain_id.py └── vehicle_gateway_worlds ├── CMakeLists.txt ├── README.md ├── package.xml └── worlds ├── aruco_px4_world.sdf ├── betaflight_iris_aruco.sdf ├── empty_px4_world.sdf └── null_island.sdf /.github/workflows/ci-format.yaml: -------------------------------------------------------------------------------- 1 | # This is a format job. Pre-commit has a first-party GitHub action, so we use 2 | # that: https://github.com/pre-commit/action 3 | 4 | name: Format 5 | 6 | on: 7 | workflow_dispatch: 8 | pull_request: 9 | 10 | jobs: 11 | pre-commit: 12 | name: Format 13 | runs-on: ubuntu-22.04 14 | steps: 15 | - uses: actions/checkout@v3 16 | - uses: actions/setup-python@v2 17 | with: 18 | python-version: 3.10.6 19 | - name: Install system hooks 20 | run: sudo apt install -qq cppcheck ament-cmake-uncrustify ament-cmake-pep257 21 | - uses: pre-commit/action@v2.0.3 22 | with: 23 | extra_args: --all-files --hook-stage manual 24 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: build 2 | on: 3 | push: 4 | schedule: 5 | - cron: "0 0 * * *" 6 | defaults: 7 | run: 8 | shell: bash 9 | jobs: 10 | build_and_test: 11 | name: Build and test 12 | runs-on: ubuntu-latest 13 | container: 14 | image: rostooling/setup-ros-docker:ubuntu-jammy-latest 15 | steps: 16 | - name: Setup ROS 2 17 | uses: ros-tooling/setup-ros@v0.3 18 | with: 19 | required-ros-distributions: humble 20 | - name: Checkout repository 21 | uses: actions/checkout@v3 22 | - name: Install rust 23 | uses: actions-rs/toolchain@v1 24 | with: 25 | toolchain: stable 26 | default: true 27 | override: true 28 | components: rustfmt, clippy 29 | - name: Test rust tools 30 | run: | 31 | rustc --version 32 | cargo --version 33 | - name: Install gazebo 34 | run: | 35 | export DEBIAN_FRONTEND=noninteractive 36 | apt update -qq 37 | apt install -qq -y lsb-release wget curl gnupg2 python3-kconfiglib python3-jinja2 python3-jsonschema ros-humble-gps-msgs gcc-arm-none-eabi libfuse2 python3-pip python3-future rsync python3-genmsg 38 | wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 39 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 40 | apt-get update && apt-get install -qq -y gz-garden 41 | - name: Build and test 42 | uses: ros-tooling/action-ros-ci@v0.3 43 | with: 44 | package-name: 45 | vcs-repo-file-url: | 46 | $GITHUB_WORKSPACE/dependencies.repos 47 | target-ros2-distro: humble 48 | rosdep-skip-keys: gz-transport12 gz-common5 gz-math7 gz-msgs9 gz-gui7 gz-cmake3 gz-sim7 49 | colcon-defaults: | 50 | { 51 | "build": { 52 | "cmake-args": [ 53 | "-DSKIP_QGROUNDCONTROL=1" 54 | ] 55 | }, 56 | "test": { 57 | "retest-until-pass": 5 58 | }, 59 | "test": { 60 | "packages-select-regex": [ 61 | "vehicle_gateway" 62 | ] 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.py[cod] 3 | install 4 | build/ 5 | log 6 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | repos: 16 | # Standard hooks 17 | - repo: https://github.com/pre-commit/pre-commit-hooks 18 | rev: v4.1.0 19 | hooks: 20 | - id: check-added-large-files 21 | - id: check-ast 22 | - id: check-case-conflict 23 | - id: check-docstring-first 24 | - id: check-merge-conflict 25 | - id: check-symlinks 26 | - id: check-xml 27 | - id: check-yaml 28 | - id: debug-statements 29 | - id: end-of-file-fixer 30 | - id: mixed-line-ending 31 | - id: trailing-whitespace 32 | exclude_types: [rst] 33 | - id: fix-byte-order-marker 34 | 35 | 36 | # Python hooks 37 | - repo: https://github.com/asottile/pyupgrade 38 | rev: v2.31.1 39 | hooks: 40 | - id: pyupgrade 41 | args: [--py36-plus] 42 | 43 | # # PEP 257 44 | - repo: local 45 | hooks: 46 | - id: ament_pep257 47 | name: ament_pep257 48 | description: Format files with pep257. 49 | entry: ament_pep257 50 | language: system 51 | args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] 52 | 53 | - repo: https://github.com/pycqa/flake8 54 | rev: 4.0.1 55 | hooks: 56 | - id: flake8 57 | args: ["--extend-ignore=E501"] 58 | 59 | # CPP hooks 60 | - repo: local 61 | hooks: 62 | - id: ament_uncrustify 63 | name: ament_uncrustify 64 | description: Format files with uncrustify. 65 | entry: ament_uncrustify 66 | language: system 67 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 68 | args: ["--reformat"] 69 | 70 | - repo: local 71 | hooks: 72 | - id: ament_cppcheck 73 | name: ament_cppcheck 74 | description: Static code analysis of C/C++ files. 75 | stages: [commit] 76 | entry: ament_cppcheck 77 | language: system 78 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 79 | 80 | # Maybe use https://github.com/cpplint/cpplint instead 81 | - repo: local 82 | hooks: 83 | - id: ament_cpplint 84 | name: ament_cpplint 85 | description: Static code analysis of C/C++ files. 86 | stages: [commit] 87 | entry: ament_cpplint 88 | language: system 89 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 90 | args: ["--linelength=100", "--filter=-whitespace/newline"] 91 | 92 | # Cmake hooks 93 | - repo: local 94 | hooks: 95 | - id: ament_lint_cmake 96 | name: ament_lint_cmake 97 | description: Check format of CMakeLists.txt files. 98 | stages: [commit] 99 | entry: ament_lint_cmake 100 | language: system 101 | files: CMakeLists\.txt$ 102 | 103 | # Copyright 104 | - repo: local 105 | hooks: 106 | - id: ament_copyright 107 | name: ament_copyright 108 | description: Check if copyright notice is available in all files. 109 | stages: [commit] 110 | entry: ament_copyright 111 | language: system 112 | 113 | # Docs - RestructuredText hooks 114 | - repo: https://github.com/PyCQA/doc8 115 | rev: 0.10.1 116 | hooks: 117 | - id: doc8 118 | args: ['--max-line-length=100', '--ignore=D001'] 119 | exclude: CHANGELOG\.rst$ 120 | 121 | - repo: https://github.com/pre-commit/pygrep-hooks 122 | rev: v1.9.0 123 | hooks: 124 | - id: rst-backticks 125 | exclude: CHANGELOG\.rst$ 126 | - id: rst-directive-colons 127 | - id: rst-inline-touching-normal 128 | 129 | # Spellcheck in comments and docs 130 | # skipping of *.svg files is not working... 131 | - repo: https://github.com/codespell-project/codespell 132 | rev: v2.1.0 133 | hooks: 134 | - id: codespell 135 | args: ['--write-changes'] 136 | exclude: CHANGELOG\.rst|\.(svg|pyc)$ 137 | -------------------------------------------------------------------------------- /BETAFLIGHT_README.md: -------------------------------------------------------------------------------- 1 | # Betaflight 2 | 3 | The Gazebo support for Betaflight was added in this [PR](https://github.com/betaflight/betaflight/pull/12346). 4 | It's already available in `master` branch. 5 | 6 | ![SITL Diagram](_images/betaflight-simulation.drawio.png) 7 | 8 | ## Packages available 9 | 10 | - **betaflight_configurator**: This package download and install in the workspace the 11 | ground control station. To connect with the SITL please use "Manual" and "Port: tcp://127.0.0.1:5761" 12 | 13 | **IMPORTANT**: you need to modify the settings of the Betaflight SITL to enable arming and the motors. In order to do this you'll need to run the file `betaflight_SITL.elf` located in `~/vg/install/betaflight_sim/share/betaflight_sim/bin/betaflight_SITL.elf` and connect to it through the program `Betaflight Configurator`: 14 | 15 | 1. Open `Betaflight Configurator` 16 | 2. In the `Port` field on the top bar insert the address `tcp://127.0.0.1:5761` and click `Connect` 17 | 3. In the side tabs you need to configure the following: 18 | - `Modes`: Configure arm in `AUX1` (range `1500` - `2100`) 19 | - `Motors`: Select `PWM` and enable `3D` 20 | Remember to click `Save` in order to commit the changes. You can shut down and reopen `betaflight_SITL.elf` to check that the settings have been saved in `Betaflight Configurator`. 21 | 4. Once you have verified that the changes have been committed you will need to copy the configuration file `eeprom.bin` that has been generated from `~/vg/install/betaflight_sim/share/betaflight_sim/bin/eeprom.bin` to `~/vg/install/betaflight_sim/share/betaflight_sim/config/eeprom.bin`: 22 | 23 | ```bash 24 | cp ~/vg/install/betaflight_sim/share/betaflight_sim/bin/eeprom.bin ~/vg/install/betaflight_sim/share/betaflight_sim/config/eeprom.bin 25 | ``` 26 | 27 | - **betaflight_controller**: When the drone is connected to SITL this requires an RC, otherwise 28 | the flight controller will generate a failsafe (RX_FAILSAFE). This program will open a 29 | UDP socket in the port 9004. Values are between [1000 - 2000] 30 | - Connect a joystick 31 | - You need to run `ros2 run joy joy_node` 32 | - And the `ros2 run betaflight_controller betaflight_joy_controller`. 33 | 34 | - **betaflight_gazebo**: This plugin will send the state data to the SITL and it will received 35 | the data from the motors [-1, 1]. 36 | - Sending state output to SITL at UDP link: 127.0.0.1:9002 37 | - Receiving PWM from SITL at UDP server: 127.0.0.1:9003 38 | 39 | - **betaflight_sim**: Download and install the betaflight flight controller. 40 | 41 | ## Run the simulation 42 | 43 | ```bash 44 | ros2 launch betaflight_sim quadcopter.launch.py 45 | ``` 46 | 47 | ## Test 48 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /Docker/dev/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | ENV DEBIAN_FRONTEND=noninteractive 4 | RUN apt-get update \ 5 | && apt-get install -y -qq \ 6 | python3-pip \ 7 | python3-kconfiglib \ 8 | python3-jinja2 \ 9 | python3-jsonschema \ 10 | python3-future \ 11 | gcc-arm-none-eabi \ 12 | rsync \ 13 | dirmngr \ 14 | build-essential \ 15 | lsb-release \ 16 | wget \ 17 | curl \ 18 | gnupg2 \ 19 | qtbase5-dev \ 20 | cmake \ 21 | git \ 22 | && apt-get clean 23 | 24 | RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg && \ 25 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null && \ 26 | apt-get update && \ 27 | apt-get install -y -qq gz-garden \ 28 | && apt-get clean 29 | 30 | RUN pip3 install vcstool pyros-genmsg 31 | 32 | RUN mkdir -p /home/vehicle_gateway/src && cd /home/vehicle_gateway && git clone https://github.com/osrf/vehicle_gateway src/vehicle_gateway --depth 1 && \ 33 | vcs import src < src/vehicle_gateway/dependencies.repos 34 | 35 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ 36 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $(lsb_release -cs)) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null & \ 37 | wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg && \ 38 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 39 | 40 | RUN apt-get update && apt-get upgrade -q -y && \ 41 | apt -y install python3-rosdep python3-colcon-common-extensions \ 42 | $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/gz\|sdf/d' | tr '\n' ' ') \ 43 | && apt-get clean 44 | 45 | RUN rosdep init && \ 46 | rosdep update && \ 47 | cd /home/vehicle_gateway && \ 48 | rosdep install --from-paths ./ -i -y --rosdistro humble --ignore-src --skip-keys="gz-transport12 gz-common5 gz-math7 gz-msgs9 gz-gui7 gz-cmake3 gz-sim7 zenohc gz-transport7 gz-plugin2" 49 | 50 | RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y && \ 51 | echo 'export PATH="$HOME/.cargo/bin:$PATH"' >> $HOME/.profile 52 | 53 | ENV PATH="/root/.cargo/bin:${PATH}" 54 | 55 | RUN rustup update 56 | 57 | RUN cd /home/vehicle_gateway && . /opt/ros/humble/local_setup.sh && colcon build --merge-install --event-handlers console_direct+ --cmake-args -DBUILD_TESTING=0 58 | 59 | RUN apt-get install -y -q ros-humble-ros2launch 60 | COPY entrypoint.sh /entrypoint.sh 61 | ENTRYPOINT ["/entrypoint.sh"] 62 | -------------------------------------------------------------------------------- /Docker/dev/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | . /opt/ros/humble/setup.sh 4 | . /home/vehicle_gateway/install/setup.sh 5 | exec "$@" 6 | -------------------------------------------------------------------------------- /Docker/nvidia_jetson/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM dustynv/ros:humble-desktop-l4t-r35.3.1 2 | 3 | # Or use the following image for the full desktop version (includes rviz, demos, tutorials, etc.): 4 | # FROM dustynv/ros:humble-ros-base-l4t-r32.7.1 5 | 6 | ENV VEHICLE_GATEWAY_WS /root/vg 7 | WORKDIR ${VEHICLE_GATEWAY_WS} 8 | 9 | # Install general dependencies 10 | RUN \ 11 | apt-get update && \ 12 | apt-get install -y curl && \ 13 | curl https://sh.rustup.rs -sSf | sh -s -- -y && \ 14 | apt-get install -y \ 15 | python3-jinja2 \ 16 | python3-jsonschema \ 17 | gcc-arm-none-eabi \ 18 | libfuse2 \ 19 | python3-pip \ 20 | git \ 21 | python3-vcstool \ 22 | python3-future \ 23 | rsync \ 24 | && \ 25 | pip3 install \ 26 | pyros-genmsg \ 27 | kconfiglib \ 28 | && \ 29 | apt-get clean 30 | 31 | # Get source code for the vehicle gateway workspace 32 | RUN \ 33 | mkdir -p ${VEHICLE_GATEWAY_WS}/src && \ 34 | cd ${VEHICLE_GATEWAY_WS}/src && \ 35 | git clone https://github.com/osrf/vehicle_gateway && \ 36 | cd ${VEHICLE_GATEWAY_WS} && \ 37 | vcs import src < src/vehicle_gateway/dependencies.repos 38 | 39 | # Install dependencies for the vehicle gateway workspace 40 | RUN \ 41 | cd ${VEHICLE_GATEWAY_WS} && \ 42 | . /opt/ros/humble/install/setup.bash && \ 43 | rosdep update && \ 44 | rosdep install \ 45 | --from-paths src \ 46 | --ignore-src \ 47 | -y \ 48 | --skip-keys="gz-transport12 gz-common5 gz-math7 gz-msgs9 gz-gui7 gz-cmake3 gz-sim7 zenohc xacro sdformat_urdf image_transport_plugins control_toolbox gps_msgs aruco_opencv_msgs aruco_opencv joy rviz2 rqt_topic rqt_plot rqt_image_view" 49 | 50 | # Build workspace 51 | # 52 | # See which packages will be built: 53 | # $ colcon graph --packages-up-to vehicle_gateway_python vehicle_gateway_models vehicle_gateway_multi 54 | # > px4_msgs + *. 55 | # > vehicle_gateway + **. 56 | # > vehicle_gateway_models + 57 | # > zenohc +* 58 | # > vehicle_gateway_multi + 59 | # > vehicle_gateway_px4 +* 60 | # > vehicle_gateway_python + 61 | RUN \ 62 | . /ros_entrypoint.sh && \ 63 | . "${HOME}/.cargo/env" && \ 64 | colcon build \ 65 | --event-handlers console_direct+ \ 66 | --packages-up-to \ 67 | vehicle_gateway_python \ 68 | vehicle_gateway_models \ 69 | vehicle_gateway_multi 70 | -------------------------------------------------------------------------------- /_images/betaflight-simulation.drawio: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /_images/betaflight-simulation.drawio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/vehicle_gateway/cfe101602d84192286de2626650cda1a8e81f971/_images/betaflight-simulation.drawio.png -------------------------------------------------------------------------------- /betaflight_configurator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(betaflight_configurator) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | include(ExternalProject) 7 | 8 | if(NOT EXISTS "${CMAKE_BINARY_DIR}/betaflight-configurator_10.9.0_linux64-portable.zip") 9 | file(DOWNLOAD 10 | https://github.com/betaflight/betaflight-configurator/releases/download/10.9.0/betaflight-configurator_10.9.0_linux64-portable.zip 11 | betaflight-configurator_10.9.0_linux64-portable.zip 12 | SHOW_PROGRESS 13 | ) 14 | file(ARCHIVE_EXTRACT 15 | INPUT ${CMAKE_BINARY_DIR}/betaflight-configurator_10.9.0_linux64-portable.zip 16 | DESTINATION ${PROJECT_BINARY_DIR}/betaflight-configurator 17 | ) 18 | endif() 19 | 20 | install(DIRECTORY 21 | "${PROJECT_BINARY_DIR}/betaflight-configurator/Betaflight Configurator/" 22 | DESTINATION 23 | ${CMAKE_INSTALL_PREFIX}/share/betaflight_configurator/bin 24 | USE_SOURCE_PERMISSIONS 25 | ) 26 | 27 | ament_environment_hooks(cmake/99_betaflight_configurator_setup.sh.in) 28 | 29 | ament_package() 30 | -------------------------------------------------------------------------------- /betaflight_configurator/cmake/99_betaflight_configurator_setup.sh.in: -------------------------------------------------------------------------------- 1 | ament_prepend_unique_value PATH "@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/bin/" 2 | -------------------------------------------------------------------------------- /betaflight_configurator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | betaflight_configurator 4 | 0.0.0 5 | The betaflight_configurator package 6 | 7 | Alejandro Hernández 8 | 9 | Apache License 2.0 10 | 11 | https://betaflight.com/ 12 | 13 | 14 | ament_cmake 15 | 16 | 17 | -------------------------------------------------------------------------------- /betaflight_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) 2 | 3 | #============================================================================ 4 | # Initialize the project 5 | #============================================================================ 6 | project(betaflight_controller) 7 | 8 | #------------------------------------------------------------------------ 9 | # Compile as C++17 10 | 11 | set(CMAKE_CXX_STANDARD 17) 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | 22 | add_executable(betaflight_joy_controller 23 | src/main.cpp 24 | src/SocketBetaflight.cpp 25 | ) 26 | ament_target_dependencies(betaflight_joy_controller 27 | rclcpp 28 | sensor_msgs 29 | ) 30 | 31 | install(TARGETS 32 | betaflight_joy_controller 33 | DESTINATION lib/${PROJECT_NAME} 34 | ) 35 | 36 | if(BUILD_TESTING) 37 | find_package(ament_lint_auto REQUIRED) 38 | # the following line skips the linter which checks for copyrights 39 | # comment the line when a copyright and license is added to all source files 40 | # set(ament_cmake_copyright_FOUND TRUE) 41 | # the following line skips cpplint (only works in a git repo) 42 | # comment the line when this package is in a git repo and when 43 | # a copyright and license is added to all source files 44 | # set(ament_cmake_cpplint_FOUND TRUE) 45 | ament_lint_auto_find_test_dependencies() 46 | endif() 47 | 48 | ament_package() 49 | -------------------------------------------------------------------------------- /betaflight_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | betaflight_controller 4 | 0.0.0 5 | betaflight controller 6 | Alejandro Hernández 7 | Alejandro Hernández 8 | Apache 2 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | sensor_msgs 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /betaflight_controller/src/SocketBetaflight.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "SocketBetaflight.hpp" 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | SocketBetaflight::SocketBetaflight(const std::string & _addr, int _port, bool _isServer) 22 | { 23 | this->addr = _addr; 24 | this->isServer = _isServer; 25 | this->port = _port; 26 | } 27 | 28 | int SocketBetaflight::init() 29 | { 30 | int one = 1; 31 | 32 | if ((this->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { 33 | return -2; 34 | } 35 | 36 | setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind 37 | fcntl(this->fd, F_SETFL, fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock 38 | 39 | this->isServer = isServer; 40 | memset(&this->si, 0, sizeof(this->si)); 41 | this->si.sin_family = AF_INET; 42 | this->si.sin_port = htons(this->port); 43 | this->port = this->port; 44 | 45 | if (this->addr.empty()) { 46 | this->si.sin_addr.s_addr = htonl(INADDR_ANY); 47 | } else { 48 | this->si.sin_addr.s_addr = inet_addr(this->addr.c_str()); 49 | } 50 | 51 | if (this->isServer) { 52 | if (bind(this->fd, (const struct sockaddr *)&this->si, sizeof(this->si)) == -1) { 53 | return -1; 54 | } 55 | } 56 | return 0; 57 | } 58 | 59 | int SocketBetaflight::udpSend(const void * data, size_t size) 60 | { 61 | return sendto(this->fd, data, size, 0, (struct sockaddr *)&this->si, sizeof(this->si)); 62 | } 63 | 64 | int SocketBetaflight::udpRecv(void * data, size_t size, uint32_t timeout_ms) 65 | { 66 | fd_set fds; 67 | struct timeval tv; 68 | 69 | FD_ZERO(&fds); 70 | FD_SET(this->fd, &fds); 71 | 72 | tv.tv_sec = timeout_ms / 1000; 73 | tv.tv_usec = (timeout_ms % 1000) * 1000UL; 74 | int ret_select = select(this->fd + 1, &fds, NULL, NULL, &tv); 75 | if (ret_select != 1) { 76 | printf("Error %d %d\n", this->fd + 1, ret_select); 77 | return -1; 78 | } 79 | 80 | socklen_t len = sizeof(this->si); 81 | int ret; 82 | ret = recvfrom(this->fd, data, size, 0, (struct sockaddr *)&this->si, &len); 83 | return ret; 84 | } 85 | -------------------------------------------------------------------------------- /betaflight_controller/src/SocketBetaflight.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOCKETBETAFLIGHT_HPP_ 16 | #define SOCKETBETAFLIGHT_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | class SocketBetaflight 27 | { 28 | public: 29 | SocketBetaflight() {} 30 | SocketBetaflight(const std::string & addr, int port, bool isServer); 31 | 32 | int init(); 33 | int udpRecv(void * data, size_t size, uint32_t timeout_ms); 34 | int udpSend(const void * data, size_t size); 35 | 36 | private: 37 | int fd; 38 | struct sockaddr_in si; 39 | int port; 40 | std::string addr; 41 | bool isServer; 42 | }; 43 | 44 | #endif // SOCKETBETAFLIGHT_HPP_ 45 | -------------------------------------------------------------------------------- /betaflight_controller/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "sensor_msgs/msg/joy.hpp" 19 | 20 | #include "SocketBetaflight.hpp" 21 | 22 | using std::placeholders::_1; 23 | #define SIMULATOR_MAX_RC_CHANNELS 16 24 | #define SIMULATOR_MAX_PWM_CHANNELS 16 25 | 26 | typedef struct 27 | { 28 | double timestamp; // in seconds 29 | uint16_t channels[SIMULATOR_MAX_RC_CHANNELS]; // (1000-2000) channel values 30 | } rc_packet; 31 | 32 | class MinimalSubscriber : public rclcpp::Node 33 | { 34 | public: 35 | MinimalSubscriber() 36 | : Node("minimal_subscriber") 37 | { 38 | std::string listen_addr("127.0.0.1"); 39 | uint32_t fdm_port_out = 9004; 40 | 41 | socketRC = SocketBetaflight(listen_addr, fdm_port_out, false); 42 | if (socketRC.init()) { 43 | std::cerr << "Not able to open socket" << '\n'; 44 | } 45 | 46 | subscription_ = this->create_subscription( 47 | "joy", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); 48 | } 49 | 50 | private: 51 | void topic_callback(const sensor_msgs::msg::Joy::SharedPtr msg) 52 | { 53 | rc_packet pkt; 54 | pkt.timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec / (1000 * 1000 * 1000); 55 | 56 | pkt.channels[0] = ((-1 * msg->axes[2] + 1) / 2 * 1000) + 1000; // roll 57 | pkt.channels[1] = ((msg->axes[3] + 1) / 2 * 1000) + 1000; // pitch 58 | pkt.channels[2] = ((msg->axes[1] + 1) / 2 * 1000) + 1000; // yaw 59 | pkt.channels[3] = ((msg->axes[0] + 1) / 2 * 1000) + 1000; // throtlle 60 | pkt.channels[4] = (msg->buttons[0] * 1000) + 1000; // aux1; 61 | pkt.channels[5] = (msg->buttons[1] * 1000) + 1000; // aux2; 62 | pkt.channels[6] = (msg->buttons[2] * 1000) + 1000; // aux3; 63 | pkt.channels[7] = (msg->buttons[3] * 1000) + 1000; // aux4; 64 | pkt.channels[8] = 1000; 65 | pkt.channels[9] = 1000; 66 | pkt.channels[10] = 1000; 67 | pkt.channels[11] = 1000; 68 | pkt.channels[12] = 1000; 69 | pkt.channels[13] = 1000; 70 | pkt.channels[14] = 1000; 71 | pkt.channels[15] = 1000; 72 | 73 | socketRC.udpSend(&pkt, sizeof(rc_packet)); 74 | } 75 | rclcpp::Subscription::SharedPtr subscription_; 76 | SocketBetaflight socketRC; 77 | }; 78 | 79 | int main(int argc, char * argv[]) 80 | { 81 | rclcpp::init(argc, argv); 82 | rclcpp::spin(std::make_shared()); 83 | rclcpp::shutdown(); 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /betaflight_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(betaflight_demo) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | find_package(ament_cmake) 8 | find_package(control_toolbox REQUIRED) 9 | find_package(pluginlib REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(vehicle_gateway_betaflight REQUIRED) 13 | 14 | find_package(PkgConfig REQUIRED) 15 | pkg_search_module(PKG_MSP REQUIRED IMPORTED_TARGET msp) 16 | include_directories(${PKG_MSP_INCLUDE_DIRS}) 17 | link_directories(${PKG_MSP_LIBRARY_DIRS}) 18 | 19 | add_executable(altitude_control 20 | src/altitude_control.cpp 21 | ) 22 | target_include_directories(altitude_control PUBLIC 23 | $ 24 | $) 25 | ament_target_dependencies(altitude_control 26 | control_toolbox 27 | pluginlib 28 | rclcpp 29 | std_msgs 30 | vehicle_gateway_betaflight 31 | ) 32 | target_link_libraries(altitude_control 33 | ${PKG_MSP_LIBRARIES} 34 | ) 35 | 36 | install(DIRECTORY 37 | launch 38 | DESTINATION 39 | share/${PROJECT_NAME} 40 | ) 41 | 42 | install( 43 | TARGETS altitude_control 44 | DESTINATION lib/${PROJECT_NAME} 45 | ) 46 | 47 | if(BUILD_TESTING) 48 | find_package(ament_lint_auto REQUIRED) 49 | ament_lint_auto_find_test_dependencies() 50 | endif() 51 | 52 | ament_package() 53 | -------------------------------------------------------------------------------- /betaflight_demo/launch/betaflight_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_index_python.packages import get_package_share_directory 16 | from launch import LaunchDescription 17 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 18 | from launch.actions import RegisterEventHandler 19 | from launch.launch_description_sources import PythonLaunchDescriptionSource 20 | from launch.substitutions import LaunchConfiguration 21 | from launch.event_handlers.on_process_io import OnProcessIO 22 | from launch_ros.actions import Node 23 | import os 24 | import time 25 | 26 | 27 | def get_betaflight_dir(): 28 | return get_package_share_directory('betaflight_sim') 29 | 30 | 31 | run_virtual_tty = ExecuteProcess(cmd=["socat", "-dd", "pty,link=/tmp/ttyS0,raw,echo=0", "tcp:127.0.0.1:5761"]), 32 | 33 | 34 | def _run_virtual_tty_check(event): 35 | """ 36 | Consider betaflight_controller ready when 'bind port 5761 for UART1...' string is printed. 37 | 38 | Launches betaflight_controller node if ready. 39 | """ 40 | target_str = 'bind port 5761 for UART1' 41 | if target_str in event.text.decode(): 42 | time.sleep(2) 43 | return run_virtual_tty 44 | 45 | 46 | def generate_launch_description(): 47 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 48 | 49 | # Launch Arguments 50 | use_sim_time_arg = DeclareLaunchArgument( 51 | 'use_sim_time', 52 | default_value=use_sim_time, 53 | description='If true, use simulated clock') 54 | 55 | os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.join(get_betaflight_dir(), "models") 56 | os.environ["GZ_SIM_RESOURCE_PATH"] += ":" + os.path.join(get_betaflight_dir(), "worlds") 57 | os.environ["GZ_SIM_RESOURCE_PATH"] += ":" + os.path.join(get_package_share_directory('vehicle_gateway_models'), "models") 58 | os.environ["GZ_SIM_RESOURCE_PATH"] += ":" + os.path.join(get_package_share_directory('vehicle_gateway_worlds'), "worlds") 59 | print("os.environ[GZ_SIM_RESOURCE_PATH]", os.environ["GZ_SIM_RESOURCE_PATH"]) 60 | world_name = LaunchConfiguration('world_name', default='empty_betaflight_world') 61 | world_name_arg = DeclareLaunchArgument('world_name', 62 | default_value=world_name, 63 | description='World name') 64 | 65 | gz_bridge = Node( 66 | package='ros_gz_bridge', 67 | executable='parameter_bridge', 68 | arguments=['/model/iris_with_Betaflight/model/iris_with_standoffs/pose@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V', 69 | '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 70 | remappings=[("/model/iris_with_Betaflight/model/iris_with_standoffs/pose", "/tf")], 71 | output='screen' 72 | ) 73 | 74 | run_betaflight_sitl = ExecuteProcess(cmd=['betaflight_SITL.elf', "127.0.0.1"], 75 | cwd=os.path.join(get_betaflight_dir(), "config"), 76 | output='screen') 77 | return LaunchDescription([ 78 | # Launch gazebo environment 79 | world_name_arg, 80 | use_sim_time_arg, 81 | gz_bridge, 82 | IncludeLaunchDescription( 83 | PythonLaunchDescriptionSource( 84 | [os.path.join(get_package_share_directory('ros_gz_sim'), 85 | 'launch', 'gz_sim.launch.py')]), 86 | launch_arguments=[('gz_args', [' -r -v 4 ', LaunchConfiguration('world_name'), ".sdf"])] 87 | ), 88 | run_betaflight_sitl, 89 | RegisterEventHandler( 90 | OnProcessIO( 91 | target_action=run_betaflight_sitl, 92 | on_stdout=_run_virtual_tty_check, 93 | on_stderr=_run_virtual_tty_check 94 | ) 95 | ), 96 | ]) 97 | -------------------------------------------------------------------------------- /betaflight_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | betaflight_demo 5 | 0.0.0 6 | Betaflight demo 7 | Alejandro Hernandez Cordero 8 | Apache License 2.0 9 | 10 | Alejandro Hernandez Cordero 11 | 12 | ament_cmake 13 | 14 | control_toolbox 15 | pluginlib 16 | rclcpp 17 | std_msgs 18 | vehicle_gateway 19 | vehicle_gateway_betaflight 20 | 21 | ament_index_python 22 | betaflight_sim 23 | launch 24 | launch_ros 25 | ros_gz_bridge 26 | ros_gz_sim 27 | vehicle_gateway_models 28 | vehicle_gateway_worlds 29 | 30 | ament_lint_auto 31 | ament_cmake_lint_cmake 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /betaflight_demo/src/altitude_control.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | using std::placeholders::_1; 28 | using namespace std::chrono_literals; 29 | 30 | template 31 | inline T clamp(T _v, T _min, T _max) 32 | { 33 | return std::max(std::min(_v, _max), _min); 34 | } 35 | 36 | class MinimalSubscriber : public rclcpp::Node 37 | { 38 | public: 39 | MinimalSubscriber(int argc, const char ** argv) 40 | : Node("minimal_subscriber"), loader_("vehicle_gateway", "vehicle_gateway::VehicleGateway") 41 | { 42 | this->subscription_ = this->create_subscription( 43 | "global_position/rel_alt", rclcpp::SensorDataQoS(), 44 | std::bind(&MinimalSubscriber::topic_callback, this, _1)); 45 | 46 | this->gateway_ = this->loader_.createSharedInstance( 47 | "vehicle_gateway_betaflight::VehicleGatewayBetaflight"); 48 | RCLCPP_INFO(this->get_logger(), "Initializing VehicleGatewayBetaflight"); 49 | this->gateway_->init(0, nullptr); 50 | this->thread_loop_ = std::thread(&MinimalSubscriber::loop, this); 51 | 52 | this->altitude_control_pid_ = control_toolbox::Pid(0.08, 0.02, 0.002); 53 | this->altitude_control_pid_.reset(); 54 | } 55 | 56 | void loop() 57 | { 58 | int count = 0; 59 | while (this->gateway_->get_arming_state() != vehicle_gateway::ARMING_STATE::ARMED) { 60 | if (!this->gateway_->set_body_rates_and_thrust_setpoint(0, 0, 0, -1)) { 61 | RCLCPP_ERROR(this->get_logger(), "Error sending RC"); 62 | } 63 | if (count == 1) { 64 | this->gateway_->arm(); 65 | } 66 | if (count == 100) { 67 | this->gateway_->disarm(); 68 | } 69 | if (count == 150) { 70 | count = 0; 71 | } 72 | count++; 73 | std::this_thread::sleep_for(50ms); 74 | RCLCPP_INFO( 75 | this->get_logger(), "Trying to arm vehicle %d", 76 | this->gateway_->get_arming_state()); 77 | } 78 | 79 | RCLCPP_INFO(this->get_logger(), "Vehicle is armed"); 80 | 81 | 82 | std::chrono::time_point last_update_time = 83 | std::chrono::system_clock::now(); 84 | 85 | while (1) { 86 | std::this_thread::sleep_for(50ms); 87 | 88 | double altitude; 89 | { 90 | std::lock_guard lock(mutex_altitude); 91 | altitude = this->altitude_; 92 | } 93 | std::chrono::time_point now = std::chrono::system_clock::now(); 94 | 95 | auto nanoseconds = 96 | std::chrono::duration_cast(now - last_update_time); 97 | 98 | last_update_time = now; 99 | 100 | double error = this->desired_altitude_ - altitude; 101 | double target_vel = 102 | this->altitude_control_pid_.computeCommand( 103 | error, 104 | nanoseconds.count()); 105 | target_vel = clamp(target_vel, -1.0, 1.0); 106 | RCLCPP_INFO( 107 | this->get_logger(), "target_vel %.2f desired alt: %.2f real alt: %.2f ", 108 | target_vel, this->desired_altitude_, altitude); 109 | if (!this->gateway_->set_body_rates_and_thrust_setpoint(0, 0, 0, target_vel)) { 110 | RCLCPP_ERROR(this->get_logger(), "Error sending RC"); 111 | } 112 | } 113 | } 114 | 115 | ~MinimalSubscriber() 116 | { 117 | this->thread_loop_.join(); 118 | this->gateway_->destroy(); 119 | } 120 | 121 | private: 122 | void topic_callback(const std_msgs::msg::Float64 & msg) 123 | { 124 | std::lock_guard lock(mutex_altitude); 125 | this->altitude_ = msg.data; 126 | RCLCPP_INFO(this->get_logger(), "I heard: '%.2f'", msg.data); 127 | } 128 | 129 | pluginlib::ClassLoader loader_; 130 | std::shared_ptr gateway_; 131 | rclcpp::Subscription::SharedPtr subscription_; 132 | std::thread thread_loop_; 133 | 134 | double altitude_{0}; 135 | double desired_altitude_{5}; 136 | std::mutex mutex_altitude; 137 | control_toolbox::Pid altitude_control_pid_; 138 | }; 139 | 140 | int main(int argc, const char * argv[]) 141 | { 142 | rclcpp::init(argc, argv); 143 | rclcpp::spin(std::make_shared(argc, argv)); 144 | rclcpp::shutdown(); 145 | return 0; 146 | } 147 | -------------------------------------------------------------------------------- /betaflight_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) 2 | 3 | #============================================================================ 4 | # Initialize the project 5 | #============================================================================ 6 | project(betaflight_gazebo) 7 | 8 | #------------------------------------------------------------------------ 9 | # Compile as C++17 10 | 11 | set(CMAKE_CXX_STANDARD 17) 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 13 | 14 | # Compiler options 15 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 16 | add_compile_options(-Wall -Wextra -Wpedantic) 17 | endif() 18 | 19 | #============================================================================ 20 | # Find gz-cmake 21 | #============================================================================ 22 | find_package(gz-cmake3 3.0.0 REQUIRED) 23 | set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR}) 24 | 25 | #============================================================================ 26 | # Search for project-specific dependencies 27 | #============================================================================ 28 | 29 | #-------------------------------------- 30 | # Find gz-sim 31 | gz_find_package(gz-sim7 REQUIRED) 32 | set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) 33 | 34 | gz_find_package(gz-plugin2 REQUIRED) 35 | set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) 36 | 37 | gz_find_package(gz-math7 REQUIRED) 38 | set(GZ_MATH_VER ${gz-math7_VERSION_MAJOR}) 39 | 40 | gz_find_package(gz-transport12 REQUIRED) 41 | set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR}) 42 | 43 | #====================================== 44 | # Build plugin 45 | 46 | add_library(BetaflightPlugin 47 | SHARED 48 | src/BetaflightPlugin.cpp 49 | src/BetaflightSocket.cpp 50 | src/Util.cpp 51 | ) 52 | target_include_directories(BetaflightPlugin PUBLIC 53 | include 54 | ) 55 | target_link_libraries(BetaflightPlugin PUBLIC 56 | gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} 57 | gz-plugin${GZ_PLUGIN_VER}::register 58 | gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} 59 | gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER} 60 | ) 61 | 62 | # Install directories 63 | install(TARGETS BetaflightPlugin 64 | DESTINATION lib 65 | ) 66 | 67 | find_package(ament_cmake REQUIRED) 68 | 69 | if(BUILD_TESTING) 70 | find_package(ament_lint_auto REQUIRED) 71 | ament_lint_auto_find_test_dependencies() 72 | endif() 73 | 74 | ament_package() 75 | -------------------------------------------------------------------------------- /betaflight_gazebo/include/betaflight_gazebo/BetaflightPlugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BETAFLIGHT_GAZEBO__BETAFLIGHTPLUGIN_HPP_ 16 | #define BETAFLIGHT_GAZEBO__BETAFLIGHTPLUGIN_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace betaflight_gazebo 24 | { 25 | // The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h. 26 | struct servo_packet 27 | { 28 | uint16_t magic; // 18458 expected magic value 29 | uint16_t frame_rate; 30 | uint32_t frame_count; 31 | uint16_t pwm[16]; 32 | }; 33 | 34 | class BetaFlightPluginPrivate; 35 | 36 | class GZ_SIM_VISIBLE BetaFlightPlugin 37 | : public gz::sim::System, 38 | public gz::sim::ISystemConfigure, 39 | public gz::sim::ISystemPostUpdate, 40 | public gz::sim::ISystemPreUpdate, 41 | public gz::sim::ISystemReset 42 | { 43 | /// \brief Constructor. 44 | 45 | public: 46 | BetaFlightPlugin(); 47 | 48 | /// \brief Destructor. 49 | 50 | public: 51 | ~BetaFlightPlugin(); 52 | 53 | public: 54 | void Reset( 55 | const gz::sim::UpdateInfo & _info, 56 | gz::sim::EntityComponentManager & _ecm) final; 57 | 58 | /// \brief Load configuration from SDF on startup. 59 | 60 | public: 61 | void Configure( 62 | const gz::sim::Entity & _entity, 63 | const std::shared_ptr & _sdf, 64 | gz::sim::EntityComponentManager & _ecm, 65 | gz::sim::EventManager & _eventMgr) final; 66 | 67 | /// \brief Do the part of one update loop that involves making 68 | /// changes to simulation. 69 | 70 | public: 71 | void PreUpdate( 72 | const gz::sim::UpdateInfo & _info, 73 | gz::sim::EntityComponentManager & _ecm) final; 74 | 75 | /// \brief Do the part of one update loop that involves 76 | /// reading results from simulation. 77 | 78 | public: 79 | void PostUpdate( 80 | const gz::sim::UpdateInfo & _info, 81 | const gz::sim::EntityComponentManager & _ecm) final; 82 | 83 | /// \brief Load control channels 84 | 85 | private: 86 | void LoadControlChannels( 87 | sdf::ElementPtr _sdf, 88 | gz::sim::EntityComponentManager & _ecm); 89 | 90 | /// \brief Load IMU sensors 91 | 92 | private: 93 | void LoadImuSensors( 94 | sdf::ElementPtr _sdf, 95 | gz::sim::EntityComponentManager & _ecm); 96 | 97 | /// \brief Update the control surfaces controllers. 98 | /// \param[in] _info Update information provided by the server. 99 | 100 | private: 101 | void OnUpdate(); 102 | 103 | /// \brief Update PID Joint controllers. 104 | /// \param[in] _dt time step size since last update. 105 | 106 | private: 107 | void ApplyMotorForces( 108 | const double _dt, 109 | gz::sim::EntityComponentManager & _ecm); 110 | 111 | /// \brief Reset PID Joint controllers. 112 | 113 | private: 114 | void ResetPIDs(); 115 | 116 | /// \brief Receive a servo packet from ArduPilot 117 | /// 118 | /// Returns true if a servo packet was received, otherwise false. 119 | 120 | private: 121 | bool ReceiveServoPacket(double _simTime, const gz::sim::EntityComponentManager & _ecm); 122 | 123 | /// \brief Send state to ArduPilot 124 | 125 | private: 126 | void SendState(double _simTime, const gz::sim::EntityComponentManager & _ecm) const; 127 | 128 | /// \brief Initialise flight dynamics model socket 129 | 130 | private: 131 | bool InitSockets(sdf::ElementPtr _sdf) const; 132 | 133 | /// \brief Private data pointer. 134 | 135 | private: 136 | std::unique_ptr dataPtr; 137 | }; 138 | 139 | } // namespace betaflight_gazebo 140 | 141 | #endif // BETAFLIGHT_GAZEBO__BETAFLIGHTPLUGIN_HPP_ 142 | -------------------------------------------------------------------------------- /betaflight_gazebo/include/betaflight_gazebo/BetaflightSocket.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BETAFLIGHT_GAZEBO__BETAFLIGHTSOCKET_HPP_ 16 | #define BETAFLIGHT_GAZEBO__BETAFLIGHTSOCKET_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace betaflight_gazebo 31 | { 32 | // Private data class 33 | class BetaflightSocket 34 | { 35 | /// \brief constructor 36 | 37 | public: 38 | BetaflightSocket(); 39 | 40 | /// \brief destructor 41 | 42 | public: 43 | ~BetaflightSocket(); 44 | 45 | /// \brief Bind to an address and port 46 | /// \param[in] _address Address to bind to. 47 | /// \param[in] _port Port to bind to. 48 | /// \return True on success. 49 | 50 | public: 51 | bool Bind(const char * _address, const uint16_t _port); 52 | 53 | /// \brief Connect to an address and port 54 | /// \param[in] _address Address to connect to. 55 | /// \param[in] _port Port to connect to. 56 | /// \return True on success. 57 | 58 | public: 59 | bool Connect(const char * _address, const uint16_t _port); 60 | 61 | /// \brief Make a socket 62 | /// \param[in] _address Socket address. 63 | /// \param[in] _port Socket port 64 | /// \param[out] _sockaddr New socket address structure. 65 | 66 | public: 67 | void MakeSockAddr( 68 | const char * _address, const uint16_t _port, 69 | struct sockaddr_in & _sockaddr); 70 | 71 | public: 72 | ssize_t Send(const void * _buf, size_t _size); 73 | 74 | /// \brief Receive data 75 | /// \param[out] _buf Buffer that receives the data. 76 | /// \param[in] _size Size of the buffer. 77 | /// \param[in] _timeoutMS Milliseconds to wait for data. 78 | 79 | public: 80 | ssize_t Recv(void * _buf, const size_t _size, uint32_t _timeoutMs); 81 | 82 | /// \brief Socket handle 83 | 84 | private: 85 | int fd; 86 | struct sockaddr_in recv; 87 | }; 88 | } // namespace betaflight_gazebo 89 | 90 | #endif // BETAFLIGHT_GAZEBO__BETAFLIGHTSOCKET_HPP_ 91 | -------------------------------------------------------------------------------- /betaflight_gazebo/include/betaflight_gazebo/Util.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 ardupilot.org 2 | // 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU Lesser General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | // 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU Lesser General Public License for more details. 12 | // 13 | // You should have received a copy of the GNU Lesser General Public License 14 | // along with this program. If not, see . 15 | 16 | #ifndef BETAFLIGHT_GAZEBO__UTIL_HPP_ 17 | #define BETAFLIGHT_GAZEBO__UTIL_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace betaflight_gazebo 28 | { 29 | 30 | /// \brief Helper function to get an entity given its unscoped name. 31 | /// 32 | /// \param[in] _name Entity's unscoped name. 33 | /// \param[in] _ecm Immutable reference to ECM. 34 | /// \param[in] _relativeTo Entity that the unscoped name is relative to. 35 | /// If not provided, the unscoped name could be relative to any entity. 36 | /// \return All entities that match the unscoped name and relative to 37 | /// requirements, or an empty set otherwise. 38 | std::unordered_set EntitiesFromUnscopedName( 39 | const std::string & _name, const gz::sim::EntityComponentManager & _ecm, 40 | gz::sim::Entity _relativeTo = gz::sim::kNullEntity); 41 | 42 | /// \brief Get the ID of a joint entity which is a descendent of this model. 43 | /// 44 | /// A replacement for gz::sim::Model::JointByName which does not resolve 45 | /// joints for nested models. 46 | /// \param[in] _ecm Entity-component manager. 47 | /// \param[in] _entity Model entity. 48 | /// \param[in] _name Scoped joint name. 49 | /// \return Joint entity. 50 | gz::sim::Entity JointByName( 51 | gz::sim::EntityComponentManager & _ecm, 52 | gz::sim::Entity _modelEntity, 53 | const std::string & _name); 54 | 55 | } // namespace betaflight_gazebo 56 | 57 | #endif // BETAFLIGHT_GAZEBO__UTIL_HPP_ 58 | -------------------------------------------------------------------------------- /betaflight_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | betaflight_gazebo 4 | 0.0.0 5 | betaflight plugin 6 | Alejandro Hernández 7 | Alejandro Hernández 8 | Apache 2 9 | 10 | ament_cmake 11 | 12 | control_toolbox 13 | gz-cmake3 14 | gz-math7 15 | gz-plugin2 16 | gz-sim7 17 | gz-transport7 18 | pluginlib 19 | rclcpp 20 | std_msgs 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /betaflight_gazebo/src/BetaflightSocket.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "betaflight_gazebo/BetaflightSocket.hpp" 16 | 17 | namespace betaflight_gazebo 18 | { 19 | /// \brief constructor 20 | BetaflightSocket::BetaflightSocket() 21 | { 22 | // initialize socket udp socket 23 | fd = socket(AF_INET, SOCK_DGRAM, 0); 24 | #ifndef _WIN32 25 | // Windows does not support FD_CLOEXEC 26 | fcntl(fd, F_SETFD, FD_CLOEXEC); 27 | #endif 28 | } 29 | 30 | /// \brief destructor 31 | BetaflightSocket::~BetaflightSocket() 32 | { 33 | if (fd != -1) { 34 | ::close(fd); 35 | fd = -1; 36 | } 37 | } 38 | 39 | /// \brief Bind to an address and port 40 | /// \param[in] _address Address to bind to. 41 | /// \param[in] _port Port to bind to. 42 | /// \return True on success. 43 | bool BetaflightSocket::Bind(const char * _address, const uint16_t _port) 44 | { 45 | struct sockaddr_in sockaddr; 46 | this->MakeSockAddr(_address, _port, sockaddr); 47 | 48 | this->recv = sockaddr; 49 | 50 | if (bind(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { 51 | shutdown(this->fd, 0); 52 | #ifdef _WIN32 53 | closesocket(this->fd); 54 | #else 55 | close(this->fd); 56 | #endif 57 | return false; 58 | } 59 | int one = 1; 60 | setsockopt( 61 | this->fd, SOL_SOCKET, SO_REUSEADDR, 62 | reinterpret_cast(&one), sizeof(one)); 63 | 64 | fcntl(this->fd, F_SETFL, fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); 65 | return true; 66 | } 67 | 68 | /// \brief Make a socket 69 | /// \param[in] _address Socket address. 70 | /// \param[in] _port Socket port 71 | /// \param[out] _sockaddr New socket address structure. 72 | void BetaflightSocket::MakeSockAddr( 73 | const char * _address, const uint16_t _port, 74 | struct sockaddr_in & _sockaddr) 75 | { 76 | memset(&_sockaddr, 0, sizeof(_sockaddr)); 77 | 78 | #ifdef HAVE_SOCK_SIN_LEN 79 | _sockaddr.sin_len = sizeof(_sockaddr); 80 | #endif 81 | 82 | _sockaddr.sin_port = htons(_port); 83 | _sockaddr.sin_family = AF_INET; 84 | _sockaddr.sin_addr.s_addr = inet_addr(_address); 85 | } 86 | 87 | ssize_t BetaflightSocket::Send(const void * _buf, size_t _size) 88 | { 89 | return send(this->fd, _buf, _size, 0); 90 | } 91 | 92 | bool BetaflightSocket::Connect(const char * _address, const uint16_t _port) 93 | { 94 | struct sockaddr_in sockaddr; 95 | this->MakeSockAddr(_address, _port, sockaddr); 96 | 97 | if (connect(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { 98 | shutdown(this->fd, 0); 99 | close(this->fd); 100 | return false; 101 | } 102 | int one = 1; 103 | setsockopt( 104 | this->fd, SOL_SOCKET, SO_REUSEADDR, 105 | reinterpret_cast(&one), sizeof(one)); 106 | 107 | fcntl(this->fd, F_SETFL, fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); 108 | return true; 109 | } 110 | 111 | /// \brief Receive data 112 | /// \param[out] _buf Buffer that receives the data. 113 | /// \param[in] _size Size of the buffer. 114 | /// \param[in] _timeoutMS Milliseconds to wait for data. 115 | ssize_t BetaflightSocket::Recv(void * _buf, const size_t _size, uint32_t _timeoutMs) 116 | { 117 | fd_set fds; 118 | struct timeval tv; 119 | 120 | FD_ZERO(&fds); 121 | FD_SET(this->fd, &fds); 122 | 123 | tv.tv_sec = _timeoutMs / 1000; 124 | tv.tv_usec = (_timeoutMs % 1000) * 1000UL; 125 | 126 | if (select(this->fd + 1, &fds, NULL, NULL, &tv) != 1) { 127 | return -1; 128 | } 129 | 130 | socklen_t len = sizeof(this->recv); 131 | return recvfrom(this->fd, _buf, _size, 0, (struct sockaddr *)&this->recv, &len); 132 | } 133 | } // namespace betaflight_gazebo 134 | -------------------------------------------------------------------------------- /betaflight_gazebo/src/Util.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 ardupilot.org 2 | // 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU Lesser General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | // 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU Lesser General Public License for more details. 12 | // 13 | // You should have received a copy of the GNU Lesser General Public License 14 | // along with this program. If not, see . 15 | 16 | 17 | #include "betaflight_gazebo/Util.hpp" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace betaflight_gazebo 26 | { 27 | ////////////////////////////////////////////////// 28 | std::unordered_set EntitiesFromUnscopedName( 29 | const std::string & _name, const gz::sim::EntityComponentManager & _ecm, 30 | gz::sim::Entity _relativeTo) 31 | { 32 | // holds entities that match 33 | std::vector entities; 34 | 35 | if (_relativeTo == gz::sim::kNullEntity) { 36 | // search everything 37 | entities = _ecm.EntitiesByComponents(gz::sim::components::Name(_name)); 38 | } else { 39 | // search all descendents 40 | auto descendents = _ecm.Descendants(_relativeTo); 41 | for (const auto & descendent : descendents) { 42 | if (_ecm.EntityHasComponentType( 43 | descendent, 44 | gz::sim::components::Name::typeId)) 45 | { 46 | auto nameComp = _ecm.Component(descendent); 47 | if (nameComp->Data() == _name) { 48 | entities.push_back(descendent); 49 | } 50 | } 51 | } 52 | } 53 | if (entities.empty()) { 54 | return {}; 55 | } 56 | 57 | return std::unordered_set(entities.begin(), entities.end()); 58 | } 59 | 60 | ////////////////////////////////////////////////// 61 | gz::sim::Entity JointByName( 62 | gz::sim::EntityComponentManager & _ecm, 63 | gz::sim::Entity _modelEntity, 64 | const std::string & _name) 65 | { 66 | // Retrieve entities from a scoped name. 67 | // See for example: 68 | // https://github.com/gazebosim/ign-gazebo/pull/955 69 | // which applies to the LiftDrag plugin 70 | auto entities = entitiesFromScopedName(_name, _ecm, _modelEntity); 71 | 72 | if (entities.empty()) { 73 | gzerr << "Joint with name [" << _name << "] not found. " 74 | << "The joint will not respond to ArduPilot commands\n"; 75 | return gz::sim::kNullEntity; 76 | } else if (entities.size() > 1) { 77 | gzwarn << "Multiple joint entities with name[" << _name << "] found. " 78 | << "Using the first one.\n"; 79 | } 80 | 81 | gz::sim::Entity joint = *entities.begin(); 82 | 83 | // Validate 84 | if (!_ecm.EntityHasComponentType( 85 | joint, 86 | gz::sim::components::Joint::typeId)) 87 | { 88 | gzerr << "Entity with name[" << _name << "] is not a joint\n"; 89 | return gz::sim::kNullEntity; 90 | } 91 | 92 | // Ensure the joint has a velocity component 93 | if (!_ecm.EntityHasComponentType( 94 | joint, 95 | gz::sim::components::JointVelocity::typeId)) 96 | { 97 | _ecm.CreateComponent( 98 | joint, 99 | gz::sim::components::JointVelocity()); 100 | } 101 | 102 | return joint; 103 | } 104 | 105 | } // namespace betaflight_gazebo 106 | -------------------------------------------------------------------------------- /betaflight_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(betaflight_sim) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | include(ExternalProject) 7 | 8 | find_program(MAKE_EXECUTABLE NAMES make REQUIRED) 9 | 10 | message(cp ./obj/main/betaflight_SITL.elf ${CMAKE_INSTALL_PREFIX}/share/betaflight_sim/bin) 11 | 12 | externalproject_add(betaflight-firmware 13 | PREFIX 14 | ${PROJECT_BINARY_DIR}/betaflight_sim 15 | GIT_REPOSITORY 16 | https://github.com/betaflight/betaflight 17 | GIT_TAG 18 | 4.5-maintenance 19 | INSTALL_COMMAND 20 | mkdir -p ${CMAKE_INSTALL_PREFIX}/share/betaflight_sim/bin/ && 21 | cp ./obj/main/betaflight_SITL.elf ${CMAKE_INSTALL_PREFIX}/share/betaflight_sim/bin/ 22 | BUILD_COMMAND pwd && ${MAKE_EXECUTABLE} TARGET=SITL -j 23 | BUILD_IN_SOURCE ON 24 | CONFIGURE_COMMAND "" 25 | ) 26 | 27 | ament_environment_hooks(cmake/99_betaflight_setup.sh.in) 28 | 29 | if(BUILD_TESTING) 30 | find_package(ament_lint_auto REQUIRED) 31 | ament_lint_auto_find_test_dependencies() 32 | endif() 33 | 34 | install(DIRECTORY 35 | launch 36 | worlds 37 | DESTINATION 38 | share/${PROJECT_NAME} 39 | ) 40 | 41 | install(DIRECTORY 42 | models 43 | config 44 | DESTINATION 45 | ${CMAKE_INSTALL_PREFIX}/share/betaflight_sim 46 | ) 47 | 48 | ament_package() 49 | -------------------------------------------------------------------------------- /betaflight_sim/README.md: -------------------------------------------------------------------------------- 1 | # Betaflight 2 | 3 | Requeriments: 4 | - arm-none-eabi-gcc 5 | -------------------------------------------------------------------------------- /betaflight_sim/cmake/99_betaflight_setup.sh.in: -------------------------------------------------------------------------------- 1 | ament_prepend_unique_value PATH "@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/bin" 2 | -------------------------------------------------------------------------------- /betaflight_sim/config/eeprom.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/vehicle_gateway/cfe101602d84192286de2626650cda1a8e81f971/betaflight_sim/config/eeprom.bin -------------------------------------------------------------------------------- /betaflight_sim/launch/quadcopter.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_index_python.packages import get_package_share_directory 16 | from launch import LaunchDescription 17 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 18 | from launch.actions import RegisterEventHandler 19 | from launch.conditions import IfCondition 20 | from launch.launch_description_sources import PythonLaunchDescriptionSource 21 | from launch.substitutions import LaunchConfiguration 22 | from launch_ros.actions import Node 23 | from launch.event_handlers.on_process_io import OnProcessIO 24 | import os 25 | import time 26 | 27 | 28 | def get_betaflight_dir(): 29 | return get_package_share_directory('betaflight_sim') 30 | 31 | 32 | run_virtual_tty = ExecuteProcess(cmd=["socat", "-dd", "pty,link=/tmp/ttyS0,raw,echo=0", 33 | "tcp:127.0.0.1:5761"]) 34 | 35 | 36 | def _run_virtual_tty_check(event): 37 | """ 38 | Consider betaflight_controller ready when 'bind port 5761 for UART1...' string is printed. 39 | 40 | Launches betaflight_controller node if ready. 41 | """ 42 | target_str = 'bind port 5761 for UART1' 43 | if target_str in event.text.decode(): 44 | time.sleep(2) 45 | return run_virtual_tty 46 | 47 | 48 | def generate_launch_description(): 49 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 50 | 51 | # Launch Arguments 52 | use_sim_time_arg = DeclareLaunchArgument( 53 | 'use_sim_time', 54 | default_value=use_sim_time, 55 | description='If true, use simulated clock') 56 | 57 | os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.join(get_betaflight_dir(), "models") 58 | os.environ["GZ_SIM_RESOURCE_PATH"] += ":" + os.path.join(get_betaflight_dir(), "worlds") 59 | 60 | world_sdf = os.path.join(get_betaflight_dir(), "worlds", "empty_betaflight_world.sdf") 61 | 62 | use_groundcontrol = DeclareLaunchArgument('groundcontrol', default_value='false', 63 | choices=['true', 'false'], 64 | description='Start ground control station.') 65 | 66 | run_betaflight_sitl = ExecuteProcess(cmd=['betaflight_SITL.elf', "127.0.0.1"], 67 | cwd=os.path.join(get_betaflight_dir(), "config"), 68 | output='screen') 69 | 70 | # Bridge 71 | bridge = Node( 72 | package='ros_gz_bridge', 73 | executable='parameter_bridge', 74 | arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], 75 | output='screen' 76 | ) 77 | 78 | return LaunchDescription([ 79 | # Launch gazebo environment 80 | IncludeLaunchDescription( 81 | PythonLaunchDescriptionSource( 82 | [os.path.join(get_package_share_directory('ros_gz_sim'), 83 | 'launch', 'gz_sim.launch.py')]), 84 | launch_arguments=[('gz_args', [' -r -v 4 ' + world_sdf])]), 85 | use_sim_time_arg, 86 | run_betaflight_sitl, 87 | use_groundcontrol, 88 | ExecuteProcess(cmd=['betaflight-configurator'], 89 | condition=IfCondition(LaunchConfiguration('groundcontrol'))), 90 | bridge, 91 | RegisterEventHandler( 92 | OnProcessIO( 93 | target_action=run_betaflight_sitl, 94 | on_stdout=_run_virtual_tty_check, 95 | on_stderr=_run_virtual_tty_check 96 | ) 97 | ), 98 | ]) 99 | -------------------------------------------------------------------------------- /betaflight_sim/launch/quadcopter_joystick.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_index_python.packages import get_package_share_directory 16 | from launch import LaunchDescription 17 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 18 | from launch.actions import RegisterEventHandler 19 | from launch.conditions import IfCondition 20 | from launch.launch_description_sources import PythonLaunchDescriptionSource 21 | from launch.substitutions import LaunchConfiguration 22 | from launch_ros.actions import Node 23 | from launch.event_handlers.on_process_io import OnProcessIO 24 | import os 25 | import time 26 | 27 | 28 | def get_betaflight_dir(): 29 | return get_package_share_directory('betaflight_sim') 30 | 31 | 32 | run_betaflight_controller = Node( 33 | package='betaflight_controller', 34 | executable='betaflight_joy_controller', 35 | output='screen') 36 | 37 | run_betaflight_sitl = ExecuteProcess( 38 | cmd=['betaflight_SITL.elf', "127.0.0.1"], 39 | cwd=os.path.join(get_betaflight_dir(), "config"), 40 | output='screen') 41 | 42 | 43 | def _run_betaflight_sitl_check(event): 44 | """ 45 | Consider betaflight_controller ready when 'Opened joystick...' string is printed. 46 | 47 | Launches betaflight_controller node if ready. 48 | """ 49 | target_str = 'Opened joystick' 50 | if target_str in event.text.decode(): 51 | time.sleep(5) 52 | return run_betaflight_sitl 53 | 54 | 55 | def _run_betaflight_controller_check(event): 56 | """ 57 | Consider betaflight_controller ready when 'bind port 5761 for UART1...' string is printed. 58 | 59 | Launches betaflight_controller node if ready. 60 | """ 61 | target_str = 'bind port 5761 for UART1' 62 | if target_str in event.text.decode(): 63 | time.sleep(2) 64 | return run_betaflight_controller 65 | 66 | 67 | def generate_launch_description(): 68 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 69 | 70 | # Launch Arguments 71 | use_sim_time_arg = DeclareLaunchArgument( 72 | 'use_sim_time', 73 | default_value=use_sim_time, 74 | description='If true, use simulated clock') 75 | 76 | os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.join(get_betaflight_dir(), "models") 77 | os.environ["GZ_SIM_RESOURCE_PATH"] += ":" + os.path.join(get_betaflight_dir(), "worlds") 78 | 79 | world_sdf = os.path.join(get_betaflight_dir(), "worlds", "empty_betaflight_world.sdf") 80 | 81 | use_groundcontrol = DeclareLaunchArgument('groundcontrol', default_value='false', 82 | choices=['true', 'false'], 83 | description='Start ground control station.') 84 | 85 | joy_node = Node( 86 | package='joy', 87 | executable='joy_node', 88 | output='screen') 89 | 90 | return LaunchDescription([ 91 | # Launch gazebo environment 92 | IncludeLaunchDescription( 93 | PythonLaunchDescriptionSource( 94 | [os.path.join(get_package_share_directory('ros_gz_sim'), 95 | 'launch', 'gz_sim.launch.py')]), 96 | launch_arguments=[('gz_args', [' -r -v 4 ' + world_sdf])]), 97 | joy_node, 98 | use_sim_time_arg, 99 | use_groundcontrol, 100 | ExecuteProcess(cmd=['betaflight-configurator'], 101 | condition=IfCondition(LaunchConfiguration('groundcontrol'))), 102 | RegisterEventHandler( 103 | OnProcessIO( 104 | target_action=joy_node, 105 | on_stdout=_run_betaflight_sitl_check, 106 | on_stderr=_run_betaflight_sitl_check 107 | ) 108 | ), 109 | RegisterEventHandler( 110 | OnProcessIO( 111 | target_action=run_betaflight_sitl, 112 | on_stdout=_run_betaflight_controller_check, 113 | on_stderr=_run_betaflight_controller_check 114 | ) 115 | ), 116 | ]) 117 | -------------------------------------------------------------------------------- /betaflight_sim/models/iris_with_standoffs/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Iris with Standoffs 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Fadri Furrer 9 | fadri.furrer@mavt.ethz.ch 10 | 11 | 12 | Michael Burri 13 | 14 | 15 | Mina Kamel 16 | 17 | 18 | Janosch Nikolic 19 | 20 | 21 | Markus Achtelik 22 | 23 | 24 | john hsu 25 | 26 | 27 | A copy of the 3DR Iris model taken from 28 | https://github.com/PX4/sitl_gazebo/tree/master/models 29 | Local modifications include adding standoffs, 30 | inertia and collision updates.. 31 | 32 | 33 | -------------------------------------------------------------------------------- /betaflight_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | betaflight_sim 4 | 0.0.0 5 | The betaflight package 6 | 7 | Alejandro Hernandez 8 | 9 | Apache 2.0 10 | 11 | socat 12 | 13 | ament_index_python 14 | betaflight_configurator 15 | betaflight_controller 16 | joy 17 | launch 18 | launch_ros 19 | ros_gz_bridge 20 | ros_gz_sim 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /build_gazebo_from_source.md: -------------------------------------------------------------------------------- 1 | # Building Gazebo from Source 2 | 3 | In order to make rebuilds faster, we suggest creating two colcon workspaces: one for this project (`vehicle_gateway`) and one for Gazebo Garden. The Gazebo workspace will take much longer to compile, but it only needs to be recompiled when changes are incorporated from Gazebo. The overlaid `vehicle_gateway` workspace is where we expect most debug-recompile cycles to occur. 4 | 5 | To keep paths short, `vg` stands for "Vehicle Gateway". After these steps, you'll have two workspaces in the `~/vg` directory, like this: 6 | 7 | ```bash 8 | vg 9 | ├── gz_ws 10 | │   ├── build 11 | │   ├── install 12 | │   ├── log 13 | │   └── src 14 | └── vg_ws 15 | ├── build 16 | ├── install 17 | ├── log 18 | └── src 19 | ``` 20 | 21 | First, install ROS 2 Humble using the `.deb` packages using APT [according to these instructions](http://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) 22 | 23 | Next, install a few dependencies and set up our workspace source directories: 24 | ```bash 25 | sudo apt install python3-kconfiglib python3-jinja2 python3-jsonschema ros-humble-gps-msgs gcc-arm-none-eabi libfuse2 26 | pip3 install pyros-genmsg 27 | mkdir -p ~/vg/vg_ws/src 28 | cd ~/vg/vg_ws/src 29 | git clone https://github.com/osrf/vehicle_gateway 30 | cd ~/vg/vg_ws 31 | vcs import src < src/vehicle_gateway/dependencies.repos 32 | ``` 33 | 34 | Next, install Gazebo Garden. The full instructions are [here](https://gazebosim.org/docs/garden/install_ubuntu), and summarized as follows. 35 | 36 | ```bash 37 | sudo apt-get update 38 | sudo apt-get install lsb-release wget gnupg 39 | sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 40 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 41 | sudo apt-get update 42 | mkdir -p ~/vg/gz_ws/src 43 | cd ~/vg/gz_ws 44 | wget https://raw.githubusercontent.com/gazebo-tooling/gazebodistro/master/collection-garden.yaml 45 | vcs import src < ../vg_ws/src/vehicle_gateway/collection-garden.yaml 46 | sudo apt install -y $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/gz\|sdf/d' | tr '\n' ' ') 47 | ``` 48 | 49 | Now we can compile Gazebo Garden: 50 | 51 | ```bash 52 | cd ~/vg/gz_ws 53 | colcon build --merge-install 54 | ``` 55 | Now you should have Gazebo available in `~/vg/gz_ws`. 56 | 57 | We can now build the Vehicle Gateway itself, by overlaying its workspace on top of the Gazebo Garden workspace as well as the ROS 2 Humble system install. The Vehicle Gateway build will also download and build the PX4 firmware, to allow software-in-the-loop (SITL) simulation: 58 | 59 | ```bash 60 | cd ~/vg/vg_ws 61 | rosdep update && rosdep install --from-paths src --ignore-src -y 62 | source ~/vg/gz_ws/install/setup.bash 63 | source /opt/ros/humble/setup.bash 64 | colcon build --event-handlers console_direct+ 65 | ``` 66 | -------------------------------------------------------------------------------- /dependencies.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros_gz: 3 | type: git 4 | url: https://github.com/gazebosim/ros_gz 5 | version: 7c62a1ce3cfb4eb3b54e1cfd092c9b63a9d5f255 6 | uros/micro_ros_msgs: 7 | type: git 8 | url: https://github.com/micro-ROS/micro_ros_msgs.git 9 | version: humble 10 | uros/micro-ROS-Agent: 11 | type: git 12 | url: https://github.com/micro-ROS/micro-ROS-Agent.git 13 | version: humble 14 | px4_msgs: 15 | type: git 16 | url: https://github.com/PX4/px4_msgs 17 | version: "release/1.14" 18 | msp: 19 | type: git 20 | url: https://github.com/christianrauch/msp 21 | version: master 22 | zenohc: 23 | type: git 24 | url: https://github.com/eclipse-zenoh/zenoh-c 25 | version: main 26 | -------------------------------------------------------------------------------- /gz_aerial_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(gz_aerial_plugins) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | if(POLICY CMP0100) 9 | cmake_policy(SET CMP0100 NEW) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | 15 | set(CMAKE_AUTOMOC ON) 16 | set(OpenGL_GL_PREFERENCE LEGACY) 17 | 18 | # Find Qt5 19 | find_package(Qt5 20 | COMPONENTS 21 | Core 22 | Quick 23 | QuickControls2 24 | REQUIRED 25 | ) 26 | 27 | #============================================================================ 28 | # Find gz-cmake 29 | #============================================================================ 30 | # If you get an error at this line, you need to install gz-cmake 31 | find_package(gz-cmake3 REQUIRED) 32 | 33 | # Find the Ignition gui library 34 | #-------------------------------------- 35 | # Find gz-gui 36 | gz_find_package(gz-gui7 REQUIRED) 37 | set(GZ_GUI_VER ${gz-gui7_VERSION_MAJOR}) 38 | 39 | #-------------------------------------- 40 | # Find gz-plugin 41 | gz_find_package(gz-plugin2 REQUIRED) 42 | set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) 43 | 44 | #-------------------------------------- 45 | # Find gz-msgs 46 | gz_find_package(gz-msgs9 REQUIRED) 47 | set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR}) 48 | 49 | #-------------------------------------- 50 | # Find gz-transport 51 | gz_find_package(gz-transport12 REQUIRED) 52 | set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR}) 53 | 54 | #-------------------------------------- 55 | # Find gz-common 56 | # Always use the profiler component to get the headers, regardless of status. 57 | gz_find_package(gz-common5 58 | REQUIRED 59 | ) 60 | set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) 61 | 62 | qt5_add_resources(resources_rcc src/DroneHmi.qrc) 63 | 64 | include_directories(SYSTEM 65 | ${Qt5Core_INCLUDE_DIRS} 66 | ${Qt5Qml_INCLUDE_DIRS} 67 | ${Qt5Quick_INCLUDE_DIRS} 68 | ${Qt5QuickControls2_INCLUDE_DIRS} 69 | ) 70 | 71 | # Generate examples 72 | add_library(DroneHmi SHARED ${headers_MOC} 73 | src/DroneHmi.cpp 74 | ${resources_rcc} 75 | ) 76 | target_link_libraries(DroneHmi 77 | gz-gui7::gz-gui7 78 | gz-common5::gz-common5 79 | gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} 80 | gz-plugin${GZ_PLUGIN_VER}::register 81 | gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER} 82 | ${Qt5Core_LIBRARIES} 83 | ${Qt5Qml_LIBRARIES} 84 | ${Qt5Quick_LIBRARIES} 85 | ${Qt5QuickControls2_LIBRARIES} 86 | ) 87 | 88 | install( 89 | TARGETS DroneHmi 90 | DESTINATION share/${PROJECT_NAME}/lib 91 | ) 92 | 93 | if(BUILD_TESTING) 94 | find_package(ament_lint_auto REQUIRED) 95 | ament_lint_auto_find_test_dependencies() 96 | endif() 97 | 98 | ament_package() 99 | -------------------------------------------------------------------------------- /gz_aerial_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gz_aerial_plugins 5 | 0.0.0 6 | Drone Gazebo Simulator GUI Plugins 7 | ahcorde 8 | Apache 2.0 9 | 10 | ament_cmake 11 | gz-common5 12 | gz-cmake3 13 | gz-gui7 14 | gz-plugin2 15 | gz-msgs9 16 | gz-transport12 17 | qml-module-qtquick-extras 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /gz_aerial_plugins/src/AttitudeWidget.qml: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | import QtQuick 2.9 18 | import QtQuick.Controls 2.2 19 | import QtQuick.Controls.Material 2.1 20 | import QtQuick.Layouts 1.3 21 | 22 | Rectangle { 23 | id: "imageDisplay" 24 | color: "transparent" 25 | anchors.fill: parent 26 | Layout.minimumWidth: 200 27 | Layout.minimumHeight: 200 28 | 29 | Connections { 30 | target: DroneHmi 31 | onNewImage: image.reload(); 32 | } 33 | 34 | ColumnLayout { 35 | id: imageDisplayColumn 36 | anchors.fill: parent 37 | anchors.margins: 10 38 | 39 | Image { 40 | id: image 41 | cache: false 42 | fillMode: Image.PreserveAspectFit 43 | Layout.fillHeight: true 44 | Layout.fillWidth: true 45 | verticalAlignment: Image.AlignTop 46 | horizontalAlignment: Image.AlignLeft 47 | function reload() { 48 | console.log("update") 49 | // Force image request to C++ 50 | source = "image://attitudedisplay/" + Math.random().toString(36).substr(2, 5); 51 | } 52 | } 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /gz_aerial_plugins/src/CompassWidget.qml: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | import QtQuick 2.9 19 | import QtQuick.Controls 2.2 20 | import QtQuick.Controls.Material 2.1 21 | import QtQuick.Controls.Styles 1.4 22 | import QtQuick.Layouts 1.3 23 | import "qrc:/qml" 24 | 25 | Rectangle 26 | { 27 | id: compassWidget 28 | width: 150 29 | height: 150 30 | 31 | Canvas 32 | { 33 | id: canvas 34 | anchors.fill: parent 35 | 36 | property double angle_: 0; 37 | property double margins_: 10.0; 38 | property variant pointText_: ["N", "NE", "E", "SE", "S", "SW", "W", "NW"]; 39 | 40 | Connections { 41 | target: DroneHmi 42 | onNewHeading: { 43 | canvas.angle_ = DroneHmi.heading 44 | canvas.requestPaint(); 45 | } 46 | } 47 | 48 | onPaint : { 49 | var ctx = getContext("2d"); 50 | ctx.reset() 51 | ctx.translate(compassWidget.width / 2, compassWidget.height / 2); 52 | var scaleCompass = Math.min((compassWidget.width - margins_)/120.0, 53 | (compassWidget.height - margins_)/120.0); 54 | ctx.scale(scale, scale); 55 | 56 | var i = 0; 57 | var j = 0; 58 | while(i < 360) 59 | { 60 | if(i % 45 == 0) 61 | { 62 | ctx.beginPath(); 63 | ctx.fillStyle = "black"; 64 | ctx.fillText(qsTr(pointText_[j]), -5, -52); 65 | ctx.moveTo(0, -40); 66 | ctx.lineTo(0, -50); 67 | ctx.moveTo(0, -40); 68 | ctx.stroke(); 69 | j++; 70 | } 71 | else 72 | { 73 | ctx.beginPath(); 74 | ctx.moveTo(0, -45); 75 | ctx.lineTo(0, -50); 76 | ctx.stroke(); 77 | } 78 | ctx.rotate(15 * 3.1416 / 180); 79 | i += 15; 80 | } 81 | 82 | ctx.lineWidth = 4 83 | ctx.strokeStyle = "black" 84 | ctx.fillStyle = "red" 85 | ctx.rotate(canvas.angle_); 86 | ctx.beginPath(); 87 | ctx.moveTo(-10, 0); 88 | ctx.lineTo(0, -45); 89 | ctx.lineTo(10, 0); 90 | ctx.lineTo(0, -15); 91 | ctx.lineTo(-10, 0); 92 | ctx.closePath(); 93 | ctx.fill(); 94 | ctx.stroke(); 95 | } 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /gz_aerial_plugins/src/DroneHmi.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /gz_aerial_plugins/src/DroneHmi.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef DRONEHMI_HPP_ 17 | #define DRONEHMI_HPP_ 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace gz 30 | { 31 | namespace aerial 32 | { 33 | namespace plugins 34 | { 35 | 36 | class AttitudeDisplayPrivate; 37 | 38 | class DroneHmi : public gz::gui::Plugin 39 | { 40 | Q_OBJECT 41 | 42 | public: 43 | /// \brief Constructor 44 | DroneHmi(); 45 | 46 | /// \brief Destructor 47 | virtual ~DroneHmi(); 48 | 49 | /// \brief Called by Gazebo GUI when plugin is instantiated. 50 | /// \param[in] _pluginElem XML configuration for this plugin. 51 | void LoadConfig(const tinyxml2::XMLElement * _pluginElem) override; 52 | 53 | /// \brief Notify that a new image has been received. 54 | 55 | void Update(); 56 | 57 | void DrawRoll(QPainter & painter); 58 | void DrawPitch(QPainter & painter); 59 | void DrawBackground(QPainter & painter); 60 | 61 | /// \brief Frequency 62 | Q_PROPERTY( 63 | double heading 64 | READ Heading 65 | WRITE SetHeading 66 | NOTIFY newHeading 67 | ) 68 | 69 | public: 70 | double Heading(); 71 | 72 | public: 73 | Q_INVOKABLE void SetHeading(const double); 74 | 75 | signals: 76 | void newImage(); 77 | void newHeading(); 78 | 79 | private: 80 | gz::transport::Node node_; 81 | 82 | void onHeadingReceived(const gz::msgs::Float & _msg); 83 | void onPitchReceived(const gz::msgs::Float & _msg); 84 | void onRollReceived(const gz::msgs::Float & _msg); 85 | 86 | /// \internal 87 | /// \brief Pointer to private data. 88 | 89 | private: 90 | std::unique_ptr dataPtr; 91 | }; 92 | 93 | } // namespace plugins 94 | } // namespace aerial 95 | } // namespace gz 96 | 97 | #endif // DRONEHMI_HPP_ 98 | -------------------------------------------------------------------------------- /gz_aerial_plugins/src/DroneHmi.qml: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | import QtQuick 2.9 18 | import QtQuick.Controls 2.2 19 | import QtQuick.Controls.Material 2.1 20 | import QtQuick.Controls.Styles 1.4 21 | import QtQuick.Layouts 1.3 22 | import "qrc:/qml" 23 | import "qrc:/CompassWidget" 24 | import "qrc:/AttitudeWidget" 25 | 26 | RowLayout 27 | { 28 | Layout.minimumWidth: 400 29 | Layout.minimumHeight: 250 30 | 31 | AttitudeWidget{} 32 | 33 | CompassWidget{} 34 | } 35 | -------------------------------------------------------------------------------- /gz_aerial_plugins/src/DroneHmi.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | DroneHmi.qml 4 | 5 | 6 | 7 | CompassWidget.qml 8 | 9 | 10 | 11 | AttitudeWidget.qml 12 | 13 | 14 | -------------------------------------------------------------------------------- /px4_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(px4_sim) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | include(ExternalProject) 7 | 8 | find_program(MAKE_EXECUTABLE NAMES make REQUIRED) 9 | 10 | externalproject_add(px4-firmware 11 | PREFIX 12 | ${PROJECT_BINARY_DIR}/px4-src 13 | GIT_REPOSITORY 14 | https://github.com/PX4/PX4-Autopilot 15 | GIT_TAG 16 | 3cc940cb06f233a22a8d1b948bc05f067491e212 17 | INSTALL_COMMAND 18 | mkdir -p ${CMAKE_INSTALL_PREFIX}/share/px4_sim && 19 | rsync -avL --exclude .git --exclude build 20 | build/px4_sitl_default/bin 21 | Tools/simulation/gz/models 22 | build/px4_sitl_default/etc 23 | ${CMAKE_INSTALL_PREFIX}/share/px4_sim && 24 | cp ${CMAKE_CURRENT_LIST_DIR}/dds_topics.yaml 25 | ${PROJECT_BINARY_DIR}/px4-src/src/px4-firmware/src/modules/uxrce_dds_client/dds_topics.yaml 26 | BUILD_COMMAND ${MAKE_EXECUTABLE} px4_ros2 -j && make px4_sitl_default -j 27 | BUILD_IN_SOURCE ON 28 | CONFIGURE_COMMAND "" 29 | ) 30 | 31 | install(DIRECTORY 32 | launch 33 | DESTINATION 34 | share/${PROJECT_NAME} 35 | ) 36 | 37 | ament_environment_hooks(cmake/99_px4_setup.sh.in) 38 | 39 | ament_export_libraries(px4) 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /px4_sim/README.md: -------------------------------------------------------------------------------- 1 | # PX4 Sim 2 | 3 | This package will download the PX4 repository and compile the required targets to 4 | simulate quadcopters, VTOLs and fixed wings planes in Gazebo. 5 | 6 | # Requirements 7 | 8 | - [Gazebo Garden](https://gazebosim.org/docs/garden/install) 9 | - [ROS 2 Humble](https://docs.ros.org/en/humble/Installation.html) 10 | - [ros_gz](https://github.com/gazebosim/ros_gz) 11 | -------------------------------------------------------------------------------- /px4_sim/cmake/99_px4_setup.sh.in: -------------------------------------------------------------------------------- 1 | ament_prepend_unique_value PATH "@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/bin:@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/etc/init.d:@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/etc/init.d-posix:@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@" 2 | -------------------------------------------------------------------------------- /px4_sim/config/multi.yaml: -------------------------------------------------------------------------------- 1 | - vehicle_id: 1 2 | frame_name: pad_1 3 | vehicle_type: x500 4 | sensor_config: stock 5 | model_pose: "" 6 | dds_domain_id: "" 7 | - vehicle_id: 2 8 | frame_name: pad_2 9 | vehicle_type: standard_vtol 10 | sensor_config: stock 11 | model_pose: "" 12 | dds_domain_id: "" 13 | -------------------------------------------------------------------------------- /px4_sim/config/two_isolated.yaml: -------------------------------------------------------------------------------- 1 | - vehicle_id: 1 2 | frame_name: pad_1 3 | vehicle_type: standard_vtol 4 | sensor_config: stock 5 | model_pose: "" 6 | dds_domain_id: 1 7 | - vehicle_id: 2 8 | frame_name: pad_2 9 | vehicle_type: standard_vtol 10 | sensor_config: stock 11 | model_pose: "" 12 | dds_domain_id: 2 13 | -------------------------------------------------------------------------------- /px4_sim/dds_topics.yaml: -------------------------------------------------------------------------------- 1 | ##### 2 | # 3 | # This file maps all the topics that are to be used on the micro DDS client. 4 | # 5 | ##### 6 | publications: 7 | 8 | - topic: /fmu/out/collision_constraints 9 | type: px4_msgs::msg::CollisionConstraints 10 | 11 | - topic: /fmu/out/failsafe_flags 12 | type: px4_msgs::msg::FailsafeFlags 13 | 14 | - topic: /fmu/out/sensor_combined 15 | type: px4_msgs::msg::SensorCombined 16 | 17 | - topic: /fmu/out/timesync_status 18 | type: px4_msgs::msg::TimesyncStatus 19 | 20 | # - topic: /fmu/out/vehicle_angular_velocity 21 | # type: px4_msgs::msg::VehicleAngularVelocity 22 | 23 | - topic: /fmu/out/vehicle_attitude 24 | type: px4_msgs::msg::VehicleAttitude 25 | 26 | - topic: /fmu/out/vehicle_control_mode 27 | type: px4_msgs::msg::VehicleControlMode 28 | 29 | - topic: /fmu/out/vehicle_global_position 30 | type: px4_msgs::msg::VehicleGlobalPosition 31 | 32 | - topic: /fmu/out/vehicle_gps_position 33 | type: px4_msgs::msg::SensorGps 34 | 35 | - topic: /fmu/out/vehicle_local_position 36 | type: px4_msgs::msg::VehicleLocalPosition 37 | 38 | - topic: /fmu/out/vehicle_odometry 39 | type: px4_msgs::msg::VehicleOdometry 40 | 41 | - topic: /fmu/out/vehicle_status 42 | type: px4_msgs::msg::VehicleStatus 43 | 44 | - topic: /fmu/out/vehicle_trajectory_waypoint_desired 45 | type: px4_msgs::msg::VehicleTrajectoryWaypoint 46 | 47 | - topic: /fmu/out/airspeed 48 | type: px4_msgs::msg::Airspeed 49 | 50 | subscriptions: 51 | 52 | - topic: /fmu/in/offboard_control_mode 53 | type: px4_msgs::msg::OffboardControlMode 54 | 55 | - topic: /fmu/in/onboard_computer_status 56 | type: px4_msgs::msg::OnboardComputerStatus 57 | 58 | - topic: /fmu/in/obstacle_distance 59 | type: px4_msgs::msg::ObstacleDistance 60 | 61 | - topic: /fmu/in/sensor_optical_flow 62 | type: px4_msgs::msg::SensorOpticalFlow 63 | 64 | - topic: /fmu/in/telemetry_status 65 | type: px4_msgs::msg::TelemetryStatus 66 | 67 | - topic: /fmu/in/trajectory_setpoint 68 | type: px4_msgs::msg::TrajectorySetpoint 69 | 70 | - topic: /fmu/in/vehicle_attitude_setpoint 71 | type: px4_msgs::msg::VehicleAttitudeSetpoint 72 | 73 | - topic: /fmu/in/vehicle_mocap_odometry 74 | type: px4_msgs::msg::VehicleOdometry 75 | 76 | - topic: /fmu/in/vehicle_rates_setpoint 77 | type: px4_msgs::msg::VehicleRatesSetpoint 78 | 79 | - topic: /fmu/in/vehicle_visual_odometry 80 | type: px4_msgs::msg::VehicleOdometry 81 | 82 | - topic: /fmu/in/vehicle_command 83 | type: px4_msgs::msg::VehicleCommand 84 | 85 | - topic: /fmu/in/vehicle_trajectory_bezier 86 | type: px4_msgs::msg::VehicleTrajectoryBezier 87 | 88 | - topic: /fmu/in/vehicle_trajectory_waypoint 89 | type: px4_msgs::msg::VehicleTrajectoryWaypoint 90 | -------------------------------------------------------------------------------- /px4_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | px4_sim 4 | 0.0.0 5 | The px4 package 6 | 7 | Alejandro Hernandez 8 | 9 | Apache 2.0 10 | 11 | ament_index_python 12 | qgroundcontrol 13 | ros_gz_sim 14 | ros_gz_bridge 15 | ros2launch 16 | launch 17 | launch_ros 18 | vehicle_gateway_python_helpers 19 | vehicle_gateway_models 20 | vehicle_gateway_worlds 21 | 22 | micro_ros_agent 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /qgroundcontrol/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(qgroundcontrol) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | include(ExternalProject) 7 | 8 | if(NOT DEFINED SKIP_QGROUNDCONTROL OR NOT ${SKIP_QGROUNDCONTROL}) 9 | if(NOT EXISTS "${CMAKE_BINARY_DIR}/QGroundControl.AppImage") 10 | file(DOWNLOAD 11 | https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.4/QGroundControl.AppImage 12 | QGroundControl.AppImage 13 | ) 14 | endif() 15 | 16 | install(PROGRAMS 17 | ${CMAKE_BINARY_DIR}/QGroundControl.AppImage 18 | TYPE BIN 19 | ) 20 | else() 21 | message(WARNING "Skipping QGroundControl") 22 | endif() 23 | 24 | ament_package() 25 | -------------------------------------------------------------------------------- /qgroundcontrol/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | qgroundcontrol 4 | 0.0.0 5 | The qgroundcontrol package 6 | 7 | Alejandro Hernández 8 | 9 | Apache License 2.0 10 | 11 | https://dev.qgroundcontrol.com 12 | 13 | 14 | ament_cmake 15 | 16 | 17 | -------------------------------------------------------------------------------- /vehicle_gateway/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vehicle_gateway) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | 16 | add_library(${PROJECT_NAME} INTERFACE) 17 | 18 | target_include_directories(${PROJECT_NAME} INTERFACE 19 | $ 20 | $ 21 | ) 22 | 23 | install(TARGETS ${PROJECT_NAME} 24 | EXPORT "export_${PROJECT_NAME}" 25 | ARCHIVE DESTINATION lib 26 | LIBRARY DESTINATION lib 27 | RUNTIME DESTINATION lib 28 | INCLUDES DESTINATION include 29 | ) 30 | 31 | install( 32 | DIRECTORY include/ 33 | DESTINATION include 34 | ) 35 | 36 | ament_export_include_directories( 37 | include 38 | ) 39 | 40 | if(BUILD_TESTING) 41 | find_package(ament_lint_auto REQUIRED) 42 | # the following line skips the linter which checks for copyrights 43 | # comment the line when a copyright and license is added to all source files 44 | # set(ament_cmake_copyright_FOUND TRUE) 45 | # the following line skips cpplint (only works in a git repo) 46 | # comment the line when this package is in a git repo and when 47 | # a copyright and license is added to all source files 48 | # set(ament_cmake_cpplint_FOUND TRUE) 49 | ament_lint_auto_find_test_dependencies() 50 | endif() 51 | 52 | ament_export_targets("export_${PROJECT_NAME}") 53 | 54 | ament_package() 55 | -------------------------------------------------------------------------------- /vehicle_gateway/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway 5 | 0.0.1 6 | A pluginlib-based system for interfacing to vehicle SDK's 7 | Morgan Quigley 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /vehicle_gateway_betaflight/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vehicle_gateway_betaflight) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | # Default to C11 9 | if(NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 11) 11 | endif() 12 | 13 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra -Wpedantic) 15 | endif() 16 | 17 | # find dependencies 18 | find_package(ament_cmake REQUIRED) 19 | find_package(pluginlib REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(sensor_msgs REQUIRED) 22 | find_package(std_msgs REQUIRED) 23 | find_package(vehicle_gateway REQUIRED) 24 | 25 | find_package(PkgConfig REQUIRED) 26 | pkg_search_module(PKG_MSP REQUIRED IMPORTED_TARGET msp) 27 | 28 | include_directories(${PKG_MSP_INCLUDE_DIRS}) 29 | link_directories(${PKG_MSP_LIBRARY_DIRS}) 30 | 31 | add_library(vehicle_gateway_betaflight src/vehicle_gateway_betaflight.cpp) 32 | target_include_directories(vehicle_gateway_betaflight PUBLIC 33 | $ 34 | $) 35 | ament_target_dependencies( 36 | vehicle_gateway_betaflight 37 | pluginlib 38 | rclcpp 39 | sensor_msgs 40 | std_msgs 41 | vehicle_gateway 42 | ) 43 | 44 | target_link_libraries(vehicle_gateway_betaflight 45 | ${PKG_MSP_LIBRARIES} 46 | ) 47 | 48 | add_executable(vehicle_gateway_betaflight_node src/vehicle_gateway_betaflight_main.cpp) 49 | target_include_directories(vehicle_gateway_betaflight_node PUBLIC 50 | $ 51 | $) 52 | ament_target_dependencies(vehicle_gateway_betaflight_node 53 | pluginlib 54 | rclcpp 55 | sensor_msgs 56 | vehicle_gateway 57 | ) 58 | 59 | target_link_libraries(vehicle_gateway_betaflight_node 60 | ${PKG_MSP_LIBRARIES} 61 | ) 62 | 63 | pluginlib_export_plugin_description_file(vehicle_gateway plugins.xml) 64 | 65 | # Causes the visibility macros to use dllexport rather than dllimport, 66 | # which is appropriate when building the dll but not consuming it. 67 | target_compile_definitions(vehicle_gateway_betaflight PRIVATE "VEHICLE_GATEWAY_BETAFLIGHT_BUILDING_LIBRARY") 68 | 69 | install( 70 | TARGETS vehicle_gateway_betaflight 71 | EXPORT export_${PROJECT_NAME} 72 | ARCHIVE DESTINATION lib 73 | LIBRARY DESTINATION lib 74 | RUNTIME DESTINATION bin 75 | ) 76 | 77 | install( 78 | TARGETS vehicle_gateway_betaflight_node 79 | DESTINATION lib/${PROJECT_NAME} 80 | ) 81 | 82 | install( 83 | DIRECTORY include/ 84 | DESTINATION include 85 | ) 86 | 87 | if(BUILD_TESTING) 88 | find_package(ament_lint_auto REQUIRED) 89 | ament_lint_auto_find_test_dependencies() 90 | endif() 91 | 92 | ament_export_libraries( 93 | vehicle_gateway_betaflight 94 | ) 95 | ament_export_targets( 96 | export_${PROJECT_NAME} 97 | ) 98 | ament_export_dependencies( 99 | pluginlib 100 | rclcpp 101 | vehicle_gateway 102 | ) 103 | ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}) 104 | 105 | ament_export_include_directories( 106 | include 107 | ) 108 | ament_package() 109 | -------------------------------------------------------------------------------- /vehicle_gateway_betaflight/README.md: -------------------------------------------------------------------------------- 1 | # vehicle_gateway_betaflight 2 | Run 3 | 4 | ```bash 5 | sudo socat -d -d pty,link=/dev/ttyS0,raw,echo=0 tcp:127.0.0.1:5761 6 | ``` 7 | -------------------------------------------------------------------------------- /vehicle_gateway_betaflight/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_betaflight 5 | 0.0.0 6 | Betaflight plugin for the Vehicle Gateway 7 | Alejandro Hernandez 8 | Apache License 2.0 9 | 10 | ament_cmake_ros 11 | 12 | msp 13 | pluginlib 14 | rclcpp 15 | sensor_msgs 16 | std_msgs 17 | vehicle_gateway 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /vehicle_gateway_betaflight/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Vehicle Gateway implementation for Betaflight 4 | 5 | 6 | -------------------------------------------------------------------------------- /vehicle_gateway_betaflight/src/vehicle_gateway_betaflight_main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | using namespace std::chrono_literals; 23 | 24 | int main(int argc, const char ** argv) 25 | { 26 | pluginlib::ClassLoader loader( 27 | "vehicle_gateway", "vehicle_gateway::VehicleGateway"); 28 | 29 | // TODO(anyone): retrieve the plugin name from a ROS 2 param in the launch file 30 | try { 31 | std::shared_ptr gateway = loader.createSharedInstance( 32 | "vehicle_gateway_betaflight::VehicleGatewayBetaflight"); 33 | std::cerr << "Initializing VehicleGatewayBetaflight" << '\n'; 34 | gateway->init(argc, argv); 35 | int count = 0; 36 | while (gateway->get_arming_state() != vehicle_gateway::ARMING_STATE::ARMED) { 37 | if (!gateway->set_body_rates_and_thrust_setpoint(0, 0, 0, -1)) { 38 | std::cerr << "Error sending RC" << '\n'; 39 | } 40 | if (count == 1) { 41 | gateway->arm(); 42 | } 43 | if (count == 30) { 44 | gateway->disarm(); 45 | count = 0; 46 | } 47 | count++; 48 | std::this_thread::sleep_for(50ms); 49 | std::cout << "Trying to arm vehicle " << gateway->get_arming_state() << '\n'; 50 | } 51 | 52 | std::cout << "Vehicle is armed" << '\n'; 53 | 54 | while (1) { 55 | std::cerr << "gateway->get_arming_state() " << 56 | static_cast(gateway->get_arming_state()) << '\n'; 57 | std::this_thread::sleep_for(50ms); 58 | if (!gateway->set_body_rates_and_thrust_setpoint(0, 0, 0, 0.1)) { 59 | std::cerr << "Error sending RC" << '\n'; 60 | } 61 | } 62 | gateway->destroy(); 63 | } catch (pluginlib::PluginlibException & ex) { 64 | printf("The plugin failed to load: %s\n", ex.what()); 65 | } 66 | return 0; 67 | } 68 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(vehicle_gateway_demo) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | find_package(ament_cmake) 8 | find_package(control_toolbox REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(std_msgs REQUIRED) 11 | find_package(vehicle_gateway_px4 REQUIRED) 12 | find_package(aruco_opencv_msgs REQUIRED) 13 | 14 | add_executable(aruco_demo 15 | src/aruco_demo.cpp 16 | ) 17 | target_include_directories(aruco_demo PUBLIC 18 | $ 19 | $) 20 | ament_target_dependencies(aruco_demo 21 | control_toolbox 22 | rclcpp 23 | std_msgs 24 | vehicle_gateway_px4 25 | aruco_opencv_msgs 26 | ) 27 | 28 | add_executable(circles 29 | src/circles.cpp 30 | ) 31 | ament_target_dependencies(circles 32 | rclcpp 33 | vehicle_gateway_px4 34 | ) 35 | 36 | add_executable(vtol_position_control 37 | src/vtol_position_control.cpp 38 | ) 39 | ament_target_dependencies(vtol_position_control 40 | rclcpp 41 | vehicle_gateway_px4 42 | ) 43 | 44 | add_executable(follower 45 | src/follower.cpp 46 | ) 47 | ament_target_dependencies(follower 48 | rclcpp 49 | vehicle_gateway_px4 50 | ) 51 | 52 | install(DIRECTORY 53 | launch 54 | config 55 | DESTINATION 56 | share/${PROJECT_NAME} 57 | ) 58 | 59 | install( 60 | TARGETS 61 | aruco_demo 62 | circles 63 | follower 64 | vtol_position_control 65 | DESTINATION lib/${PROJECT_NAME} 66 | ) 67 | 68 | if(BUILD_TESTING) 69 | find_package(ament_lint_auto REQUIRED) 70 | ament_lint_auto_find_test_dependencies() 71 | endif() 72 | 73 | ament_package() 74 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/README.md: -------------------------------------------------------------------------------- 1 | # Wingman demo 2 | 3 | Launch the simulation with two VTOLs typing the following commands: 4 | 5 | ```bash 6 | export MULTIROBOT_CONFIG=`ros2 pkg prefix vehicle_gateway_demo`/share/vehicle_gateway_demo/config/two_vtols.yaml 7 | ros2 launch px4_sim px4_sim_multi.launch.py 8 | ``` 9 | 10 | When the simulation is running we need to launch the leader vehicle. Open one terminal and run: 11 | 12 | ```bash 13 | ros2 run vehicle_gateway_demo vtol_position_control 1 `ros2 pkg prefix vehicle_gateway_demo`/share/vehicle_gateway_demo/config/leader_follower_multicast_discovery.json 14 | ``` 15 | 16 | Then you can run the wingman vehicle or follower with this other command 17 | 18 | ```bash 19 | ros2 run vehicle_gateway_demo follower 2 `ros2 pkg prefix vehicle_gateway_demo`/share/vehicle_gateway_demo/config/leader_follower_multicast_discovery.json 20 | ``` 21 | 22 | 23 | If you want to visualize both drones you can also run QGroundControl 24 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/config/leader_follower_multicast_discovery.json: -------------------------------------------------------------------------------- 1 | { 2 | "state_transmit_interval_seconds": 0.5, 3 | "zenoh": { 4 | "mode": "peer", 5 | "connect": { 6 | "endpoints": [ 7 | ] 8 | }, 9 | "listen": { 10 | "endpoints": [ 11 | "tcp/0.0.0.0:0" 12 | ] 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/config/single_marker_tracker.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | cam_base_topic: /world/aruco_px4_world/model/x500_camera_0/link/camera_link/sensor/camera/image 4 | output_frame: 'aruco_px4_world' 5 | #output_frame: 'x500_0/camera_link/camera' 6 | use_sim_time: true 7 | marker_dict: 4X4_50 8 | 9 | image_sub_qos: 10 | reliability: 2 # 0 - system default, 1 - reliable, 2 - best effort 11 | durability: 2 # 0 - system default, 1 - transient local, 2 - volatile 12 | depth: 1 13 | 14 | publish_tf: true 15 | marker_size: 4.0 16 | 17 | # Dynamically reconfigurable Detector parameters 18 | # https://docs.opencv.org/4.2.0/d5/dae/tutorial_aruco_detection.html 19 | aruco: 20 | adaptiveThreshWinSizeMin: 3 21 | adaptiveThreshWinSizeMax: 23 22 | adaptiveThreshWinSizeStep: 10 23 | adaptiveThreshConstant: 7.0 24 | minMarkerPerimeterRate: 0.03 25 | maxMarkerPerimeterRate: 4.0 26 | polygonalApproxAccuracyRate: 0.05 27 | minCornerDistanceRate: 0.05 28 | minDistanceToBorder: 3 29 | minMarkerDistanceRate: 0.05 30 | markerBorderBits: 1 31 | perspectiveRemovePixelPerCell: 4 32 | perspectiveRemoveIgnoredMarginPerCell: 0.13 33 | maxErroneousBitsInBorderRate: 0.35 34 | minOtsuStdDev: 5.0 35 | errorCorrectionRate: 0.6 36 | cornerRefinementMethod: 2 # 0 - None, 1 - Subpix, 2 - Contour 37 | cornerRefinementWinSize: 5 38 | cornerRefinementMaxIterations: 30 39 | cornerRefinementMinAccuracy: 0.1 40 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/config/two_vtols.yaml: -------------------------------------------------------------------------------- 1 | - vehicle_id: 1 2 | frame_name: pad_1 3 | vehicle_type: standard_vtol 4 | sensor_config: stock 5 | model_pose: "" 6 | dds_domain_id: "" 7 | - vehicle_id: 2 8 | frame_name: pad_2 9 | vehicle_type: standard_vtol 10 | sensor_config: stock 11 | model_pose: "" 12 | dds_domain_id: "" 13 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_demo 5 | 0.0.0 6 | Betaflight demo 7 | Alejandro Hernandez Cordero 8 | Apache License 2.0 9 | 10 | Alejandro Hernandez Cordero 11 | 12 | ament_cmake 13 | 14 | aruco_opencv_msgs 15 | control_toolbox 16 | pluginlib 17 | rclcpp 18 | std_msgs 19 | vehicle_gateway 20 | vehicle_gateway_px4 21 | 22 | ament_index_python 23 | aruco_opencv 24 | betaflight_sim 25 | micro_ros_agent 26 | launch 27 | launch_ros 28 | px4_sim 29 | ros_gz_bridge 30 | ros_gz_sim 31 | rviz2 32 | tf2_ros 33 | vehicle_gateway_models 34 | vehicle_gateway_worlds 35 | 36 | 37 | ament_lint_auto 38 | ament_cmake_lint_cmake 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /vehicle_gateway_demo/src/circles.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | using std::placeholders::_1; 26 | using namespace std::chrono_literals; 27 | 28 | class DroneCircles : public rclcpp::Node 29 | { 30 | public: 31 | DroneCircles(int argc, const char ** argv) 32 | : Node("minimal_subscriber"), loader_("vehicle_gateway", "vehicle_gateway::VehicleGateway") 33 | { 34 | this->gateway_ = this->loader_.createSharedInstance( 35 | "vehicle_gateway_px4::VehicleGatewayPX4"); 36 | RCLCPP_INFO(this->get_logger(), "Initializing VehicleGatewayPX4"); 37 | this->gateway_->init(0, nullptr); 38 | this->thread_loop_ = std::thread(&DroneCircles::loop, this); 39 | } 40 | 41 | void loop() 42 | { 43 | int offboard_setpoint_counter_ = 0; 44 | 45 | while (offboard_setpoint_counter_ < 6) { 46 | offboard_setpoint_counter_++; 47 | std::this_thread::sleep_for(100ms); 48 | this->gateway_->set_offboard_control_mode(vehicle_gateway::POSITION); 49 | this->gateway_->set_local_position_setpoint(0, 0, -3, 0); 50 | if (offboard_setpoint_counter_ == 5) { 51 | while (true) { 52 | RCLCPP_INFO(this->get_logger(), "Try to set offboard mode and arm vehicle"); 53 | this->gateway_->set_offboard_mode(); 54 | this->gateway_->arm(); 55 | std::this_thread::sleep_for(200ms); 56 | if ((this->gateway_->get_flight_mode() == vehicle_gateway::FLIGHT_MODE::OFFBOARD && 57 | this->gateway_->get_arming_state() == vehicle_gateway::ARMING_STATE::ARMED) || 58 | this->stopped_) 59 | { 60 | RCLCPP_INFO(this->get_logger(), "Vehicle is in Offboard mode"); 61 | RCLCPP_INFO(this->get_logger(), "Vehicle is armed"); 62 | break; 63 | } 64 | std::this_thread::sleep_for(200ms); 65 | this->gateway_->disarm(); 66 | std::this_thread::sleep_for(200ms); 67 | } 68 | } 69 | } 70 | 71 | while (this->gateway_->get_altitude() > -2.5 && !this->stopped_) { 72 | std::this_thread::sleep_for(50ms); 73 | this->gateway_->set_offboard_control_mode(vehicle_gateway::POSITION); 74 | this->gateway_->set_local_position_setpoint(0, 0, -5, 0); 75 | RCLCPP_INFO(this->get_logger(), "altitude %.2f", this->gateway_->get_altitude()); 76 | } 77 | 78 | std::chrono::time_point last_update_time = 79 | std::chrono::system_clock::now(); 80 | 81 | while (!this->stopped_) { 82 | this->gateway_->set_offboard_control_mode(vehicle_gateway::POSITION); 83 | this->gateway_->set_local_position_setpoint( 84 | this->radius * cos(this->theta), 85 | this->radius * sin(this->theta), 86 | -3, 87 | 0); 88 | 89 | std::chrono::time_point now = std::chrono::system_clock::now(); 90 | 91 | auto diff = 92 | std::chrono::duration_cast(now - last_update_time); 93 | this->theta = this->theta + this->omega * diff.count() / 1000.0; 94 | 95 | last_update_time = now; 96 | std::this_thread::sleep_for(50ms); 97 | } 98 | RCLCPP_INFO(this->get_logger(), "Vehicle end"); 99 | } 100 | 101 | ~DroneCircles() 102 | { 103 | this->stopped_ = true; 104 | this->thread_loop_.join(); 105 | this->gateway_->destroy(); 106 | } 107 | 108 | private: 109 | pluginlib::ClassLoader loader_; 110 | std::shared_ptr gateway_; 111 | std::thread thread_loop_; 112 | bool stopped_ = false; 113 | 114 | float theta = 0.0; 115 | float radius = 4.0; 116 | float omega = 0.5; 117 | }; 118 | 119 | int main(int argc, const char * argv[]) 120 | { 121 | rclcpp::init(argc, argv); 122 | rclcpp::spin(std::make_shared(argc, argv)); 123 | rclcpp::shutdown(); 124 | return 0; 125 | } 126 | -------------------------------------------------------------------------------- /vehicle_gateway_integration_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(vehicle_gateway_integration_test) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | if(BUILD_TESTING) 7 | find_package(ament_lint_auto REQUIRED) 8 | ament_lint_auto_find_test_dependencies() 9 | 10 | find_package(launch_testing_ament_cmake REQUIRED) 11 | 12 | add_subdirectory(test) 13 | endif() 14 | 15 | ament_package() 16 | -------------------------------------------------------------------------------- /vehicle_gateway_integration_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_integration_test 5 | 0.0.0 6 | Vehicle gateway integration tests 7 | Alejandro Hernández 8 | Apache License 2.0 9 | 10 | ament_index_python 11 | ament_lint_auto 12 | ament_lint_common 13 | launch 14 | launch_ros 15 | launch_testing 16 | launch_testing_ament_cmake 17 | micro_ros_agent 18 | ros_gz_sim 19 | ros_gz_bridge 20 | px4_sim 21 | vehicle_gateway_models 22 | vehicle_gateway_python 23 | vehicle_gateway_python_helpers 24 | vehicle_gateway_worlds 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /vehicle_gateway_integration_test/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_launch_test(test_px4.py 2 | TARGET test_arm_disarm_vtol 3 | ARGS "vehicle_type:=standard_vtol" "script_test:=arm_disarm.py" 4 | TIMEOUT 120 5 | ) 6 | 7 | add_launch_test(test_px4.py 8 | TARGET test_arm_disarm_x500 9 | ARGS "vehicle_type:=x500" "script_test:=arm_disarm.py" 10 | TIMEOUT 120 11 | ) 12 | 13 | add_launch_test(test_px4.py 14 | TARGET test_takeoff_land_vtol 15 | ARGS "vehicle_type:=standard_vtol" "script_test:=takeoff_land.py" 16 | TIMEOUT 120 17 | ) 18 | 19 | add_launch_test(test_px4.py 20 | TARGET test_takeoff_land_x500 21 | ARGS "vehicle_type:=x500" "script_test:=takeoff_land.py" 22 | TIMEOUT 120 23 | ) 24 | 25 | add_launch_test(test_px4_multirobot.py 26 | TARGET test_takeoff_land_multirobot_x500 27 | ARGS "vehicle_type:=x500" "script_test:=takeoff_land_multirobot.py" 28 | TIMEOUT 120 29 | ) 30 | 31 | install( 32 | PROGRAMS 33 | script/arm_disarm.py 34 | script/takeoff_land_multirobot.py 35 | script/takeoff_land.py 36 | DESTINATION lib/${PROJECT_NAME} 37 | ) 38 | -------------------------------------------------------------------------------- /vehicle_gateway_integration_test/test/script/arm_disarm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import sys 17 | import time 18 | 19 | import vehicle_gateway 20 | from vehicle_gateway import ArmingState 21 | 22 | vg = vehicle_gateway.init(args=sys.argv, plugin_type='px4') 23 | while vg.get_arming_state() != ArmingState.ARMED: 24 | vg.arm() 25 | time.sleep(0.1) 26 | 27 | while vg.get_arming_state() != ArmingState.STANDBY: 28 | vg.disarm() 29 | time.sleep(0.1) 30 | 31 | vg.destroy() 32 | -------------------------------------------------------------------------------- /vehicle_gateway_integration_test/test/script/takeoff_land.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import sys 17 | import time 18 | 19 | import vehicle_gateway 20 | from vehicle_gateway import ArmingState 21 | 22 | 23 | vg = vehicle_gateway.init(args=sys.argv, plugin_type='px4') 24 | while vg.get_arming_state() != ArmingState.ARMED: 25 | vg.arm() 26 | time.sleep(0.5) 27 | 28 | vg.takeoff() 29 | 30 | while vg.get_altitude() > -3.0: 31 | print(f'altitude: {vg.get_altitude()}') 32 | sys.stdout.flush() 33 | time.sleep(0.25) 34 | 35 | vg.land() 36 | 37 | while vg.get_altitude() < -1.0: 38 | print(f'altitude: {vg.get_altitude()}') 39 | sys.stdout.flush() 40 | time.sleep(0.25) 41 | 42 | print('Landed. Disarming...') 43 | sys.stdout.flush() 44 | 45 | while vg.get_arming_state() != ArmingState.STANDBY: 46 | vg.disarm() 47 | time.sleep(0.5) 48 | vg.destroy() 49 | -------------------------------------------------------------------------------- /vehicle_gateway_integration_test/test/script/takeoff_land_multirobot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import sys 17 | import time 18 | 19 | import vehicle_gateway 20 | from vehicle_gateway import ArmingState 21 | 22 | for i in range(1, 3): 23 | vg = vehicle_gateway.init(args=sys.argv, plugin_type='px4', vehicle_id=i) 24 | while vg.get_arming_state() != ArmingState.ARMED: 25 | print('arming') 26 | vg.arm() 27 | time.sleep(0.5) 28 | 29 | vg.takeoff() 30 | 31 | while vg.get_altitude() > -3.0: 32 | print(f'altitude: {vg.get_altitude()}') 33 | sys.stdout.flush() 34 | time.sleep(0.25) 35 | 36 | vg.land() 37 | 38 | while vg.get_altitude() < -1.0: 39 | print(f'altitude: {vg.get_altitude()}') 40 | sys.stdout.flush() 41 | time.sleep(0.25) 42 | 43 | print('Landed. Disarming...') 44 | sys.stdout.flush() 45 | 46 | while vg.get_arming_state() != ArmingState.STANDBY: 47 | vg.disarm() 48 | time.sleep(0.5) 49 | vg.destroy() 50 | -------------------------------------------------------------------------------- /vehicle_gateway_models/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(vehicle_gateway_models) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | install( 8 | DIRECTORY 9 | configs_px4 10 | models 11 | DESTINATION 12 | share/${PROJECT_NAME}/ 13 | ) 14 | 15 | if(BUILD_TESTING) 16 | find_package(ament_lint_auto REQUIRED) 17 | ament_lint_auto_find_test_dependencies() 18 | endif() 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/rc_cessna_stock/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | RC Cessna 182 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Jenn Nguyen 9 | jenn@openrobotics.org 10 | 11 | 12 | 13 | This is importing the rc_cessna model from px4 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/rc_cessna_stock/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://rc_cessna 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/standard_vtol_camera/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | standard_vtol_camera 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Morgan Quigley 9 | morgan@openrobotics.org 10 | 11 | 12 | 13 | standard_vtol with camera 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/standard_vtol_camera/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | true 7 | true 8 | true 9 | 30 10 | 11 | 12 | 13 | model://standard_vtol_stock 14 | 15 | 16 | 17 | model://camera 18 | 0 0 0.18 0 1.57 0 19 | 20 | 21 | 22 | base_link 23 | camera_link 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/standard_vtol_stock/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Standard VTOL 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Jenn Nguyen 9 | jenn@openrobotics.org 10 | 11 | 12 | 13 | This is importing the standard_vtol model from px4 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/standard_vtol_stock/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://standard_vtol 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/x500_camera/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | x500_camera 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Alejandro Hernandez Cordero 9 | alejandro@openrobotics.org 10 | 11 | 12 | 13 | x500 with camera 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/x500_camera/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | true 7 | true 8 | true 9 | 62 10 | 11 | 12 | 13 | model://x500 14 | 15 | 16 | 17 | model://camera 18 | 19 | 20 | 21 | base_link 22 | camera_link 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/x500_stock/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | x500 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Jenn Nguyen 9 | jenn@openrobotics.org 10 | 11 | 12 | 13 | This is importing the x500 model from px4 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/configs_px4/x500_stock/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://x500 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /vehicle_gateway_models/models/aruco_marker/materials/aruco_4x4_50_id_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/vehicle_gateway/cfe101602d84192286de2626650cda1a8e81f971/vehicle_gateway_models/models/aruco_marker/materials/aruco_4x4_50_id_1.png -------------------------------------------------------------------------------- /vehicle_gateway_models/models/aruco_marker/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | aruco_marker 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Alejandro Hernandez Cordero 9 | alejandro@openrobotics.org 10 | 11 | 12 | 13 | Aruco marker 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/models/aruco_marker/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | 4 4 0.1 11 | 12 | 13 | 14 | 1.0 1.0 1.0 15 | 1.0 1.0 1.0 16 | 1.0 1.0 1.0 17 | 18 | 19 | model://aruco_marker/materials/aruco_4x4_50_id_1.png 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /vehicle_gateway_models/models/camera/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Alejandro Hernandez Cordero 9 | alejandro@openrobotics.org 10 | 11 | 12 | 13 | Camera 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/models/camera/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.1 0 1.57 0 5 | 6 | 7 | 0.01 0.025 0.025 0 0 0 8 | 0.01 9 | 10 | 4.15e-6 11 | 0 12 | 0 13 | 2.407e-6 14 | 0 15 | 2.407e-6 16 | 17 | 18 | 19 | 20 | 21 | 0.02 0.05 0.05 22 | 23 | 24 | 25 | 26 | 27 | true 28 | 30.0 29 | true 30 | 31 | 1.047 32 | 33 | 640 34 | 480 35 | 36 | 37 | 0.1 38 | 100 39 | 40 | 41 | gaussian 42 | 0.0 43 | 0.001 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /vehicle_gateway_models/models/standard_vtol_camera/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | standard_vtol_camera 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Morgan Quigley 9 | morgan@openrobotics.org 10 | 11 | 12 | 13 | standard_vtol with camera 14 | 15 | 16 | -------------------------------------------------------------------------------- /vehicle_gateway_models/models/standard_vtol_camera/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | true 7 | true 8 | true 9 | 30 10 | 11 | 12 | 13 | model://standard_vtol 14 | 15 | 16 | 17 | model://camera 18 | 19 | 20 | 21 | base_link 22 | camera_link 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /vehicle_gateway_models/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_models 5 | 0.0.0 6 | Provides drone models 7 | Alejandro Hernandez Cordero 8 | Apache License 2.0 9 | 10 | Alejandro Hernandez Cordero 11 | 12 | ament_cmake 13 | 14 | ament_lint_auto 15 | ament_cmake_lint_cmake 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /vehicle_gateway_multi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(vehicle_gateway_multi) 4 | 5 | #------------------------------------------------------------------------ 6 | # Compile as C++17 7 | 8 | set(CMAKE_CXX_STANDARD 17) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | 11 | # Compiler options 12 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 13 | add_compile_options(-Wall -Wextra -Wpedantic) 14 | endif() 15 | 16 | find_package(ament_cmake REQUIRED) 17 | 18 | find_package(ament_cmake) 19 | find_package(px4_msgs REQUIRED) 20 | find_package(nlohmann_json REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(std_msgs REQUIRED) 23 | find_package(yaml_cpp_vendor REQUIRED) 24 | find_package(zenohc REQUIRED) 25 | 26 | add_executable(vehicle_gateway_multi_bridge 27 | src/vehicle_gateway_multi_bridge.cpp 28 | ) 29 | 30 | ament_target_dependencies(vehicle_gateway_multi_bridge 31 | px4_msgs 32 | nlohmann_json 33 | rclcpp 34 | std_msgs 35 | yaml_cpp_vendor 36 | ) 37 | 38 | target_link_libraries(vehicle_gateway_multi_bridge 39 | zenohc::lib 40 | ) 41 | 42 | add_executable(vehicle_gateway_multi_bridge_client 43 | src/vehicle_gateway_multi_bridge_client.cpp 44 | ) 45 | target_link_libraries(vehicle_gateway_multi_bridge_client 46 | zenohc::lib 47 | ) 48 | 49 | install(DIRECTORY 50 | config 51 | DESTINATION 52 | share/${PROJECT_NAME} 53 | ) 54 | 55 | install( 56 | TARGETS 57 | vehicle_gateway_multi_bridge 58 | vehicle_gateway_multi_bridge_client 59 | DESTINATION lib/${PROJECT_NAME} 60 | ) 61 | 62 | if(BUILD_TESTING) 63 | find_package(ament_lint_auto REQUIRED) 64 | ament_lint_auto_find_test_dependencies() 65 | endif() 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /vehicle_gateway_multi/README.md: -------------------------------------------------------------------------------- 1 | # vehicle_gateway_multi 2 | 3 | This package will create a bridge between the ROS 2 topic `/px4_/fmu/out/vehicle_gps_position"` 4 | and the zenoh key `/vehicle_gateway//telemetry`. 5 | 6 | # Test it 7 | 8 | Launch the `px4_sim_multi.launch.py` example from the `px4_sim` package: 9 | 10 | ```bash 11 | export MULTIROBOT_CONFIG=/src/vehicle_gateway/px4_sim/config/multi.yaml 12 | ros2 launch px4_sim px4_sim_multi.launch.py 13 | ``` 14 | 15 | This launch file will launch a Gazebo simulation with two vehicles: one quadcopter and one vtol both in the same DDS domain id. 16 | If you want to use a different DDS domain id you can easily modify this in the `multi.yaml` file. 17 | 18 | Then you should run the ROS 2 <-> Zenoh bridge. Type this for the vehicle one: 19 | 20 | ```bash 21 | # export ROS_DOMAIN_ID= 22 | ros2 run vehicle_gateway_multi vehicle_gateway_multi_bridge /src/vehicle_gateway/vehicle_gateway_multi/config/zenoh_all_localhost.json5 1 23 | ``` 24 | 25 | Type this for the vehicle two: 26 | ```bash 27 | # export ROS_DOMAIN_ID= 28 | ros2 run vehicle_gateway_multi vehicle_gateway_multi_bridge /src/vehicle_gateway/vehicle_gateway_multi/config/zenoh_all_localhost.json5 2 29 | ``` 30 | 31 | Now we can check that everything is working if we use the following program to subscribe the zenoh key `vehicle_gateway/*/telemetry` 32 | 33 | ```bash 34 | ros2 run vehicle_gateway_multi vehicle_gateway_multi_bridge_client 35 | ``` 36 | -------------------------------------------------------------------------------- /vehicle_gateway_multi/config/zenoh_all_localhost.json5: -------------------------------------------------------------------------------- 1 | { 2 | mode: "peer", 3 | connect: { 4 | endpoints: [ 5 | ], 6 | }, 7 | listen: { 8 | endpoints: [ 9 | "tcp/0.0.0.0:0", 10 | ], 11 | }, 12 | } 13 | -------------------------------------------------------------------------------- /vehicle_gateway_multi/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_multi 5 | 0.0.0 6 | Multiple vehicle communications for Vehicle Gateway 7 | Morgan Quigley 8 | Apache License 2.0 9 | 10 | Morgan Quigley 11 | 12 | ament_cmake 13 | 14 | nlohmann-json-dev 15 | px4_msgs 16 | rclcpp 17 | std_msgs 18 | vehicle_gateway 19 | yaml_cpp_vendor 20 | zenohc 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /vehicle_gateway_multi/src/vehicle_gateway_multi_bridge_client.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | const char * kind_to_str(z_sample_kind_t kind); 20 | 21 | void data_handler(const z_sample_t * sample, void * /*arg*/) 22 | { 23 | z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); 24 | printf( 25 | ">> [Subscriber] Received %s ('%s': '%.*s')\n", kind_to_str(sample->kind), z_loan(keystr), 26 | static_cast(sample->payload.len), sample->payload.start); 27 | z_drop(z_move(keystr)); 28 | } 29 | 30 | int main(int argc, char ** argv) 31 | { 32 | const char * expr = "vehicle_gateway/*/state"; 33 | if (argc > 1) { 34 | expr = argv[1]; 35 | } 36 | 37 | z_owned_config_t config = z_config_default(); 38 | if (argc > 2) { 39 | if (zc_config_insert_json(z_loan(config), Z_CONFIG_LISTEN_KEY, argv[2]) < 0) { 40 | printf( 41 | "Couldn't insert value `%s` in configuration at `%s`. " 42 | "This is likely because `%s` expects a " 43 | "JSON-serialized list of strings\n", 44 | argv[2], Z_CONFIG_LISTEN_KEY, 45 | Z_CONFIG_LISTEN_KEY); 46 | exit(-1); 47 | } 48 | } 49 | 50 | printf("Opening session...\n"); 51 | z_owned_session_t s = z_open(z_move(config)); 52 | if (!z_check(s)) { 53 | printf("Unable to open session!\n"); 54 | exit(-1); 55 | } 56 | 57 | z_owned_closure_sample_t callback = z_closure(data_handler); 58 | printf("Declaring Subscriber on '%s'...\n", expr); 59 | z_owned_subscriber_t sub = 60 | z_declare_subscriber(z_loan(s), z_keyexpr(expr), z_move(callback), NULL); 61 | if (!z_check(sub)) { 62 | printf("Unable to declare subscriber.\n"); 63 | exit(-1); 64 | } 65 | 66 | printf("Enter 'q' to quit...\n"); 67 | char c = 0; 68 | while (c != 'q') { 69 | c = getchar(); 70 | if (c == -1) { 71 | sleep(1); 72 | } 73 | } 74 | 75 | z_undeclare_subscriber(z_move(sub)); 76 | z_close(z_move(s)); 77 | return 0; 78 | } 79 | 80 | const char * kind_to_str(z_sample_kind_t kind) 81 | { 82 | switch (kind) { 83 | case Z_SAMPLE_KIND_PUT: 84 | return "PUT"; 85 | case Z_SAMPLE_KIND_DELETE: 86 | return "DELETE"; 87 | default: 88 | return "UNKNOWN"; 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /vehicle_gateway_px4/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vehicle_gateway_px4) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | # Default to C11 9 | if(NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 11) 11 | endif() 12 | 13 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra -Wpedantic) 15 | endif() 16 | 17 | # find dependencies 18 | find_package(ament_cmake REQUIRED) 19 | find_package(ament_cmake_ros REQUIRED) 20 | find_package(vehicle_gateway REQUIRED) 21 | find_package(pluginlib REQUIRED) 22 | find_package(px4_msgs REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(tf2 REQUIRED) 25 | find_package(zenohc REQUIRED) 26 | 27 | add_library(vehicle_gateway_px4 src/vehicle_gateway_px4.cpp) 28 | target_include_directories(vehicle_gateway_px4 PUBLIC 29 | $ 30 | $) 31 | ament_target_dependencies( 32 | vehicle_gateway_px4 33 | pluginlib 34 | px4_msgs 35 | rclcpp 36 | vehicle_gateway 37 | tf2 38 | ) 39 | target_link_libraries(vehicle_gateway_px4 40 | zenohc::lib 41 | ) 42 | 43 | pluginlib_export_plugin_description_file(vehicle_gateway plugins.xml) 44 | 45 | # Causes the visibility macros to use dllexport rather than dllimport, 46 | # which is appropriate when building the dll but not consuming it. 47 | target_compile_definitions(vehicle_gateway_px4 PRIVATE "VEHICLE_GATEWAY_PX4_BUILDING_LIBRARY") 48 | 49 | install( 50 | TARGETS vehicle_gateway_px4 51 | EXPORT export_${PROJECT_NAME} 52 | ARCHIVE DESTINATION lib 53 | LIBRARY DESTINATION lib 54 | RUNTIME DESTINATION bin 55 | ) 56 | 57 | install( 58 | DIRECTORY include/ 59 | DESTINATION include 60 | ) 61 | 62 | install( 63 | DIRECTORY launch/ 64 | DESTINATION share/${PROJECT_NAME} 65 | ) 66 | 67 | if(BUILD_TESTING) 68 | find_package(ament_lint_auto REQUIRED) 69 | ament_lint_auto_find_test_dependencies() 70 | endif() 71 | 72 | ament_export_libraries( 73 | vehicle_gateway_px4 74 | ) 75 | ament_export_targets( 76 | export_${PROJECT_NAME} 77 | ) 78 | ament_export_dependencies( 79 | px4_msgs 80 | pluginlib 81 | rclcpp 82 | vehicle_gateway 83 | tf2 84 | zenohc 85 | ) 86 | 87 | ament_export_include_directories( 88 | include 89 | ) 90 | ament_package() 91 | -------------------------------------------------------------------------------- /vehicle_gateway_px4/launch/px4.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /vehicle_gateway_px4/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_px4 5 | 0.0.0 6 | PX4 plugin for the Vehicle Gateway 7 | Morgan Quigley 8 | Apache License 2.0 9 | 10 | ament_cmake_ros 11 | 12 | vehicle_gateway 13 | pluginlib 14 | rclcpp 15 | px4_msgs 16 | tf2 17 | zenohc 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /vehicle_gateway_px4/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Vehicle Gateway implementation for PX4 4 | 5 | 6 | -------------------------------------------------------------------------------- /vehicle_gateway_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(vehicle_gateway_python) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | endif() 9 | # Default to C11 10 | if(NOT CMAKE_C_STANDARD) 11 | set(CMAKE_C_STANDARD 11) 12 | endif() 13 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra) 15 | endif() 16 | 17 | # Figure out Python3 debug/release before anything else can find_package it 18 | if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") 19 | find_package(python_cmake_module REQUIRED) 20 | find_package(PythonExtra REQUIRED) 21 | 22 | # Force FindPython3 to use the debug interpreter where ROS 2 expects it 23 | set(Python3_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}") 24 | endif() 25 | 26 | find_package(ament_cmake REQUIRED) 27 | find_package(ament_cmake_python REQUIRED) 28 | find_package(pluginlib REQUIRED) 29 | find_package(vehicle_gateway REQUIRED) 30 | find_package(vehicle_gateway_px4 REQUIRED) 31 | 32 | # Find python before pybind11 33 | find_package(Python3 REQUIRED COMPONENTS Interpreter Development) 34 | 35 | find_package(pybind11_vendor REQUIRED) 36 | find_package(pybind11 REQUIRED) 37 | 38 | ament_python_install_package(vehicle_gateway) 39 | 40 | # Set the build location and install location for a CPython extension 41 | function(configure_build_install_location _library_name) 42 | # Install library for actual use 43 | install(TARGETS ${_library_name} 44 | DESTINATION "${PYTHON_INSTALL_DIR}/vehicle_gateway" 45 | ) 46 | endfunction() 47 | 48 | # Split from main extension and converted to pybind11 49 | pybind11_add_module(_vehicle_gateway_pybind11 SHARED 50 | src/vehicle_gateway_python/_vehicle_gateway_pybind11.cpp 51 | src/vehicle_gateway_python/destroyable.cpp 52 | src/vehicle_gateway_python/vehicle_gateway.cpp 53 | ) 54 | 55 | target_link_libraries(_vehicle_gateway_pybind11 PRIVATE 56 | pluginlib::pluginlib 57 | vehicle_gateway::vehicle_gateway 58 | vehicle_gateway_px4::vehicle_gateway_px4 59 | ) 60 | 61 | configure_build_install_location(_vehicle_gateway_pybind11) 62 | 63 | if(NOT WIN32) 64 | ament_environment_hooks( 65 | "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}" 66 | ) 67 | endif() 68 | 69 | if(BUILD_TESTING) 70 | find_package(ament_lint_auto REQUIRED) 71 | # TODO(anyone): This test is not working in CI 72 | list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_pep257) 73 | ament_lint_auto_find_test_dependencies() 74 | endif() 75 | 76 | ament_package() 77 | -------------------------------------------------------------------------------- /vehicle_gateway_python/examples/param_tuning_attempts/4004_gz_standard_vtol: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # 3 | # @name Standard VTOL 4 | # 5 | # @type Standard VTOL 6 | # 7 | 8 | . ${R}etc/init.d/rc.vtol_defaults 9 | 10 | PX4_SIMULATOR=${PX4_SIMULATOR:=gz} 11 | PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} 12 | PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} 13 | 14 | param set-default SIM_GZ_EN 1 15 | 16 | param set-default SENS_EN_GPSSIM 1 17 | param set-default SENS_EN_BAROSIM 0 18 | param set-default SENS_EN_MAGSIM 1 19 | param set-default SENS_EN_ARSPDSIM 1 20 | 21 | # TODO: Enable motor failure detection when the 22 | # VTOL no longer reports 0A for all ESCs in SITL 23 | param set-default FD_ACT_EN 0 24 | param set-default FD_ACT_MOT_TOUT 500 25 | 26 | param set-default CA_AIRFRAME 2 27 | 28 | param set-default COM_PREARM_MODE 2 29 | 30 | param set-default CA_ROTOR_COUNT 5 31 | param set-default CA_ROTOR0_PX 0.1515 32 | param set-default CA_ROTOR0_PY 0.245 33 | param set-default CA_ROTOR0_KM 0.05 34 | param set-default CA_ROTOR1_PX -0.1515 35 | param set-default CA_ROTOR1_PY -0.1875 36 | param set-default CA_ROTOR1_KM 0.05 37 | param set-default CA_ROTOR2_PX 0.1515 38 | param set-default CA_ROTOR2_PY -0.245 39 | param set-default CA_ROTOR2_KM -0.05 40 | param set-default CA_ROTOR3_PX -0.1515 41 | param set-default CA_ROTOR3_PY 0.1875 42 | param set-default CA_ROTOR3_KM -0.05 43 | param set-default CA_ROTOR4_AX 1.0 44 | param set-default CA_ROTOR4_AZ 0.0 45 | param set-default CA_ROTOR4_PX 0.2 46 | 47 | param set-default SIM_GZ_EC_FUNC1 101 48 | param set-default SIM_GZ_EC_MIN1 10 49 | param set-default SIM_GZ_EC_MAX1 1500 50 | param set-default SIM_GZ_EC_FUNC2 102 51 | param set-default SIM_GZ_EC_MIN2 10 52 | param set-default SIM_GZ_EC_MAX2 1500 53 | param set-default SIM_GZ_EC_FUNC3 103 54 | param set-default SIM_GZ_EC_MIN3 10 55 | param set-default SIM_GZ_EC_MAX3 1500 56 | param set-default SIM_GZ_EC_FUNC4 104 57 | param set-default SIM_GZ_EC_MIN4 10 58 | param set-default SIM_GZ_EC_MAX4 1500 59 | 60 | param set-default SIM_GZ_EC_FUNC5 105 61 | param set-default SIM_GZ_EC_MIN5 0 62 | param set-default SIM_GZ_EC_MAX5 3500 63 | 64 | param set-default SIM_GZ_SV_FUNC1 201 65 | param set-default SIM_GZ_SV_FUNC2 202 66 | param set-default SIM_GZ_SV_FUNC3 203 67 | 68 | param set-default COM_RC_IN_MODE 1 69 | param set-default ASPD_PRIMARY 1 70 | 71 | param set-default CA_SV_CS_COUNT 3 72 | param set-default CA_SV_CS0_TYPE 1 73 | param set-default CA_SV_CS0_TRQ_R -0.5 74 | param set-default CA_SV_CS1_TYPE 2 75 | param set-default CA_SV_CS1_TRQ_R 0.5 76 | param set-default CA_SV_CS2_TYPE 3 77 | param set-default CA_SV_CS2_TRQ_P 1.0 78 | 79 | param set-default FW_L1_PERIOD 25 80 | param set-default FW_PR_FF 0.2 81 | param set-default FW_PR_P 0.9 82 | param set-default FW_PSP_OFF 2 83 | param set-default FW_P_LIM_MIN -15 84 | param set-default FW_RR_FF 0.1 85 | param set-default FW_RR_P 0.3 86 | param set-default FW_THR_TRIM 0.25 87 | param set-default FW_THR_MAX 0.6 88 | param set-default FW_THR_MIN 0.05 89 | param set-default FW_T_CLMB_MAX 8 90 | param set-default FW_T_SINK_MAX 2.7 91 | param set-default FW_T_SINK_MIN 2.2 92 | 93 | param set-default MC_AIRMODE 1 94 | param set-default MC_ROLLRATE_P 0.3 95 | param set-default MC_YAW_P 1.6 96 | 97 | param set-default MIS_TAKEOFF_ALT 10 98 | 99 | param set-default MPC_ACC_HOR_MAX 2 100 | param set-default MPC_XY_P 0.8 101 | param set-default MPC_XY_VEL_P_ACC 3 102 | param set-default MPC_XY_VEL_I_ACC 4 103 | param set-default MPC_XY_VEL_D_ACC 0.1 104 | 105 | param set-default NAV_ACC_RAD 5 106 | 107 | param set-default VT_FWD_THRUST_EN 4 108 | param set-default VT_F_TRANS_THR 0.75 109 | param set-default VT_B_TRANS_DUR 8 110 | param set-default VT_TYPE 2 111 | param set-default FD_ESCS_EN 0 112 | -------------------------------------------------------------------------------- /vehicle_gateway_python/examples/position_control.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | import sys 17 | import time 18 | 19 | import vehicle_gateway 20 | from vehicle_gateway import ArmingState, ControllerType, FlightMode 21 | 22 | px4_gateway = vehicle_gateway.init(args=sys.argv, plugin_type='px4') 23 | 24 | print(px4_gateway.get_arming_state()) 25 | 26 | px4_gateway.disarm() 27 | 28 | _start_time = time.perf_counter() 29 | offboard_setpoint_counter_ = 0 30 | while 1: 31 | current_time = time.perf_counter() 32 | elapsed_time = current_time - _start_time 33 | if elapsed_time >= 1: 34 | if offboard_setpoint_counter_ < 6: 35 | offboard_setpoint_counter_ += 1 36 | 37 | if offboard_setpoint_counter_ == 5: 38 | while px4_gateway.get_flight_mode() != FlightMode.OFFBOARD: 39 | px4_gateway.set_offboard_mode() 40 | print('Setting offboard mode') 41 | time.sleep(0.01) 42 | while px4_gateway.get_arming_state() != ArmingState.ARMED: 43 | px4_gateway.arm() 44 | print('Arming') 45 | time.sleep(0.01) 46 | 47 | px4_gateway.set_offboard_control_mode(ControllerType.POSITION) 48 | px4_gateway.set_local_position_setpoint(0, -5, -5, 0) 49 | 50 | _start_time = current_time 51 | time.sleep(0.1) 52 | -------------------------------------------------------------------------------- /vehicle_gateway_python/examples/test_takeoff_land.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import sys 16 | import time 17 | 18 | import vehicle_gateway 19 | 20 | px4_gateway = vehicle_gateway.init(args=sys.argv, plugin_type='px4', vehicle_id=0) 21 | 22 | print('Arming...') 23 | px4_gateway.arm_sync() 24 | 25 | time.sleep(2) # not sure why this is needed - perhaps some internal state setting 26 | 27 | print('Takeoff!') 28 | px4_gateway.takeoff() 29 | 30 | time.sleep(10) 31 | 32 | x, y, z = px4_gateway.get_local_position() 33 | print(f'Current position: (x: {x:.2f}, y: {y:.2f}, z: {z:.2f})') 34 | 35 | print('Landing...') 36 | px4_gateway.land() 37 | time.sleep(10) 38 | 39 | print('Disarming...') 40 | px4_gateway.disarm_sync() 41 | 42 | px4_gateway.destroy() 43 | print('Takeoff and land demo complete.') 44 | -------------------------------------------------------------------------------- /vehicle_gateway_python/examples/velocity_control.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | import sys 17 | import time 18 | 19 | import vehicle_gateway 20 | from vehicle_gateway import ArmingState, ControllerType, FlightMode 21 | 22 | px4_gateway = vehicle_gateway.init(args=sys.argv, plugin_type='px4') 23 | 24 | print(px4_gateway.get_arming_state()) 25 | 26 | px4_gateway.disarm() 27 | 28 | _start_time = time.perf_counter() 29 | offboard_setpoint_counter_ = 0 30 | while 1: 31 | current_time = time.perf_counter() 32 | elapsed_time = current_time - _start_time 33 | if elapsed_time >= 1: 34 | if offboard_setpoint_counter_ < 6: 35 | offboard_setpoint_counter_ += 1 36 | 37 | if offboard_setpoint_counter_ == 5: 38 | while px4_gateway.get_flight_mode() != FlightMode.OFFBOARD: 39 | px4_gateway.set_offboard_mode() 40 | print('Setting offboard mode') 41 | time.sleep(0.01) 42 | while px4_gateway.get_arming_state() != ArmingState.ARMED: 43 | px4_gateway.arm() 44 | print('Arming') 45 | time.sleep(0.01) 46 | 47 | px4_gateway.set_offboard_control_mode(ControllerType.VELOCITY) 48 | px4_gateway.set_local_velocity_setpoint(0, 0, -5, 5) 49 | 50 | _start_time = current_time 51 | time.sleep(0.1) 52 | -------------------------------------------------------------------------------- /vehicle_gateway_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_python 5 | 0.0.0 6 | Vehicle gateway python wrapper 7 | 8 | Alejandro Hernandez Cordero 9 | Apache License 2.0 10 | 11 | Alejandro Hernandez Cordero 12 | 13 | ament_cmake 14 | python_cmake_module 15 | 16 | pluginlib 17 | vehicle_gateway 18 | vehicle_gateway_px4 19 | 20 | pybind11_vendor 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | python3-sphinx 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /vehicle_gateway_python/src/vehicle_gateway_python/_vehicle_gateway_pybind11.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | 18 | #include "destroyable.hpp" 19 | #include "exceptions.hpp" 20 | #include "vehicle_gateway.hpp" 21 | 22 | namespace py = pybind11; 23 | 24 | PYBIND11_MODULE(_vehicle_gateway_pybind11, m) { 25 | m.doc() = "Vehicle gateway Python client library."; 26 | 27 | vehicle_gateway_python::define_destroyable(m); 28 | 29 | vehicle_gateway_python::define_vehicle_gateway(m); 30 | } 31 | -------------------------------------------------------------------------------- /vehicle_gateway_python/src/vehicle_gateway_python/destroyable.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "destroyable.hpp" 21 | #include "exceptions.hpp" 22 | 23 | namespace vehicle_gateway_python 24 | { 25 | Destroyable::Destroyable(const Destroyable &) 26 | { 27 | // When a destroyable is copied, it does not matter if someone asked 28 | // to destroy the original. The copy has its own lifetime. 29 | use_count = 0; 30 | please_destroy_ = false; 31 | } 32 | 33 | void 34 | Destroyable::enter() 35 | { 36 | if (please_destroy_) { 37 | throw InvalidHandle("cannot use Destroyable because destruction was requested"); 38 | } 39 | ++use_count; 40 | } 41 | 42 | void 43 | Destroyable::exit(py::object, py::object, py::object) 44 | { 45 | if (0u == use_count) { 46 | throw std::runtime_error("Internal error: Destroyable use_count would be negative"); 47 | } 48 | 49 | --use_count; 50 | if (please_destroy_ && 0u == use_count) { 51 | destroy(); 52 | } 53 | } 54 | 55 | void 56 | Destroyable::destroy() 57 | { 58 | // Normally would be pure virtual, but then pybind11 can't create bindings for this class 59 | throw NotImplementedError("Internal error: Destroyable subclass didn't override destroy()"); 60 | } 61 | 62 | void 63 | Destroyable::destroy_when_not_in_use() 64 | { 65 | if (please_destroy_) { 66 | // already asked to destroy 67 | return; 68 | } 69 | please_destroy_ = true; 70 | if (0u == use_count) { 71 | destroy(); 72 | } 73 | } 74 | 75 | void 76 | define_destroyable(py::object module) 77 | { 78 | py::class_>(module, "Destroyable") 79 | .def("__enter__", &Destroyable::enter) 80 | .def("__exit__", &Destroyable::exit) 81 | .def( 82 | "destroy_when_not_in_use", &Destroyable::destroy_when_not_in_use, 83 | "Forcefully destroy the rcl object as soon as it's not actively being used"); 84 | } 85 | } // namespace vehicle_gateway_python 86 | -------------------------------------------------------------------------------- /vehicle_gateway_python/src/vehicle_gateway_python/destroyable.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef VEHICLE_GATEWAY_PYTHON__DESTROYABLE_HPP_ 16 | #define VEHICLE_GATEWAY_PYTHON__DESTROYABLE_HPP_ 17 | 18 | #include 19 | 20 | namespace py = pybind11; 21 | 22 | namespace vehicle_gateway_python 23 | { 24 | /// This class blocks destruction when in use 25 | class Destroyable 26 | { 27 | public: 28 | /// Default constructor 29 | Destroyable() = default; 30 | 31 | /// Copy constructor 32 | Destroyable(const Destroyable & other); 33 | 34 | /// Context manager __enter__ - block destruction 35 | void 36 | enter(); 37 | 38 | /// Context manager __exit__ - unblock destruction 39 | void 40 | exit(py::object pytype, py::object pyvalue, py::object pytraceback); 41 | 42 | /// Signal that the object should be destroyed as soon as it's not in use 43 | void 44 | destroy_when_not_in_use(); 45 | 46 | /// Override this to destroy an object 47 | virtual 48 | void 49 | destroy(); 50 | 51 | virtual 52 | ~Destroyable() = default; 53 | 54 | private: 55 | size_t use_count = 0u; 56 | bool please_destroy_ = false; 57 | }; 58 | 59 | /// Define a pybind11 wrapper for an vehicle_gateway_python::Destroyable 60 | /** 61 | * \param[in] module a pybind11 module to add the definition to 62 | */ 63 | void define_destroyable(py::object module); 64 | } // namespace vehicle_gateway_python 65 | 66 | #endif // VEHICLE_GATEWAY_PYTHON__DESTROYABLE_HPP_ 67 | -------------------------------------------------------------------------------- /vehicle_gateway_python/src/vehicle_gateway_python/exceptions.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef VEHICLE_GATEWAY_PYTHON__EXCEPTIONS_HPP_ 16 | #define VEHICLE_GATEWAY_PYTHON__EXCEPTIONS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace vehicle_gateway_python 22 | { 23 | class VehicleGatewayError : public std::runtime_error 24 | { 25 | public: 26 | explicit VehicleGatewayError(const std::string & error_text); 27 | 28 | ~VehicleGatewayError() = default; 29 | }; 30 | 31 | class NotImplementedError : public std::runtime_error 32 | { 33 | using std::runtime_error::runtime_error; 34 | }; 35 | 36 | class InvalidHandle : public std::runtime_error 37 | { 38 | using std::runtime_error::runtime_error; 39 | }; 40 | 41 | } // namespace vehicle_gateway_python 42 | 43 | #endif // VEHICLE_GATEWAY_PYTHON__EXCEPTIONS_HPP_ 44 | -------------------------------------------------------------------------------- /vehicle_gateway_python/src/vehicle_gateway_python/vehicle_gateway.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef VEHICLE_GATEWAY_PYTHON__VEHICLE_GATEWAY_HPP_ 16 | #define VEHICLE_GATEWAY_PYTHON__VEHICLE_GATEWAY_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "destroyable.hpp" 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace py = pybind11; 32 | 33 | namespace vehicle_gateway_python 34 | { 35 | class VehicleGatewayPython : public Destroyable, 36 | public std::enable_shared_from_this 37 | { 38 | public: 39 | VehicleGatewayPython( 40 | const std::vector & _args, 41 | const std::string & _plugin_name, 42 | unsigned int _vehicle_id); 43 | 44 | ~VehicleGatewayPython(); 45 | 46 | /// Get VehicleID 47 | unsigned int GetVehicleID(); 48 | 49 | /// Takeoff 50 | void Takeoff(); 51 | 52 | /// Land 53 | void Land(); 54 | 55 | /// Arm 56 | void Arm(); 57 | 58 | /// Arm synchronously 59 | void ArmSync(); 60 | 61 | /// Disarm 62 | void Disarm(); 63 | 64 | /// Disarm synchronously 65 | void DisarmSync(); 66 | 67 | /// Transition to multicopter 68 | void TransitionToMultiCopter(); 69 | 70 | /// Transition to multicopter synchronously 71 | void TransitionToMultiCopterSync(); 72 | 73 | /// Transition to fixed wings 74 | void TransitionToFixedWings(); 75 | 76 | /// Transition to fixed wings synchronously 77 | void TransitionToFixedWingsSync(); 78 | 79 | /// Go to latitude and longitude coordinates 80 | void PublishLatLonSetpoint(double lat, double lon, float alt_amsl); 81 | 82 | /// Get latitude, longitude, and altitude 83 | std::vector GetLatLon(); 84 | 85 | void PublishLocalPositionSetpoint(float x, float y, float z, float yaw); 86 | 87 | void PublishLocalVelocitySetpoint(float vx, float vy, float vz, float yaw_rate); 88 | 89 | void PublishBodyRatesAndThrustSetpoint( 90 | float roll_rate, float pitch_rate, float yaw_rate, 91 | float thrust); 92 | 93 | /// Get flight mode 94 | /// \return Flight mode 95 | vehicle_gateway::FLIGHT_MODE GetFlightMode(); 96 | 97 | /// Get Vehicle type 98 | /// \return Vehicle type 99 | vehicle_gateway::VEHICLE_TYPE GetVehicleType(); 100 | 101 | /// Get the arm state 102 | /// \return Arming state of the robot 103 | vehicle_gateway::ARMING_STATE GetArmingState(); 104 | 105 | /// Get the arm reason 106 | /// \return Arm reason 107 | vehicle_gateway::ARM_DISARM_REASON GetArmReason(); 108 | 109 | /// Get the disarm reason 110 | /// \return Disarm reason 111 | vehicle_gateway::ARM_DISARM_REASON GetDisarmReason(); 112 | 113 | /// Get current failure 114 | /// \return Current failure 115 | vehicle_gateway::FAILURE GetFailure(); 116 | 117 | /// Get local position 118 | /// \return Local position vector 119 | std::vector GetLocalPosition(); 120 | 121 | /// Get Euler attitude 122 | /// \return vector of euler RPY 123 | std::vector GetEulerRPY(); 124 | 125 | /// Get vtol state 126 | /// \return VTOL state 127 | vehicle_gateway::VTOL_STATE GetVtolState(); 128 | 129 | /// Set offboard_control_mode 130 | void SetOffboardControlMode(vehicle_gateway::CONTROLLER_TYPE type); 131 | 132 | /// Set offboard control mode 133 | void SetOffboardMode(); 134 | 135 | /// Set onboard control mode 136 | void SetOnboardMode(); 137 | 138 | /// Get ground speed 139 | /// \return Get ground speed 140 | float GetGroundSpeed(); 141 | 142 | /// Set ground speed speed m/s 143 | /// \param[in] Desired speed in m/s 144 | void SetGroundSpeed(float speed); 145 | 146 | /// Get air speed 147 | /// \return Get air speed 148 | float GetAirSpeed(); 149 | 150 | /// Set air speed speed m/s 151 | /// \param[in] Desired air in m/s 152 | void SetAirSpeed(float speed); 153 | 154 | // Destroy autopilot API 155 | void Destroy(); 156 | 157 | float GetAltitude(); 158 | 159 | private: 160 | std::shared_ptr gateway_; 161 | std::shared_ptr> loader_; 162 | }; 163 | 164 | void 165 | define_vehicle_gateway(py::object module); 166 | 167 | } // namespace vehicle_gateway_python 168 | 169 | #endif // VEHICLE_GATEWAY_PYTHON__VEHICLE_GATEWAY_HPP_ 170 | -------------------------------------------------------------------------------- /vehicle_gateway_python/vehicle_gateway/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from typing import List, Optional 16 | 17 | from vehicle_gateway.impl.implementation_singleton import vehicle_gateway_implementation \ 18 | as _vehicle_gateway 19 | 20 | ArmingState = _vehicle_gateway.ArmingState 21 | ArmDisarmReason = _vehicle_gateway.ArmDisarmReason 22 | FlightMode = _vehicle_gateway.FlightMode 23 | Failure = _vehicle_gateway.Failure 24 | VehicleType = _vehicle_gateway.VehicleType 25 | ControllerType = _vehicle_gateway.ControllerType 26 | VtolState = _vehicle_gateway.VtolState 27 | 28 | 29 | def init( 30 | args: Optional[List[str]] = None, 31 | plugin_type: str = None, 32 | vehicle_id: int = 0, 33 | ) -> None: 34 | args if args is None else [] 35 | return _vehicle_gateway.VehicleGatewayPython(args, plugin_type, vehicle_id) 36 | -------------------------------------------------------------------------------- /vehicle_gateway_python/vehicle_gateway/impl/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | -------------------------------------------------------------------------------- /vehicle_gateway_python/vehicle_gateway/impl/implementation_singleton.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """ 16 | Provide singleton access to the vehicle_gateway_python C modules. 17 | 18 | For example, you might use it like this: 19 | 20 | .. code:: 21 | 22 | from vehicle_gateway_python.impl.implementation_singleton \ 23 | import vehicle_gateway_python_implementation as _vehicle_gateway 24 | 25 | _vehicle_gateway_python.init() 26 | """ 27 | 28 | from rpyutils import import_c_library 29 | package = 'vehicle_gateway' 30 | 31 | vehicle_gateway_implementation = import_c_library('._vehicle_gateway_pybind11', package) 32 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_python_helpers 5 | 0.0.0 6 | Vehicle gateway Python helpers 7 | Alejandro Hernandez Cordero 8 | Apache License 2.0 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/resource/vehicle_gateway_python_helpers: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/vehicle_gateway/cfe101602d84192286de2626650cda1a8e81f971/vehicle_gateway_python_helpers/resource/vehicle_gateway_python_helpers -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/vehicle_gateway_python_helpers 3 | [install] 4 | install_scripts=$base/lib/vehicle_gateway_python_helpers 5 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'vehicle_gateway_python_helpers' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='Alejandro Hernandez Cordero', 17 | maintainer_email='alejandro@osrfoundation.org', 18 | description='Vehicle gateway Python helpers', 19 | license='Apache License 2.0', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /vehicle_gateway_python_helpers/vehicle_gateway_python_helpers/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/vehicle_gateway/cfe101602d84192286de2626650cda1a8e81f971/vehicle_gateway_python_helpers/vehicle_gateway_python_helpers/__init__.py -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vehicle_gateway_sim_performance) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | # Default to C11 9 | if(NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 11) 11 | endif() 12 | 13 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra -Wpedantic) 15 | endif() 16 | 17 | find_package(ament_cmake REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(vehicle_gateway_px4 REQUIRED) 20 | 21 | #============================================================================ 22 | # Find gz-cmake 23 | #============================================================================ 24 | # If you get an error at this line, you need to install gz-cmake 25 | find_package(gz-cmake3 REQUIRED) 26 | 27 | # Find the Gazebo transport library 28 | #-------------------------------------- 29 | # Find gz-transport 30 | gz_find_package(gz-transport12 REQUIRED) 31 | set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR}) 32 | 33 | add_executable(system_metric_collector 34 | src/main.cpp 35 | src/linux_cpu_system_measurement.cpp 36 | src/linux_memory_system_measurement.cpp 37 | ) 38 | target_include_directories(system_metric_collector PUBLIC 39 | $ 40 | $ 41 | ) 42 | 43 | target_link_libraries(system_metric_collector 44 | gz-transport${GZ_TRANSPORT_VER}::core 45 | ) 46 | 47 | ament_export_include_directories( 48 | include 49 | ) 50 | 51 | add_executable(vtol_position_control 52 | src/vtol_position_control.cpp 53 | ) 54 | ament_target_dependencies(vtol_position_control 55 | rclcpp 56 | vehicle_gateway_px4 57 | ) 58 | 59 | install( 60 | TARGETS 61 | system_metric_collector 62 | vtol_position_control 63 | EXPORT 64 | export_${PROJECT_NAME} 65 | DESTINATION 66 | lib/${PROJECT_NAME} 67 | ) 68 | 69 | if(BUILD_TESTING) 70 | find_package(ament_lint_auto REQUIRED) 71 | ament_lint_auto_find_test_dependencies() 72 | 73 | find_package(launch_testing_ament_cmake REQUIRED) 74 | 75 | add_subdirectory(test) 76 | endif() 77 | 78 | ament_package() 79 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/vehicle_gateway/cfe101602d84192286de2626650cda1a8e81f971/vehicle_gateway_sim_performance/COLCON_IGNORE -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/include/linux_cpu_system_measurement.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef LINUX_CPU_SYSTEM_MEASUREMENT_HPP_ 15 | #define LINUX_CPU_SYSTEM_MEASUREMENT_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | enum CPUStates 27 | { 28 | S_USER = 0, 29 | S_NICE, 30 | S_SYSTEM, 31 | S_IDLE, 32 | S_IOWAIT, 33 | S_IRQ, 34 | S_SOFTIRQ, 35 | S_STEAL, 36 | S_GUEST, 37 | S_GUEST_NICE, 38 | }; 39 | 40 | const int NUM_CPU_STATES = 10; 41 | 42 | typedef struct CPUData 43 | { 44 | std::string cpu; 45 | size_t times[NUM_CPU_STATES]; 46 | } CPUData; 47 | 48 | class LinuxCPUSystemMeasurement 49 | { 50 | public: 51 | LinuxCPUSystemMeasurement(); 52 | void ReadStatsCPU(std::vector & entries); 53 | float getCPUSystemCurrentlyUsed(); 54 | 55 | private: 56 | size_t GetIdleTime(const CPUData & e); 57 | size_t GetActiveTime(const CPUData & e); 58 | void initCPUProcess(); 59 | void makeReading(); 60 | 61 | std::vector entries1_; 62 | std::vector entries2_; 63 | int numProcessors_; 64 | }; 65 | 66 | #endif // LINUX_CPU_SYSTEM_MEASUREMENT_HPP_ 67 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/include/linux_memory_system_measurement.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef LINUX_MEMORY_SYSTEM_MEASUREMENT_HPP_ 15 | #define LINUX_MEMORY_SYSTEM_MEASUREMENT_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | class LinuxMemorySystemMeasurement 25 | { 26 | public: 27 | LinuxMemorySystemMeasurement(); 28 | double getTotalMemorySystem(); 29 | double getFreeMemorySystem(); 30 | double getAvailableMemorySystem(); 31 | }; 32 | 33 | #endif // LINUX_MEMORY_SYSTEM_MEASUREMENT_HPP_ 34 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_sim_performance 5 | 0.0.0 6 | Vehicle Gateway simualtion performance 7 | Morgan Quigley 8 | Apache License 2.0 9 | 10 | gz-cmake3 11 | gz-transport12 12 | pluginlib 13 | rclcpp 14 | vehicle_gateway_px4 15 | 16 | ament_index_python 17 | ament_lint_auto 18 | ament_lint_common 19 | launch 20 | launch_ros 21 | launch_testing 22 | launch_testing_ament_cmake 23 | micro_ros_agent 24 | ros_gz_bridge 25 | ros_gz_sim 26 | px4_sim 27 | vehicle_gateway_models 28 | vehicle_gateway_python 29 | vehicle_gateway_worlds 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/script/show_data.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnuplot 4 | 5 | set term qt 1 size 640,480 6 | 7 | plot './build/vehicle_gateway_sim_performance/test/system_collector_1.csv' using 1:2 with lines title 'CPU - 1 vehicle vehicles', \ 8 | './build/vehicle_gateway_sim_performance/test/system_collector_3.csv' using 1:2 with lines title 'CPU - 3 vehicles', \ 9 | './build/vehicle_gateway_sim_performance/test/system_collector_5.csv' using 1:2 with lines title 'CPU - 5 vehicles', \ 10 | './build/vehicle_gateway_sim_performance/test/system_collector_7.csv' using 1:2 with lines title 'CPU - 7 vehicles', \ 11 | './build/vehicle_gateway_sim_performance/test/system_collector_9.csv' using 1:2 with lines title 'CPU - 9' 12 | set term qt 2 size 640,480 13 | 14 | plot './build/vehicle_gateway_sim_performance/test/system_collector_1.csv' using 1:3 with lines title 'Memory - 1 vehicles', \ 15 | './build/vehicle_gateway_sim_performance/test/system_collector_3.csv' using 1:3 with lines title 'Memory - 3 vehicles', \ 16 | './build/vehicle_gateway_sim_performance/test/system_collector_5.csv' using 1:3 with lines title 'Memory - 5 vehicles', \ 17 | './build/vehicle_gateway_sim_performance/test/system_collector_7.csv' using 1:3 with lines title 'Memory - 7 vehicles', \ 18 | './build/vehicle_gateway_sim_performance/test/system_collector_9.csv' using 1:3 with lines title 'Memory - 9 vehicles' 19 | 20 | set term qt 3 size 640,480 21 | 22 | plot './build/vehicle_gateway_sim_performance/test/system_collector_1.csv' using 1:4 with lines title 'RFT - 1 vehicles', \ 23 | './build/vehicle_gateway_sim_performance/test/system_collector_3.csv' using 1:4 with lines title 'RFT - 3 vehicles', \ 24 | './build/vehicle_gateway_sim_performance/test/system_collector_5.csv' using 1:4 with lines title 'RFT - 5 vehicles', \ 25 | './build/vehicle_gateway_sim_performance/test/system_collector_7.csv' using 1:4 with lines title 'RFT - 7 vehicles', \ 26 | './build/vehicle_gateway_sim_performance/test/system_collector_9.csv' using 1:4 with lines title 'RFT - 9 vehicles' 27 | 28 | 29 | set term qt 4 size 640,480 30 | 31 | plot './build/vehicle_gateway_sim_performance/test/system_collector_1_multi_dds_domain.csv' using 1:2 with lines title 'Multi Domain ID - CPU - 1 vehicle', \ 32 | './build/vehicle_gateway_sim_performance/test/system_collector_3_multi_dds_domain.csv' using 1:2 with lines title 'Multi Domain ID - CPU - 3 vehicles', \ 33 | './build/vehicle_gateway_sim_performance/test/system_collector_5_multi_dds_domain.csv' using 1:2 with lines title 'Multi Domain ID - CPU - 5 vehicles', \ 34 | './build/vehicle_gateway_sim_performance/test/system_collector_7_multi_dds_domain.csv' using 1:2 with lines title 'Multi Domain ID - CPU - 7 vehicles', \ 35 | './build/vehicle_gateway_sim_performance/test/system_collector_9_multi_dds_domain.csv' using 1:2 with lines title 'Multi Domain ID - CPU - 9' 36 | set term qt 5 size 640,480 37 | 38 | plot './build/vehicle_gateway_sim_performance/test/system_collector_1_multi_dds_domain.csv' using 1:3 with lines title 'Multi Domain ID - Memory - 1 vehicles', \ 39 | './build/vehicle_gateway_sim_performance/test/system_collector_3_multi_dds_domain.csv' using 1:3 with lines title 'Multi Domain ID - Memory - 3 vehicles', \ 40 | './build/vehicle_gateway_sim_performance/test/system_collector_5_multi_dds_domain.csv' using 1:3 with lines title 'Multi Domain ID - Memory - 5 vehicles', \ 41 | './build/vehicle_gateway_sim_performance/test/system_collector_7_multi_dds_domain.csv' using 1:3 with lines title 'Multi Domain ID - Memory - 7 vehicles', \ 42 | './build/vehicle_gateway_sim_performance/test/system_collector_9_multi_dds_domain.csv' using 1:3 with lines title 'Multi Domain ID - Memory - 9 vehicles' 43 | 44 | set term qt 6 size 640,480 45 | 46 | plot './build/vehicle_gateway_sim_performance/test/system_collector_1_multi_dds_domain.csv' using 1:4 with lines title 'Multi Domain ID - RFT - 1 vehicles', \ 47 | './build/vehicle_gateway_sim_performance/test/system_collector_3_multi_dds_domain.csv' using 1:4 with lines title 'Multi Domain ID - RFT - 3 vehicles', \ 48 | './build/vehicle_gateway_sim_performance/test/system_collector_5_multi_dds_domain.csv' using 1:4 with lines title 'Multi Domain ID - RFT - 5 vehicles', \ 49 | './build/vehicle_gateway_sim_performance/test/system_collector_7_multi_dds_domain.csv' using 1:4 with lines title 'Multi Domain ID - RFT - 7 vehicles', \ 50 | './build/vehicle_gateway_sim_performance/test/system_collector_9_multi_dds_domain.csv' using 1:4 with lines title 'Multi Domain ID - RFT - 9 vehicles' 51 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/src/linux_cpu_system_measurement.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "linux_cpu_system_measurement.hpp" 15 | 16 | #include 17 | #include 18 | 19 | LinuxCPUSystemMeasurement::LinuxCPUSystemMeasurement() 20 | : numProcessors_(0) 21 | { 22 | initCPUProcess(); 23 | makeReading(); 24 | } 25 | 26 | void LinuxCPUSystemMeasurement::initCPUProcess() 27 | { 28 | std::string line; 29 | std::ifstream myfile(std::string("/proc/cpuinfo")); 30 | if (myfile.is_open()) { 31 | while (std::getline(myfile, line) ) { 32 | if (line.find("processor") != std::string::npos) { 33 | this->numProcessors_++; 34 | } 35 | } 36 | } 37 | myfile.close(); 38 | } 39 | 40 | size_t LinuxCPUSystemMeasurement::GetIdleTime(const CPUData & e) 41 | { 42 | return e.times[S_IDLE] + 43 | e.times[S_IOWAIT]; 44 | } 45 | size_t LinuxCPUSystemMeasurement::GetActiveTime(const CPUData & e) 46 | { 47 | return e.times[S_USER] + 48 | e.times[S_NICE] + 49 | e.times[S_SYSTEM] + 50 | e.times[S_IRQ] + 51 | e.times[S_SOFTIRQ] + 52 | e.times[S_STEAL] + 53 | e.times[S_GUEST] + 54 | e.times[S_GUEST_NICE]; 55 | } 56 | 57 | void LinuxCPUSystemMeasurement::makeReading() 58 | { 59 | ReadStatsCPU(entries1_); 60 | } 61 | 62 | float LinuxCPUSystemMeasurement::getCPUSystemCurrentlyUsed() 63 | { 64 | ReadStatsCPU(entries2_); 65 | 66 | const size_t NUM_ENTRIES = entries1_.size(); 67 | 68 | double total = 0; 69 | 70 | for (size_t i = 0; i < NUM_ENTRIES; ++i) { 71 | const CPUData & e1 = entries1_[i]; 72 | const CPUData & e2 = entries2_[i]; 73 | 74 | const float ACTIVE_TIME = static_cast(GetActiveTime(e2) - GetActiveTime(e1)); 75 | const float IDLE_TIME = static_cast(GetIdleTime(e2) - GetIdleTime(e1)); 76 | const float TOTAL_TIME = ACTIVE_TIME + IDLE_TIME; 77 | 78 | if (TOTAL_TIME > 0.0) { 79 | total += (100.f * ACTIVE_TIME / TOTAL_TIME); 80 | } 81 | } 82 | 83 | entries1_ = entries2_; 84 | 85 | return total / this->numProcessors_; 86 | } 87 | 88 | void LinuxCPUSystemMeasurement::ReadStatsCPU(std::vector & entries) 89 | { 90 | entries.clear(); 91 | std::ifstream fileStat("/proc/stat"); 92 | 93 | std::string line; 94 | 95 | const std::string STR_CPU("cpu"); 96 | const std::size_t LEN_STR_CPU = STR_CPU.size(); 97 | 98 | while (std::getline(fileStat, line)) { 99 | // cpu stats line found 100 | if (!line.compare(0, LEN_STR_CPU, STR_CPU)) { 101 | std::istringstream ss(line); 102 | 103 | // read cpu label 104 | std::string label; 105 | ss >> label; 106 | 107 | if (label.size() > LEN_STR_CPU) { 108 | label.erase(0, LEN_STR_CPU); 109 | } else { 110 | continue; 111 | } 112 | 113 | // store entry 114 | entries.emplace_back(CPUData()); 115 | CPUData & entry = entries.back(); 116 | entry.cpu = label; 117 | 118 | // read times 119 | for (int i = 0; i < NUM_CPU_STATES; ++i) { 120 | ss >> entry.times[i]; 121 | } 122 | } 123 | } 124 | } 125 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/src/linux_memory_system_measurement.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "linux_memory_system_measurement.hpp" 15 | #include 16 | #include 17 | #include 18 | 19 | LinuxMemorySystemMeasurement::LinuxMemorySystemMeasurement() {} 20 | 21 | double LinuxMemorySystemMeasurement::getTotalMemorySystem() 22 | { 23 | std::string token; 24 | std::ifstream file("/proc/meminfo"); 25 | while (file >> token) { 26 | if (token == "MemTotal:") { 27 | uint64_t mem; 28 | if (file >> mem) { 29 | return mem / 1024.0; // Mb 30 | } else { 31 | return 0; 32 | } 33 | } 34 | // ignore rest of the line 35 | file.ignore(std::numeric_limits::max(), '\n'); 36 | } 37 | return 0; 38 | } 39 | 40 | double LinuxMemorySystemMeasurement::getFreeMemorySystem() 41 | { 42 | std::string token; 43 | std::ifstream file("/proc/meminfo"); 44 | while (file >> token) { 45 | if (token == "MemFree:") { 46 | uint64_t mem; 47 | if (file >> mem) { 48 | return mem / 1024.0; // Mb 49 | } else { 50 | return 0; 51 | } 52 | } 53 | // ignore rest of the line 54 | file.ignore(std::numeric_limits::max(), '\n'); 55 | } 56 | return 0; 57 | } 58 | 59 | double LinuxMemorySystemMeasurement::getAvailableMemorySystem() 60 | { 61 | std::string token; 62 | std::ifstream file("/proc/meminfo"); 63 | while (file >> token) { 64 | if (token == "MemAvailable:") { 65 | uint64_t mem; 66 | if (file >> mem) { 67 | return mem / 1024.0; // Mb 68 | } else { 69 | return 0; 70 | } 71 | } 72 | // ignore rest of the line 73 | file.ignore(std::numeric_limits::max(), '\n'); 74 | } 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _MSC_VER 21 | #pragma warning(push) 22 | #pragma warning(disable: 4251) 23 | #endif 24 | #include 25 | #ifdef _MSC_VER 26 | #pragma warning(pop) 27 | #endif 28 | 29 | #include 30 | 31 | #include "linux_memory_system_measurement.hpp" 32 | #include "linux_cpu_system_measurement.hpp" 33 | 34 | static sem_t sentinel; 35 | 36 | std::mutex mutex_stats; 37 | double gazebo_rtf = 0.0; 38 | int num_reading = 0; 39 | 40 | static void post_sentinel(int signum) 41 | { 42 | (void)signum; 43 | const char msg[] = "\nGot signal - shutting down\n"; 44 | (void)!write(STDERR_FILENO, msg, sizeof(msg)); 45 | sem_post(&sentinel); 46 | } 47 | 48 | ////////////////////////////////////////////////// 49 | /// \brief Function called each time a topic update is received. 50 | void cb(const gz::msgs::WorldStatistics & _msg) 51 | { 52 | mutex_stats.lock(); 53 | gazebo_rtf += _msg.real_time_factor(); 54 | num_reading++; 55 | mutex_stats.unlock(); 56 | } 57 | 58 | int main(int /*argc*/, char * argv[]) 59 | { 60 | std::ofstream m_os; 61 | std::string process_pid; 62 | struct timespec wait_until; 63 | 64 | signal(SIGINT, post_sentinel); 65 | 66 | std::string filename = argv[1]; 67 | m_os.open(filename, std::ofstream::out); 68 | std::cout << "file_name " << filename << std::endl; 69 | if (m_os.is_open()) { 70 | m_os << "T_experiment" << ",system_cpu_usage (%)" 71 | << ",system virtual memory (Mb)" << ", Gazebo RTF" << std::endl; 72 | } 73 | 74 | auto start = std::chrono::high_resolution_clock::now(); 75 | 76 | LinuxCPUSystemMeasurement linux_cpu_system_measurement; 77 | LinuxMemorySystemMeasurement linux_memory_system_measurement; 78 | 79 | gz::transport::Node node; 80 | std::string topic = "/stats"; 81 | 82 | // Subscribe to a topic by registering a callback. 83 | if (!node.Subscribe(topic, cb)) { 84 | std::cerr << "Error subscribing to topic [" << topic << "]" << std::endl; 85 | return -1; 86 | } 87 | 88 | while (true) { 89 | double cpu_system_percentage = linux_cpu_system_measurement.getCPUSystemCurrentlyUsed(); 90 | double phy_mem_system_usage = linux_memory_system_measurement.getTotalMemorySystem() - 91 | linux_memory_system_measurement.getAvailableMemorySystem(); 92 | 93 | clock_gettime(CLOCK_REALTIME, &wait_until); 94 | wait_until.tv_sec += 1; 95 | if (sem_timedwait(&sentinel, &wait_until) == 0 || errno != ETIMEDOUT) { 96 | break; 97 | } 98 | 99 | auto finish = std::chrono::high_resolution_clock::now(); 100 | std::chrono::duration elapsed = finish - start; 101 | float seconds_running = elapsed.count() / 1000; 102 | 103 | if (m_os.is_open()) { 104 | mutex_stats.lock(); 105 | m_os << seconds_running << ", " << cpu_system_percentage << 106 | ", " << phy_mem_system_usage << ", " << gazebo_rtf / num_reading << std::endl; 107 | num_reading = 0; 108 | gazebo_rtf = 0.0; 109 | mutex_stats.unlock(); 110 | } 111 | } 112 | 113 | return 0; 114 | } 115 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | foreach(val RANGE 1 12 2) 2 | add_launch_test(test_px4_multirobot.py 3 | TARGET test_performance_position_control_vtol_${val} 4 | ENV "NUMBER_OF_VEHICLES=${val}" 5 | ARGS "vehicle_type:=standard_vtol" "script_test:=vtol_position_control" 6 | TIMEOUT 700 7 | ) 8 | endforeach() 9 | 10 | foreach(val RANGE 1 10 2) 11 | add_launch_test(test_px4_multirobot_multiple_domain_id.py 12 | TARGET test_performance_position_control_vtol_different_domain_id_${val} 13 | ENV "ROS_DOMAIN_ID=${val};NUMBER_OF_VEHICLES=${val}" 14 | ARGS "vehicle_type:=standard_vtol" "script_test:=vtol_position_control" 15 | TIMEOUT 700 16 | ) 17 | endforeach() 18 | -------------------------------------------------------------------------------- /vehicle_gateway_sim_performance/test/common.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | from shlex import split 18 | import subprocess 19 | import xml.etree.ElementTree as ET 20 | 21 | from ament_index_python.packages import get_package_share_directory 22 | 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import Node 25 | 26 | import psutil 27 | 28 | from vehicle_gateway_python_helpers.helpers import get_px4_process 29 | 30 | 31 | def create_px4_instance(vehicle_id, additional_env, model_name): 32 | gateway_models_dir = get_package_share_directory('vehicle_gateway_models') 33 | 34 | run_px4 = get_px4_process( 35 | vehicle_id, 36 | additional_env, 37 | ['sleep', str(5 + int(vehicle_id)), '&&']) 38 | 39 | model_sdf_filename = [ 40 | gateway_models_dir, 41 | '/configs_px4/', 42 | LaunchConfiguration('vehicle_type'), 43 | '_', 44 | 'stock', 45 | '/model.sdf'] 46 | 47 | world_sdf_path = os.path.join( 48 | get_package_share_directory('vehicle_gateway_worlds'), 49 | 'worlds', 50 | 'empty_px4_world.sdf') 51 | # I couldn't get the libsdformat binding to work as expected due 52 | # to various troubles. Let's simplify and just treat SDF as 53 | # regular XML and do an XPath query 54 | sdf_root = ET.parse(world_sdf_path).getroot() 55 | frame_node = sdf_root.find(f'.//frame[@name=\"pad_{vehicle_id}\"]') 56 | if not frame_node: 57 | raise ValueError(f'Could not find a frame named pad_{vehicle_id}') 58 | pose_node = frame_node.find('pose') 59 | pose_str = pose_node.text 60 | x, y, z, roll, pitch, yaw = pose_str.split(' ') 61 | 62 | spawn_entity = Node( 63 | package='ros_gz_sim', 64 | executable='create', 65 | output='screen', 66 | arguments=['-file', model_sdf_filename, 67 | '-name', model_name, 68 | '-allow_renaming', 'true', 69 | '-x', x, 70 | '-y', y, 71 | '-z', z, 72 | '-R', roll, 73 | '-P', pitch, 74 | '-Y', yaw], 75 | on_exit=[run_px4]) 76 | 77 | return [run_px4, spawn_entity] 78 | 79 | 80 | def kill_all_process(NUMBER_OF_VEHICLES): 81 | for i in range(1, NUMBER_OF_VEHICLES + 1): 82 | # shutdown px4 83 | p = subprocess.Popen(split(f'px4-shutdown --instance {i}'), 84 | stdout=subprocess.PIPE, 85 | stderr=subprocess.PIPE) 86 | p.wait() 87 | for proc in psutil.process_iter(): 88 | # check whether the process name matches 89 | if proc.name() == 'ruby' or \ 90 | proc.name() == 'micro_ros_agent' or \ 91 | proc.name() == 'system_metric_collector': 92 | proc.kill() 93 | -------------------------------------------------------------------------------- /vehicle_gateway_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(vehicle_gateway_worlds) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY 8 | worlds 9 | DESTINATION 10 | share/${PROJECT_NAME}/ 11 | ) 12 | 13 | if(BUILD_TESTING) 14 | find_package(ament_lint_auto REQUIRED) 15 | ament_lint_auto_find_test_dependencies() 16 | endif() 17 | 18 | ament_package() 19 | -------------------------------------------------------------------------------- /vehicle_gateway_worlds/README.md: -------------------------------------------------------------------------------- 1 | It is convenient to annotate the world SDF with pre-defined spawn positions. This can be done using `frame` tags in SDF, which are children of the `world` tag, like this: 2 | 3 | ```xml 4 | 5 | -9.7948 -8.31848 2.0 0.0 0.0 0.0 6 | 7 | ``` 8 | 9 | The ordering is (x, y, z, roll, pitch, yaw), as defined in the [SDF documentation](http://sdformat.org/spec?ver=1.9&elem=world#world_frame) documentation. 10 | -------------------------------------------------------------------------------- /vehicle_gateway_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vehicle_gateway_worlds 5 | 0.0.0 6 | Some demo worlds for the Vehicle Gateway 7 | Morgan Quigley 8 | Apache License 2.0 9 | 10 | 11 | ament_cmake 12 | 13 | 14 | -------------------------------------------------------------------------------- /vehicle_gateway_worlds/worlds/aruco_px4_world.sdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 10 | 11 | 14 | 15 | 18 | 19 | 22 | ogre2 23 | 24 | 27 | 28 | 31 | 32 | 35 | 36 | 37 | 38 | true 39 | 0 0 20 0 0 0 40 | 0.8 0.8 0.8 1 41 | 1.0 1.0 1.0 1 42 | 0.2 0.2 0.2 1 43 | 44 | 1000 45 | 0.9 46 | 0.01 47 | 0.001 48 | 49 | -0.5 0.1 -0.9 50 | 51 | 52 | 53 | true 54 | 55 | 56 | 57 | 58 | 0 0 1 59 | 100 100 60 | 61 | 62 | 63 | 64 | 65 | 66 | 0 0 1 67 | 100 100 68 | 69 | 70 | 71 | 0.2 0.2 0.2 1 72 | 0.2 0.4 0.2 1 73 | 0.8 0.8 0.8 1 74 | 75 | 76 | 77 | 78 | 79 | 80 | model://aruco_marker 81 | 82 | 83 | 84 | 85 | --------------------------------------------------------------------------------