diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index c4ad1ac591..ba9b8a1523 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -69,24 +69,18 @@ pipeline { "matek_h743-slim_default", "matek_h743_default", "modalai_fc-v1_default", - "modalai_fc-v1_rtps", "modalai_fc-v2_default", "modalai_voxl2-io_default", "mro_ctrl-zero-f7_default", "mro_ctrl-zero-f7-oem_default", "mro_ctrl-zero-h7-oem_default", - "mro_ctrl-zero-h7-oem_rtps", "mro_ctrl-zero-h7_default", - "mro_ctrl-zero-h7_rtps", "mro_pixracerpro_default", - "mro_pixracerpro_rtps", "mro_x21-777_default", "mro_x21_default", "nxp_fmuk66-e_default", - "nxp_fmuk66-e_rtps", "nxp_fmuk66-e_socketcan", "nxp_fmuk66-v3_default", - "nxp_fmuk66-v3_rtps", "nxp_fmuk66-v3_socketcan", "nxp_fmurt1062-v1_default", "nxp_ucans32k146_canbootloader", @@ -104,7 +98,6 @@ pipeline { "px4_fmu-v5_debug", "px4_fmu-v5_default", "px4_fmu-v5_lto", - "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", "px4_fmu-v5x_default", diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index 9c72c966fa..ef15d32171 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -23,7 +23,6 @@ jobs: "shellcheck_all", "NO_NINJA_BUILD=1 px4_fmu-v5_default", "NO_NINJA_BUILD=1 px4_sitl_default", - "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps", "airframe_metadata", "module_documentation", "parameters_metadata", diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml index 4372a34da1..10799ba38f 100644 --- a/.github/workflows/metadata.yml +++ b/.github/workflows/metadata.yml @@ -116,7 +116,7 @@ jobs: ls -ls * # TODO: deploy graph_px4_sitl.json to S3 px4-travis:Firmware/master/ - micrortps_agent: + ROS2_msgs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: @@ -124,34 +124,8 @@ jobs: with: token: ${{ secrets.ACCESS_TOKEN }} - - name: microRTPS agent - run: | - make px4_sitl_rtps - git clone https://github.com/PX4/micrortps_agent.git - cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent - - ROS_msgs: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: PX4 ROS msgs + - name: PX4 ROS2 msgs run: | git clone https://github.com/PX4/px4_msgs.git - python3 msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/ - - ROS2_bridge: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: PX4 ROS2 bridge - run: | - git clone https://github.com/PX4/px4_ros_com.git - ./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml + rm px4_msgs/msg/*.msg + cp msg/*.msg px4_msgs/msg/ diff --git a/.gitmodules b/.gitmodules index a1b3d788af..4b9627a94c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,10 +18,6 @@ path = src/drivers/gps/devices url = https://github.com/PX4/PX4-GPSDrivers.git branch = main -[submodule "src/modules/micrortps_bridge/micro-CDR"] - path = src/modules/micrortps_bridge/micro-CDR - url = https://github.com/PX4/Micro-CDR.git - branch = master [submodule "platforms/nuttx/NuttX/nuttx"] path = platforms/nuttx/NuttX/nuttx url = https://github.com/PX4/NuttX.git @@ -64,4 +60,5 @@ branch = px4 [submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"] path = src/modules/microdds_client/Micro-XRCE-DDS-Client - url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git + url = https://github.com/PX4/Micro-XRCE-DDS-Client.git + branch = px4 diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index d47e962fd6..1f93a5ce3e 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -6,11 +6,11 @@ CONFIG: buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default - px4_sitl_rtps: - short: px4_sitl_rtps + px4_sitl_nolockstep: + short: px4_sitl_nolockstep buildType: RelWithDebInfo settings: - CONFIG: px4_sitl_rtps + CONFIG: px4_sitl_nolockstep px4_sitl_asan: short: px4_sitl (AddressSanitizer) buildType: AddressSanitizer diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in index f6d12430c7..0874d152cf 100644 --- a/Documentation/Doxyfile.in +++ b/Documentation/Doxyfile.in @@ -829,7 +829,6 @@ RECURSIVE = YES # run. EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \ - @CMAKE_SOURCE_DIR@/src/modules/micrortps_bridge/micro-CDR \ @CMAKE_SOURCE_DIR@/src/examples \ @CMAKE_SOURCE_DIR@/src/templates diff --git a/Jenkinsfile b/Jenkinsfile index b25af16dff..30deefc365 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -13,76 +13,6 @@ pipeline { } } parallel { - // stage('Catkin build on ROS workspace') { - // agent { - // docker { - // image 'px4io/px4-dev-ros-melodic:2021-08-18' - // args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' - // } - // } - // steps { - // sh 'ls -l' - // sh '''#!/bin/bash -l - // echo $0; - // mkdir -p catkin_ws/src; - // cd catkin_ws; - // git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo - // git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/simulation/gazebo/sitl_gazebo src/mavlink_sitl_gazebo; - // git -C ${WORKSPACE}/catkin_ws/src/Firmware fetch --tags; - // source /opt/ros/melodic/setup.bash; - // export PYTHONPATH=/opt/ros/$ROS_DISTRO/lib/python2.7/dist-packages:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages; - // catkin init; - // catkin build -j$(nproc) -l$(nproc); - // ''' - // // test if the binary was correctly installed and runs using 'mavros_posix_sitl.launch' - // sh '''#!/bin/bash -l - // echo $0; - // source catkin_ws/devel/setup.bash; - // rostest px4 pub_test.launch; - // ''' - // } - // post { - // always { - // sh 'rm -rf catkin_ws' - // } - // failure { - // archiveArtifacts(allowEmptyArchive: false, artifacts: '.ros/**/*.xml, .ros/**/*.log') - // } - // } - // options { - // checkoutToSubdirectory('catkin_ws/src/Firmware') - // } - // } - - stage('Colcon build on ROS2 workspace') { - agent { - docker { - image 'px4io/px4-dev-ros2-foxy:2021-08-18' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' - } - } - steps { - sh 'ls -l' - sh '''#!/bin/bash -l - echo $0; - unset ROS_DISTRO; - mkdir -p colcon_ws/src; - cd colcon_ws; - git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo; - git -C ${WORKSPACE}/colcon_ws/src/Firmware fetch --tags; - source /opt/ros/foxy/setup.sh; - colcon build --event-handlers console_direct+ --symlink-install; - ''' - } - post { - always { - sh 'rm -rf colcon_ws' - } - } - options { - checkoutToSubdirectory('colcon_ws/src/Firmware') - } - } stage('Airframe') { agent { @@ -149,7 +79,7 @@ pipeline { docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { - sh 'mkdir -p build/msg_docs; ./msg/tools/generate_msg_docs.py -d build/msg_docs' + sh 'mkdir -p build/msg_docs; ./Tools/msg/generate_msg_docs.py -d build/msg_docs' dir('build') { archiveArtifacts(artifacts: 'msg_docs/*.md') stash includes: 'msg_docs/*.md', name: 'msg_documentation' @@ -288,37 +218,6 @@ pipeline { } } - stage('microRTPS agent') { - agent { - docker { image 'px4io/px4-dev-base-focal:2021-08-18' } - } - steps { - sh('export') - sh('git fetch --all --tags') - sh('make distclean; git clean -ff -x -d .') - sh('make px4_sitl_rtps') - withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { - sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/micrortps_agent.git -b ${BRANCH_NAME}") - sh("rm -rf micrortps_agent/src micrortps_agent/idl") - sh('cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent') - sh('cd micrortps_agent; git status; git add src; git commit -a -m "Update microRTPS agent source code `date`" || true') - sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true') - sh('cd micrortps_agent; git status; git add idl; git commit -a -m "Update IDL definitions `date`" || true') - sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true') - sh('cd micrortps_agent; git status; git add CMakeLists.txt; git commit -a -m "Update CMakeLists.txt `date`" || true') - sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true') - sh('rm -rf micrortps_agent') - } - } - when { - anyOf { - branch 'main' - branch 'master' // should be removed, but in case there is something going on... - branch 'pr-jenkins' // for testing - } - } - } - stage('PX4 ROS msgs') { agent { docker { image 'px4io/px4-dev-base-focal:2021-08-18' } @@ -329,66 +228,16 @@ pipeline { withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_msgs.git") // 'main' branch - sh('./msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/') + sh('rm -f px4_msgs/msg/*.msg') + sh('cp msg/*.msg px4_msgs/msg/') sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') sh('cd px4_msgs; git push origin main || true') - // 'ros1' branch - sh('cd px4_msgs; git checkout ros1') - sh('./msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/') - sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') - sh('cd px4_msgs; git push origin ros1 || true') sh('rm -rf px4_msgs') } } when { anyOf { branch 'main' - branch 'master' // should be removed, but in case there is something going on... - branch 'pr-jenkins' // for testing - } - } - } - - stage('PX4 ROS2 bridge') { - agent { - docker { image 'px4io/px4-dev-base-focal:2021-08-18' } - } - steps { - sh('export') - sh('make distclean; git clean -ff -x -d .') - withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { - sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_ros_com.git -b ${BRANCH_NAME}") - // deploy uORB RTPS ID map - sh('./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml') - sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS ID map `date`" || true') - sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true') - // deploy uORB RTPS required tools - sh('cp msg/tools/uorb_rtps_classifier.py px4_ros_com/scripts/uorb_rtps_classifier.py') - sh('cp msg/tools/generate_microRTPS_bridge.py px4_ros_com/scripts/generate_microRTPS_bridge.py') - sh('cp msg/tools/px_generate_uorb_topic_files.py px4_ros_com/scripts/px_generate_uorb_topic_files.py') - sh('cp msg/tools/px_generate_uorb_topic_helper.py px4_ros_com/scripts/px_generate_uorb_topic_helper.py') - // deploy templates - sh('cp msg/templates/urtps/microRTPS_agent.cpp.em px4_ros_com/templates/microRTPS_agent.cpp.em') - sh('cp msg/templates/urtps/microRTPS_timesync.cpp.em px4_ros_com/templates/microRTPS_timesync.cpp.em') - sh('cp msg/templates/urtps/microRTPS_timesync.h.em px4_ros_com/templates/microRTPS_timesync.h.em') - sh('cp msg/templates/urtps/microRTPS_transport.cpp px4_ros_com/templates/microRTPS_transport.cpp') - sh('cp msg/templates/urtps/microRTPS_transport.h px4_ros_com/templates/microRTPS_transport.h') - sh('cp msg/templates/urtps/Publisher.cpp.em px4_ros_com/templates/Publisher.cpp.em') - sh('cp msg/templates/urtps/Publisher.h.em px4_ros_com/templates/Publisher.h.em') - sh('cp msg/templates/urtps/Subscriber.cpp.em px4_ros_com/templates/Subscriber.cpp.em') - sh('cp msg/templates/urtps/Subscriber.h.em px4_ros_com/templates/Subscriber.h.em') - sh('cp msg/templates/urtps/RtpsTopics.cpp.em px4_ros_com/templates/RtpsTopics.cpp.em') - sh('cp msg/templates/urtps/RtpsTopics.h.em px4_ros_com/templates/RtpsTopics.h.em') - sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS script tools `date`" || true') - sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true') - sh('rm -rf px4_msgs') - } - } - when { - anyOf { - branch 'main' - branch 'master' // should be removed, but in case there is something going on... - branch 'pr-jenkins' // for testing } } } diff --git a/Makefile b/Makefile index 34aa1c1273..c61ea751d0 100644 --- a/Makefile +++ b/Makefile @@ -172,11 +172,6 @@ ifdef PYTHON_EXECUTABLE override CMAKE_ARGS += -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} endif -# Check if the microRTPS agent is to be built -ifdef BUILD_MICRORTPS_AGENT - override CMAKE_ARGS += -DBUILD_MICRORTPS_AGENT=ON -endif - # Functions # -------------------------------------------------------------------- # describe how to build a cmake config @@ -253,7 +248,7 @@ endef # Other targets # -------------------------------------------------------------------- -.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware check_rtps +.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware # QGroundControl flashable NuttX firmware qgc_firmware: px4fmu_firmware misc_qgc_extra_firmware @@ -278,15 +273,7 @@ misc_qgc_extra_firmware: \ check_airmind_mindpx-v2_default \ sizes -# builds with RTPS -check_rtps: \ - check_px4_fmu-v3_rtps \ - check_px4_fmu-v4_rtps \ - check_px4_fmu-v4pro_rtps \ - check_px4_sitl_rtps \ - sizes - -.PHONY: sizes check quick_check check_rtps uorb_graphs +.PHONY: sizes check quick_check uorb_graphs sizes: @-find build -name *.elf -type f | xargs size 2> /dev/null || : @@ -495,7 +482,9 @@ shellcheck_all: validate_module_configs: @find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f \ - -not -path "$(SRC_DIR)/src/lib/mixer_module/*" -not -path "$(SRC_DIR)/src/lib/crypto/libtommath/*" -print0 | \ + -not -path "$(SRC_DIR)/src/lib/mixer_module/*" \ + -not -path "$(SRC_DIR)/src/modules/microdds_client/dds_topics.yaml" \ + -not -path "$(SRC_DIR)/src/lib/crypto/libtommath/*" -print0 | \ xargs -0 "$(SRC_DIR)"/Tools/validate_yaml.py --schema-file "$(SRC_DIR)"/validation/module_schema.yaml # Cleanup diff --git a/README.md b/README.md index a7daa142c4..edc263b4e7 100644 --- a/README.md +++ b/README.md @@ -75,7 +75,7 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con * [Paul Riseborough](https://github.com/priseborough) * Vision based navigation and Obstacle Avoidance * [Markus Achtelik](https://github.com/markusachtelik) - * RTPS/ROS2 Interface + * DDS/ROS2 Interface * [Nuno Marques](https://github.com/TSC21) See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 419ed0ed18..1ba7ccde27 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -36,7 +36,6 @@ add_subdirectory(airframes) px4_add_romfs_files( px4-rc.mavlink px4-rc.params - px4-rc.rtps px4-rc.simulator rc.replay rcS diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps b/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps deleted file mode 100644 index d3cfa4f817..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# shellcheck disable=SC2154 - -micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance)) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 09ae36ce4f..4e6acb9c56 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -259,11 +259,8 @@ fi navigator start -# Try to start the micrortps_client with UDP transport if module exists -if px4-micrortps_client status > /dev/null 2>&1 -then - . px4-rc.rtps -fi +# Try to start the microdds_client with UDP transport if module exists +microdds_client start -t udp -p 8888 if param greater -s MNT_MODE_IN -1 then diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 33a98569ae..6681bf84ac 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -8,7 +8,6 @@ if [ $# -gt 0 ]; then fi exec find boards msg src platforms test \ - -path msg/templates/urtps -prune -o \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ @@ -21,8 +20,6 @@ exec find boards msg src platforms test \ -path src/modules/ekf2/EKF -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ -path src/modules/mavlink/mavlink -prune -o \ - -path src/modules/micrortps_bridge/micro-CDR -prune -o \ - -path src/modules/micrortps_bridge/microRTPS_client -prune -o \ -path test/mavsdk_tests/catch2 -prune -o \ -path src/lib/crypto/monocypher -prune -o \ -path src/lib/crypto/libtomcrypt -prune -o \ diff --git a/Tools/build_micrortps_agent.sh b/Tools/build_micrortps_agent.sh deleted file mode 100755 index 6d45eaeb41..0000000000 --- a/Tools/build_micrortps_agent.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash -set -e - -SCRIPT_DIR=$0 -if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then - SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) -fi - -PX4_DIR=$(cd "$(dirname $(dirname $SCRIPT_DIR))" && pwd) - -if [ -d $PX4_DIR/build/*_rtps ]; then - cd $PX4_DIR/build/*_rtps/src/modules/micrortps_bridge/micrortps_agent/ - cmake -Bbuild - cmake --build build -j$(nproc --all) -fi diff --git a/msg/tools/__init__.py b/Tools/msg/__init__.py similarity index 100% rename from msg/tools/__init__.py rename to Tools/msg/__init__.py diff --git a/msg/tools/generate_msg_docs.py b/Tools/msg/generate_msg_docs.py similarity index 98% rename from msg/tools/generate_msg_docs.py rename to Tools/msg/generate_msg_docs.py index 8dd961968b..cd80971586 100755 --- a/msg/tools/generate_msg_docs.py +++ b/Tools/msg/generate_msg_docs.py @@ -66,7 +66,7 @@ if __name__ == "__main__": with open(msg_filename, 'r') as source_file: msg_contents = source_file.read() - #Format markdown using msg name, comment, url, contents. + #Format markdown using msg name, comment, url, contents. markdown_output="""# %s (UORB message) %s @@ -90,7 +90,7 @@ if __name__ == "__main__": readme_text="""# uORB Message Reference :::note -This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/msg/tools/generate_msg_docs.py) from the source code. +This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code. ::: This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)). diff --git a/Tools/msg/px_generate_uorb_topic_files.py b/Tools/msg/px_generate_uorb_topic_files.py new file mode 100755 index 0000000000..4a26cbf10a --- /dev/null +++ b/Tools/msg/px_generate_uorb_topic_files.py @@ -0,0 +1,253 @@ +#!/usr/bin/env python3 +############################################################################# +# +# Copyright (C) 2013-2022 PX4 Pro Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +px_generate_uorb_topic_files.py +Generates c/cpp header/source files for uorb topics from .msg +message files +""" + +import os +import argparse +import re +import sys + +try: + import em +except ImportError as e: + print("Failed to import em: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user empy") + print("") + sys.exit(1) + +try: + import genmsg.template_tools +except ImportError as e: + print("Failed to import genmsg: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyros-genmsg") + print("") + sys.exit(1) + + +__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" +__copyright__ = "Copyright (C) 2013-2022 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + + +TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] +TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] +OUTPUT_FILE_EXT = ['.h', '.cpp'] +INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] +PACKAGE = 'px4' +TOPICS_TOKEN = '# TOPICS ' + + +def get_topics(filename): + """ + Get TOPICS names from a "# TOPICS" line + """ + ofile = open(filename, 'r') + text = ofile.read() + result = [] + for each_line in text.split('\n'): + if each_line.startswith(TOPICS_TOKEN): + topic_names_str = each_line.strip() + topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") + topic_names_list = topic_names_str.split(" ") + for topic in topic_names_list: + # topic name PascalCase (file name) to snake_case (topic name) + topic_name = re.sub(r'(?= 0): type_name = type_name[sl_pos + 1:] + type_name = re.sub(r'(?'%(name)) + name_snake = re.sub(r'(?'%(name_snake)) }@ static inline constexpr int ucdr_topic_size_@(topic)() @@ -87,11 +124,8 @@ static inline constexpr int ucdr_topic_size_@(topic)() return @(struct_size); } -bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf) +bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf, int64_t time_offset = 0) { - if (ucdr_buffer_remaining(&buf) < @(struct_size)) { - return false; - } @{ for field_type, field_name, field_size, padding in fields: if padding > 0: @@ -99,7 +133,18 @@ for field_type, field_name, field_size, padding in fields: print('\tbuf.offset += {:}; // padding'.format(padding)) print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size)) - print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name)) + + if field_type == 'uint64' and field_name == 'timestamp': + print('\tconst uint64_t timestamp_adjusted = topic.timestamp + time_offset;') + print('\tmemcpy(buf.iterator, ×tamp_adjusted, sizeof(topic.{0}));'.format(field_name)) + + elif field_type == 'uint64' and field_name == 'timestamp_sample': + print('\tconst uint64_t timestamp_sample_adjusted = topic.timestamp_sample + time_offset;') + print('\tmemcpy(buf.iterator, ×tamp_sample_adjusted, sizeof(topic.{0}));'.format(field_name)) + + else: + print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name)) + print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name)) print('\tbuf.offset += sizeof(topic.{:});'.format(field_name)) @@ -107,11 +152,8 @@ for field_type, field_name, field_size, padding in fields: return true; } -bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic) +bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic, int64_t time_offset = 0) { - if (ucdr_buffer_remaining(&buf) < @(struct_size)) { - return false; - } @{ for field_type, field_name, field_size, padding in fields: if padding > 0: @@ -120,6 +162,10 @@ for field_type, field_name, field_size, padding in fields: print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size)) print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name)) + + if field_type == 'uint64' and (field_name == 'timestamp' or field_name == 'timestamp_sample'): + print('\ttopic.{0} -= time_offset;'.format(field_name)) + print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name)) print('\tbuf.offset += sizeof(topic.{:});'.format(field_name)) diff --git a/msg/templates/uorb/msg.cpp.em b/Tools/msg/templates/uorb/msg.cpp.em similarity index 85% rename from msg/templates/uorb/msg.cpp.em rename to Tools/msg/templates/uorb/msg.cpp.em index 03776549f5..e613d9511f 100644 --- a/msg/templates/uorb/msg.cpp.em +++ b/Tools/msg/templates/uorb/msg.cpp.em @@ -13,12 +13,11 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - search_path (dict) search paths for genmsg -@# - topics (List of String) multi-topic names -@# - constrained_flash set to true if flash is constrained +@# - topics (List of String) topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,8 +55,7 @@ import genmsg.msgs from px_generate_uorb_topic_helper import * # this is in Tools/ -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name +uorb_struct = '%s_s'%name_snake_case sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) @@ -67,7 +65,7 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field #include #include #include -#include +#include #include #include #include @@ -76,10 +74,10 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" @# This is used for the logger -constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );"; +constexpr char __orb_@(name_snake_case)_fields[] = "@( ";".join(topic_fields) );"; -@[for multi_topic in topics]@ -ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast(ORB_ID::@multi_topic)); +@[for topic in topics]@ +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(name_snake_case)_fields, static_cast(ORB_ID::@topic)); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/msg/templates/uorb/msg.h.em b/Tools/msg/templates/uorb/msg.h.em similarity index 90% rename from msg/templates/uorb/msg.h.em rename to Tools/msg/templates/uorb/msg.h.em index 9854e1eaef..69aa1347a1 100644 --- a/msg/templates/uorb/msg.h.em +++ b/Tools/msg/templates/uorb/msg.h.em @@ -13,11 +13,11 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - search_path (dict) search paths for genmsg -@# - topics (List of String) multi-topic names +@# - topics (List of String) topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -52,12 +52,12 @@ @{ import genmsg.msgs +import re from px_generate_uorb_topic_helper import * # this is in Tools/ -uorb_struct = '%s_s'%spec.short_name -uorb_struct_upper = spec.short_name.upper() -topic_name = spec.short_name +uorb_struct = '%s_s'%name_snake_case +uorb_struct_upper = name_snake_case.upper() }@ #pragma once @@ -77,6 +77,8 @@ for field in spec.parsed_fields(): if (not field.is_header): (package, name) = genmsg.names.package_resource_name(field.base_type) package = package or spec.package # convert '' to package + + name = re.sub(r'(?'%(name)) }@ @@ -127,9 +129,17 @@ for constant in spec.constants: #endif }; +#ifdef __cplusplus +namespace px4 { + namespace msg { + using @(spec.short_name) = @(uorb_struct); + } // namespace msg +} // namespace px4 +#endif + /* register this as object request broker structure */ -@[for multi_topic in topics]@ -ORB_DECLARE(@multi_topic); +@[for topic in topics]@ +ORB_DECLARE(@topic); @[end for] #ifdef __cplusplus diff --git a/msg/templates/uorb/uORBTopics.cpp.em b/Tools/msg/templates/uorb/uORBTopics.cpp.em similarity index 84% rename from msg/templates/uorb/uORBTopics.cpp.em rename to Tools/msg/templates/uorb/uORBTopics.cpp.em index c71082e354..c8b1c88397 100644 --- a/msg/templates/uorb/uORBTopics.cpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.cpp.em @@ -8,11 +8,11 @@ @# @# Context: @# - msgs (List) list of all msg files -@# - topics (List) list of all topic names +@# - multi_topics (List) list of all multi-topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,19 +46,22 @@ #include #include @{ -msg_names = [mn.replace(".msg", "") for mn in msgs] +msg_names = list(set([mn.replace(".msg", "") for mn in msgs])) # set() filters duplicates +msg_names.sort() msgs_count = len(msg_names) -topics_all = topics -topics_all.sort() -topics_count_all = len(topics_all) + +topic_names = list(set(topics)) # set() filters duplicates +topic_names.sort() +topics_count = len(topics) + }@ @[for msg_name in msg_names]@ #include @[end for] const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = { -@[for idx, topic_name in enumerate(topics_all, 1)]@ - ORB_ID(@(topic_name))@[if idx != topics_count_all], @[end if] +@[for idx, topic_name in enumerate(topic_names, 1)]@ + ORB_ID(@(topic_name))@[if idx != topic_names], @[end if] @[end for] }; diff --git a/msg/templates/uorb/uORBTopics.hpp.em b/Tools/msg/templates/uorb/uORBTopics.hpp.em similarity index 85% rename from msg/templates/uorb/uORBTopics.hpp.em rename to Tools/msg/templates/uorb/uORBTopics.hpp.em index 528b95acee..aea7015cc7 100644 --- a/msg/templates/uorb/uORBTopics.hpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.hpp.em @@ -7,12 +7,11 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files @# - topics (List) list of all topic names @############################################### /**************************************************************************** * - * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,11 +43,9 @@ ****************************************************************************/ @{ -msg_names = [mn.replace(".msg", "") for mn in msgs] -msgs_count = len(msg_names) -topics_all = topics -topics_all.sort() -topics_count_all = len(topics_all) +topics_count = len(topics) +topic_names_all = list(set(topics)) # set() filters duplicates +topic_names_all.sort() }@ #pragma once @@ -57,7 +54,7 @@ topics_count_all = len(topics_all) #include -static constexpr size_t ORB_TOPICS_COUNT{@(topics_count_all)}; +static constexpr size_t ORB_TOPICS_COUNT{@(topics_count)}; static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } /* @@ -66,8 +63,8 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } extern const struct orb_metadata *const *orb_get_topics() __EXPORT; enum class ORB_ID : uint8_t { -@[for idx, msg_name in enumerate(topics_all)]@ - @(msg_name) = @(idx), +@[for idx, topic_name in enumerate(topic_names_all)]@ + @(topic_name) = @(idx), @[end for] INVALID }; diff --git a/Tools/setup/arch.sh b/Tools/setup/arch.sh index 83bc225d49..f27d7e756f 100755 --- a/Tools/setup/arch.sh +++ b/Tools/setup/arch.sh @@ -9,8 +9,6 @@ ## - jMAVSim simulator (omit with arg: --no-sim-tools) ## - Gazebo simulator (not by default, use --gazebo) ## -## Not Installs: -## - FastRTPS and FastCDR INSTALL_NUTTX="true" INSTALL_SIM="true" @@ -122,7 +120,7 @@ if [[ $INSTALL_SIM == "true" ]]; then echo echo "Installing PX4 simulation dependencies" - # java (jmavsim or fastrtps) + # java (jmavsim) sudo pacman -S --noconfirm --needed \ ant \ jdk-openjdk \ diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 6d1439bb6c..b17539de6f 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -10,8 +10,6 @@ set -e ## - NuttX toolchain (omit with arg: --no-nuttx) ## - jMAVSim and Gazebo9 simulator (omit with arg: --no-sim-tools) ## -## Not Installs: -## - FastRTPS and FastCDR INSTALL_NUTTX="true" INSTALL_SIM="true" @@ -216,7 +214,7 @@ if [[ $INSTALL_SIM == "true" ]]; then else java_version=14 fi - # Java (jmavsim or fastrtps) + # Java (jmavsim) sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ ant \ openjdk-$java_version-jre \ diff --git a/Tools/update_px4_ros2_bridge.sh b/Tools/update_px4_ros2_bridge.sh deleted file mode 100755 index 58ebca5b7c..0000000000 --- a/Tools/update_px4_ros2_bridge.sh +++ /dev/null @@ -1,135 +0,0 @@ -#!/usr/bin/env bash -set -e - -agent_template_files_updated=0 -code_generator_files_updated=0 - -# parse help argument -if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then - echo -e "Usage: update_px4_ros2_bridge.bash [options...] \t This script allows to update px4_ros_com and px4_msgs in a specified directory." >&2 - echo - echo -e "\t--ws_dir \t\t Location of the ament/colcon workspace. Default: $HOME/colcon_ws." - echo -e "\t--px4_ros_com \t\t Updates px4_ros_com microRTPS agent code generation and templates." - echo -e "\t--px4_msgs \t\t Updates px4_msgs messages definition files." - echo -e "\t--all \t\t Updates both px4_ros_com and px4_msgs." - echo - exit 0 -fi - -# parse the arguments -while [ $# -gt 0 ]; do - if [[ $1 == *"--"* ]]; then - v="${1/--/}" - if [ ! -z $2 ]; then - declare $v="$2" - else - declare $v=1 - fi - fi - shift -done - -# get script directory -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" - -# get PX4-Autopilot directory -PX4_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) - -function compare_and_update () { - cmp -s "$1" "$2" - if [ $? -eq 1 ]; then - cp "$1" "$2" - return 0 - fi - - return 1; -} - -# update microRTPS agent code generators / helpers -function update_agent_code { - declare -a templates=( \ - "microRTPS_agent.cpp.em" \ - "microRTPS_timesync.cpp.em" \ - "microRTPS_timesync.h.em" \ - "microRTPS_transport.cpp" \ - "microRTPS_transport.h" \ - "Publisher.cpp.em" \ - "Publisher.h.em" \ - "Subscriber.cpp.em" \ - "Subscriber.h.em" \ - "RtpsTopics.cpp.em" \ - "RtpsTopics.h.em" \ - ) - - for file in ${templates[@]}; do - compare_and_update "$PX4_DIR/msg/templates/urtps/$file" "$ws_dir/src/px4_ros_com/templates/$file" \ - && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/templates/$file' updated" && ((agent_template_files_updated+=1)) - done - if [ $agent_template_files_updated -eq 0 ]; then - echo -e "--\t\t- No template files updated" - elif [ $agent_template_files_updated -eq 1 ]; then - echo -e "--\t\t - 1 template file updated" - else - echo -e "--\t\t - $agent_template_files_updated template files updated" - fi -} - -# update microRTPS agent code templates -function update_agent_templates { - declare -a code_generators=( \ - "uorb_rtps_classifier.py" \ - "generate_microRTPS_bridge.py" \ - "px_generate_uorb_topic_files.py" \ - ) - for file in ${code_generators[@]}; do - compare_and_update "$PX4_DIR/msg/tools/$file $ws_dir/src/px4_ros_com/scripts/$file" \ - && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/scripts/$file' updated" && ((code_generator_files_updated+=1)) - done - if [ $code_generator_files_updated -eq 0 ]; then - echo -e "--\t\t- No code generators / helpers files updated" - elif [ $code_generator_files_updated -eq 1 ]; then - echo -e "--\t\t - 1 code generator / helper file updated" - else - echo -e "--\t\t - $code_generator_files_updated code generator / helper files updated" - fi -} - -# update px4_ros_com files -function update_px4_ros_com { - python3 $PX4_DIR/msg/tools/uorb_to_ros_urtps_topics.py -i $PX4_DIR/msg/tools/urtps_bridge_topics.yaml -o $ws_dir/src/px4_ros_com/templates/urtps_bridge_topics.yaml - echo -e "--------------- \033[1mmicroRTPS agent code generation and templates update\033[0m ----------------" - echo "-------------------------------------------------------------------------------------------------------" - update_agent_code - update_agent_templates - return 0 -} - -# function to update px4_msgs -function update_px4_msgs { - find "$ws_dir/src/px4_msgs/msg/" -maxdepth 1 -type f -delete - python3 $PX4_DIR/msg/tools/uorb_to_ros_msgs.py $PX4_DIR/msg/ $ws_dir/src/px4_msgs/msg/ -} - -# decisor -echo "-------------------------------------------------------------------------------------------------------" -if [ -d "${ws_dir}" ]; then - ws_dir=$(cd "$ws_dir" && pwd) - if [ ! -z ${all} ]; then - update_px4_ros_com - echo "-------------------------------------------------------------------------------------------------------" - update_px4_msgs - elif [ ! -z ${px4_ros_com} ]; then - update_px4_ros_com - echo "-------------------------------------------------------------------------------------------------------" - elif [ ! -z ${px4_msgs} ]; then - update_px4_msgs - fi - echo -e "-------------------------------- \033[0;32mUpdate successful!\033[0m ---------------------------------" - echo "-------------------------------------------------------------------------------------------------------" - exit 0 -else - echo -e "-- \033[0;31mWorkspace directory doesn't exist...\033[0m" - echo -e "---------------------------------- \033[0;31mUpdate failed!\033[0m -----------------------------------" - echo "-------------------------------------------------------------------------------------------------------" - exit $ERRCODE -fi diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index e28a13c13b..edb01b3b89 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -48,6 +48,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index fe85f68e6a..1f12767b7c 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -34,7 +34,6 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y CONFIG_DRIVERS_UAVCAN=y CONFIG_MODULES_AIRSPEED_SELECTOR=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -53,7 +52,6 @@ CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y @@ -63,6 +61,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 7fcd4a1a81..6655e8e388 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -15,7 +15,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y -CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 48faec33f6..0eee7a73c3 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -50,6 +50,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 455d95d091..c4d16ff985 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 629c1a557f..9e53c4a38d 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index a9069bb741..e654a21f97 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -53,6 +53,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index bcff78646b..06c0cd20c3 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -52,6 +52,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board index 87395dd23d..0e3b91b463 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -53,6 +53,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index dbf4338931..8c3e771211 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -66,6 +66,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/modalai/fc-v1/rtps.px4board b/boards/modalai/fc-v1/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/modalai/fc-v1/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index a49105cf96..104d5e9947 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -36,7 +36,6 @@ CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y @@ -54,7 +53,6 @@ CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y @@ -64,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index d592c791f4..f9bcd91e0a 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index 5c64e1af3c..9fa47c2e4b 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7-oem/rtps.px4board b/boards/mro/ctrl-zero-h7-oem/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/mro/ctrl-zero-h7-oem/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 5fc4ad7dd8..11f35c83b1 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -63,6 +63,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7/rtps.px4board b/boards/mro/ctrl-zero-h7/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/mro/ctrl-zero-h7/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index f3c6137964..72b170ffcb 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/pixracerpro/rtps.px4board b/boards/mro/pixracerpro/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/mro/pixracerpro/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 3772be3f2b..f0d2e8fe15 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -41,6 +41,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -49,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -64,7 +66,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y @@ -75,7 +77,6 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_I2CDETECT=y diff --git a/boards/nxp/fmuk66-e/rtps.px4board b/boards/nxp/fmuk66-e/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/nxp/fmuk66-e/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 63032fb003..40ef605364 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -68,6 +68,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/nxp/fmuk66-v3/rtps.px4board b/boards/nxp/fmuk66-v3/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/nxp/fmuk66-v3/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board index cea42ef564..bdc42a1cc6 100644 --- a/boards/nxp/fmurt1062-v1/default.px4board +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 3c5dacf2f4..3de33fb7eb 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/px4/fmu-v4/rtps.px4board b/boards/px4/fmu-v4/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/px4/fmu-v4/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index 274e5b45ca..c023a5cb75 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -1,5 +1,7 @@ +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_BOARD_CRYPTO=y -CONFIG_DRIVERS_SW_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_DRIVERS_SW_CRYPTO=y CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 0ac4351440..7d3514675c 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -73,6 +73,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index d51a7314d5..284cb67dbc 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -4,9 +4,13 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_OPTICAL_FLOW=n CONFIG_COMMON_TELEMETRY=n -CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_ROBOCLAW=n -CONFIG_DRIVERS_RPM=n CONFIG_DRIVERS_UAVCAN=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_AIRSPEED_SELECTOR=n @@ -17,8 +21,8 @@ CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_FW_POS_CONTROL_L1=n CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n -CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n @@ -30,7 +34,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=n CONFIG_SYSTEMCMDS_GPIO=n CONFIG_SYSTEMCMDS_I2CDETECT=n CONFIG_SYSTEMCMDS_LED_CONTROL=n -CONFIG_SYSTEMCMDS_MOTOR_TEST=n CONFIG_SYSTEMCMDS_MTD=n CONFIG_SYSTEMCMDS_NSHTERM=n CONFIG_SYSTEMCMDS_REFLECT=n diff --git a/boards/px4/fmu-v5/rtps.px4board b/boards/px4/fmu-v5/rtps.px4board deleted file mode 100644 index e77de930f2..0000000000 --- a/boards/px4/fmu-v5/rtps.px4board +++ /dev/null @@ -1,3 +0,0 @@ -CONFIG_DRIVERS_HEATER=n -CONFIG_DRIVERS_OSD=n -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index 213d5767a8..e6cc948fde 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -1,43 +1,43 @@ # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_BAROMETERS=n +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n +CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_HYGROMETERS=n CONFIG_COMMON_TELEMETRY=n +CONFIG_DRIVERS_ADC_ADS1115=n CONFIG_DRIVERS_BATT_SMBUS=n CONFIG_DRIVERS_CAMERA_CAPTURE=n CONFIG_DRIVERS_CAMERA_TRIGGER=n -CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n -CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n CONFIG_DRIVERS_IRLOCK=n -CONFIG_DRIVERS_OSD=n CONFIG_DRIVERS_PCA9685=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_POWER_MONITOR_INA226=n CONFIG_DRIVERS_PWM_INPUT=n -CONFIG_DRIVERS_PWM_OUT_SIM=n CONFIG_DRIVERS_ROBOCLAW=n -CONFIG_DRIVERS_RPM=n CONFIG_DRIVERS_SMART_BATTERY_BATMON=n CONFIG_DRIVERS_UAVCAN=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_GIMBAL=n CONFIG_MODULES_GYRO_CALIBRATION=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n -CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n -CONFIG_SYSTEMCMDS_MOTOR_TEST=n CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_BAROMETER_MS5611=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index 59d08a9b56..9578fb422a 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -4,8 +4,8 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_HYGROMETERS=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_ADC_ADS1115=n -CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_GPS=n +CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n CONFIG_DRIVERS_IRLOCK=n @@ -14,15 +14,14 @@ CONFIG_DRIVERS_PCA9685=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_ROBOCLAW=n -CONFIG_DRIVERS_RPM=n CONFIG_DRIVERS_SMART_BATTERY_BATMON=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MICRODDS_CLIENT=n CONFIG_MODULES_ROVER_POS_CONTROL=n -CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index f0ff95bb6d..09b04fbcd6 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -9,7 +9,8 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3" CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y -CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y @@ -30,12 +31,9 @@ CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_COMMON_OSD=y -CONFIG_DRIVERS_PCA9685=y -CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y @@ -76,6 +74,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y @@ -88,7 +87,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y -CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_I2CDETECT=y @@ -103,7 +101,6 @@ CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SD_STRESS=y -CONFIG_SYSTEMCMDS_SERIAL_TEST=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y @@ -112,4 +109,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v5x/rtps.px4board b/boards/px4/fmu-v5x/rtps.px4board deleted file mode 100644 index 272e47aa57..0000000000 --- a/boards/px4/fmu-v5x/rtps.px4board +++ /dev/null @@ -1,15 +0,0 @@ -CONFIG_COMMON_BAROMETERS=n -CONFIG_COMMON_TELEMETRY=n -CONFIG_DRIVERS_OSD=n -CONFIG_EXAMPLES_FAKE_GPS=n -CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n -CONFIG_SYSTEMCMDS_DUMPFILE=n -CONFIG_SYSTEMCMDS_GPIO=n -CONFIG_SYSTEMCMDS_MOTOR_TEST=n -CONFIG_SYSTEMCMDS_REFLECT=n -CONFIG_SYSTEMCMDS_SD_BENCH=n -CONFIG_SYSTEMCMDS_SD_STRESS=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n -CONFIG_DRIVERS_BAROMETER_BMP388=y -CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board index 5279a489dc..1c71c5afbc 100644 --- a/boards/px4/fmu-v5x/test.px4board +++ b/boards/px4/fmu-v5x/test.px4board @@ -1,13 +1,11 @@ CONFIG_DRIVERS_ADC_ADS1115=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n -CONFIG_DRIVERS_PCA9685=n -CONFIG_DRIVERS_PCA9685_PWM_OUT=n -CONFIG_DRIVERS_RPM=n -CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_TEST_PPM=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 9f550cf3c0..ff3292e15d 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -57,6 +57,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 992238ede2..ef2ff976cb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -8,8 +8,8 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y -CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y +CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y @@ -24,7 +24,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -CONFIG_DRIVERS_MAGNETOMETER_RM3100=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y @@ -53,7 +52,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y -CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y @@ -66,11 +64,10 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_VTOL_ATT_CONTROL=y @@ -87,7 +84,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 32bc968c23..1dcc9d36ca 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -35,6 +35,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y diff --git a/boards/px4/sitl/rtps.px4board b/boards/px4/sitl/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/px4/sitl/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board index 943191213e..6d4ae08ccf 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -69,6 +69,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board index 75ef687ce5..f44aeac552 100644 --- a/boards/spracing/h7extreme/default.px4board +++ b/boards/spracing/h7extreme/default.px4board @@ -37,6 +37,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/msg/action_request.msg b/msg/ActionRequest.msg similarity index 100% rename from msg/action_request.msg rename to msg/ActionRequest.msg diff --git a/msg/actuator_armed.msg b/msg/ActuatorArmed.msg similarity index 100% rename from msg/actuator_armed.msg rename to msg/ActuatorArmed.msg diff --git a/msg/actuator_controls.msg b/msg/ActuatorControls.msg similarity index 88% rename from msg/actuator_controls.msg rename to msg/ActuatorControls.msg index 010e72790d..5a183a2cdc 100644 --- a/msg/actuator_controls.msg +++ b/msg/ActuatorControls.msg @@ -20,5 +20,5 @@ uint8 GROUP_INDEX_GIMBAL = 2 uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[9] control -# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 +# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/actuator_controls_status.msg b/msg/ActuatorControlsStatus.msg similarity index 66% rename from msg/actuator_controls_status.msg rename to msg/ActuatorControlsStatus.msg index 4e3bf83450..287fac8a4d 100644 --- a/msg/actuator_controls_status.msg +++ b/msg/ActuatorControlsStatus.msg @@ -7,4 +7,4 @@ uint8 INDEX_THROTTLE = 3 float32[4] control_power -# TOPICS actuator_controls_status actuator_controls_status_0 actuator_controls_status_1 +# TOPICS actuator_controls_status_0 actuator_controls_status_1 diff --git a/msg/actuator_motors.msg b/msg/ActuatorMotors.msg similarity index 100% rename from msg/actuator_motors.msg rename to msg/ActuatorMotors.msg diff --git a/msg/actuator_outputs.msg b/msg/ActuatorOutputs.msg similarity index 100% rename from msg/actuator_outputs.msg rename to msg/ActuatorOutputs.msg diff --git a/msg/actuator_servos.msg b/msg/ActuatorServos.msg similarity index 100% rename from msg/actuator_servos.msg rename to msg/ActuatorServos.msg diff --git a/msg/actuator_servos_trim.msg b/msg/ActuatorServosTrim.msg similarity index 100% rename from msg/actuator_servos_trim.msg rename to msg/ActuatorServosTrim.msg diff --git a/msg/actuator_test.msg b/msg/ActuatorTest.msg similarity index 100% rename from msg/actuator_test.msg rename to msg/ActuatorTest.msg diff --git a/msg/adc_report.msg b/msg/AdcReport.msg similarity index 100% rename from msg/adc_report.msg rename to msg/AdcReport.msg diff --git a/msg/airspeed.msg b/msg/Airspeed.msg similarity index 100% rename from msg/airspeed.msg rename to msg/Airspeed.msg diff --git a/msg/airspeed_validated.msg b/msg/AirspeedValidated.msg similarity index 100% rename from msg/airspeed_validated.msg rename to msg/AirspeedValidated.msg diff --git a/msg/airspeed_wind.msg b/msg/AirspeedWind.msg similarity index 100% rename from msg/airspeed_wind.msg rename to msg/AirspeedWind.msg diff --git a/msg/autotune_attitude_control_status.msg b/msg/AutotuneAttitudeControlStatus.msg similarity index 100% rename from msg/autotune_attitude_control_status.msg rename to msg/AutotuneAttitudeControlStatus.msg diff --git a/msg/battery_status.msg b/msg/BatteryStatus.msg similarity index 100% rename from msg/battery_status.msg rename to msg/BatteryStatus.msg diff --git a/msg/button_event.msg b/msg/ButtonEvent.msg similarity index 85% rename from msg/button_event.msg rename to msg/ButtonEvent.msg index bc745b9017..bbca356aab 100644 --- a/msg/button_event.msg +++ b/msg/ButtonEvent.msg @@ -3,4 +3,4 @@ bool triggered # Set to true if the event is triggered # TOPICS button_event safety_button -uint8 ORB_QUEUE_LENGTH = 2 \ No newline at end of file +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d459488399..756f2f454c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2016-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,223 +37,188 @@ cmake_policy(SET CMP0057 NEW) include(px4_list_make_absolute) set(msg_files - action_request.msg - actuator_armed.msg - actuator_controls.msg - actuator_controls_status.msg - actuator_motors.msg - actuator_outputs.msg - actuator_servos.msg - actuator_servos_trim.msg - actuator_test.msg - adc_report.msg - airspeed.msg - airspeed_validated.msg - airspeed_wind.msg - autotune_attitude_control_status.msg - battery_status.msg - button_event.msg - camera_capture.msg - camera_status.msg - camera_trigger.msg - cellular_status.msg - collision_constraints.msg - collision_report.msg - control_allocator_status.msg - cpuload.msg - differential_pressure.msg - distance_sensor.msg - ekf2_timestamps.msg - esc_report.msg - esc_status.msg - estimator_aid_source_1d.msg - estimator_aid_source_2d.msg - estimator_aid_source_3d.msg - estimator_bias.msg - estimator_event_flags.msg - estimator_gps_status.msg - estimator_innovations.msg - estimator_selector_status.msg - estimator_sensor_bias.msg - estimator_states.msg - estimator_status.msg - estimator_status_flags.msg - event.msg - failure_detector_status.msg - failsafe_flags.msg - follow_target.msg - follow_target_estimator.msg - follow_target_status.msg - generator_status.msg - geofence_result.msg - gimbal_device_attitude_status.msg - gimbal_device_information.msg - gimbal_device_set_attitude.msg - gimbal_manager_information.msg - gimbal_manager_set_attitude.msg - gimbal_manager_set_manual_control.msg - gimbal_manager_status.msg - gps_dump.msg - gps_inject_data.msg - health_report.msg - gripper.msg - heater_status.msg - home_position.msg - hover_thrust_estimate.msg - input_rc.msg - internal_combustion_engine_status.msg - iridiumsbd_status.msg - irlock_report.msg - landing_gear.msg - landing_target_innovations.msg - landing_target_pose.msg - led_control.msg - log_message.msg - logger_status.msg - mag_worker_data.msg - magnetometer_bias_estimate.msg - manual_control_setpoint.msg - manual_control_switches.msg - mavlink_log.msg - mavlink_tunnel.msg - mission.msg - mission_result.msg - mount_orientation.msg - navigator_mission_item.msg - npfg_status.msg - obstacle_distance.msg - offboard_control_mode.msg - onboard_computer_status.msg - orbit_status.msg - parameter_update.msg - ping.msg - position_controller_landing_status.msg - position_controller_status.msg - position_setpoint.msg - position_setpoint_triplet.msg - power_button_state.msg - power_monitor.msg - pps_capture.msg - pwm_input.msg - px4io_status.msg - radio_status.msg - rate_ctrl_status.msg - rc_channels.msg - rc_parameter_map.msg - rpm.msg - rtl_time_estimate.msg - satellite_info.msg - sensor_accel.msg - sensor_accel_fifo.msg - sensor_baro.msg - sensor_combined.msg - sensor_correction.msg - sensor_gnss_relative.msg - sensor_gps.msg - sensor_gyro.msg - sensor_gyro_fft.msg - sensor_gyro_fifo.msg - sensor_hygrometer.msg - sensor_mag.msg - sensor_optical_flow.msg - sensor_preflight_mag.msg - sensor_selection.msg - sensors_status.msg - sensors_status_imu.msg - system_power.msg - takeoff_status.msg - task_stack_info.msg - tecs_status.msg - telemetry_status.msg - timesync.msg - timesync_status.msg - trajectory_bezier.msg - trajectory_setpoint.msg - trajectory_waypoint.msg - transponder_report.msg - tune_control.msg - uavcan_parameter_request.msg - uavcan_parameter_value.msg - ulog_stream.msg - ulog_stream_ack.msg - uwb_distance.msg - uwb_grid.msg - vehicle_acceleration.msg - vehicle_air_data.msg - vehicle_angular_acceleration.msg - vehicle_angular_acceleration_setpoint.msg - vehicle_angular_velocity.msg - vehicle_attitude.msg - vehicle_attitude_setpoint.msg - vehicle_command.msg - vehicle_command_ack.msg - vehicle_constraints.msg - vehicle_control_mode.msg - vehicle_global_position.msg - vehicle_imu.msg - vehicle_imu_status.msg - vehicle_land_detected.msg - vehicle_local_position.msg - vehicle_local_position_setpoint.msg - vehicle_magnetometer.msg - vehicle_odometry.msg - vehicle_optical_flow.msg - vehicle_optical_flow_vel.msg - vehicle_rates_setpoint.msg - vehicle_roi.msg - vehicle_status.msg - vehicle_thrust_setpoint.msg - vehicle_torque_setpoint.msg - vehicle_trajectory_bezier.msg - vehicle_trajectory_waypoint.msg - vtol_vehicle_status.msg - wheel_encoders.msg - wind.msg - yaw_estimator_status.msg + ActionRequest.msg + ActuatorArmed.msg + ActuatorControls.msg + ActuatorControlsStatus.msg + ActuatorMotors.msg + ActuatorOutputs.msg + ActuatorServos.msg + ActuatorServosTrim.msg + ActuatorTest.msg + AdcReport.msg + Airspeed.msg + AirspeedValidated.msg + AirspeedWind.msg + AutotuneAttitudeControlStatus.msg + BatteryStatus.msg + ButtonEvent.msg + CameraCapture.msg + CameraStatus.msg + CameraTrigger.msg + CellularStatus.msg + CollisionConstraints.msg + CollisionReport.msg + ControlAllocatorStatus.msg + Cpuload.msg + DebugArray.msg + DebugKeyValue.msg + DebugValue.msg + DebugVect.msg + DifferentialPressure.msg + DistanceSensor.msg + Ekf2Timestamps.msg + EscReport.msg + EscStatus.msg + EstimatorAidSource1d.msg + EstimatorAidSource2d.msg + EstimatorAidSource3d.msg + EstimatorBias.msg + EstimatorEventFlags.msg + EstimatorGpsStatus.msg + EstimatorInnovations.msg + EstimatorSelectorStatus.msg + EstimatorSensorBias.msg + EstimatorStates.msg + EstimatorStatus.msg + EstimatorStatusFlags.msg + Event.msg + FailsafeFlags.msg + FailureDetectorStatus.msg + FollowTarget.msg + FollowTargetEstimator.msg + FollowTargetStatus.msg + GeneratorStatus.msg + GeofenceResult.msg + GimbalDeviceAttitudeStatus.msg + GimbalDeviceInformation.msg + GimbalDeviceSetAttitude.msg + GimbalManagerInformation.msg + GimbalManagerSetAttitude.msg + GimbalManagerSetManualControl.msg + GimbalManagerStatus.msg + GpsDump.msg + GpsInjectData.msg + Gripper.msg + HealthReport.msg + HeaterStatus.msg + HomePosition.msg + HoverThrustEstimate.msg + InputRc.msg + InternalCombustionEngineStatus.msg + IridiumsbdStatus.msg + IrlockReport.msg + LandingGear.msg + LandingTargetInnovations.msg + LandingTargetPose.msg + LedControl.msg + LoggerStatus.msg + LogMessage.msg + MagnetometerBiasEstimate.msg + MagWorkerData.msg + ManualControlSetpoint.msg + ManualControlSwitches.msg + MavlinkLog.msg + MavlinkTunnel.msg + Mission.msg + MissionResult.msg + MountOrientation.msg + NavigatorMissionItem.msg + NpfgStatus.msg + ObstacleDistance.msg + OffboardControlMode.msg + OnboardComputerStatus.msg + OrbitStatus.msg + OrbTest.msg + OrbTestLarge.msg + OrbTestMedium.msg + ParameterUpdate.msg + Ping.msg + PositionControllerLandingStatus.msg + PositionControllerStatus.msg + PositionSetpoint.msg + PositionSetpointTriplet.msg + PowerButtonState.msg + PowerMonitor.msg + PpsCapture.msg + PwmInput.msg + Px4ioStatus.msg + RadioStatus.msg + RateCtrlStatus.msg + RcChannels.msg + RcParameterMap.msg + Rpm.msg + RtlTimeEstimate.msg + SatelliteInfo.msg + SensorAccel.msg + SensorAccelFifo.msg + SensorBaro.msg + SensorCombined.msg + SensorCorrection.msg + SensorGnssRelative.msg + SensorGps.msg + SensorGyro.msg + SensorGyroFft.msg + SensorGyroFifo.msg + SensorHygrometer.msg + SensorMag.msg + SensorOpticalFlow.msg + SensorPreflightMag.msg + SensorSelection.msg + SensorsStatus.msg + SensorsStatusImu.msg + SystemPower.msg + TakeoffStatus.msg + TaskStackInfo.msg + TecsStatus.msg + TelemetryStatus.msg + TimesyncStatus.msg + TrajectoryBezier.msg + TrajectorySetpoint.msg + TrajectoryWaypoint.msg + TransponderReport.msg + TuneControl.msg + UavcanParameterRequest.msg + UavcanParameterValue.msg + UlogStream.msg + UlogStreamAck.msg + UwbDistance.msg + UwbGrid.msg + VehicleAcceleration.msg + VehicleAirData.msg + VehicleAngularAcceleration.msg + VehicleAngularAccelerationSetpoint.msg + VehicleAngularVelocity.msg + VehicleAttitude.msg + VehicleAttitudeSetpoint.msg + VehicleCommand.msg + VehicleCommandAck.msg + VehicleConstraints.msg + VehicleControlMode.msg + VehicleGlobalPosition.msg + VehicleImu.msg + VehicleImuStatus.msg + VehicleLandDetected.msg + VehicleLocalPosition.msg + VehicleLocalPositionSetpoint.msg + VehicleMagnetometer.msg + VehicleOdometry.msg + VehicleOpticalFlow.msg + VehicleOpticalFlowVel.msg + VehicleRatesSetpoint.msg + VehicleRoi.msg + VehicleStatus.msg + VehicleThrustSetpoint.msg + VehicleTorqueSetpoint.msg + VehicleTrajectoryBezier.msg + VehicleTrajectoryWaypoint.msg + VtolVehicleStatus.msg + WheelEncoders.msg + Wind.msg + YawEstimatorStatus.msg ) - -if(NOT px4_constrained_flash_build) - list(APPEND msg_files - debug_array.msg - debug_key_value.msg - debug_value.msg - debug_vect.msg - ) -endif() - -if(PX4_TESTING) - list(APPEND msg_files - orb_test.msg - orb_test_large.msg - orb_test_medium.msg - ) -endif() - list(SORT msg_files) -set(deprecated_msgs - ekf2_innovations.msg # 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'. - ) - -foreach(msg IN LISTS deprecated_msgs) - if(msg IN_LIST msg_files) - get_filename_component(msg_we ${msg} NAME_WE) - list(APPEND invalid_msgs ${msg_we}) - endif() -endforeach() -if(invalid_msgs) - list(LENGTH invalid_msgs invalid_msgs_size) - if(${invalid_msgs_size} GREATER 1) - foreach(msg IN LISTS invalid_msgs) - string(CONCAT invalid_msgs_cs ${invalid_msgs_cs} "'${msg}', ") - endforeach() - STRING(REGEX REPLACE ", +$" "" invalid_msgs_cs ${invalid_msgs_cs}) - message(FATAL_ERROR "${invalid_msgs_cs} are listed as deprecated. Please use different names for the messages.") - else() - message(FATAL_ERROR "'${invalid_msgs}' is listed as deprecated. Please use a different name for the message.") - endif() -endif() - px4_list_make_absolute(msg_files ${CMAKE_CURRENT_SOURCE_DIR} ${msg_files}) if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "") @@ -271,63 +236,64 @@ endif() # headers set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics) set(ucdr_out_path ${PX4_BINARY_DIR}/uORB/ucdr) -set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) +set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) -set(uorb_headers ${msg_out_path}/uORBTopics.hpp) -set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp) +set(uorb_headers) +set(uorb_sources) set(uorb_ucdr_headers) foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) + + # Pascal case to snake case (MsgFile -> msg_file) + string(REGEX REPLACE "(.)([A-Z][a-z]+)" "\\1_\\2" msg "${msg}") + string(REGEX REPLACE "([a-z0-9])([A-Z])" "\\1_\\2" msg "${msg}") + string(TOLOWER "${msg}" msg) + list(APPEND uorb_headers ${msg_out_path}/${msg}.h) list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp) list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h) endforeach() -if (px4_constrained_flash_build) - set(added_arguments --constrained-flash) -endif() - # set parent scope msg_files for other modules to consume (eg topic_listener) set(msg_files ${msg_files} PARENT_SCOPE) # Generate uORB headers -add_custom_command(OUTPUT ${uorb_headers} - COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py +add_custom_command( + OUTPUT + ${uorb_headers} + ${msg_out_path}/uORBTopics.hpp + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} -i ${CMAKE_CURRENT_SOURCE_DIR} -o ${msg_out_path} - -e templates/uorb - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers - -q - ${added_arguments} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS ${msg_files} - templates/uorb/msg.h.em - templates/uorb/uORBTopics.hpp.em - tools/px_generate_uorb_topic_files.py - tools/px_generate_uorb_topic_helper.py + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/msg.h.em + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/uORBTopics.hpp.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py COMMENT "Generating uORB topic headers" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} VERBATIM ) add_custom_target(uorb_headers DEPENDS ${uorb_headers}) -add_custom_command(OUTPUT ${uorb_ucdr_headers} - COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py +# Generate microcdr headers +add_custom_command( + OUTPUT ${uorb_ucdr_headers} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} -i ${CMAKE_CURRENT_SOURCE_DIR} -o ${ucdr_out_path} - -e templates/ucdr - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/ucdr_headers - -q - ${added_arguments} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/ucdr DEPENDS ${msg_files} - templates/ucdr/msg.h.em - tools/px_generate_uorb_topic_files.py - tools/px_generate_uorb_topic_helper.py + ${PX4_SOURCE_DIR}/Tools/msg/templates/ucdr/msg.h.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py COMMENT "Generating uORB topic ucdr headers" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} VERBATIM @@ -335,26 +301,27 @@ add_custom_command(OUTPUT ${uorb_ucdr_headers} add_custom_target(uorb_ucdr_headers DEPENDS ${uorb_ucdr_headers}) # Generate uORB sources -add_custom_command(OUTPUT ${uorb_sources} - COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py +add_custom_command( + OUTPUT + ${uorb_sources} + ${msg_source_out_path}/uORBTopics.cpp + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --sources -f ${msg_files} -i ${CMAKE_CURRENT_SOURCE_DIR} -o ${msg_source_out_path} - -e templates/uorb - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources - -q - ${added_arguments} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS ${msg_files} - templates/uorb/msg.cpp.em - templates/uorb/uORBTopics.cpp.em - tools/px_generate_uorb_topic_files.py - tools/px_generate_uorb_topic_helper.py + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/msg.cpp.em + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/uORBTopics.cpp.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py COMMENT "Generating uORB topic sources" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} VERBATIM ) -add_library(uorb_msgs ${uorb_sources}) +add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp) +target_link_libraries(uorb_msgs PRIVATE m) add_dependencies(uorb_msgs prebuild_targets uorb_headers) diff --git a/msg/camera_capture.msg b/msg/CameraCapture.msg similarity index 100% rename from msg/camera_capture.msg rename to msg/CameraCapture.msg diff --git a/msg/camera_status.msg b/msg/CameraStatus.msg similarity index 100% rename from msg/camera_status.msg rename to msg/CameraStatus.msg diff --git a/msg/camera_trigger.msg b/msg/CameraTrigger.msg similarity index 89% rename from msg/camera_trigger.msg rename to msg/CameraTrigger.msg index 0da1dd109d..abfdac6dda 100644 --- a/msg/camera_trigger.msg +++ b/msg/CameraTrigger.msg @@ -5,5 +5,3 @@ uint32 seq # Image sequence number bool feedback # Trigger feedback from camera uint32 ORB_QUEUE_LENGTH = 2 - -# TOPICS camera_trigger \ No newline at end of file diff --git a/msg/cellular_status.msg b/msg/CellularStatus.msg similarity index 100% rename from msg/cellular_status.msg rename to msg/CellularStatus.msg diff --git a/msg/collision_constraints.msg b/msg/CollisionConstraints.msg similarity index 100% rename from msg/collision_constraints.msg rename to msg/CollisionConstraints.msg diff --git a/msg/collision_report.msg b/msg/CollisionReport.msg similarity index 100% rename from msg/collision_report.msg rename to msg/CollisionReport.msg diff --git a/msg/control_allocator_status.msg b/msg/ControlAllocatorStatus.msg similarity index 100% rename from msg/control_allocator_status.msg rename to msg/ControlAllocatorStatus.msg diff --git a/msg/cpuload.msg b/msg/Cpuload.msg similarity index 100% rename from msg/cpuload.msg rename to msg/Cpuload.msg diff --git a/msg/debug_array.msg b/msg/DebugArray.msg similarity index 100% rename from msg/debug_array.msg rename to msg/DebugArray.msg diff --git a/msg/debug_key_value.msg b/msg/DebugKeyValue.msg similarity index 100% rename from msg/debug_key_value.msg rename to msg/DebugKeyValue.msg diff --git a/msg/debug_value.msg b/msg/DebugValue.msg similarity index 100% rename from msg/debug_value.msg rename to msg/DebugValue.msg diff --git a/msg/debug_vect.msg b/msg/DebugVect.msg similarity index 100% rename from msg/debug_vect.msg rename to msg/DebugVect.msg diff --git a/msg/differential_pressure.msg b/msg/DifferentialPressure.msg similarity index 100% rename from msg/differential_pressure.msg rename to msg/DifferentialPressure.msg diff --git a/msg/distance_sensor.msg b/msg/DistanceSensor.msg similarity index 100% rename from msg/distance_sensor.msg rename to msg/DistanceSensor.msg diff --git a/msg/ekf2_timestamps.msg b/msg/Ekf2Timestamps.msg similarity index 100% rename from msg/ekf2_timestamps.msg rename to msg/Ekf2Timestamps.msg diff --git a/msg/esc_report.msg b/msg/EscReport.msg similarity index 100% rename from msg/esc_report.msg rename to msg/EscReport.msg diff --git a/msg/esc_status.msg b/msg/EscStatus.msg similarity index 98% rename from msg/esc_status.msg rename to msg/EscStatus.msg index f786f6d351..e5e220ce0d 100644 --- a/msg/esc_status.msg +++ b/msg/EscStatus.msg @@ -25,4 +25,4 @@ uint8 esc_online_flags # Bitmask indicating which ESC is online/offline uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set. -esc_report[8] esc +EscReport[8] esc diff --git a/msg/estimator_aid_source_1d.msg b/msg/EstimatorAidSource1d.msg similarity index 96% rename from msg/estimator_aid_source_1d.msg rename to msg/EstimatorAidSource1d.msg index be544f2102..46faddafac 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -18,7 +18,6 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip # TOPICS estimator_aid_src_fake_hgt diff --git a/msg/estimator_aid_source_2d.msg b/msg/EstimatorAidSource2d.msg similarity index 95% rename from msg/estimator_aid_source_2d.msg rename to msg/EstimatorAidSource2d.msg index ada6be79d9..a70c9acaf5 100644 --- a/msg/estimator_aid_source_2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -18,6 +18,5 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_source_2d # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow diff --git a/msg/estimator_aid_source_3d.msg b/msg/EstimatorAidSource3d.msg similarity index 95% rename from msg/estimator_aid_source_3d.msg rename to msg/EstimatorAidSource3d.msg index e5c082eeec..bb53ae9823 100644 --- a/msg/estimator_aid_source_3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -18,7 +18,6 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_source_3d # TOPICS estimator_aid_src_ev_vel # TOPICS estimator_aid_src_gnss_vel # TOPICS estimator_aid_src_mag diff --git a/msg/estimator_bias.msg b/msg/EstimatorBias.msg similarity index 96% rename from msg/estimator_bias.msg rename to msg/EstimatorBias.msg index 988a6ce557..3dbadb94e1 100644 --- a/msg/estimator_bias.msg +++ b/msg/EstimatorBias.msg @@ -9,5 +9,4 @@ float32 innov # innovation of the last measurement fusion (m) float32 innov_var # innovation variance of the last measurement fusion (m^2) float32 innov_test_ratio # normalized innovation squared test ratio -# TOPICS estimator_bias # TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias diff --git a/msg/estimator_event_flags.msg b/msg/EstimatorEventFlags.msg similarity index 100% rename from msg/estimator_event_flags.msg rename to msg/EstimatorEventFlags.msg diff --git a/msg/estimator_gps_status.msg b/msg/EstimatorGpsStatus.msg similarity index 100% rename from msg/estimator_gps_status.msg rename to msg/EstimatorGpsStatus.msg diff --git a/msg/estimator_innovations.msg b/msg/EstimatorInnovations.msg similarity index 100% rename from msg/estimator_innovations.msg rename to msg/EstimatorInnovations.msg diff --git a/msg/estimator_selector_status.msg b/msg/EstimatorSelectorStatus.msg similarity index 100% rename from msg/estimator_selector_status.msg rename to msg/EstimatorSelectorStatus.msg diff --git a/msg/estimator_sensor_bias.msg b/msg/EstimatorSensorBias.msg similarity index 100% rename from msg/estimator_sensor_bias.msg rename to msg/EstimatorSensorBias.msg diff --git a/msg/estimator_states.msg b/msg/EstimatorStates.msg similarity index 100% rename from msg/estimator_states.msg rename to msg/EstimatorStates.msg diff --git a/msg/estimator_status.msg b/msg/EstimatorStatus.msg similarity index 100% rename from msg/estimator_status.msg rename to msg/EstimatorStatus.msg diff --git a/msg/estimator_status_flags.msg b/msg/EstimatorStatusFlags.msg similarity index 100% rename from msg/estimator_status_flags.msg rename to msg/EstimatorStatusFlags.msg diff --git a/msg/event.msg b/msg/Event.msg similarity index 100% rename from msg/event.msg rename to msg/Event.msg diff --git a/msg/failsafe_flags.msg b/msg/FailsafeFlags.msg similarity index 100% rename from msg/failsafe_flags.msg rename to msg/FailsafeFlags.msg diff --git a/msg/failure_detector_status.msg b/msg/FailureDetectorStatus.msg similarity index 100% rename from msg/failure_detector_status.msg rename to msg/FailureDetectorStatus.msg diff --git a/msg/follow_target.msg b/msg/FollowTarget.msg similarity index 100% rename from msg/follow_target.msg rename to msg/FollowTarget.msg diff --git a/msg/follow_target_estimator.msg b/msg/FollowTargetEstimator.msg similarity index 100% rename from msg/follow_target_estimator.msg rename to msg/FollowTargetEstimator.msg diff --git a/msg/follow_target_status.msg b/msg/FollowTargetStatus.msg similarity index 100% rename from msg/follow_target_status.msg rename to msg/FollowTargetStatus.msg diff --git a/msg/generator_status.msg b/msg/GeneratorStatus.msg similarity index 100% rename from msg/generator_status.msg rename to msg/GeneratorStatus.msg diff --git a/msg/geofence_result.msg b/msg/GeofenceResult.msg similarity index 100% rename from msg/geofence_result.msg rename to msg/GeofenceResult.msg diff --git a/msg/gimbal_device_attitude_status.msg b/msg/GimbalDeviceAttitudeStatus.msg similarity index 100% rename from msg/gimbal_device_attitude_status.msg rename to msg/GimbalDeviceAttitudeStatus.msg diff --git a/msg/gimbal_device_information.msg b/msg/GimbalDeviceInformation.msg similarity index 100% rename from msg/gimbal_device_information.msg rename to msg/GimbalDeviceInformation.msg diff --git a/msg/gimbal_device_set_attitude.msg b/msg/GimbalDeviceSetAttitude.msg similarity index 100% rename from msg/gimbal_device_set_attitude.msg rename to msg/GimbalDeviceSetAttitude.msg diff --git a/msg/gimbal_manager_information.msg b/msg/GimbalManagerInformation.msg similarity index 100% rename from msg/gimbal_manager_information.msg rename to msg/GimbalManagerInformation.msg diff --git a/msg/gimbal_manager_set_attitude.msg b/msg/GimbalManagerSetAttitude.msg similarity index 100% rename from msg/gimbal_manager_set_attitude.msg rename to msg/GimbalManagerSetAttitude.msg diff --git a/msg/gimbal_manager_set_manual_control.msg b/msg/GimbalManagerSetManualControl.msg similarity index 100% rename from msg/gimbal_manager_set_manual_control.msg rename to msg/GimbalManagerSetManualControl.msg diff --git a/msg/gimbal_manager_status.msg b/msg/GimbalManagerStatus.msg similarity index 100% rename from msg/gimbal_manager_status.msg rename to msg/GimbalManagerStatus.msg diff --git a/msg/gps_dump.msg b/msg/GpsDump.msg similarity index 100% rename from msg/gps_dump.msg rename to msg/GpsDump.msg diff --git a/msg/gps_inject_data.msg b/msg/GpsInjectData.msg similarity index 100% rename from msg/gps_inject_data.msg rename to msg/GpsInjectData.msg diff --git a/msg/gripper.msg b/msg/Gripper.msg similarity index 100% rename from msg/gripper.msg rename to msg/Gripper.msg diff --git a/msg/health_report.msg b/msg/HealthReport.msg similarity index 99% rename from msg/health_report.msg rename to msg/HealthReport.msg index 5d38e6db2a..189518052b 100644 --- a/msg/health_report.msg +++ b/msg/HealthReport.msg @@ -10,4 +10,3 @@ uint64 health_error_flags uint64 arming_check_warning_flags uint64 arming_check_error_flags - diff --git a/msg/heater_status.msg b/msg/HeaterStatus.msg similarity index 100% rename from msg/heater_status.msg rename to msg/HeaterStatus.msg diff --git a/msg/home_position.msg b/msg/HomePosition.msg similarity index 100% rename from msg/home_position.msg rename to msg/HomePosition.msg diff --git a/msg/hover_thrust_estimate.msg b/msg/HoverThrustEstimate.msg similarity index 100% rename from msg/hover_thrust_estimate.msg rename to msg/HoverThrustEstimate.msg diff --git a/msg/input_rc.msg b/msg/InputRc.msg similarity index 100% rename from msg/input_rc.msg rename to msg/InputRc.msg diff --git a/msg/internal_combustion_engine_status.msg b/msg/InternalCombustionEngineStatus.msg similarity index 100% rename from msg/internal_combustion_engine_status.msg rename to msg/InternalCombustionEngineStatus.msg diff --git a/msg/iridiumsbd_status.msg b/msg/IridiumsbdStatus.msg similarity index 100% rename from msg/iridiumsbd_status.msg rename to msg/IridiumsbdStatus.msg diff --git a/msg/irlock_report.msg b/msg/IrlockReport.msg similarity index 100% rename from msg/irlock_report.msg rename to msg/IrlockReport.msg diff --git a/msg/landing_gear.msg b/msg/LandingGear.msg similarity index 100% rename from msg/landing_gear.msg rename to msg/LandingGear.msg diff --git a/msg/landing_target_innovations.msg b/msg/LandingTargetInnovations.msg similarity index 100% rename from msg/landing_target_innovations.msg rename to msg/LandingTargetInnovations.msg diff --git a/msg/landing_target_pose.msg b/msg/LandingTargetPose.msg similarity index 100% rename from msg/landing_target_pose.msg rename to msg/LandingTargetPose.msg diff --git a/msg/led_control.msg b/msg/LedControl.msg similarity index 100% rename from msg/led_control.msg rename to msg/LedControl.msg diff --git a/msg/log_message.msg b/msg/LogMessage.msg similarity index 100% rename from msg/log_message.msg rename to msg/LogMessage.msg diff --git a/msg/logger_status.msg b/msg/LoggerStatus.msg similarity index 100% rename from msg/logger_status.msg rename to msg/LoggerStatus.msg diff --git a/msg/mag_worker_data.msg b/msg/MagWorkerData.msg similarity index 100% rename from msg/mag_worker_data.msg rename to msg/MagWorkerData.msg diff --git a/msg/magnetometer_bias_estimate.msg b/msg/MagnetometerBiasEstimate.msg similarity index 100% rename from msg/magnetometer_bias_estimate.msg rename to msg/MagnetometerBiasEstimate.msg diff --git a/msg/manual_control_setpoint.msg b/msg/ManualControlSetpoint.msg similarity index 100% rename from msg/manual_control_setpoint.msg rename to msg/ManualControlSetpoint.msg diff --git a/msg/manual_control_switches.msg b/msg/ManualControlSwitches.msg similarity index 100% rename from msg/manual_control_switches.msg rename to msg/ManualControlSwitches.msg diff --git a/msg/mavlink_log.msg b/msg/MavlinkLog.msg similarity index 100% rename from msg/mavlink_log.msg rename to msg/MavlinkLog.msg diff --git a/msg/mavlink_tunnel.msg b/msg/MavlinkTunnel.msg similarity index 100% rename from msg/mavlink_tunnel.msg rename to msg/MavlinkTunnel.msg diff --git a/msg/mission.msg b/msg/Mission.msg similarity index 100% rename from msg/mission.msg rename to msg/Mission.msg diff --git a/msg/mission_result.msg b/msg/MissionResult.msg similarity index 100% rename from msg/mission_result.msg rename to msg/MissionResult.msg diff --git a/msg/mount_orientation.msg b/msg/MountOrientation.msg similarity index 100% rename from msg/mount_orientation.msg rename to msg/MountOrientation.msg diff --git a/msg/navigator_mission_item.msg b/msg/NavigatorMissionItem.msg similarity index 100% rename from msg/navigator_mission_item.msg rename to msg/NavigatorMissionItem.msg diff --git a/msg/npfg_status.msg b/msg/NpfgStatus.msg similarity index 100% rename from msg/npfg_status.msg rename to msg/NpfgStatus.msg diff --git a/msg/obstacle_distance.msg b/msg/ObstacleDistance.msg similarity index 100% rename from msg/obstacle_distance.msg rename to msg/ObstacleDistance.msg diff --git a/msg/offboard_control_mode.msg b/msg/OffboardControlMode.msg similarity index 100% rename from msg/offboard_control_mode.msg rename to msg/OffboardControlMode.msg diff --git a/msg/onboard_computer_status.msg b/msg/OnboardComputerStatus.msg similarity index 100% rename from msg/onboard_computer_status.msg rename to msg/OnboardComputerStatus.msg diff --git a/msg/orb_test.msg b/msg/OrbTest.msg similarity index 100% rename from msg/orb_test.msg rename to msg/OrbTest.msg diff --git a/msg/orb_test_large.msg b/msg/OrbTestLarge.msg similarity index 100% rename from msg/orb_test_large.msg rename to msg/OrbTestLarge.msg diff --git a/msg/orb_test_medium.msg b/msg/OrbTestMedium.msg similarity index 100% rename from msg/orb_test_medium.msg rename to msg/OrbTestMedium.msg diff --git a/msg/orbit_status.msg b/msg/OrbitStatus.msg similarity index 100% rename from msg/orbit_status.msg rename to msg/OrbitStatus.msg diff --git a/msg/parameter_update.msg b/msg/ParameterUpdate.msg similarity index 100% rename from msg/parameter_update.msg rename to msg/ParameterUpdate.msg diff --git a/msg/ping.msg b/msg/Ping.msg similarity index 100% rename from msg/ping.msg rename to msg/Ping.msg diff --git a/msg/position_controller_landing_status.msg b/msg/PositionControllerLandingStatus.msg similarity index 100% rename from msg/position_controller_landing_status.msg rename to msg/PositionControllerLandingStatus.msg diff --git a/msg/position_controller_status.msg b/msg/PositionControllerStatus.msg similarity index 100% rename from msg/position_controller_status.msg rename to msg/PositionControllerStatus.msg diff --git a/msg/position_setpoint.msg b/msg/PositionSetpoint.msg similarity index 100% rename from msg/position_setpoint.msg rename to msg/PositionSetpoint.msg diff --git a/msg/position_setpoint_triplet.msg b/msg/PositionSetpointTriplet.msg similarity index 67% rename from msg/position_setpoint_triplet.msg rename to msg/PositionSetpointTriplet.msg index 7c9a2ca78b..6f9ac4d2a2 100644 --- a/msg/position_setpoint_triplet.msg +++ b/msg/PositionSetpointTriplet.msg @@ -3,6 +3,6 @@ uint64 timestamp # time since system start (microseconds) -px4/position_setpoint previous -px4/position_setpoint current -px4/position_setpoint next +PositionSetpoint previous +PositionSetpoint current +PositionSetpoint next diff --git a/msg/power_button_state.msg b/msg/PowerButtonState.msg similarity index 100% rename from msg/power_button_state.msg rename to msg/PowerButtonState.msg diff --git a/msg/power_monitor.msg b/msg/PowerMonitor.msg similarity index 100% rename from msg/power_monitor.msg rename to msg/PowerMonitor.msg diff --git a/msg/pps_capture.msg b/msg/PpsCapture.msg similarity index 100% rename from msg/pps_capture.msg rename to msg/PpsCapture.msg diff --git a/msg/pwm_input.msg b/msg/PwmInput.msg similarity index 100% rename from msg/pwm_input.msg rename to msg/PwmInput.msg diff --git a/msg/px4io_status.msg b/msg/Px4ioStatus.msg similarity index 100% rename from msg/px4io_status.msg rename to msg/Px4ioStatus.msg diff --git a/msg/radio_status.msg b/msg/RadioStatus.msg similarity index 100% rename from msg/radio_status.msg rename to msg/RadioStatus.msg diff --git a/msg/rate_ctrl_status.msg b/msg/RateCtrlStatus.msg similarity index 100% rename from msg/rate_ctrl_status.msg rename to msg/RateCtrlStatus.msg diff --git a/msg/rc_channels.msg b/msg/RcChannels.msg similarity index 100% rename from msg/rc_channels.msg rename to msg/RcChannels.msg diff --git a/msg/rc_parameter_map.msg b/msg/RcParameterMap.msg similarity index 100% rename from msg/rc_parameter_map.msg rename to msg/RcParameterMap.msg diff --git a/msg/rpm.msg b/msg/Rpm.msg similarity index 100% rename from msg/rpm.msg rename to msg/Rpm.msg diff --git a/msg/rtl_time_estimate.msg b/msg/RtlTimeEstimate.msg similarity index 100% rename from msg/rtl_time_estimate.msg rename to msg/RtlTimeEstimate.msg diff --git a/msg/satellite_info.msg b/msg/SatelliteInfo.msg similarity index 100% rename from msg/satellite_info.msg rename to msg/SatelliteInfo.msg diff --git a/msg/sensor_accel.msg b/msg/SensorAccel.msg similarity index 100% rename from msg/sensor_accel.msg rename to msg/SensorAccel.msg diff --git a/msg/sensor_accel_fifo.msg b/msg/SensorAccelFifo.msg similarity index 100% rename from msg/sensor_accel_fifo.msg rename to msg/SensorAccelFifo.msg diff --git a/msg/sensor_baro.msg b/msg/SensorBaro.msg similarity index 100% rename from msg/sensor_baro.msg rename to msg/SensorBaro.msg diff --git a/msg/sensor_combined.msg b/msg/SensorCombined.msg similarity index 100% rename from msg/sensor_combined.msg rename to msg/SensorCombined.msg diff --git a/msg/sensor_correction.msg b/msg/SensorCorrection.msg similarity index 100% rename from msg/sensor_correction.msg rename to msg/SensorCorrection.msg diff --git a/msg/sensor_gnss_relative.msg b/msg/SensorGnssRelative.msg similarity index 100% rename from msg/sensor_gnss_relative.msg rename to msg/SensorGnssRelative.msg diff --git a/msg/sensor_gps.msg b/msg/SensorGps.msg similarity index 100% rename from msg/sensor_gps.msg rename to msg/SensorGps.msg diff --git a/msg/sensor_gyro.msg b/msg/SensorGyro.msg similarity index 100% rename from msg/sensor_gyro.msg rename to msg/SensorGyro.msg diff --git a/msg/sensor_gyro_fft.msg b/msg/SensorGyroFft.msg similarity index 100% rename from msg/sensor_gyro_fft.msg rename to msg/SensorGyroFft.msg diff --git a/msg/sensor_gyro_fifo.msg b/msg/SensorGyroFifo.msg similarity index 100% rename from msg/sensor_gyro_fifo.msg rename to msg/SensorGyroFifo.msg diff --git a/msg/sensor_hygrometer.msg b/msg/SensorHygrometer.msg old mode 100755 new mode 100644 similarity index 100% rename from msg/sensor_hygrometer.msg rename to msg/SensorHygrometer.msg diff --git a/msg/sensor_mag.msg b/msg/SensorMag.msg similarity index 100% rename from msg/sensor_mag.msg rename to msg/SensorMag.msg diff --git a/msg/sensor_optical_flow.msg b/msg/SensorOpticalFlow.msg similarity index 100% rename from msg/sensor_optical_flow.msg rename to msg/SensorOpticalFlow.msg diff --git a/msg/sensor_preflight_mag.msg b/msg/SensorPreflightMag.msg similarity index 100% rename from msg/sensor_preflight_mag.msg rename to msg/SensorPreflightMag.msg diff --git a/msg/sensor_selection.msg b/msg/SensorSelection.msg similarity index 100% rename from msg/sensor_selection.msg rename to msg/SensorSelection.msg diff --git a/msg/sensors_status.msg b/msg/SensorsStatus.msg similarity index 87% rename from msg/sensors_status.msg rename to msg/SensorsStatus.msg index 28c2797812..c16bf1c6a2 100644 --- a/msg/sensors_status.msg +++ b/msg/SensorsStatus.msg @@ -12,4 +12,4 @@ uint8[4] priority bool[4] enabled bool[4] external -# TOPICS sensors_status sensors_status_baro sensors_status_mag +# TOPICS sensors_status_baro sensors_status_mag diff --git a/msg/sensors_status_imu.msg b/msg/SensorsStatusImu.msg similarity index 100% rename from msg/sensors_status_imu.msg rename to msg/SensorsStatusImu.msg diff --git a/msg/system_power.msg b/msg/SystemPower.msg similarity index 100% rename from msg/system_power.msg rename to msg/SystemPower.msg diff --git a/msg/takeoff_status.msg b/msg/TakeoffStatus.msg similarity index 100% rename from msg/takeoff_status.msg rename to msg/TakeoffStatus.msg diff --git a/msg/task_stack_info.msg b/msg/TaskStackInfo.msg similarity index 100% rename from msg/task_stack_info.msg rename to msg/TaskStackInfo.msg diff --git a/msg/tecs_status.msg b/msg/TecsStatus.msg similarity index 100% rename from msg/tecs_status.msg rename to msg/TecsStatus.msg diff --git a/msg/telemetry_status.msg b/msg/TelemetryStatus.msg similarity index 100% rename from msg/telemetry_status.msg rename to msg/TelemetryStatus.msg diff --git a/msg/timesync_status.msg b/msg/TimesyncStatus.msg similarity index 72% rename from msg/timesync_status.msg rename to msg/TimesyncStatus.msg index 9d557352dc..71e84e85ab 100644 --- a/msg/timesync_status.msg +++ b/msg/TimesyncStatus.msg @@ -1,8 +1,9 @@ uint64 timestamp # time since system start (microseconds) -uint8 SOURCE_PROTOCOL_MAVLINK = 0 -uint8 SOURCE_PROTOCOL_RTPS = 1 -uint8 source_protocol # timesync source. Source can be MAVLink or the microRTPS bridge +uint8 SOURCE_PROTOCOL_UNKNOWN = 0 +uint8 SOURCE_PROTOCOL_MAVLINK = 1 +uint8 SOURCE_PROTOCOL_DDS = 2 +uint8 source_protocol # timesync source uint64 remote_timestamp # remote system timestamp (microseconds) int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) diff --git a/msg/trajectory_bezier.msg b/msg/TrajectoryBezier.msg similarity index 100% rename from msg/trajectory_bezier.msg rename to msg/TrajectoryBezier.msg diff --git a/msg/trajectory_setpoint.msg b/msg/TrajectorySetpoint.msg similarity index 100% rename from msg/trajectory_setpoint.msg rename to msg/TrajectorySetpoint.msg diff --git a/msg/trajectory_waypoint.msg b/msg/TrajectoryWaypoint.msg similarity index 100% rename from msg/trajectory_waypoint.msg rename to msg/TrajectoryWaypoint.msg diff --git a/msg/transponder_report.msg b/msg/TransponderReport.msg similarity index 100% rename from msg/transponder_report.msg rename to msg/TransponderReport.msg diff --git a/msg/tune_control.msg b/msg/TuneControl.msg similarity index 100% rename from msg/tune_control.msg rename to msg/TuneControl.msg diff --git a/msg/uavcan_parameter_request.msg b/msg/UavcanParameterRequest.msg similarity index 100% rename from msg/uavcan_parameter_request.msg rename to msg/UavcanParameterRequest.msg diff --git a/msg/uavcan_parameter_value.msg b/msg/UavcanParameterValue.msg similarity index 100% rename from msg/uavcan_parameter_value.msg rename to msg/UavcanParameterValue.msg diff --git a/msg/ulog_stream.msg b/msg/UlogStream.msg similarity index 100% rename from msg/ulog_stream.msg rename to msg/UlogStream.msg diff --git a/msg/ulog_stream_ack.msg b/msg/UlogStreamAck.msg similarity index 100% rename from msg/ulog_stream_ack.msg rename to msg/UlogStreamAck.msg diff --git a/msg/uwb_distance.msg b/msg/UwbDistance.msg similarity index 100% rename from msg/uwb_distance.msg rename to msg/UwbDistance.msg diff --git a/msg/uwb_grid.msg b/msg/UwbGrid.msg similarity index 100% rename from msg/uwb_grid.msg rename to msg/UwbGrid.msg diff --git a/msg/vehicle_acceleration.msg b/msg/VehicleAcceleration.msg similarity index 100% rename from msg/vehicle_acceleration.msg rename to msg/VehicleAcceleration.msg diff --git a/msg/vehicle_air_data.msg b/msg/VehicleAirData.msg similarity index 100% rename from msg/vehicle_air_data.msg rename to msg/VehicleAirData.msg diff --git a/msg/vehicle_angular_acceleration.msg b/msg/VehicleAngularAcceleration.msg similarity index 100% rename from msg/vehicle_angular_acceleration.msg rename to msg/VehicleAngularAcceleration.msg diff --git a/msg/vehicle_angular_acceleration_setpoint.msg b/msg/VehicleAngularAccelerationSetpoint.msg similarity index 100% rename from msg/vehicle_angular_acceleration_setpoint.msg rename to msg/VehicleAngularAccelerationSetpoint.msg diff --git a/msg/vehicle_angular_velocity.msg b/msg/VehicleAngularVelocity.msg similarity index 100% rename from msg/vehicle_angular_velocity.msg rename to msg/VehicleAngularVelocity.msg diff --git a/msg/vehicle_attitude.msg b/msg/VehicleAttitude.msg similarity index 86% rename from msg/vehicle_attitude.msg rename to msg/VehicleAttitude.msg index f704d71933..6eeb20a21e 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/VehicleAttitude.msg @@ -8,5 +8,5 @@ float32[4] q # Quaternion rotation from the FRD body frame to float32[4] delta_q_reset # Amount by which quaternion has changed during last reset uint8 quat_reset_counter # Quaternion reset counter -# TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude +# TOPICS vehicle_attitude vehicle_attitude_groundtruth # TOPICS estimator_attitude diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/VehicleAttitudeSetpoint.msg similarity index 100% rename from msg/vehicle_attitude_setpoint.msg rename to msg/VehicleAttitudeSetpoint.msg diff --git a/msg/vehicle_command.msg b/msg/VehicleCommand.msg similarity index 100% rename from msg/vehicle_command.msg rename to msg/VehicleCommand.msg diff --git a/msg/vehicle_command_ack.msg b/msg/VehicleCommandAck.msg similarity index 100% rename from msg/vehicle_command_ack.msg rename to msg/VehicleCommandAck.msg diff --git a/msg/vehicle_constraints.msg b/msg/VehicleConstraints.msg similarity index 100% rename from msg/vehicle_constraints.msg rename to msg/VehicleConstraints.msg diff --git a/msg/vehicle_control_mode.msg b/msg/VehicleControlMode.msg similarity index 100% rename from msg/vehicle_control_mode.msg rename to msg/VehicleControlMode.msg diff --git a/msg/vehicle_global_position.msg b/msg/VehicleGlobalPosition.msg similarity index 100% rename from msg/vehicle_global_position.msg rename to msg/VehicleGlobalPosition.msg diff --git a/msg/vehicle_imu.msg b/msg/VehicleImu.msg similarity index 100% rename from msg/vehicle_imu.msg rename to msg/VehicleImu.msg diff --git a/msg/vehicle_imu_status.msg b/msg/VehicleImuStatus.msg similarity index 100% rename from msg/vehicle_imu_status.msg rename to msg/VehicleImuStatus.msg diff --git a/msg/vehicle_land_detected.msg b/msg/VehicleLandDetected.msg similarity index 100% rename from msg/vehicle_land_detected.msg rename to msg/VehicleLandDetected.msg diff --git a/msg/vehicle_local_position.msg b/msg/VehicleLocalPosition.msg similarity index 100% rename from msg/vehicle_local_position.msg rename to msg/VehicleLocalPosition.msg diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/VehicleLocalPositionSetpoint.msg similarity index 100% rename from msg/vehicle_local_position_setpoint.msg rename to msg/VehicleLocalPositionSetpoint.msg diff --git a/msg/vehicle_magnetometer.msg b/msg/VehicleMagnetometer.msg similarity index 100% rename from msg/vehicle_magnetometer.msg rename to msg/VehicleMagnetometer.msg diff --git a/msg/vehicle_odometry.msg b/msg/VehicleOdometry.msg similarity index 100% rename from msg/vehicle_odometry.msg rename to msg/VehicleOdometry.msg diff --git a/msg/vehicle_optical_flow.msg b/msg/VehicleOpticalFlow.msg similarity index 100% rename from msg/vehicle_optical_flow.msg rename to msg/VehicleOpticalFlow.msg diff --git a/msg/vehicle_optical_flow_vel.msg b/msg/VehicleOpticalFlowVel.msg similarity index 100% rename from msg/vehicle_optical_flow_vel.msg rename to msg/VehicleOpticalFlowVel.msg diff --git a/msg/vehicle_rates_setpoint.msg b/msg/VehicleRatesSetpoint.msg similarity index 100% rename from msg/vehicle_rates_setpoint.msg rename to msg/VehicleRatesSetpoint.msg diff --git a/msg/vehicle_roi.msg b/msg/VehicleRoi.msg similarity index 100% rename from msg/vehicle_roi.msg rename to msg/VehicleRoi.msg diff --git a/msg/vehicle_status.msg b/msg/VehicleStatus.msg similarity index 100% rename from msg/vehicle_status.msg rename to msg/VehicleStatus.msg diff --git a/msg/vehicle_thrust_setpoint.msg b/msg/VehicleThrustSetpoint.msg similarity index 100% rename from msg/vehicle_thrust_setpoint.msg rename to msg/VehicleThrustSetpoint.msg diff --git a/msg/vehicle_torque_setpoint.msg b/msg/VehicleTorqueSetpoint.msg similarity index 100% rename from msg/vehicle_torque_setpoint.msg rename to msg/VehicleTorqueSetpoint.msg diff --git a/msg/vehicle_trajectory_bezier.msg b/msg/VehicleTrajectoryBezier.msg similarity index 93% rename from msg/vehicle_trajectory_bezier.msg rename to msg/VehicleTrajectoryBezier.msg index 421ff1a015..d4bf99b469 100644 --- a/msg/vehicle_trajectory_bezier.msg +++ b/msg/VehicleTrajectoryBezier.msg @@ -12,7 +12,7 @@ uint8 POINT_4 = 4 uint8 NUMBER_POINTS = 5 -trajectory_bezier[5] control_points +TrajectoryBezier[5] control_points uint8 bezier_order # TOPICS vehicle_trajectory_bezier diff --git a/msg/vehicle_trajectory_waypoint.msg b/msg/VehicleTrajectoryWaypoint.msg similarity index 95% rename from msg/vehicle_trajectory_waypoint.msg rename to msg/VehicleTrajectoryWaypoint.msg index 182f548d23..6bff1cec84 100644 --- a/msg/vehicle_trajectory_waypoint.msg +++ b/msg/VehicleTrajectoryWaypoint.msg @@ -16,6 +16,6 @@ uint8 POINT_4 = 4 uint8 NUMBER_POINTS = 5 -trajectory_waypoint[5] waypoints +TrajectoryWaypoint[5] waypoints # TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/msg/vtol_vehicle_status.msg b/msg/VtolVehicleStatus.msg similarity index 100% rename from msg/vtol_vehicle_status.msg rename to msg/VtolVehicleStatus.msg diff --git a/msg/wheel_encoders.msg b/msg/WheelEncoders.msg similarity index 100% rename from msg/wheel_encoders.msg rename to msg/WheelEncoders.msg diff --git a/msg/wind.msg b/msg/Wind.msg similarity index 100% rename from msg/wind.msg rename to msg/Wind.msg diff --git a/msg/yaw_estimator_status.msg b/msg/YawEstimatorStatus.msg similarity index 100% rename from msg/yaw_estimator_status.msg rename to msg/YawEstimatorStatus.msg diff --git a/msg/templates/module.yaml b/msg/templates/module.yaml deleted file mode 100644 index 099976c11e..0000000000 --- a/msg/templates/module.yaml +++ /dev/null @@ -1,6 +0,0 @@ -module_name: Sagtech MXS -serial_config: - - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} - port_config_param: - name: TRANS_MXS_CFG - group: Transponders diff --git a/msg/templates/px4/ros/msg.h.em b/msg/templates/px4/ros/msg.h.em deleted file mode 100644 index e102a087d4..0000000000 --- a/msg/templates/px4/ros/msg.h.em +++ /dev/null @@ -1,94 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# This file generates the multiplatform wrapper -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - - -cpp_class = 'px4_%s'%spec.short_name -native_type = spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include "px4/@(topic_name).h" -#include "platforms/px4_message.h" - -@############################## -@# Class -@############################## - -namespace px4 -{ - -class @(cpp_class) : - public PX4Message<@(native_type)> -{ -public: - @(cpp_class)() : - PX4Message<@(native_type)>() - {} - - @(cpp_class)(@(native_type) msg) : - PX4Message<@(native_type)>(msg) - {} - - ~@(cpp_class)() {} - - static PX4TopicHandle handle() {return "@(topic_name)";} -}; - -}; diff --git a/msg/templates/px4/uorb/msg.h.em b/msg/templates/px4/uorb/msg.h.em deleted file mode 100644 index deed11912d..0000000000 --- a/msg/templates/px4/uorb/msg.h.em +++ /dev/null @@ -1,99 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# This file generates the multiplatform wrapper -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - - -native_type = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include "platforms/px4_message.h" - -@############################## -@# Class -@############################## - -namespace px4 -{ -@[for multi_topic in topics]@ -@{ -cpp_class = 'px4_%s'%multi_topic -}@ - - -class __EXPORT @(cpp_class) : - public PX4Message<@(native_type)> -{ -public: - @(cpp_class)() : - PX4Message<@(native_type)>() - {} - - @(cpp_class)(@(native_type) msg) : - PX4Message<@(native_type)>(msg) - {} - - ~@(cpp_class)() {} - - static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));} -}; -@[end for] - -}; diff --git a/msg/templates/uorb_microcdr/dds_topics.h.em b/msg/templates/uorb_microcdr/dds_topics.h.em deleted file mode 100644 index 8d7b5e7a67..0000000000 --- a/msg/templates/uorb_microcdr/dds_topics.h.em +++ /dev/null @@ -1,225 +0,0 @@ -@############################################### -@# -@# EmPy template -@# -@############################################### -@# Generates publications and subscriptions for XRCE -@# -@# Context: -@# - msgs (List) list of all RTPS messages -@# - topics (List) list of all topic names -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import os - -import genmsg.msgs - -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -topic_names = [s.short_name for s in spec] -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ - - -#include -#include - -#include -#include -@[for idx, topic in enumerate(send_topics)]@ -#include -@[end for]@ -@[for idx, topic in enumerate(recv_topics)]@ -#include -@[end for]@ - -// Subscribers for messages to send -struct SendTopicsSubs { -@[ for idx, topic in enumerate(send_topics)]@ - uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; - uxrObjectId @(topic)_data_writer; -@[ end for]@ - - uxrSession* session; - - uint32_t num_payload_sent{}; - - bool init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id); - void update(uxrStreamId stream_id); -}; - -bool SendTopicsSubs::init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id) -{ - session = session_; - -@[ for idx, topic in enumerate(send_topics)]@ -@{ -topic_pascal = topic.replace("_", " ").title().replace(" ", "") -}@ - { - - uxrObjectId topic_id = uxr_object_id(@(idx)+1, UXR_TOPIC_ID); - const char* topic_xml = "" - "" - "rt/fmu/out/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - ""; - uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, - UXR_REPLACE); - - uxrObjectId publisher_id = uxr_object_id(@(idx)+1, UXR_PUBLISHER_ID); - const char* publisher_xml = ""; - uint16_t publisher_req = uxr_buffer_create_publisher_xml(session, stream_id, publisher_id, participant_id, - publisher_xml, UXR_REPLACE); - - uxrObjectId datawriter_id = uxr_object_id(@(idx)+1, UXR_DATAWRITER_ID); - @(topic)_data_writer = datawriter_id; - const char* datawriter_xml = "" - "" - "" - "NO_KEY" - "rt/fmu/out/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - "" - ""; - uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(session, stream_id, datawriter_id, publisher_id, - datawriter_xml, UXR_REPLACE); - - // Send create entities message and wait its status - uint8_t status[3]; - uint16_t requests[3] = { - topic_req, publisher_req, datawriter_req - }; - if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { - PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", "@(topic_pascal)", status[0], status[1], status[2]); - return false; - } - } - -@[ end for]@ - - return true; -} - -void SendTopicsSubs::update(uxrStreamId stream_id) -{ -@[ for idx, topic in enumerate(send_topics)]@ - { - @(send_base_types[idx])_s data; - - if (@(topic)_sub.update(&data)) { - ucdrBuffer ub{}; - uint32_t topic_size = ucdr_topic_size_@(send_base_types[idx])(); - uxr_prepare_output_stream(session, stream_id, @(topic)_data_writer, &ub, topic_size); - ucdr_serialize_@(send_base_types[idx])(data, ub); - // TODO: fill up the MTU and then flush, which reduces the packet overhead - uxr_flash_output_streams(session); - num_payload_sent += topic_size; - } - } -@[ end for]@ - -} - -static void on_topic_update(uxrSession* session, uxrObjectId object_id, - uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t - length, void* args); - -// Publishers for received messages -struct RcvTopicsPubs { -@[ for idx, topic in enumerate(recv_topics)]@ - uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; -@[ end for]@ - - uxrSession* session; - - uint32_t num_payload_received{}; - - bool init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id); -}; - -bool RcvTopicsPubs::init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id) -{ - session = session_; - uxr_set_topic_callback(session, on_topic_update, this); - - -@[ for idx, topic in enumerate(recv_topics)]@ -@{ -topic_pascal = topic.replace("_", " ").title().replace(" ", "") -}@ - { - - uxrObjectId subscriber_id = uxr_object_id(@(idx)+1, UXR_SUBSCRIBER_ID); - const char* subscriber_xml = ""; - uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(session, stream_id, subscriber_id, participant_id, subscriber_xml, UXR_REPLACE); - - uxrObjectId topic_id = uxr_object_id(1000+@(idx), UXR_TOPIC_ID); - const char* topic_xml = "" - "" - "rt/fmu/in/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - ""; - uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, UXR_REPLACE); - - uxrObjectId datareader_id = uxr_object_id(@(idx)+1, UXR_DATAREADER_ID); - const char* datareader_xml = "" - "" - "" - "NO_KEY" - "rt/fmu/in/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - "" - ""; - uint16_t datareader_req = uxr_buffer_create_datareader_xml(session, stream_id, datareader_id, subscriber_id, datareader_xml, UXR_REPLACE); - - uint8_t status[3]; - uint16_t requests[3] = {topic_req, subscriber_req, datareader_req }; - if(!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) - { - PX4_ERR("create entities failed: %s %i %i %i", "@(topic)", status[0], status[1], status[2]); - return false; - } - - uxrDeliveryControl delivery_control = {0}; - delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED; - uxr_buffer_request_data(session, stream_id, datareader_id, input_stream, &delivery_control); - - } - -@[ end for]@ - - return true; -} - -void on_topic_update(uxrSession* session, uxrObjectId object_id, - uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t - length, void* args) -{ - RcvTopicsPubs* pubs = (RcvTopicsPubs*)args; - pubs->num_payload_received += length; - - switch (object_id.id) { -@[ for idx, topic in enumerate(recv_topics)]@ - case @(idx)+1: { - @(receive_base_types[idx])_s topic; - if (ucdr_deserialize_@(receive_base_types[idx])(*ub, topic)) { - pubs->@(topic)_pub.publish(topic); - } - } - break; -@[ end for]@ - - default: - PX4_ERR("unknown object id: %i", object_id.id); - break; - } -} diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em deleted file mode 100644 index ba49bfbcb8..0000000000 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ /dev/null @@ -1,310 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_client.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - msgs (List) list of all RTPS messages -@# - topics (List) list of all topic names -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import os - -import genmsg.msgs - -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -topic_names = [s.short_name for s in spec] -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ -/**************************************************************************** - * - * Copyright (c) 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -@[for topic in list(set(topic_names))]@ -#include -#include -@[end for]@ - -using namespace time_literals; - -@[if recv_topics]@ -// Publishers for received messages -struct RcvTopicsPubs { -@[ for idx, topic in enumerate(recv_topics)]@ - uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; -@[ end for]@ -}; -@[end if]@ - -@[if send_topics]@ -// Subscribers for messages to send -struct SendTopicsSubs { -@[ for idx, topic in enumerate(send_topics)]@ - uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; -@[ end for]@ -}; - -struct SendThreadArgs { - const uint32_t &datarate; - uint64_t &total_sent; - uint64_t &sent_last_sec; - uint64_t &sent; - int &sent_loop; - SendThreadArgs(const uint32_t &datarate_, uint64_t &total_sent_, - uint64_t &sent_last_sec_, uint64_t &sent_, int &sent_loop_) - : datarate(datarate_), - total_sent(total_sent_), - sent_last_sec(sent_last_sec_), - sent(sent_), - sent_loop(sent_loop_) {} -}; - -void *send(void *args) -{ - char data_buffer[BUFFER_SIZE]{}; - int read{0}; - uint32_t length{0}; - size_t header_length{0}; - uint8_t last_msg_seq{0}; - uint8_t last_remote_msg_seq{0}; - - struct SendThreadArgs *data = reinterpret_cast(args); - SendTopicsSubs *subs = new SendTopicsSubs(); - - float bandwidth_mult{0}; - float tx_interval{1.f}; - uint64_t tx_last_sec_read{0}; - hrt_abstime last_stats_update{0}; - - // ucdrBuffer to serialize using the user defined buffer - ucdrBuffer writer; - header_length = transport_node->get_header_length(); - ucdr_init_buffer(&writer, reinterpret_cast(&data_buffer[header_length]), BUFFER_SIZE - header_length); - - while (!_should_exit_task) { -@[ for idx, topic in enumerate(send_topics)]@ - { - @(send_base_types[idx])_s @(topic)_data; - - if (subs->@(topic)_sub.update(&@(topic)_data)) - { -@[ if topic == 'Timesync' or topic == 'timesync']@ - if (@(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) { - last_remote_msg_seq = @(topic)_data.seq; - - @(topic)_data.timestamp = hrt_absolute_time(); - @(topic)_data.seq = last_msg_seq; - @(topic)_data.tc1 = hrt_absolute_time() * 1000ULL; - @(topic)_data.ts1 = @(topic)_data.ts1; - - last_msg_seq++; -@[ end if]@ - // copy raw data into local buffer. Payload is shifted by header length to make room for header - serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); - - if (0 < (read = transport_node->write(static_cast(@(msgs[0].index(topic) + 1)), data_buffer, length))) { - data->total_sent += read; - tx_last_sec_read += read; - ++data->sent; - } - -@[ if topic == 'Timesync' or topic == 'timesync']@ - } - -@[ end if]@ - } - } -@[ end for]@ - - if (hrt_absolute_time() - last_stats_update >= 1_s) { - data->sent_last_sec = tx_last_sec_read; - if (data->datarate > 0) { - bandwidth_mult = static_cast(data->datarate) / static_cast(tx_last_sec_read); - // Apply a low-pass filter to determine the new TX interval - tx_interval += 0.5f * (tx_interval / bandwidth_mult - tx_interval); - // Clamp the interval between 1 and 1000 ms - tx_interval = math::constrain(tx_interval, MIN_TX_INTERVAL_US, MAX_TX_INTERVAL_US); - } - tx_last_sec_read = 0; - last_stats_update = hrt_absolute_time(); - } - - px4_usleep(tx_interval); - - ++data->sent_loop; - } - - delete(data); - delete(subs); - - return nullptr; -} - -static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args) -{ - pthread_attr_t sender_thread_attr; - pthread_attr_init(&sender_thread_attr); - pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(2250)); - struct sched_param param; - (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_DEFAULT; - (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - int rc = pthread_create(&sender_thread, &sender_thread_attr, &send, (void *)&args); - if (rc != 0) { - errno = rc; - PX4_ERR("Could not create send thread (%d)", errno); - return -1; - } - rc = pthread_setname_np(sender_thread, "urtpsclient_snd"); - if (pthread_setname_np(sender_thread, "urtpsclient_snd")) { - errno = rc; - PX4_ERR("Could not set pthread name for the send thread (%d)", errno); - } - pthread_attr_destroy(&sender_thread_attr); - - return 0; -} -@[end if]@ - -void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd, - uint64_t &total_sent, uint64_t &sent_last_sec, - uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop) -{ - px4_clock_gettime(CLOCK_REALTIME, &begin); - _should_exit_task = false; - -@[if recv_topics]@ - char data_buffer[BUFFER_SIZE]{}; - int read{0}; - uint8_t topic_ID{255}; - - uint64_t rx_last_sec_read{0}; - hrt_abstime last_stats_update{0}; - - RcvTopicsPubs *pubs = new RcvTopicsPubs(); - - // Set the main task name to 'urtpsclient_rcv' in case there is - // data to receive - px4_prctl(PR_SET_NAME, "urtpsclient_rcv", px4_getpid()); - - // ucdrBuffer to deserialize using the user defined buffer - ucdrBuffer reader; - ucdr_init_buffer(&reader, reinterpret_cast(data_buffer), BUFFER_SIZE); -@[end if]@ - -@[if send_topics]@ - // var struct to be updated on the thread - SendThreadArgs *sender_thread_args = new SendThreadArgs(datarate, total_sent, sent_last_sec, sent, sent_loop); - - // create a thread for sending data - pthread_t sender_thread; - launch_send_thread(sender_thread, (*sender_thread_args)); -@[end if]@ - - while (!_should_exit_task) { -@[if recv_topics]@ - - read = transport_node->read(); - if (read > 0) { - total_rcvd += read; - rx_last_sec_read += read; - } - - while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) { - uint64_t read_time = hrt_absolute_time(); - - switch (topic_ID) { -@[ for idx, topic in enumerate(recv_topics)]@ - case @(msgs[0].index(topic) + 1): { - @(receive_base_types[idx])_s @(topic)_data; - deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer); - - if (@(topic)_data.timestamp > read_time) { - // don't allow timestamps from the future - @(topic)_data.timestamp = read_time; - } - - pubs->@(topic)_pub.publish(@(topic)_data); - ++received; - } - break; -@[ end for]@ - default: - PX4_WARN("Unexpected topic ID '%hhu' to getMsg. Please make sure the client is capable of parsing the message associated to the topic ID '%hhu'", - topic_ID, topic_ID); - break; - } - } -@[end if]@ - - if (hrt_absolute_time() - last_stats_update >= 1_s) { - rcvd_last_sec = rx_last_sec_read; - rx_last_sec_read = 0; - last_stats_update = hrt_absolute_time(); - } - - // loop forever if informed loop number is negative - if (_options.loops >= 0 && rcvd_loop >= _options.loops) { break; } - - px4_usleep(_options.sleep_us); - ++rcvd_loop; - } - -@[if send_topics]@ - _should_exit_task = true; - pthread_join(sender_thread, nullptr); -@[end if]@ -@[if recv_topics]@ - delete(pubs); -@[end if]@ -} diff --git a/msg/templates/uorb_microcdr/msg.cpp.em b/msg/templates/uorb_microcdr/msg.cpp.em deleted file mode 100644 index 458f455f49..0000000000 --- a/msg/templates/uorb_microcdr/msg.cpp.em +++ /dev/null @@ -1,156 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - search_path (dict) search paths for genmsg -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ - -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#include -#include -#include -#include - -@################################################# -@# Searching for serialize function per each field -@################################################# -@{ - -def print_info(field): - print("type: ", field.type, "name: ", field.name, "base_type: ", \ - field.base_type, "field.is_array:", ('0', '1')[field.is_array], " array_len: ", field.array_len, \ - "is_builtin:", ('0', '1')[field.is_builtin], "is_header:", ('0', '1')[field.is_header]) - -def print_level_info(fields): - for field in fields: - print_info(field) - if (not field.is_builtin): - print("\n") - children_fields = get_children_fields(field.base_type, search_path) - print_level_info(children_fields) - print("\n") - -def walk_through_parsed_fields(): - print_level_info(spec.parsed_fields()) - -def get_serialization_type_name(type_name): - if type_name in type_serialize_map: - return type_serialize_map[type_name] - else: - raise Exception("Type {0} not supported, add to type_serialize_map!".format(type_name)) - -def add_serialize_functions(fields, scope_name): - for field in fields: - if (not field.is_header): - if (field.is_builtin): - if (not field.is_array): - print(" ucdr_serialize_" + str(get_serialization_type_name(field.type)) + "(writer, input->" + scope_name+str(field.name) + ");") - else: - print(" ucdr_serialize_array_" + str(get_serialization_type_name(field.base_type)) + "(writer, input->" + scope_name+str(field.name) + ", " + str(field.array_len) + ");") - else: - name = field.name - children_fields = get_children_fields(field.base_type, search_path) - if (scope_name): name = scope_name + name - if (not field.is_array): - add_serialize_functions(children_fields, name + '.') - else: - for i in range(field.array_len): - add_serialize_functions(children_fields, name + ('[%d].' %i)) - -def add_deserialize_functions(fields, scope_name): - for field in fields: - if (not field.is_header): - if (field.is_builtin): - if (not field.is_array): - print(" ucdr_deserialize_" + str(get_serialization_type_name(field.type)) + "(reader, &output->" + scope_name+str(field.name) + ");") - else: - print(" ucdr_deserialize_array_" + str(get_serialization_type_name(field.base_type)) + "(reader, output->" + scope_name+str(field.name) + ", " + str(field.array_len) + ");") - else: - name = field.name - children_fields = get_children_fields(field.base_type, search_path) - if (scope_name): name = scope_name + name - if (not field.is_array): - add_deserialize_functions(children_fields, name + '.') - else: - for i in range(field.array_len): - add_deserialize_functions(children_fields, name + ('[%d].' %i)) - -def add_code_to_serialize(): - add_serialize_functions(spec.parsed_fields(), "") - -def add_code_to_deserialize(): - add_deserialize_functions(spec.parsed_fields(), "") -}@ - -void serialize_@(topic_name)(ucdrBuffer *writer, const struct @(uorb_struct) *input, char *output, uint32_t *length) -{ - if (nullptr == writer || nullptr == input || nullptr == output || nullptr == length) - return; - - ucdr_reset_buffer(writer); - -@add_code_to_serialize() - (*length) = ucdr_buffer_length(writer); -} - -void deserialize_@(topic_name)(ucdrBuffer *reader, struct @(uorb_struct) *output, const char *input) -{ - if (nullptr == reader || nullptr == output || nullptr == input) - return; - - ucdr_reset_buffer(reader); - -@add_code_to_deserialize() -} diff --git a/msg/templates/uorb_microcdr/msg.h.em b/msg/templates/uorb_microcdr/msg.h.em deleted file mode 100644 index 76b793ab70..0000000000 --- a/msg/templates/uorb_microcdr/msg.h.em +++ /dev/null @@ -1,69 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ - -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include - -void serialize_@(topic_name)(ucdrBuffer *writer, const struct @(uorb_struct) *input, char *output, uint32_t *length); -void deserialize_@(topic_name)(ucdrBuffer *reader, struct @(uorb_struct) *output, const char *input); diff --git a/msg/templates/uorb_microcdr/uORBTopics.cpp.em b/msg/templates/uorb_microcdr/uORBTopics.cpp.em deleted file mode 100644 index 88dc5255d8..0000000000 --- a/msg/templates/uorb_microcdr/uORBTopics.cpp.em +++ /dev/null @@ -1,42 +0,0 @@ -@############################################### -@# -@# EmPy template for generating uORBTopics.cpp file -@# for logging purposes -@# -@############################################### -@# Start of Template -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include diff --git a/msg/templates/uorb_microcdr/uORBTopics.hpp.em b/msg/templates/uorb_microcdr/uORBTopics.hpp.em deleted file mode 100644 index 6eebd31544..0000000000 --- a/msg/templates/uorb_microcdr/uORBTopics.hpp.em +++ /dev/null @@ -1,44 +0,0 @@ -@############################################### -@# -@# EmPy template for generating uORBTopics.cpp file -@# for logging purposes -@# -@############################################### -@# Start of Template -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em deleted file mode 100644 index 86687f06a1..0000000000 --- a/msg/templates/urtps/Publisher.cpp.em +++ /dev/null @@ -1,219 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - fastrtps_version (str) FastRTPS version installed on the system -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name - -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass - -topic_name = topic - -# For ROS, use the topic pattern convention defined in -# http://wiki.ros.org/ROS/Patterns/Conventions -if ros2_distro: - topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() - topic_name = topic_name_split[0] - for w in topic_name_split[1:]: - topic_name += "_" + w - topic_name = topic_name.lower() -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Publisher.cpp - * This file contains the implementation of the publisher functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - -#include "@(topic)_Publisher.h" - -#include -#include -#include -#include -#include -#include -@[if version.parse(fastrtps_version) >= version.parse('2.0')]@ -#include - -using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; -@[end if]@ - - -@(topic)_Publisher::@(topic)_Publisher() - : mp_participant(nullptr), - mp_publisher(nullptr) -{ } - -@(topic)_Publisher::~@(topic)_Publisher() -{ - Domain::removeParticipant(mp_participant); -} - -bool @(topic)_Publisher::init(const std::string &ns, std::string topic_name) -{ - // Create RTPSParticipant - ParticipantAttributes PParam; -@[if version.parse(fastrtps_version) < version.parse('2.0')]@ - PParam.rtps.builtin.domainId = 0; -@[else]@ - PParam.domainId = 0; -@[end if]@ -@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@ - PParam.rtps.builtin.leaseDuration = c_TimeInfinite; -@[else]@ - PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; -@[end if]@ -@[if ros2_distro]@ - // ROS2 default memory management policy - PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE; -@[end if]@ - std::string nodeName = ns; - nodeName.append("@(topic)_publisher"); - PParam.rtps.setName(nodeName.c_str()); - -@[if ros2_distro]@ - // Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only - // the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and - // RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used - const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); - const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION"); - const char* ros_distro = std::getenv("ROS_DISTRO"); - if (localhost_only && strcmp(localhost_only, "1") == 0 - && ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0) - || (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0))) - || (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) { - // Create a custom network UDPv4 transport descriptor - // to whitelist the localhost - auto localhostUdpTransport = std::make_shared(); - localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1"); - - // Disable the built-in Transport Layer - PParam.rtps.useBuiltinTransports = false; - - // Add the descriptor as a custom user transport - PParam.rtps.userTransports.push_back(localhostUdpTransport); - -@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@ - // Add shared memory transport when available - auto shmTransport = std::make_shared(); - PParam.rtps.userTransports.push_back(shmTransport); -@[ end if]@ - } -@[end if]@ - - mp_participant = Domain::createParticipant(PParam); - - if (mp_participant == nullptr) { - return false; - } - - // Register the type - Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); - - // Create Publisher - PublisherAttributes Wparam; - Wparam.topic.topicKind = NO_KEY; - Wparam.topic.topicDataType = @(topic)DataType.getName(); -@[if ros2_distro]@ -@[ if ros2_distro == "ardent"]@ - Wparam.qos.m_partition.push_back("rt"); - std::string topicName = ns; -@[ else]@ - std::string topicName = "rt/"; - topicName.append(ns); -@[ end if]@ - // ROS2 default publish mode QoS policy - Wparam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE; -@[else]@ - std::string topicName = ns; -@[end if]@ - topic_name.empty() ? topicName.append("fmu/@(topic_name)/out") : topicName.append(topic_name); - Wparam.topic.topicName = topicName; - mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast(&m_listener)); - - if (mp_publisher == nullptr) { - return false; - } - - return true; -} - -void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher *pub, MatchingInfo &info) -{ - // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain - // are the same for all its subcomponents (publishers, subscribers) - bool is_different_endpoint = false; - - for (size_t i = 0; i < 6; i++) { - if (pub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { - is_different_endpoint = true; - break; - } - } - - // If the matching happens for the same entity, do not make a match - if (is_different_endpoint) { - if (info.status == MATCHED_MATCHING) { - n_matched++; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher matched\033[0m" << std::endl; - - } else { - n_matched--; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher unmatched\033[0m" << std::endl; - } - } -} - -void @(topic)_Publisher::publish(@(topic)_msg_t *st) -{ - mp_publisher->write(st); -} diff --git a/msg/templates/urtps/Publisher.h.em b/msg/templates/urtps/Publisher.h.em deleted file mode 100644 index 6851497d2a..0000000000 --- a/msg/templates/urtps/Publisher.h.em +++ /dev/null @@ -1,120 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Publisher.h - * This header file contains the declaration of the publisher functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - - -#ifndef _@(topic)__PUBLISHER_H_ -#define _@(topic)__PUBLISHER_H_ - -#include -#include - -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -#include "@(topic)_PubSubTypes.h" -@[else]@ -#include "@(topic)PubSubTypes.h" -@[end if]@ - -using namespace eprosima::fastrtps; -using namespace eprosima::fastrtps::rtps; - -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; -using @(topic)_msg_datatype = @(package)::msg::dds_::@(topic)_PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic)_; -using @(topic)_msg_datatype = @(topic)_PubSubType; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::@(topic); -using @(topic)_msg_datatype = @(package)::msg::@(topic)PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic); -using @(topic)_msg_datatype = @(topic)PubSubType; -@[ end if]@ -@[end if]@ - -class @(topic)_Publisher -{ -public: - @(topic)_Publisher(); - virtual ~@(topic)_Publisher(); - bool init(const std::string &ns, std::string topic_name = ""); - void run(); - void publish(@(topic)_msg_t *st); -private: - Participant *mp_participant; - Publisher *mp_publisher; - - class PubListener : public PublisherListener - { - public: - PubListener() : n_matched(0) {}; - ~PubListener() {}; - void onPublicationMatched(Publisher *pub, MatchingInfo &info); - int n_matched; - } m_listener; - @(topic)_msg_datatype @(topic)DataType; -}; - -#endif // _@(topic)__PUBLISHER_H_ diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em deleted file mode 100644 index be1d8d2045..0000000000 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ /dev/null @@ -1,187 +0,0 @@ -@############################################### -@# -@# EmPy template for generating RtpsTopics.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -import os -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "RtpsTopics.h" - -bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, - std::queue *t_send_queue, const std::string &ns) -{ -@[if recv_topics]@ - // Initialise subscribers - std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl; -@[for topic in recv_topics]@ - - if (_@(topic)_sub.init(@(msgs[0].index(topic) + 1), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) { - std::cout << "- @(topic) subscriber started" << std::endl; - - } else { - std::cerr << "Failed starting @(topic) subscriber" << std::endl; - return false; - } - -@[end for]@ - std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl; -@[end if]@ -@[if send_topics]@ - - // Initialise publishers - std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl; -@[for topic in send_topics]@ - -@[ if topic == 'Timesync' or topic == 'timesync']@ - if (_@(topic)_pub.init(ns)) { - if (_@(topic)_fmu_in_pub.init(ns, std::string("fmu/timesync/in"))) { - _timesync->start(&_@(topic)_fmu_in_pub); - std::cout << "- @(topic) publishers started" << std::endl; - } -@[ elif topic == 'TimesyncStatus' or topic == 'timesync_status']@ - if (_@(topic)_pub.init(ns, std::string("timesync_status"))) { - _timesync->init_status_pub(&_@(topic)_pub); - std::cout << "- @(topic) publisher started" << std::endl; -@[ else]@ - if (_@(topic)_pub.init(ns)) { - std::cout << "- @(topic) publisher started" << std::endl; -@[ end if]@ - - } else { - std::cerr << "ERROR starting @(topic) publisher" << std::endl; - return false; - } - -@[end for]@ - std::cout << "\033[0;36m-----------------------\033[0m" << std::endl; -@[end if]@ - return true; -} - -@[if send_topics]@ -template -void RtpsTopics::sync_timestamp_of_incoming_data(T &msg) { - uint64_t timestamp = getMsgTimestamp(&msg); - uint64_t timestamp_sample = getMsgTimestampSample(&msg); - _timesync->subtractOffset(timestamp); - setMsgTimestamp(&msg, timestamp); - _timesync->subtractOffset(timestamp_sample); - setMsgTimestampSample(&msg, timestamp_sample); -} - -void RtpsTopics::publish(const uint8_t topic_ID, char data_buffer[], size_t len) -{ - switch (topic_ID) { -@[for topic in send_topics]@ - - case @(msgs[0].index(topic) + 1): { // @(topic) publisher - @(topic)_msg_t st; - eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); - eprosima::fastcdr::Cdr cdr_des(cdrbuffer); - st.deserialize(cdr_des); -@[ if topic == 'Timesync' or topic == 'timesync']@ - _timesync->processTimesyncMsg(&st, &_@(topic)_pub); -@[ end if]@ - - // apply timestamp offset - sync_timestamp_of_incoming_data(st); - - _@(topic)_pub.publish(&st); - } - break; -@[end for]@ - - default: - printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to publish. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", - topic_ID, topic_ID); - break; - } -} -@[end if]@ -@[if recv_topics]@ -template -void RtpsTopics::sync_timestamp_of_outgoing_data(T &msg) { - uint64_t timestamp = getMsgTimestamp(&msg); - uint64_t timestamp_sample = getMsgTimestampSample(&msg); - _timesync->addOffset(timestamp); - setMsgTimestamp(&msg, timestamp); - _timesync->addOffset(timestamp_sample); - setMsgTimestampSample(&msg, timestamp_sample); -} - -bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) -{ - bool ret = false; - - switch (topic_ID) { -@[for topic in recv_topics]@ - - case @(msgs[0].index(topic) + 1): // @(topic) subscriber - if (_@(topic)_sub.hasMsg()) { - @(topic)_msg_t msg = _@(topic)_sub.getMsg(); - - // apply timestamp offset - sync_timestamp_of_outgoing_data(msg); - - msg.serialize(scdr); - ret = true; - - _@(topic)_sub.unlockMsg(); - } - - break; -@[end for]@ - - default: - printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to getMsg. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", - topic_ID, topic_ID); - break; - } - - return ret; -} -@[end if]@ diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em deleted file mode 100644 index b82c7b20c8..0000000000 --- a/msg/templates/urtps/RtpsTopics.h.em +++ /dev/null @@ -1,220 +0,0 @@ -@############################################### -@# -@# EmPy template for generating RtpsTopics.h file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - fastrtps_version (List[str]) FastRTPS version installed on the system -@# - package (List[str]) messages package name. Defaulted to 'px4' -@# - ros2_distro (List[str]) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -import os -from packaging import version -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -package = package[0] -fastrtps_version = fastrtps_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include - -#include "microRTPS_timesync.h" - -@[for topic in send_topics]@ -#include "@(topic)_Publisher.h" -@[end for]@ -@[for topic in recv_topics]@ -#include "@(topic)_Subscriber.h" -@[end for]@ - - -@[for topic in (recv_topics + send_topics)]@ -@[ if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; -@[ else]@ -using @(topic)_msg_t = @(topic)_; -@[ end if]@ -@[ else]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::@(topic); -@[ else]@ -using @(topic)_msg_t = @(topic); -@[ end if]@ -@[ end if]@ -@[end for]@ - -class RtpsTopics -{ -public: - bool init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, std::queue *t_send_queue, - const std::string &ns); - void set_timesync(const std::shared_ptr ×ync) { _timesync = timesync; }; -@[if send_topics]@ - template - void sync_timestamp_of_incoming_data(T &msg); - void publish(const uint8_t topic_ID, char data_buffer[], size_t len); -@[end if]@ -@[if recv_topics]@ - template - void sync_timestamp_of_outgoing_data(T &msg); - bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr); -@[end if]@ - -private: -@[if send_topics]@ - /** Publishers **/ -@[for topic in send_topics]@ -@[ if topic == 'Timesync' or topic == 'timesync']@ - @(topic)_Publisher _@(topic)_pub; - @(topic)_Publisher _@(topic)_fmu_in_pub; -@[ else]@ - @(topic)_Publisher _@(topic)_pub; -@[ end if]@ -@[end for]@ -@[end if]@ - -@[if recv_topics]@ - /** Subscribers **/ -@[for topic in recv_topics]@ - @(topic)_Subscriber _@(topic)_sub; -@[end for]@ -@[end if]@ - - // SFINAE - template struct hasTimestampSample{ - private: - template().timestamp_sample(int64_t()))> - static std::true_type detect(int); - template - static std::false_type detect(...); - public: - static constexpr bool value = decltype(detect(0))::value; - }; - - template - inline typename std::enable_if < !hasTimestampSample::value, uint64_t >::type - getMsgTimestampSample_impl(const T *) { return 0; } - - /** Msg metada Getters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp_(); } - - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample_(); } - - template - inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id_(); } - - template - inline uint8_t getMsgSeq(const T *msg) { return msg->seq_(); } -@[elif ros2_distro]@ - template - inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp(); } - - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample(); } - - template - inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id(); } - - template - inline uint8_t getMsgSeq(const T *msg) { return msg->seq(); } -@[end if]@ - - template - inline uint64_t getMsgTimestampSample(const T *msg) { return getMsgTimestampSample_impl(msg); } - - template - inline typename std::enable_if ::value, void>::type - setMsgTimestampSample_impl(T *, const uint64_t &) {} - - /** Msg metadata Setters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp_() = timestamp; } - - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T *msg, const uint64_t ×tamp_sample) { msg->timestamp_sample_() = timestamp_sample; } - - template - inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id_() = sys_id; } - - template - inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq_() = seq; } -@[elif ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp() = timestamp; } - - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T *msg, const uint64_t ×tamp_sample) { msg->timestamp_sample() = timestamp_sample; } - - template - inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id() = sys_id; } - - template - inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq() = seq; } -@[end if]@ - - template - inline void setMsgTimestampSample(T *msg, const uint64_t ×tamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); } - - /** - * @@brief Timesync object ptr. - * This object is used to compuyte and apply the time offsets to the - * messages timestamps. - */ - std::shared_ptr _timesync; -}; diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em deleted file mode 100644 index ab0c5f2d82..0000000000 --- a/msg/templates/urtps/Subscriber.cpp.em +++ /dev/null @@ -1,284 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - fastrtps_version (str) FastRTPS version installed on the system -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass - -topic_name = topic - -# For ROS, use the topic pattern convention defined in -# http://wiki.ros.org/ROS/Patterns/Conventions -if ros2_distro: - topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() - topic_name = topic_name_split[0] - for w in topic_name_split[1:]: - topic_name += "_" + w - topic_name = topic_name.lower() -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Subscriber.cpp - * This file contains the implementation of the subscriber functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - -#include "@(topic)_Subscriber.h" - -#include -#include -#include -#include -#include -#include -@[if version.parse(fastrtps_version) >= version.parse('2.0')]@ -#include - -using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; -@[end if]@ - - -@(topic)_Subscriber::@(topic)_Subscriber() - : mp_participant(nullptr), - mp_subscriber(nullptr) -{ } - -@(topic)_Subscriber::~@(topic)_Subscriber() -{ - Domain::removeParticipant(mp_participant); -} - -bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, - std::mutex *t_send_queue_mutex, std::queue *t_send_queue, const std::string &ns, - std::string topic_name) -{ - m_listener.topic_ID = topic_ID; - m_listener.t_send_queue_cv = t_send_queue_cv; - m_listener.t_send_queue_mutex = t_send_queue_mutex; - m_listener.t_send_queue = t_send_queue; - - // Create RTPSParticipant - ParticipantAttributes PParam; -@[if version.parse(fastrtps_version) < version.parse('2.0')]@ - PParam.rtps.builtin.domainId = 0; -@[else]@ - PParam.domainId = 0; -@[end if]@ -@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@ - PParam.rtps.builtin.leaseDuration = c_TimeInfinite; -@[else]@ - PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; -@[end if]@ -@[if ros2_distro]@ - // ROS2 default memory management policy - PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE; -@[end if]@ - std::string nodeName = ns; - nodeName.append("@(topic)_subscriber"); - PParam.rtps.setName(nodeName.c_str()); - -@[if ros2_distro]@ - // Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only - // the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and - // RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used - const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); - const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION"); - const char* ros_distro = std::getenv("ROS_DISTRO"); - if (localhost_only && strcmp(localhost_only, "1") == 0 - && ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0) - || (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0))) - || (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) { - // Create a custom network UDPv4 transport descriptor - // to whitelist the localhost - auto localhostUdpTransport = std::make_shared(); - localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1"); - - // Disable the built-in Transport Layer - PParam.rtps.useBuiltinTransports = false; - - // Add the descriptor as a custom user transport - PParam.rtps.userTransports.push_back(localhostUdpTransport); - -@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@ - // Add shared memory transport when available - auto shmTransport = std::make_shared(); - PParam.rtps.userTransports.push_back(shmTransport); -@[ end if]@ - } -@[end if]@ - - mp_participant = Domain::createParticipant(PParam); - - if (mp_participant == nullptr) { - return false; - } - - // Register the type - Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); - - // Create Subscriber - SubscriberAttributes Rparam; - Rparam.topic.topicKind = NO_KEY; - Rparam.topic.topicDataType = @(topic)DataType.getName(); -@[if ros2_distro]@ -@[ if ros2_distro == "ardent"]@ - Rparam.qos.m_partition.push_back("rt"); - std::string topicName = ns; -@[ else]@ - std::string topicName = "rt/"; - topicName.append(ns); -@[ end if]@ -@[else]@ - std::string topicName = ns; -@[end if]@ - topic_name.empty() ? topicName.append("fmu/@(topic_name)/in") : topicName.append(topic_name); - Rparam.topic.topicName = topicName; - mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast(&m_listener)); - - if (mp_subscriber == nullptr) { - return false; - } - - return true; -} - -void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber *sub, MatchingInfo &info) -{ -@# Since the time sync runs on the bridge itself, it is required that there is a -@# match between two topics of the same entity -@[if topic != 'Timesync' and topic != 'timesync' and topic != 'TimesyncStatus' and topic != 'timesync_status']@ - // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain - // are the same for all its subcomponents (publishers, subscribers) - bool is_different_endpoint = false; - - for (size_t i = 0; i < 6; i++) { - if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { - is_different_endpoint = true; - break; - } - } - - // If the matching happens for the same entity, do not make a match - if (is_different_endpoint) { - if (info.status == MATCHED_MATCHING) { - n_matched++; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl; - - } else { - n_matched--; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl; - } - } - -@[else]@ - (void)sub; - - if (info.status == MATCHED_MATCHING) { - n_matched++; - - } else { - n_matched--; - } -@[end if]@ -} - -void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber *sub) -{ - if (n_matched > 0) { - std::unique_lock has_msg_lock(has_msg_mutex); - - if (has_msg.load() == true) { // Check if msg has been fetched - has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched - } - - has_msg_lock.unlock(); - - // Take data - if (sub->takeNextData(&msg, &m_info)) { - if (m_info.sampleKind == ALIVE) { - std::unique_lock lk(*t_send_queue_mutex); - - ++n_msg; - has_msg = true; - - t_send_queue->push(topic_ID); - lk.unlock(); - t_send_queue_cv->notify_one(); - - } - } - } -} - -bool @(topic)_Subscriber::hasMsg() -{ - if (m_listener.n_matched > 0) { - return m_listener.has_msg.load(); - } - - return false; -} - -@(topic)_msg_t @(topic)_Subscriber::getMsg() -{ - return m_listener.msg; -} - -void @(topic)_Subscriber::unlockMsg() -{ - if (m_listener.n_matched > 0) { - std::unique_lock has_msg_lock(m_listener.has_msg_mutex); - m_listener.has_msg = false; - has_msg_lock.unlock(); - m_listener.has_msg_cv.notify_one(); - } -} diff --git a/msg/templates/urtps/Subscriber.h.em b/msg/templates/urtps/Subscriber.h.em deleted file mode 100644 index 703112bef3..0000000000 --- a/msg/templates/urtps/Subscriber.h.em +++ /dev/null @@ -1,140 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Subscriber.h - * This header file contains the declaration of the subscriber functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - - -#ifndef _@(topic)__SUBSCRIBER_H_ -#define _@(topic)__SUBSCRIBER_H_ - -#include -#include -#include -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -#include "@(topic)_PubSubTypes.h" -@[else]@ -#include "@(topic)PubSubTypes.h" -@[end if]@ - -#include -#include -#include - -using namespace eprosima::fastrtps; -using namespace eprosima::fastrtps::rtps; - -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; -using @(topic)_msg_datatype = @(package)::msg::dds_::@(topic)_PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic)_; -using @(topic)_msg_datatype = @(topic)_PubSubType; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::@(topic); -using @(topic)_msg_datatype = @(package)::msg::@(topic)PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic); -using @(topic)_msg_datatype = @(topic)PubSubType; -@[ end if]@ -@[end if]@ - -class @(topic)_Subscriber -{ -public: - @(topic)_Subscriber(); - virtual ~@(topic)_Subscriber(); - bool init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, - std::queue *t_send_queue, const std::string &ns, std::string topic_name = ""); - void run(); - bool hasMsg(); - @(topic)_msg_t getMsg(); - void unlockMsg(); - -private: - Participant *mp_participant; - Subscriber *mp_subscriber; - - class SubListener : public SubscriberListener - { - public: - SubListener() : n_matched(0), n_msg(0), has_msg(false) {}; - ~SubListener() {}; - void onSubscriptionMatched(Subscriber *sub, MatchingInfo &info); - void onNewDataMessage(Subscriber *sub); - SampleInfo_t m_info; - int n_matched; - int n_msg; - @(topic)_msg_t msg; - std::atomic_bool has_msg; - uint8_t topic_ID; - std::condition_variable *t_send_queue_cv; - std::mutex *t_send_queue_mutex; - std::queue *t_send_queue; - std::condition_variable has_msg_cv; - std::mutex has_msg_mutex; - - } m_listener; - @(topic)_msg_datatype @(topic)DataType; -}; - -#endif // _@(topic)__SUBSCRIBER_H_ diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em deleted file mode 100644 index 224bf3dcd7..0000000000 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ /dev/null @@ -1,408 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_agent.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - ros2_distro (List[str]) ROS2 distro name -@############################################### -@{ -import genmsg.msgs -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] - -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "microRTPS_transport.h" -#include "microRTPS_timesync.h" -#include "RtpsTopics.h" - -@[if ros2_distro]@ -#include -@[end if]@ - -// Default values -#define SLEEP_US 1 -#define MAX_SLEEP_US 1000000 -#define BAUDRATE 460800 -#define MAX_DATA_RATE 10000000 -#define DEVICE "/dev/ttyACM0" -#define POLL_MS 1 -#define MAX_POLL_MS 1000 -#define DEFAULT_RECV_PORT 2020 -#define DEFAULT_SEND_PORT 2019 -#define DEFAULT_IP "127.0.0.1" - -using namespace eprosima; -using namespace eprosima::fastrtps; - -volatile sig_atomic_t running = 1; -std::unique_ptr transport_node; -std::unique_ptr topics; -uint32_t total_sent = 0, sent = 0; - -struct options { - enum class eTransports { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - int sleep_us = SLEEP_US; - uint32_t baudrate = BAUDRATE; - int poll_ms = POLL_MS; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; - char ip[16] = DEFAULT_IP; - bool sw_flow_control = false; - bool hw_flow_control = false; - bool verbose_debug = false; - std::string ns = ""; -} _options; - -static void usage(const char *name) -{ - printf("usage: %s [options]\n\n" - " -b UART device baudrate. Defaults to 460800\n" - " -d UART device. Defaults to /dev/ttyACM0\n" - " -f Activates UART link SW flow control\n" - " -g Activates UART link HW flow control\n" - " -i Target remote IP address for UDP. Defaults to 127.0.0.1\n" - " -n Topics namespace. Identifies the vehicle in a multi-agent network\n" - " -o UART polling timeout in milliseconds. Defaults to 1ms\n" - " -r UDP port for receiving (local). Defaults to 2020\n" - " -s UDP port for sending (remote). Defaults to 2019\n" - " -t [UART|UDP] Defaults to UART\n" - " -v Add more verbosity\n" - " -w Iteration time for data publishing to the DDS world, in microseconds.\n" - " Defaults to 1us\n" - " (ROS2 only) Allows to pass arguments to the timesync ROS2 node.\n" - " Currently used for setting the usage of simulation time by the node using\n" - " '--ros-args -p use_sim_time:=true'\n", - name); -} - -static int parse_options(int argc, char **argv) -{ - static const struct option options[] = { - {"baudrate", required_argument, NULL, 'b'}, - {"device", required_argument, NULL, 'd'}, - {"sw-flow-control", no_argument, NULL, 'f'}, - {"hw-flow-control", no_argument, NULL, 'g'}, - {"ip-address", required_argument, NULL, 'i'}, - {"namespace", required_argument, NULL, 'n'}, - {"poll-ms", required_argument, NULL, 'o'}, - {"reception-port", required_argument, NULL, 'r'}, - {"sending-port", required_argument, NULL, 's'}, - {"transport", required_argument, NULL, 't'}, - {"increase-verbosity", no_argument, NULL, 'v'}, - {"sleep-time-us", required_argument, NULL, 'w'}, - {"ros-args", required_argument, NULL, 0}, - {"help", no_argument, NULL, 'h'}, - {NULL, 0, NULL, 0}}; - - int ch; - - while ((ch = getopt_long(argc, argv, "t:d:w:b:o:r:s:i:fghvn:", options, nullptr)) >= 0) { - switch (ch) { - case 't': _options.transport = strcmp(optarg, "UDP") == 0 ? - options::eTransports::UDP - : options::eTransports::UART; break; - - case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break; - - case 'w': _options.sleep_us = strtoul(optarg, nullptr, 10); break; - - case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; - - case 'o': _options.poll_ms = strtol(optarg, nullptr, 10); break; - - case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; - - case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; - - case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break; - - case 'f': _options.sw_flow_control = true; break; - - case 'g': _options.hw_flow_control = true; break; - - case 'h': usage(argv[0]); exit(0); break; - - case 'v': _options.verbose_debug = true; break; - - case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break; - - default: -@[if ros2_distro]@ - break; -@[else]@ - usage(argv[0]); - return -1; -@[end if]@ - } - } - - if (_options.poll_ms < POLL_MS) { - _options.poll_ms = POLL_MS; - printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low. Using %d ms instead\033[0m\n", POLL_MS); - } else if (_options.poll_ms > MAX_POLL_MS) { - _options.poll_ms = MAX_POLL_MS; - printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too high. Using %d ms instead\033[0m\n", MAX_POLL_MS); - } - - if (_options.sleep_us > MAX_SLEEP_US) { - _options.sleep_us = MAX_SLEEP_US; - printf("\033[1;33m[ micrortps_agent ]\tPublishing iteration cycle too slow. Using %d us instead\033[0m\n", MAX_SLEEP_US); - } - - if (_options.hw_flow_control && _options.sw_flow_control) { - printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m\n"); - return -1; - } - - return 0; -} - -@[if recv_topics]@ -std::atomic exit_sender_thread(false); -std::condition_variable t_send_queue_cv; -std::mutex t_send_queue_mutex; -std::queue t_send_queue; - -void t_send(void *) -{ - char data_buffer[BUFFER_SIZE] = {}; - uint32_t length = 0; - uint8_t topic_ID = 255; - - while (running && !exit_sender_thread) { - std::unique_lock lk(t_send_queue_mutex); - - while (t_send_queue.empty() && !exit_sender_thread) { - t_send_queue_cv.wait(lk); - } - - topic_ID = t_send_queue.front(); - t_send_queue.pop(); - lk.unlock(); - - size_t header_length = transport_node->get_header_length(); - /* make room for the header to fill in later */ - eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer) - header_length); - eprosima::fastcdr::Cdr scdr(cdrbuffer); - - if (!exit_sender_thread) { - if (topics->getMsg(topic_ID, scdr)) { - length = scdr.getSerializedDataLength(); - - if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) { - total_sent += length; - ++sent; - } - } - } - } -} -@[end if]@ - -void signal_handler(int signum) -{ - printf("\n\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum); - running = 0; - transport_node->close(); -} - -int main(int argc, char **argv) -{ - printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n"); - - if (-1 == parse_options(argc, argv)) { - printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - topics = std::make_unique(); - -@[if ros2_distro]@ - // Initialize communications via the rmw implementation and set up a global signal handler. - rclcpp::init(argc, argv, rclcpp::InitOptions()); -@[end if]@ - - // register signal SIGINT and signal handler - signal(SIGINT, signal_handler); - - printf("[ micrortps_agent ]\tStarting link...\n"); - - const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); - - if (localhost_only && strcmp(localhost_only, "1") == 0) { - printf("[ micrortps_agent ]\tUsing only the localhost network...\n"); - } - - /** - * Set the system ID to Mission Computer, in order to identify the agent side - * - * Note: theoretically a multi-agent system is possible, but this would require - * adjustments in the way the timesync is done (would have to create a timesync - * instance per agent). Keeping it contained for a 1:1 link for now is reasonable. - */ - const uint8_t sys_id = static_cast(MicroRtps::System::MISSION_COMPUTER); - - switch (_options.transport) { - case options::eTransports::UART: { - transport_node = std::make_unique(_options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control, _options.hw_flow_control, sys_id, _options.verbose_debug); - printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; poll: %dms; flow_control: %s\n", - _options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); - } - break; - - case options::eTransports::UDP: { - transport_node = std::make_unique(_options.ip, _options.recv_port, _options.send_port, - sys_id, _options.verbose_debug); - printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u\n", - _options.ip, _options.recv_port, _options.send_port); - } - break; - - default: - printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - if (0 > transport_node->init()) { - printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - sleep(1); - -@[if send_topics]@ - char data_buffer[BUFFER_SIZE] = {}; - int received = 0, loop = 0; - int length = 0, total_read = 0; - bool receiving = false; - uint8_t topic_ID = 255; - std::chrono::time_point start, end; -@[end if]@ - - // Init timesync - topics->set_timesync(std::make_shared(_options.verbose_debug)); - -@[if recv_topics]@ - topics->init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns); -@[end if]@ - - running = true; -@[if recv_topics]@ - std::thread sender_thread(t_send, nullptr); -@[end if]@ - - while (running) { -@[if send_topics]@ - ++loop; - if (!receiving) { start = std::chrono::steady_clock::now(); } - - // Publish messages received from UART - length = transport_node->read(); - if (length > 0) { - total_read += length; - } - - while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) { - topics->publish(topic_ID, data_buffer, sizeof(data_buffer)); - ++received; - receiving = true; - end = std::chrono::steady_clock::now(); - } -@[else]@ - usleep(_options.sleep_us); -@[end if]@ - } - -@[if recv_topics]@ - exit_sender_thread = true; - t_send_queue_cv.notify_one(); - sender_thread.join(); - - std::chrono::duration elapsed_secs = end - start; - if (received > 0) { - printf("[ micrortps_agent ]\tRECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n", - received, total_read, loop, elapsed_secs.count(), static_cast(total_read) / (1000 * elapsed_secs.count())); - } -@[end if]@ -@[if recv_topics]@ - if (sent > 0) { - printf("[ micrortps_agent ]\tSENT: %lu messages - %lu bytes\n", static_cast(sent), - static_cast(total_sent)); - } -@[end if]@ - -@[if ros2_distro]@ - rclcpp::shutdown(); -@[end if]@ - transport_node.reset(); - topics.reset(); - - return 0; -} diff --git a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em deleted file mode 100644 index f75d13f5d1..0000000000 --- a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em +++ /dev/null @@ -1,63 +0,0 @@ -################################################################################ -# -# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -################################################################################ - -cmake_minimum_required(VERSION 2.8.12) -project(micrortps_agent) - -# Find requirements -find_package(fastrtps REQUIRED) -find_package(fastcdr REQUIRED) - -# Set C++14 -include(CheckCXXCompilerFlag) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR - CMAKE_CXX_COMPILER_ID MATCHES "Clang") - check_cxx_compiler_flag(--std=c++14 SUPPORTS_CXX14) - if(SUPPORTS_CXX14) - add_compile_options(--std=c++14) - else() - message(FATAL_ERROR "Compiler doesn't support C++14") - endif() -endif() - -file(GLOB MICRORTPS_AGENT_SOURCES src/*.cpp src/*.h) -add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES}) -target_link_libraries(micrortps_agent fastrtps fastcdr) - -# Install to '/usr/local/bin' if `make install` is used -install( - TARGETS micrortps_agent - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib -) diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em deleted file mode 100644 index a441339fb1..0000000000 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ /dev/null @@ -1,295 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_timesync.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - package (List[str]) messages package name. Defaulted to 'px4' -@# - ros2_distro (List[str]) ROS2 distro name -@############################################### -@{ -import genmsg.msgs -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -package = package[0] -fastrtps_version = fastrtps_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] -}@ -/**************************************************************************** - * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file microRTPS_timesync.cpp - * @@brief source code for time sync implementation - */ - -#include -#include -#include - -#include "microRTPS_timesync.h" - -TimeSync::TimeSync(bool debug) - : _offset_ns(-1), -@[if ros2_distro]@ - _timesync_node(std::make_shared("timesync_node")), -@[end if]@ - _skew_ns_per_sync(0.0), - _num_samples(0), - _request_reset_counter(0), - _last_msg_seq(0), - _last_remote_msg_seq(0), - _debug(debug) -{ } - -TimeSync::~TimeSync() { stop(); } - -void TimeSync::start(TimesyncPublisher *pub) -{ - stop(); - -@[if ros2_distro]@ - auto spin_node = [this]() { - rclcpp::spin(_timesync_node); - }; -@[end if]@ - - auto run = [this, pub]() { - while (!_request_stop) { - timesync_msg_t msg = newTimesyncMsg(); - - pub->publish(&msg); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - }; - _request_stop = false; -@[if ros2_distro]@ - _timesync_node_thread.reset(new std::thread(spin_node)); -@[end if]@ - _send_timesync_thread.reset(new std::thread(run)); -} - -void TimeSync::init_status_pub(TimesyncStatusPublisher *status_pub) -{ - auto run = [this, status_pub]() { - while (!_request_stop) { - timesync_status_msg_t status_msg = newTimesyncStatusMsg(); - - status_pub->publish(&status_msg); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - }; - _request_stop = false; - _send_timesync_status_thread.reset(new std::thread(run)); -} - -void TimeSync::stop() -{ - _request_stop = true; - -@[if ros2_distro]@ - if (_timesync_node_thread && _timesync_node_thread->joinable()) { _timesync_node_thread->join(); } -@[end if]@ - if (_send_timesync_thread && _send_timesync_thread->joinable()) { _send_timesync_thread->join(); } - if (_send_timesync_status_thread && _send_timesync_status_thread->joinable()) { _send_timesync_status_thread->join(); } -} - -void TimeSync::reset() -{ - _num_samples = 0; - _request_reset_counter = 0; -} - -@[if ros2_distro]@ -uint64_t TimeSync::getROSTimeNSec() const -{ - return _timesync_node->now().nanoseconds(); -} - -uint64_t TimeSync::getROSTimeUSec() const -{ - return RCL_NS_TO_US(getROSTimeNSec()); -} -@[else]@ -uint64_t TimeSync::getSteadyTimeNSec() const -{ - auto time = std::chrono::steady_clock::now(); - return std::chrono::time_point_cast(time).time_since_epoch().count(); -} - -uint64_t TimeSync::getSteadyTimeUSec() const -{ - auto time = std::chrono::steady_clock::now(); - return std::chrono::time_point_cast(time).time_since_epoch().count(); -} -@[end if]@ - -bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns) -{ - _rtti = local_t3_ns - local_t1_ns; - _remote_time_stamp = remote_t2_ns; - - // assume rtti is evenly split both directions - int64_t remote_t3_ns = remote_t2_ns + _rtti.load() / 2ll; - - int64_t measurement_offset = remote_t3_ns - local_t3_ns; - - if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) { - reset(); - - if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl; } - } - - if (_num_samples == 0) { - updateOffset(measurement_offset); - _skew_ns_per_sync = 0; - } - - if (_num_samples >= WINDOW_SIZE) { - if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) { - _request_reset_counter++; - - if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl; } - - return false; - - } else { - _request_reset_counter = 0; - } - } - - // ignore if rtti > 50ms - if (_rtti.load() > 50ll * 1000ll * 1000ll) { - if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << _rtti.load() / (1000ll * 1000ll) << "ms\033[0m" << std::endl; } - - return false; - } - - double alpha = ALPHA_FINAL; - double beta = BETA_FINAL; - - if (_num_samples < WINDOW_SIZE) { - double schedule = (double)_num_samples / WINDOW_SIZE; - double s = 1. - exp(.5 * (1. - 1. / (1. - schedule))); - alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL; - beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; - } - - _offset_prev = _offset_ns.load(); - updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + - measurement_offset * alpha)); - _skew_ns_per_sync = - static_cast(beta * (_offset_ns.load() - _offset_prev.load()) + (1. - beta) * _skew_ns_per_sync); - - _num_samples++; - - return true; -} - -void TimeSync::processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub) -{ - if (getMsgSeq(msg) != _last_remote_msg_seq) { - _last_remote_msg_seq = getMsgSeq(msg); - - if (getMsgTC1(msg) > 0) { -@[if ros2_distro]@ - if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getROSTimeNSec())) { -@[else]@ - if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getSteadyTimeNSec())) { -@[end if]@ - if (_debug) { std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl; } - } - - } else if (getMsgTC1(msg) == 0) { -@[if ros2_distro]@ - setMsgTimestamp(msg, getROSTimeUSec()); -@[else]@ - setMsgTimestamp(msg, getSteadyTimeUSec()); -@[end if]@ - setMsgSeq(msg, getMsgSeq(msg) + 1); -@[if ros2_distro]@ - setMsgTC1(msg, getROSTimeNSec()); -@[else]@ - setMsgTC1(msg, getSteadyTimeNSec()); -@[end if]@ - - pub->publish(msg); - } - } -} - -timesync_msg_t TimeSync::newTimesyncMsg() -{ - timesync_msg_t msg{}; - -@[if ros2_distro]@ - setMsgTimestamp(&msg, getROSTimeUSec()); -@[else]@ - setMsgTimestamp(&msg, getSteadyTimeUSec()); -@[end if]@ - setMsgSeq(&msg, _last_msg_seq); - setMsgTC1(&msg, 0); -@[if ros2_distro]@ - setMsgTS1(&msg, getROSTimeNSec()); -@[else]@ - setMsgTS1(&msg, getSteadyTimeNSec()); -@[end if]@ - - _last_msg_seq++; - - return msg; -} - -timesync_status_msg_t TimeSync::newTimesyncStatusMsg() -{ - timesync_status_msg_t msg{}; - -@[if ros2_distro]@ - setMsgTimestamp(&msg, getROSTimeUSec()); -@[else]@ - setMsgTimestamp(&msg, getSteadyTimeUSec()); -@[end if]@ - setMsgSourceProtocol(&msg, 1); // SOURCE_PROTOCOL_RTPS - setMsgRemoteTimeStamp(&msg, _remote_time_stamp.load() / 1000ULL); - setMsgObservedOffset(&msg, _offset_prev.load()); - setMsgEstimatedOffset(&msg, _offset_ns.load()); - setMsgRoundTripTime(&msg, _rtti.load() / 1000ll); - - return msg; -} diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em deleted file mode 100644 index ffe4541f18..0000000000 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ /dev/null @@ -1,303 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_timesync.h file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - package (List[str]) messages package name. Defaulted to 'px4' -@# - ros2_distro (List[str]) ROS2 distro name -@############################################### -@{ -import genmsg.msgs -from packaging import version -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -package = package[0] -fastrtps_version = fastrtps_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] -}@ -/**************************************************************************** - * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file microRTPS_timesync.h - * @@brief Adds time sync for the microRTPS bridge - * @@author Nuno Marques - * @@author Julian Kent - */ - -#pragma once - -#include -#include -#include - -@[if ros2_distro]@ -#include "Timesync_Publisher.h" -#include "TimesyncStatus_Publisher.h" - -#include -#include -#include -@[else]@ -#include "timesync_Publisher.h" -#include "timesync_status_Publisher.h" -@[end if]@ - -static constexpr double ALPHA_INITIAL = 0.05; -static constexpr double ALPHA_FINAL = 0.003; -static constexpr double BETA_INITIAL = 0.05; -static constexpr double BETA_FINAL = 0.003; -static constexpr int WINDOW_SIZE = 500; -static constexpr int64_t UNKNOWN = 0; -static constexpr int64_t TRIGGER_RESET_THRESHOLD_NS = 100ll * 1000ll * 1000ll; -static constexpr int REQUEST_RESET_COUNTER_THRESHOLD = 5; - -@# Sets the timesync DDS type according to the FastRTPS and ROS2 version -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using timesync_msg_t = @(package)::msg::dds_::Timesync_; -using timesync_status_msg_t = @(package)::msg::dds_::TimesyncStatus_; -@[ else]@ -using timesync_msg_t = timesync_; -using timesync_status_msg_t = timesync_status_; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -using timesync_msg_t = @(package)::msg::Timesync; -using timesync_status_msg_t = @(package)::msg::TimesyncStatus; -@[ else]@ -using timesync_msg_t = timesync; -using timesync_status_msg_t = timesync_status; -@[ end if]@ -@[end if]@ -@# Sets the timesync publisher entity depending on using ROS2 or not -@[if ros2_distro]@ -using TimesyncPublisher = Timesync_Publisher; -using TimesyncStatusPublisher = TimesyncStatus_Publisher; -@[else]@ -using TimesyncPublisher = timesync_Publisher; -using TimesyncStatusPublisher = timesync_status_Publisher; -@[end if]@ - -class TimeSync -{ -public: - TimeSync(bool debug); - virtual ~TimeSync(); - - /** - * @@brief Starts the timesync publishing thread - * @@param[in] pub The timesync publisher entity to use - */ - void start(TimesyncPublisher *pub); - - /** - * @@brief Init and run the timesync status publisher thread - * @@param[in] pub The timesync status publisher entity to use - */ - void init_status_pub(TimesyncStatusPublisher *status_pub); - - /** - * @@brief Resets the filter - */ - void reset(); - - /** - * @@brief Stops the timesync publishing thread - */ - void stop(); - -@[if ros2_distro]@ - /** - * @@brief Get ROS time in nanoseconds. This will match the system time, which - * corresponds to the system-wide real time since epoch. If use_sim_time - * is set, the simulation time is grabbed by the node and used instead - * More info about ROS2 clock and time in: - * https://design.ros2.org/articles/clock_and_time.html - * @@return ROS time in nanoseconds - */ - uint64_t getROSTimeNSec() const; - - /** - * @@brief Get ROS time in microseconds. Fetches the time from getROSTimeNSec() - * and converts it to microseconds - * @@return ROS time in microseconds - */ - uint64_t getROSTimeUSec() const; -@[else]@ - /** - * @@brief Get clock monotonic time (raw) in nanoseconds - * @@return Steady CLOCK_MONOTONIC time in nanoseconds - */ - uint64_t getSteadyTimeNSec() const; - - /** - * @@brief Get clock monotonic time (raw) in microseconds - * @@return Steady CLOCK_MONOTONIC time in microseconds - */ - uint64_t getSteadyTimeUSec() const; -@[end if]@ - - /** - * @@brief Adds a time offset measurement to be filtered - * @@param[in] local_t1_ns The agent CLOCK_MONOTONIC_RAW time in nanoseconds when the message was sent - * @@param[in] remote_t2_ns The (client) remote CLOCK_MONOTONIC time in nanoseconds - * @@param[in] local_t3_ns The agent current CLOCK_MONOTONIC time in nanoseconds - * @@return true or false depending if the time offset was updated - */ - bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns); - - /** - * @@brief Processes DDS timesync message - * @@param[in,out] msg The timestamp msg to be processed - */ - void processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub); - - /** - * @@brief Creates a new timesync DDS message to be sent from the agent to the client - * @@return A new timesync message with the origin in the agent and with the agent timestamp - */ - timesync_msg_t newTimesyncMsg(); - - /** - * @@brief Creates a new timesync status DDS message to be sent from the agent to the client - * @@return A new timesync status message with the origin in the agent and with the agent timestamp - */ - timesync_status_msg_t newTimesyncStatusMsg(); - - /** - * @@brief Get the time sync offset in nanoseconds - * @@return The offset in nanoseconds - */ - inline int64_t getOffset() { return _offset_ns.load(); } - - /** - * @@brief Sums the time sync offset to the timestamp - * @@param[in,out] timestamp The timestamp to add the offset to - */ - inline void addOffset(uint64_t ×tamp) { timestamp = (timestamp * 1000LL + _offset_ns.load()) / 1000ULL; } - - /** - * @@brief Substracts the time sync offset to the timestamp - * @@param[in,out] timestamp The timestamp to subtract the offset of - */ - inline void subtractOffset(uint64_t ×tamp) { timestamp = (timestamp * 1000LL - _offset_ns.load()) / 1000ULL; } - -private: - std::atomic _offset_ns; - std::atomic _offset_prev; - std::atomic _remote_time_stamp; - std::atomic _rtti; - -@[if ros2_distro]@ - /** - * @@brief A ROS2 node to fetch the ROS time to be used for timesync - */ - std::shared_ptr _timesync_node; -@[end if]@ - - int64_t _skew_ns_per_sync; - int64_t _num_samples; - - int32_t _request_reset_counter; - uint8_t _last_msg_seq; - uint8_t _last_remote_msg_seq; - - bool _debug; - - std::unique_ptr _send_timesync_thread; - std::unique_ptr _send_timesync_status_thread; -@[if ros2_distro]@ - std::unique_ptr _timesync_node_thread; -@[end if]@ - std::atomic _request_stop{false}; - - /** - * @@brief Updates the offset of the time sync filter - * @@param[in] offset The value of the offset to update to - */ - inline void updateOffset(const uint64_t &offset) { _offset_ns.store(offset, std::memory_order_relaxed); } - - /** Timesync msg Getters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp_(); } - inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq_(); } - inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1_(); } - inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1_(); } -@[elif ros2_distro]@ - inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp(); } - inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq(); } - inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1(); } - inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1(); } - @[end if]@ - - /** Common timestamp setter **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp_() = timestamp; } -@[elif ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp() = timestamp; } -@[end if]@ - - /** Timesync msg Setters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq_() = seq; } - inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1_() = tc1; } - inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1_() = ts1; } -@[elif ros2_distro]@ - inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq() = seq; } - inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1() = tc1; } - inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1() = ts1; } -@[end if]@ - - /** Timesync Status msg Setters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol_() = source_protocol; } - inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp_() = remote_timestamp; } - inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset_() = observed_offset; } - inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset_() = estimated_offset; } - inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; } -@[elif ros2_distro]@ - inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol() = source_protocol; } - inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp() = remote_timestamp; } - inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset() = observed_offset; } - inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset() = estimated_offset; } - inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; } -@[end if]@ -}; diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp deleted file mode 100644 index 51bddadbac..0000000000 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ /dev/null @@ -1,841 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#if __has_include("px4_platform_common/log.h") && __has_include("px4_platform_common/time.h") -#include -#include -#endif - -#if defined(__linux__) || defined(__PX4_LINUX) -#include -#endif /* __linux__ */ - -#include "microRTPS_transport.h" - - -/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */ -uint16_t const crc16_table[256] = { - 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, - 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, - 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, - 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, - 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, - 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, - 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, - 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, - 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, - 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, - 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, - 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, - 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, - 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, - 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, - 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, - 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, - 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, - 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, - 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, - 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, - 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, - 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, - 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, - 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, - 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, - 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, - 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, - 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, - 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, - 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, - 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 -}; - -Transport_node::Transport_node(const uint8_t sys_id, const bool debug): - _rx_buff_pos(0), - _debug(debug), - _sys_id(sys_id) -{ -} - -Transport_node::~Transport_node() -{ -} - -uint16_t Transport_node::crc16_byte(uint16_t crc, const uint8_t data) -{ - return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff]; -} - -uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len) -{ - uint16_t crc = 0; - - while (len--) { - crc = crc16_byte(crc, *buffer++); - } - - return crc; -} - -ssize_t Transport_node::read() -{ - ssize_t len = node_read((void *)(_rx_buffer + _rx_buff_pos), sizeof(_rx_buffer) - _rx_buff_pos); - - if (len < 0) { - int errsv = errno; - - if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { -#ifndef PX4_DEBUG - - if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv); } - -#else - - if (_debug) { PX4_DEBUG("Read fail %d", errsv); } - -#endif /* PX4_DEBUG */ - } - - return len; - } - - _rx_buff_pos += len; - - return len; -} - -#ifndef PX4_DEBUG - -void Transport_node::print_buffer_debug() -{ - for (uint32_t i = 0; i < BUFFER_SIZE; ++i) { - if (i >= _rx_buff_pos) { - printf("."); - continue; - } - - printf("%" PRIi8, _rx_buffer[i]); - - if (_rx_buffer[i] == '>') { - printf("(X)"); - } - - printf(" "); - } - - printf("\n_rx_buff_pos: %" PRIu32 "\n", _rx_buff_pos); -} - -#endif /* PX4_DEBUG */ - -bool Transport_node::parse( - uint8_t *topic_id, char out_buffer[], size_t buffer_len) -{ - if (nullptr == out_buffer || nullptr == topic_id || !fds_OK()) { - return false; - } - - *topic_id = 255; - - static constexpr size_t header_size = sizeof(struct Header); - - // No need to look for a message if not even a header fits into the buffer. - if (_rx_buff_pos < header_size) { - return false; - } - - // Try to find the start of a message using the magic start sequence of '>>>'. - int32_t msg_start_pos = -1; - - for (uint32_t i = 0; i <= _rx_buff_pos; ++i) { - if ('>' == _rx_buffer[i] && memcmp(_rx_buffer + i, ">>>", 3) == 0) { - msg_start_pos = i; - break; - } - } - - auto DropDataBeforeIndex = [&](uint32_t index) { - if (index == 0) { - return; - } - - if (index > _rx_buff_pos) { - index = _rx_buff_pos; - -#ifndef PX4_DEBUG - - if (_debug) { printf("\033[0;31m[ micrortps_transport ]\t clamped index from %" PRIu32 " to %" PRIu32 "\033[0m\n", index, _rx_buff_pos); } - -#else - - if (_debug) { PX4_ERR("ERROR: clamped index from %" PRIu32 " to %" PRIu32 "\n", index, _rx_buff_pos); } - -#endif /* PX4_DEBUG */ - } - - memmove(_rx_buffer, _rx_buffer + index, _rx_buff_pos - index); - _rx_buff_pos -= index; - }; - - // No start sequence has been found in the buffer. We can drop all data up to - // 2 characters before the end (to account for a non-complete start sequence). - if (msg_start_pos == -1) { - // Note, _rx_buff_pos should always be larger than 2 as we enforce a min. - // buffer content corresponding to a header length some lines above. - int32_t drop_before_idx = _rx_buff_pos - 2; - DropDataBeforeIndex(drop_before_idx); - return false; - } - - // A start sequence has been found. We can drop everything before this index - // in the buffer. This could happen at the startup when syncing to the - // stream or with noise on the data stream. - DropDataBeforeIndex(msg_start_pos); - - // We have found a start sequence. Let's check if the header is in the buffer. - // [>,>,>,topic_id,sys_id,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, - // ... ,payloadEnd] - if (header_size > _rx_buff_pos) { - // Header not yet in the buffer. - return false; - } - - struct Header *header = (struct Header *)&_rx_buffer[0]; - - uint32_t payload_len = - ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; - - // The message won't fit the output or processing buffer. This could happen - // with a corrupted header or by an actual message that is too large for our - // buffers. Let's drop it. - uint32_t message_length = header_size + payload_len; - - if (message_length > buffer_len || (message_length + 3) > BUFFER_SIZE) { - DropDataBeforeIndex(3 + header_size); - return false; - } - - // Let's check if all payload data according to the message length in the - // header is already in the buffer. - if ((header_size + payload_len) > _rx_buff_pos) { - // The buffer does not yet contain the full message. We need to wait for - // more data. - return false; - } - - // The received message comes from this system. Discard it. - // This might happen when: - // 1. The same UDP port is being used to send a rcv packets or - // 2. The same topic on the agent is being used for outgoing and incoming - // data - if (header->sys_id == _sys_id) { - DropDataBeforeIndex(3); - return false; - } - - // Check the CRC of the message. - uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l; - uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + header_size, payload_len); - - if (read_crc != calc_crc) { -#ifndef PX4_DEBUG - - if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tBad CRC %" PRIu16 " != %" PRIu16 "\t\t(↓ %lu)\033[0m\n", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); } - -#else - - if (_debug) { PX4_DEBUG("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); } - -#endif /* PX4_DEBUG */ - - DropDataBeforeIndex(3); - return false; - } - - // Copy message to output buffer and drop it from the buffer. - memmove(out_buffer, _rx_buffer + header_size, payload_len); - *topic_id = header->topic_id; - - DropDataBeforeIndex(header_size + payload_len); - return true; -} - -size_t Transport_node::get_header_length() -{ - return sizeof(struct Header); -} - -ssize_t Transport_node::write(const uint8_t topic_id, char buffer[], size_t length) -{ - if (!fds_OK()) { - return -1; - } - - static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u, 0u}; - - // [>,>,>,topic_id,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] - uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); - - header.topic_id = topic_id; - header.sys_id = _sys_id; - header.seq = _seq_number++; - header.payload_len_h = (length >> 8) & 0xff; - header.payload_len_l = length & 0xff; - header.crc_h = (crc >> 8) & 0xff; - header.crc_l = crc & 0xff; - - /* Headroom for header is created in client */ - /* Fill in the header in the same payload buffer to call a single node_write */ - memcpy(buffer, &header, sizeof(header)); - ssize_t len = node_write(buffer, length + sizeof(header)); - - if (len != ssize_t(length + sizeof(header))) { - return len; - } - - return len + sizeof(header); -} - -UART_node::UART_node(const char *uart_name, const uint32_t baudrate, - const uint32_t poll_ms, const bool hw_flow_control, - const bool sw_flow_control, const uint8_t sys_id, - const bool debug): - Transport_node(sys_id, debug), - _uart_fd(-1), - _baudrate(baudrate), - _poll_ms(poll_ms), - _hw_flow_control(hw_flow_control), - _sw_flow_control(sw_flow_control) -{ - - if (nullptr != uart_name) { - strcpy(_uart_name, uart_name); - } -} - -UART_node::~UART_node() -{ - close(); -} - -int UART_node::init() -{ - // Open a serial port - _uart_fd = open(_uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (_uart_fd < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: Failed to open device: %s (%d)\033[0m\n", _uart_name, errno); -#else - PX4_ERR("UART transport: Failed to open device: %s (%d)", _uart_name, errno); -#endif /* PX4_ERR */ - return -errno; - } - - // If using shared UART, no need to set it up - if (_baudrate == 0) { - _poll_fd[0].fd = _uart_fd; - _poll_fd[0].events = POLLIN; - return _uart_fd; - } - - // Try to set baud rate - struct termios uart_config; - int termios_state; - - // Back up the original uart configuration to restore it after exit - if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) { - int errno_bkp = errno; -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR GET CONF %s: %d (%d)\n\033[0m", _uart_name, termios_state, - errno); -#else - PX4_ERR("UART transport: ERR GET CONF %s: %d (%d)", _uart_name, termios_state, errno); -#endif /* PX4_ERR */ - close(); - return -errno_bkp; - } - -#if defined(__linux__) || defined(__PX4_LINUX) - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); - uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); - - uart_config.c_lflag &= ~(ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | ECHONL | ICANON | IEXTEN | ISIG); - - // never send SIGTTOU - uart_config.c_lflag &= ~(TOSTOP); - - // ignore modem control lines - uart_config.c_cflag |= CLOCAL; - - // 8 bits - uart_config.c_cflag |= CS8; -#else /* __linux__ */ - - // Clear ONLCR flag (which appends a CR for every LF) - uart_config.c_oflag &= ~ONLCR; -#endif - - // Flow control - if (_hw_flow_control) { - // HW flow control - uart_config.c_cflag |= CRTSCTS; - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - } else if (_sw_flow_control) { - // SW flow control - uart_config.c_cflag &= ~CRTSCTS; - uart_config.c_lflag |= (IXON | IXOFF | IXANY); - } else { - uart_config.c_cflag &= ~CRTSCTS; - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - } - - // Set baud rate - speed_t speed; - - if (!baudrate_to_speed(_baudrate, &speed)) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: Unsupported _baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\033[0m\n", - _uart_name, _baudrate); -#else - PX4_ERR("UART transport: ERR SET BAUD %s: Unsupported baudrate: %" PRIu32 "\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n", - _uart_name, _baudrate); -#endif /* PX4_ERR */ - close(); - return -EINVAL; - } - - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - int errno_bkp = errno; -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: %d (%d)\033[0m\n", _uart_name, termios_state, - errno); -#else - PX4_ERR("ERR SET BAUD %s: %d (%d)", _uart_name, termios_state, errno); -#endif /* PX4_ERR */ - close(); - return -errno_bkp; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - int errno_bkp = errno; -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET CONF %s (%d)\033[0m\n", _uart_name, errno); -#else - PX4_ERR("UART transport: ERR SET CONF %s (%d)", _uart_name, errno); -#endif /* PX4_ERR */ - close(); - return -errno_bkp; - } - -#if defined(__linux__) || defined(__PX4_LINUX) - // For Linux, set high speed polling at the chip level. Since this routine relies on a USB latency - // change at the chip level it may fail on certain chip sets if their driver does not support this - // configuration request - { - struct serial_struct serial_ctl; - - if (ioctl(_uart_fd, TIOCGSERIAL, &serial_ctl) < 0) { - printf("\033[0;31m[ micrortps_transport ]\tError while trying to read serial port configuration: %d\033[0m\n", errno); - - if (ioctl(_uart_fd, TCFLSH, TCIOFLUSH) == -1) { - int errno_bkp = errno; - printf("\033[0;31m[ protocol__splitter ]\tCould not flush terminal\033[0m\n"); - close(); - return -errno_bkp; - } - } - - serial_ctl.flags |= ASYNC_LOW_LATENCY; - - if (ioctl(_uart_fd, TIOCSSERIAL, &serial_ctl) < 0) { - int errno_bkp = errno; - printf("\033[0;31m[ micrortps_transport ]\tError while trying to write serial port latency: %d\033[0m\n", errno); - close(); - return -errno_bkp; - } - } -#endif /* __linux__ */ - - char aux[64]; - bool flush = false; - - while (0 < ::read(_uart_fd, (void *)&aux, 64)) { - flush = true; -#ifndef px4_usleep - usleep(1000); -#else - /* With PX4 px4_usleep() should be used. */ - px4_usleep(1000); -#endif /* px4_usleep */ - } - - if (flush) { -#ifndef PX4_DEBUG - - if (_debug) { printf("[ micrortps_transport ]\tUART transport: Flush\n"); } - -#else - - if (_debug) { PX4_DEBUG("UART transport: Flush"); } - -#endif /* PX4_DEBUG */ - - } else { -#ifndef PX4_DEBUG - - if (_debug) { printf("[ micrortps_transport ]\tUART transport: No flush\n"); } - -#else - - if (_debug) { PX4_DEBUG("UART transport: No flush"); } - -#endif /* PX4_INFO */ - } - - _poll_fd[0].fd = _uart_fd; - _poll_fd[0].events = POLLIN; - - return _uart_fd; -} - -bool UART_node::fds_OK() -{ - return (-1 != _uart_fd); -} - -uint8_t UART_node::close() -{ - if (-1 != _uart_fd) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\tClosed UART.\n\033[0m"); -#else - PX4_WARN("Closed UART."); -#endif /* PX4_WARN */ - ::close(_uart_fd); - _uart_fd = -1; - memset(&_poll_fd, 0, sizeof(_poll_fd)); - } - - return 0; -} - -ssize_t UART_node::node_read(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - ssize_t ret = 0; - int r = poll(_poll_fd, 1, _poll_ms); - - if (r == 1 && (_poll_fd[0].revents & POLLIN)) { - ret = ::read(_uart_fd, buffer, len); - } - - return ret; -} - -ssize_t UART_node::node_write(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - return ::write(_uart_fd, buffer, len); -} - -bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed) -{ -#ifndef B460800 -#define B460800 460800 -#endif - -#ifndef B500000 -#define B500000 500000 -#endif - -#ifndef B921600 -#define B921600 921600 -#endif - -#ifndef B1000000 -#define B1000000 1000000 -#endif - -#ifndef B1500000 -#define B1500000 1500000 -#endif - -#ifndef B2000000 -#define B2000000 2000000 -#endif - - switch (bauds) { - case 0: *speed = B0; break; - - case 50: *speed = B50; break; - - case 75: *speed = B75; break; - - case 110: *speed = B110; break; - - case 134: *speed = B134; break; - - case 150: *speed = B150; break; - - case 200: *speed = B200; break; - - case 300: *speed = B300; break; - - case 600: *speed = B600; break; - - case 1200: *speed = B1200; break; - - case 1800: *speed = B1800; break; - - case 2400: *speed = B2400; break; - - case 4800: *speed = B4800; break; - - case 9600: *speed = B9600; break; - - case 19200: *speed = B19200; break; - - case 38400: *speed = B38400; break; - - case 57600: *speed = B57600; break; - - case 115200: *speed = B115200; break; - - case 230400: *speed = B230400; break; - - case 460800: *speed = B460800; break; - - case 500000: *speed = B500000; break; - - case 921600: *speed = B921600; break; - - case 1000000: *speed = B1000000; break; - - case 1500000: *speed = B1500000; break; - - case 2000000: *speed = B2000000; break; - -#ifdef B3000000 - case 3000000: *speed = B3000000; break; - -#endif -#ifdef B3500000 - case 3500000: *speed = B3500000; break; - -#endif -#ifdef B4000000 - case 4000000: *speed = B4000000; break; - -#endif - default: - return false; - } - - return true; -} - -UDP_node::UDP_node(const char *udp_ip, uint16_t udp_port_recv, - uint16_t udp_port_send, const uint8_t sys_id, const bool debug): - Transport_node(sys_id, debug), - _sender_fd(-1), - _receiver_fd(-1), - _udp_port_recv(udp_port_recv), - _udp_port_send(udp_port_send) -{ - if (nullptr != udp_ip) { - strcpy(_udp_ip, udp_ip); - } -} - -UDP_node::~UDP_node() -{ - close(); -} - -int UDP_node::init() -{ - if (0 > init_receiver(_udp_port_recv) || 0 > init_sender(_udp_port_send)) { - return -1; - } - - return 0; -} - -bool UDP_node::fds_OK() -{ - return (-1 != _sender_fd && -1 != _receiver_fd); -} - -int UDP_node::init_receiver(uint16_t udp_port) -{ -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - // udp socket data - memset((char *)&_receiver_inaddr, 0, sizeof(_receiver_inaddr)); - _receiver_inaddr.sin_family = AF_INET; - _receiver_inaddr.sin_port = htons(udp_port); - _receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); - - if ((_receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n"); -#else - PX4_ERR("UDP transport: Create socket failed"); -#endif /* PX4_ERR */ - return -1; - } - -#ifndef PX4_INFO - printf("[ micrortps_transport ]\tUDP transport: Trying to connect...\n"); -#else - PX4_INFO("UDP transport: Trying to connect..."); -#endif /* PX4_INFO */ - - if (bind(_receiver_fd, (struct sockaddr *)&_receiver_inaddr, sizeof(_receiver_inaddr)) < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Bind failed\033[0m\n"); -#else - PX4_ERR("UDP transport: Bind failed"); -#endif /* PX4_ERR */ - return -1; - } - -#ifndef PX4_INFO - printf("[ micrortps_transport ]\tUDP transport: Connected to server!\n\n"); -#else - PX4_INFO("UDP transport: Connected to server!"); -#endif /* PX4_INFO */ -#endif /* __PX4_NUTTX */ - return 0; -} - -int UDP_node::init_sender(uint16_t udp_port) -{ -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - - if ((_sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n"); -#else - PX4_ERR("UDP transport: Create socket failed"); -#endif /* PX4_ERR */ - return -1; - } - - memset((char *) &_sender_outaddr, 0, sizeof(_sender_outaddr)); - _sender_outaddr.sin_family = AF_INET; - _sender_outaddr.sin_port = htons(udp_port); - - if (inet_aton(_udp_ip, &_sender_outaddr.sin_addr) == 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: inet_aton() failed\033[0m\n"); -#else - PX4_ERR("UDP transport: inet_aton() failed"); -#endif /* PX4_ERR */ - return -1; - } - -#endif /* __PX4_NUTTX */ - - return 0; -} - -uint8_t UDP_node::close() -{ -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - - if (_sender_fd != -1) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed sender socket!\033[0m\n"); -#else - PX4_WARN("UDP transport: Closed sender socket!"); -#endif /* PX4_WARN */ - shutdown(_sender_fd, SHUT_RDWR); - ::close(_sender_fd); - _sender_fd = -1; - } - - if (_receiver_fd != -1) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed receiver socket!\033[0m\n"); -#else - PX4_WARN("UDP transport: Closed receiver socket!"); -#endif /* PX4_WARN */ - shutdown(_receiver_fd, SHUT_RDWR); - ::close(_receiver_fd); - _receiver_fd = -1; - } - -#endif /* __PX4_NUTTX */ - return 0; -} - -ssize_t UDP_node::node_read(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - ssize_t ret = 0; -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - // Blocking call - static socklen_t addrlen = sizeof(_receiver_outaddr); - ret = recvfrom(_receiver_fd, buffer, len, 0, (struct sockaddr *)&_receiver_outaddr, &addrlen); -#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */ - return ret; -} - -ssize_t UDP_node::node_write(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - ssize_t ret = 0; -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - ret = sendto(_sender_fd, buffer, len, 0, (struct sockaddr *)&_sender_outaddr, sizeof(_sender_outaddr)); -#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */ - return ret; -} diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h deleted file mode 100644 index a9c3567e6d..0000000000 --- a/msg/templates/urtps/microRTPS_transport.h +++ /dev/null @@ -1,164 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#define BUFFER_SIZE 1024 -#define DEFAULT_UART "/dev/ttyACM0" - -namespace MicroRtps { - enum class System { - FMU, - MISSION_COMPUTER - }; -} - -class Transport_node -{ -public: - Transport_node(const uint8_t sys_id, const bool debug); - virtual ~Transport_node(); - - virtual int init() {return 0;} - virtual uint8_t close() {return 0;} - - ssize_t read(); - bool parse(uint8_t *topic_id, char out_buffer[], size_t buffer_len); - - /** - * write a buffer - * @param topic_id - * @param buffer buffer to write: it must leave get_header_length() bytes free at the beginning. This will be - * filled with the header. length does not include get_header_length(). So buffer looks like this: - * ------------------------------------------------- - * | header (leave free) | payload data | - * | get_header_length() bytes | length bytes | - * ------------------------------------------------- - * @param length buffer length excluding header length - * @return length on success, <0 on error - */ - ssize_t write(const uint8_t topic_id, char buffer[], size_t length); - - /** Get the Length of struct Header to make headroom for the size of struct Header along with payload */ - size_t get_header_length(); - -private: -#ifndef PX4_DEBUG - void print_buffer_debug(); -#endif /* PX4_DEBUG */ - - struct __attribute__((packed)) Header { - char marker[3]; - uint8_t topic_id; - uint8_t sys_id; - uint8_t seq; - uint8_t payload_len_h; - uint8_t payload_len_l; - uint8_t crc_h; - uint8_t crc_l; - }; - -protected: - virtual ssize_t node_read(void *buffer, size_t len) = 0; - virtual ssize_t node_write(void *buffer, size_t len) = 0; - virtual bool fds_OK() = 0; - uint16_t crc16_byte(uint16_t crc, const uint8_t data); - uint16_t crc16(uint8_t const *buffer, size_t len); - - uint32_t _rx_buff_pos; - char _rx_buffer[BUFFER_SIZE]{}; - - bool _debug; - - uint8_t _sys_id; - uint8_t _seq_number{0}; -}; - -class UART_node: public Transport_node -{ -public: - UART_node(const char *uart_name, const uint32_t baudrate, - const uint32_t poll_ms, const bool hw_flow_control, - const bool sw_flow_control, const uint8_t sys_id, - const bool debug); - virtual ~UART_node(); - - int init(); - uint8_t close(); - -protected: - ssize_t node_read(void *buffer, size_t len); - ssize_t node_write(void *buffer, size_t len); - bool fds_OK(); - bool baudrate_to_speed(uint32_t bauds, speed_t *speed); - - int _uart_fd; - char _uart_name[64]{}; - uint32_t _baudrate; - uint32_t _poll_ms; - bool _hw_flow_control{false}; - bool _sw_flow_control{false}; - struct pollfd _poll_fd[1]{}; -}; - -class UDP_node: public Transport_node -{ -public: - UDP_node(const char *udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send, - const uint8_t sys_id, const bool debug); - virtual ~UDP_node(); - - int init(); - uint8_t close(); - -protected: - int init_receiver(uint16_t udp_port); - int init_sender(uint16_t udp_port); - ssize_t node_read(void *buffer, size_t len); - ssize_t node_write(void *buffer, size_t len); - bool fds_OK(); - - int _sender_fd; - int _receiver_fd; - char _udp_ip[16]{}; - uint16_t _udp_port_recv; - uint16_t _udp_port_send; - struct sockaddr_in _sender_outaddr; - struct sockaddr_in _receiver_inaddr; - struct sockaddr_in _receiver_outaddr; -}; diff --git a/msg/templates/urtps/msg.idl.em b/msg/templates/urtps/msg.idl.em deleted file mode 100644 index 1fa20e3fe6..0000000000 --- a/msg/templates/urtps/msg.idl.em +++ /dev/null @@ -1,154 +0,0 @@ -@############################################### -@# -@# ROS message to IDL converter -@# -@# EmPy template for generating .idl files -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@################################################################################ -@# -@# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -@# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -@# -@# Redistribution and use in source and binary forms, with or without -@# modification, are permitted provided that the following conditions are met: -@# -@# 1. Redistributions of source code must retain the above copyright notice, this -@# list of conditions and the following disclaimer. -@# -@# 2. Redistributions in binary form must reproduce the above copyright notice, -@# this list of conditions and the following disclaimer in the documentation -@# and/or other materials provided with the distribution. -@# -@# 3. Neither the name of the copyright holder nor the names of its contributors -@# may be used to endorse or promote products derived from this software without -@# specific prior written permission. -@# -@# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -@# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -@# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -@# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -@# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -@# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -@# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -@# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -@# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -@# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -@# POSSIBILITY OF SUCH DAMAGE. -@# -@################################################################################ -@{ -import genmsg.msgs -from packaging import version -from px_generate_uorb_topic_helper import * # this is in Tools/ - -builtin_types = set() -array_types = set() - -topic = alias if alias else spec.short_name -}@ -@################################################# -@# Searching for serialize function per each field -@################################################# -@{ - -def get_include_directives(spec): - include_directives = set() - for field in (spec.parsed_fields()): - if genmsg.msgs.is_valid_constant_type(genmsg.msgs.bare_msg_type(field.type)): - continue - builtin_type = str(field.base_type).replace('px4/', '') - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - include_directive = '#include "%s_.idl"' % builtin_type - else: - include_directive = '#include "%s.idl"' % builtin_type - builtin_types.add(builtin_type) - include_directives.add(include_directive) - return sorted(include_directives) - - -def get_idl_type_name(field_type): - if field_type in type_idl_map: - return type_idl_map[field_type] - else: - (package, name) = genmsg.names.package_resource_name(field_type) - return name - - -def add_msg_field(field): - if (not field.is_header): - if field.is_array: - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - print(' {0}__{1}_array_{2} {3}_;'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name)) - else: - print(' {0}__{1}_array_{2} {3};'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name)) - else: - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - else: - base_type = get_idl_type_name(field.base_type) if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - print(' {0} {1}_;'.format(base_type, field.name)) - -def add_msg_fields(): - for field in spec.parsed_fields(): - add_msg_field(field) - - -def add_array_typedefs(): - for field in spec.parsed_fields(): - if not field.is_header and field.is_array: - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - else: - base_type = get_idl_type_name(field.base_type) if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - array_type = 'typedef {0} {1}__{2}_array_{3}[{4}];'.format(base_type, topic, get_idl_type_name(field.base_type).replace(" ", "_"), field.array_len, field.array_len) - if array_type not in array_types: - array_types.add(array_type) - for atype in array_types: - print(atype) - - -def add_msg_constants(): - sorted_constants = sorted(spec.constants, - key=sizeof_field_type, reverse=True) - for constant in sorted_constants: - print('const {0} {1}__{2} = {3};'.format(get_idl_type_name(constant.type), topic, constant.name, constant.val)) - -} -#ifndef __@(topic)__idl__ -#define __@(topic)__idl__ - -@############################# -@# Include dependency messages -@############################# -@[for line in get_include_directives(spec)]@ -@(line)@ -@[end for]@ - - -@# Constants -@add_msg_constants() -@# Array types -@add_array_typedefs() -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -struct @(topic)_ -@[else]@ -struct @(topic) -@[end if]@ -{ -@add_msg_fields() -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -}; // struct @(topic)_ - -#pragma keylist @(topic)_ -@[else]@ -}; // struct @(topic) - -#pragma keylist @(topic) -@[end if]@ - -#endif // __@(topic)__idl__ diff --git a/msg/timesync.msg b/msg/timesync.msg deleted file mode 100644 index 67f415c744..0000000000 --- a/msg/timesync.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint8 seq # timesync msg sequence -int64 tc1 # time sync timestamp 1 -int64 ts1 # time sync timestamp 2 diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py deleted file mode 100644 index a1a0b31519..0000000000 --- a/msg/tools/generate_microRTPS_bridge.py +++ /dev/null @@ -1,465 +0,0 @@ -#!/usr/bin/env python3 -################################################################################ -# -# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -################################################################################ - -# This script can generate the client and agent code based on a set of topics -# to sent and set to receive. It uses fastrtpsgen to generate the code from the -# IDL for the topic messages. The PX4 msg definitions are used to create the IDL -# used by fastrtpsgen using templates. - -import sys -import os -import argparse -import shutil -import px_generate_uorb_topic_files -from uorb_rtps_classifier import Classifier -import subprocess -import glob -import errno -import re - -try: - from six.moves import input -except ImportError as e: - print("Failed to import six: " + e) - print("") - print("You may need to install it using:") - print(" pip3 install --user six") - print("") - sys.exit(1) - -try: - from packaging import version -except ImportError as e: - print("Failed to import packaging: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user packaging") - print("") - sys.exit(1) - - -default_client_out = "src/modules/micrortps_bridge/micrortps_client" -default_agent_out = "src/modules/micrortps_bridge/micrortps_agent" -default_uorb_templates_dir = "templates/uorb_microcdr" -default_urtps_templates_dir = "templates/urtps" -default_urtps_topics_file = "tools/urtps_bridge_topics.yaml" -default_package_name = px_generate_uorb_topic_files.PACKAGE - -parser = argparse.ArgumentParser() - -parser.add_argument("-a", "--agent", dest='agent', action="store_true", - help="Flag for generate the agent, by default is true if -c is not specified") -parser.add_argument("-c", "--client", dest='client', action="store_true", - help="Flag for generate the client, by default is true if -a is not specified") -parser.add_argument("-i", "--generate-idl", dest='gen_idl', - action="store_true", help="Flag for generate idl files for each msg") -parser.add_argument("-j", "--idl-dir", dest='idl_dir', - type=str, help="IDL files dir", default='') -parser.add_argument("-m", "--mkdir-build", dest='mkdir_build', - action="store_true", help="Flag to create 'build' dir") -parser.add_argument("-l", "--generate-cmakelists", dest='cmakelists', - action="store_true", help="Flag to generate a CMakeLists.txt file for the micro-RTPS agent") -parser.add_argument("-t", "--topic-msg-dir", dest='msgdir', type=str, - help="Topics message, by default using relative path 'msg/'", default="msg") -parser.add_argument("-b", "--uorb-templates-dir", dest='uorb_templates', type=str, - help="uORB templates, by default using relative path to msgdir 'templates/uorb_microcdr'", default=default_uorb_templates_dir) -parser.add_argument("-q", "--urtps-templates-dir", dest='urtps_templates', type=str, - help="uRTPS templates, by default using relative path to msgdir 'templates/urtps'", default=default_urtps_templates_dir) -parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str, - help="Setup uRTPS bridge topics file path, by default using relative path to msgdir 'tools/urtps_bridge_topics.yaml'", default=default_urtps_topics_file) -parser.add_argument("-p", "--package", dest='package', type=str, - help="Msg package naming, by default px4", default=default_package_name) -parser.add_argument("-o", "--agent-outdir", dest='agentdir', type=str, - help="Agent output dir, by default using relative path 'src/modules/micrortps_bridge/micrortps_agent'", default=default_agent_out) -parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str, - help="Client output dir, by default using relative path 'src/modules/micrortps_bridge/micrortps_client'", default=default_client_out) -parser.add_argument("-f", "--fastrtpsgen-dir", dest='fastrtpsgen', type=str, nargs='?', - help="fastrtpsgen installation dir, only needed if fastrtpsgen is not in PATH, by default empty", default="") -parser.add_argument("-g", "--fastrtpsgen-include", dest='fastrtpsgen_include', type=str, - help="directory(ies) to add to preprocessor include paths of fastrtpsgen, by default empty", default="") -parser.add_argument("-r", "--ros2-distro", dest='ros2_distro', type=str, nargs='?', - help="ROS2 distro, only required if generating the agent for usage with ROS2 nodes, by default empty", default="") -parser.add_argument("--delete-tree", dest='del_tree', - action="store_true", help="Delete dir tree output dir(s)") - - -if len(sys.argv) <= 1: - parser.print_usage() - exit(-1) - -# Parse arguments -args = parser.parse_args() -agent = args.agent -client = args.client -cmakelists = args.cmakelists -del_tree = args.del_tree -gen_idl = args.gen_idl -mkdir_build = args.mkdir_build -package = args.package - -# Msg files path -msg_dir = os.path.abspath(args.msgdir) -px_generate_uorb_topic_files.append_to_include_path( - {msg_dir}, px_generate_uorb_topic_files.INCL_DEFAULT, package) - -# Agent files output path -agent_out_dir = os.path.abspath(args.agentdir) - -# Client files output path -client_out_dir = os.path.abspath(args.clientdir) - -# IDL files path -idl_dir = args.idl_dir -if idl_dir != '': - idl_dir = os.path.abspath(args.idl_dir) -else: - idl_dir = os.path.join(agent_out_dir, "idl") - -if args.fastrtpsgen is None or args.fastrtpsgen == '': - # Assume fastrtpsgen is in PATH - fastrtpsgen_path = 'fastrtpsgen' - for dirname in os.environ['PATH'].split(':'): - candidate = os.path.join(dirname, 'fastrtpsgen') - if os.path.isfile(candidate): - fastrtpsgen_path = candidate -else: - # Path to fastrtpsgen is explicitly specified - if os.path.isdir(args.fastrtpsgen): - fastrtpsgen_path = os.path.join( - os.path.abspath(args.fastrtpsgen), 'fastrtpsgen') - else: - fastrtpsgen_path = args.fastrtpsgen - -fastrtpsgen_include = args.fastrtpsgen_include -if fastrtpsgen_include is not None and fastrtpsgen_include != '': - fastrtpsgen_include = "-I " + \ - os.path.abspath( - args.fastrtpsgen_include) + " " - -# get FastRTPSGen version -# .. note:: since Fast-RTPS 1.8.0 release, FastRTPSGen is a separated repository -# and not included in the Fast-RTPS project. -# The starting version since this separation is 1.0.0, which follows its own -# versioning -fastrtpsgen_version = version.Version("1.0.0") -if(os.path.exists(fastrtpsgen_path)): - try: - fastrtpsgen_version_out = subprocess.check_output( - [fastrtpsgen_path, "-version"]).decode("utf-8").strip()[-5:] - except OSError: - raise - - try: - fastrtpsgen_version = version.parse(fastrtpsgen_version_out) - except version.InvalidVersion: - raise Exception( - "'fastrtpsgen -version' returned None or an invalid version") -else: - raise Exception( - "FastRTPSGen not found. Specify the location of fastrtpsgen with the -f flag") - - -# get ROS 2 version, if exists -ros2_distro = '' -ros_version = os.environ.get('ROS_VERSION') -if ros_version == '2': - if args.ros2_distro != '': - ros2_distro = args.ros2_distro - else: - ros2_distro = os.environ.get('ROS_DISTRO') - -# get FastRTPS version -fastrtps_version = '' -if not ros2_distro: - # grab the version installed system wise - fastrtps_version = subprocess.check_output( - "ldconfig -v 2>/dev/null | grep libfastrtps", shell=True).decode("utf-8").strip().split('so.')[-1] -else: - try: - # grab the version of the ros--fastrtps package - fastrtps_version = re.search(r'Version:\s*([\dd.]+)', subprocess.check_output( - "dpkg -s ros-" + ros2_distro + "-fastrtps 2>/dev/null | grep -i version", shell=True).decode("utf-8").strip()).group(1) - except subprocess.CalledProcessError: - # if ROS2 was installed from sources the command above fails, get the system-wide version instead - try: - fastrtps_version = subprocess.check_output( - "ldconfig -v 2>/dev/null | grep libfastrtps", shell=True).decode("utf-8").strip().split('so.')[-1] - except subprocess.CalledProcessError: - # if no system-wide version is found, look for the one provided by the source-installed ROS2 - fastrtps_path = re.search(r'(?!\:)[^\:]*fastrtps\/lib(?=\:)', os.environ.get('LD_LIBRARY_PATH')).group(0) - fastrtps_version = subprocess.check_output( - "ls " + fastrtps_path, shell=True).decode("utf-8").strip().split('so.')[-1] - - -# If nothing specified it's generated both -if agent == False and client == False: - agent = True - client = True - -if del_tree: - if agent: - _continue = str(input("\nFiles in " + agent_out_dir + - " will be erased, continue?[Y/n]\n")) - if _continue == "N" or _continue == "n": - print("Aborting execution...") - exit(-1) - else: - if agent and os.path.isdir(agent_out_dir): - shutil.rmtree(agent_out_dir) - - if client: - _continue = str(input( - "\nFiles in " + client_out_dir + " will be erased, continue?[Y/n]\n")) - if _continue.strip() in ("N", "n"): - print("Aborting execution...") - exit(-1) - else: - if client and os.path.isdir(client_out_dir): - shutil.rmtree(client_out_dir) - -if agent and os.path.isdir(os.path.join(agent_out_dir, "idl")): - shutil.rmtree(os.path.join(agent_out_dir, "idl")) - -# uORB templates path -uorb_templates_dir = (args.uorb_templates if os.path.isabs(args.uorb_templates) - else os.path.join(msg_dir, args.uorb_templates)) - -# uRTPS templates path -urtps_templates_dir = (args.urtps_templates if os.path.isabs(args.urtps_templates) - else os.path.join(msg_dir, args.urtps_templates)) - -# parse yaml file into a map of ids and messages to send and receive -classifier = (Classifier(os.path.abspath(args.yaml_file), msg_dir) if os.path.isabs(args.yaml_file) - else Classifier(os.path.join(msg_dir, args.yaml_file), msg_dir)) - - -uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.em' -uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.em' -uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE = 'RtpsTopics.cpp.em' -uRTPS_AGENT_TEMPL_FILE = 'microRTPS_agent.cpp.em' -uRTPS_TIMESYNC_CPP_TEMPL_FILE = 'microRTPS_timesync.cpp.em' -uRTPS_TIMESYNC_H_TEMPL_FILE = 'microRTPS_timesync.h.em' -uRTPS_AGENT_CMAKELISTS_TEMPL_FILE = 'microRTPS_agent_CMakeLists.txt.em' -uRTPS_PUBLISHER_SRC_TEMPL_FILE = 'Publisher.cpp.em' -uRTPS_PUBLISHER_H_TEMPL_FILE = 'Publisher.h.em' -uRTPS_SUBSCRIBER_SRC_TEMPL_FILE = 'Subscriber.cpp.em' -uRTPS_SUBSCRIBER_H_TEMPL_FILE = 'Subscriber.h.em' - - -def generate_agent(out_dir): - global fastrtps_version - - if classifier.msgs_to_send: - for msg_file in classifier.msgs_to_send: - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) - - if classifier.alias_msgs_to_send: - for msg_file in classifier.alias_msgs_to_send: - msg_alias = msg_file[0] - msg_name = msg_file[1] - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) - - if classifier.msgs_to_receive: - for msg_file in classifier.msgs_to_receive: - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) - - if classifier.alias_msgs_to_receive: - for msg_file in classifier.alias_msgs_to_receive: - msg_alias = msg_file[0] - msg_name = msg_file[1] - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) - - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_CPP_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_H_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_H_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE) - if cmakelists: - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, os.path.dirname(out_dir), - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_CMAKELISTS_TEMPL_FILE) - - # Final steps to install agent - mkdir_p(os.path.join(out_dir, "fastrtpsgen")) - prev_cwd_path = os.getcwd() - os.chdir(os.path.join(out_dir, "fastrtpsgen")) - if not glob.glob(os.path.join(idl_dir, "*.idl")): - raise Exception("No IDL files found in %s" % idl_dir) - - # If it is generating the bridge code for interfacing with ROS2, then set - # the '-typeros2' option in fastrtpsgen. - # .. note:: This is only available in FastRTPSGen 1.0.4 and above - gen_ros2_typename = "" - if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'humble', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"): - gen_ros2_typename = "-typeros2 " - - idl_files = [] - for idl_file in glob.glob(os.path.join(idl_dir, "*.idl")): - # Only run the generator for the topics that are actually marked to be - # used by the bridge - if os.path.splitext(os.path.basename(idl_file))[0] in classifier.msg_list: - idl_files.append(idl_file) - - try: - ret = subprocess.check_call(fastrtpsgen_path + " -d " + out_dir + - "/fastrtpsgen -example x64Linux2.6gcc " + gen_ros2_typename + fastrtpsgen_include + " ".join(str(idl_file) for idl_file in idl_files), shell=True) - except OSError: - raise - - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*PubSubMain*")) - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/makefile*")) - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*Publisher*")) - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*Subscriber*")) - for f in glob.glob(os.path.join(out_dir, "fastrtpsgen/*.cxx")): - os.rename(f, f.replace(".cxx", ".cpp")) - cp_wildcard(os.path.join(out_dir, "fastrtpsgen/*"), out_dir) - if os.path.isdir(os.path.join(out_dir, "fastrtpsgen")): - shutil.rmtree(os.path.join(out_dir, "fastrtpsgen")) - cp_wildcard(os.path.join(urtps_templates_dir, - "microRTPS_transport.*"), agent_out_dir) - if cmakelists: - os.rename(os.path.join(os.path.dirname(out_dir), "microRTPS_agent_CMakeLists.txt"), - os.path.join(os.path.dirname(out_dir), "CMakeLists.txt")) - if (mkdir_build): - mkdir_p(os.path.join(os.path.dirname(out_dir), "build")) - os.chdir(prev_cwd_path) - return 0 - - -def rm_wildcard(pattern): - for f in glob.glob(pattern): - os.remove(f) - - -def cp_wildcard(pattern, destdir): - for f in glob.glob(pattern): - shutil.copy(f, destdir) - - -def mkdir_p(dirpath): - try: - os.makedirs(dirpath) - except OSError as e: - if e.errno == errno.EEXIST and os.path.isdir(dirpath): - pass - else: - raise - - -def generate_client(out_dir): - global fastrtps_version - - # Rename work in the default path - if default_client_out != out_dir: - def_file = os.path.join(default_client_out, "microRTPS_client.cpp") - if os.path.isfile(def_file): - os.rename(def_file, def_file.replace(".cpp", ".cpp_")) - def_file = os.path.join(default_client_out, "microRTPS_transport.cpp") - if os.path.isfile(def_file): - os.rename(def_file, def_file.replace(".cpp", ".cpp_")) - def_file = os.path.join(default_client_out, "microRTPS_transport.h") - if os.path.isfile(def_file): - os.rename(def_file, def_file.replace(".h", ".h_")) - - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, - out_dir, uorb_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_CLIENT_TEMPL_FILE) - - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, - out_dir, - uorb_templates_dir, - package, - px_generate_uorb_topic_files.INCL_DEFAULT, - classifier.msg_list, - fastrtps_version, - ros2_distro, - 'dds_topics.h.em') - - # Final steps to install client - cp_wildcard(os.path.join(urtps_templates_dir, - "microRTPS_transport.*"), out_dir) - - return 0 - - -if agent: - generate_agent(agent_out_dir) - print(("\nAgent created in: " + agent_out_dir)) - -if client: - generate_client(client_out_dir) - print(("\nClient created in: " + client_out_dir)) diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py deleted file mode 100755 index 2a038f381c..0000000000 --- a/msg/tools/px_generate_uorb_topic_files.py +++ /dev/null @@ -1,561 +0,0 @@ -#!/usr/bin/env python3 -############################################################################# -# -# Copyright (C) 2013-2018 PX4 Pro Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################# - -""" -px_generate_uorb_topic_files.py -Generates c/cpp header/source files for uorb topics from .msg (ROS syntax) -message files -""" - -import os -import shutil -import filecmp -import argparse -import sys - -try: - import em -except ImportError as e: - print("Failed to import em: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user empy") - print("") - sys.exit(1) - -try: - import genmsg.template_tools -except ImportError as e: - print("Failed to import genmsg: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user pyros-genmsg") - print("") - sys.exit(1) - -try: - from packaging import version -except ImportError as e: - print("Failed to import packaging: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user packaging") - print("") - sys.exit(1) - - -__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" -__copyright__ = "Copyright (C) 2013-2016 PX4 Development Team." -__license__ = "BSD" -__email__ = "thomasgubler@gmail.com" - - -TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] -TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] -OUTPUT_FILE_EXT = ['.h', '.cpp'] -INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] -PACKAGE = 'px4' -TOPICS_TOKEN = '# TOPICS ' -IDL_TEMPLATE_FILE = 'msg.idl.em' - -CONSTRAINED_FLASH = False - - -class MsgScope: - NONE = 0 - SEND = 1 - RECEIVE = 2 - - -def get_topics(filename, msg_name): - """ - Get TOPICS names from a "# TOPICS" line. If there are no multi topics defined, - set topic name same as the message name, since the user doesn't expect any new - custom topic names. - """ - ofile = open(filename, 'r') - text = ofile.read() - result = [] - for each_line in text.split('\n'): - if each_line.startswith(TOPICS_TOKEN): - topic_names_str = each_line.strip() - topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") - result.extend(topic_names_str.split(" ")) - ofile.close() - - if len(result) == 0: - result.append(msg_name) - - return result - - -def get_msgs_list(msgdir): - """ - Makes list of msg files in the given directory - """ - return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] - - -def generate_output_from_file(format_idx, filename, outputdir, package, templatedir, includepath): - """ - Converts a single .msg file to an uorb header/source file - """ - msg_context = genmsg.msg_loader.MsgContext.create_default() - full_type_name = genmsg.gentools.compute_full_type_name( - package, os.path.basename(filename)) - spec = genmsg.msg_loader.load_msg_from_file( - msg_context, filename, full_type_name) - field_name_and_type = {} - for field in spec.parsed_fields(): - field_name_and_type.update({field.name: field.type}) - # assert if the timestamp field exists - try: - assert 'timestamp' in field_name_and_type - except AssertionError: - print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " + - spec.short_name + " msg definition!") - exit(1) - # assert if the timestamp field is of type uint64 - try: - assert field_name_and_type.get('timestamp') == 'uint64' - except AssertionError: - print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name + - " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!") - exit(1) - - # Get topics used for the message - topics = get_topics(filename, spec.short_name) - - if includepath: - search_path = genmsg.command_line.includepath_to_dict(includepath) - else: - search_path = {} - genmsg.msg_loader.load_depends(msg_context, spec, search_path) - md5sum = genmsg.gentools.compute_md5(msg_context, spec) - em_globals = { - "file_name_in": filename, - "md5sum": md5sum, - "search_path": search_path, - "msg_context": msg_context, - "spec": spec, - "topics": topics, - "constrained_flash": CONSTRAINED_FLASH - } - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) - output_file = os.path.join(outputdir, spec.short_name + - OUTPUT_FILE_EXT[format_idx]) - - return generate_by_template(output_file, template_file, em_globals) - - -def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, fastrtps_version, ros2_distro, msgs): - """ - Generates an .idl from .msg file - """ - msg = os.path.join(msg_dir, filename_msg + ".msg") - - if (alias != ""): - em_globals = get_em_globals( - msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = alias - else: - em_globals = get_em_globals( - msg, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = em_globals["spec"].short_name - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, IDL_TEMPLATE_FILE) - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace( - "msg.idl.em", str(spec_short_name + "_.idl"))) - else: - output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace( - "msg.idl.em", str(spec_short_name + ".idl"))) - - return generate_by_template(output_file, template_file, em_globals) - - -def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filename_receive_msgs, filename_alias_receive_msgs, - msg_dir, outputdir, templatedir, package, includepath, msgs, fastrtps_version, ros2_distro, template_name): - """ - Generates source file by msg content - """ - send_msgs = list(os.path.join(msg_dir, msg + ".msg") - for msg in filename_send_msgs) - receive_msgs = list(os.path.join(msg_dir, msg + ".msg") - for msg in filename_receive_msgs) - - alias_send_msgs = list([os.path.join( - msg_dir, msg[1] + ".msg"), msg[0]] for msg in filename_alias_send_msgs) - - alias_receive_msgs = list([os.path.join( - msg_dir, msg[1] + ".msg"), msg[0]] for msg in filename_alias_receive_msgs) - - em_globals_list = [] - if send_msgs: - em_globals_list.extend([get_em_globals( - f, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.SEND) for f in send_msgs]) - - if alias_send_msgs: - em_globals_list.extend([get_em_globals( - f[0], f[1], package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.SEND) for f in alias_send_msgs]) - - if receive_msgs: - em_globals_list.extend([get_em_globals( - f, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in receive_msgs]) - - if alias_receive_msgs: - em_globals_list.extend([get_em_globals( - f[0], f[1], package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in alias_receive_msgs]) - - merged_em_globals = merge_em_globals_list(em_globals_list) - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, template_name) - output_file = os.path.join( - outputdir, template_name.replace(".em", "")) - - return generate_by_template(output_file, template_file, merged_em_globals) - - -def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, msgs, fastrtps_version, ros2_distro, template_name): - """ - Generates a sources and headers from .msg file - """ - msg = os.path.join(msg_dir, filename_msg + ".msg") - - if (alias): - em_globals = get_em_globals( - msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = alias - else: - em_globals = get_em_globals( - msg, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = em_globals["spec"].short_name - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, template_name) - output_file = os.path.join( - outputdir, spec_short_name + "_" + template_name.replace(".em", "")) - - return generate_by_template(output_file, template_file, em_globals) - - -def get_em_globals(filename_msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, scope): - """ - Generates em globals dictionary - """ - msg_context = genmsg.msg_loader.MsgContext.create_default() - full_type_name = genmsg.gentools.compute_full_type_name( - package, os.path.basename(filename_msg)) - spec = genmsg.msg_loader.load_msg_from_file( - msg_context, filename_msg, full_type_name) - - # Get topics used for the message - topics = get_topics(filename_msg, spec.short_name) - - if includepath: - search_path = genmsg.command_line.includepath_to_dict(includepath) - else: - search_path = {} - genmsg.msg_loader.load_depends(msg_context, spec, search_path) - md5sum = genmsg.gentools.compute_md5(msg_context, spec) - em_globals = { - "file_name_in": filename_msg, - "md5sum": md5sum, - "search_path": search_path, - "msg_context": msg_context, - "spec": spec, - "topics": topics, - "msgs": msgs, - "scope": scope, - "package": package, - "alias": alias, - "fastrtps_version": fastrtps_version, - "ros2_distro": ros2_distro - } - - return em_globals - - -def merge_em_globals_list(em_globals_list): - """ - Merges a list of em_globals to a single dictionary where each attribute is a list - """ - if len(em_globals_list) < 1: - return {} - - merged_em_globals = {} - for name in em_globals_list[0]: - merged_em_globals[name] = [em_globals[name] - for em_globals in em_globals_list] - - return merged_em_globals - - -def generate_by_template(output_file, template_file, em_globals): - """ - Invokes empy intepreter to geneate output_file by the - given template_file and predefined em_globals dict - """ - # check if folder exists: - folder_name = os.path.dirname(output_file) - if not os.path.exists(folder_name): - os.makedirs(folder_name) - - ofile = open(output_file, 'w') - # todo, reuse interpreter - interpreter = em.Interpreter(output=ofile, globals=em_globals, options={ - em.RAW_OPT: True, em.BUFFERED_OPT: True}) - try: - interpreter.file(open(template_file)) - except OSError as e: - ofile.close() - os.remove(output_file) - raise - interpreter.shutdown() - ofile.close() - return True - - -def convert_dir(format_idx, inputdir, outputdir, package, templatedir): - """ - Converts all .msg files in inputdir to uORB header/source files - """ - - # Find the most recent modification time in input dir - maxinputtime = 0 - for f in os.listdir(inputdir): - fni = os.path.join(inputdir, f) - if os.path.isfile(fni): - it = os.path.getmtime(fni) - if it > maxinputtime: - maxinputtime = it - - # Find the most recent modification time in output dir - maxouttime = 0 - if os.path.isdir(outputdir): - for f in os.listdir(outputdir): - fni = os.path.join(outputdir, f) - if os.path.isfile(fni): - it = os.path.getmtime(fni) - if it > maxouttime: - maxouttime = it - - # Do not generate if nothing changed on the input - if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): - return False - - includepath = INCL_DEFAULT + [':'.join([package, inputdir])] - for f in os.listdir(inputdir): - # Ignore hidden files - if f.startswith("."): - continue - - fn = os.path.join(inputdir, f) - # Only look at actual files - if not os.path.isfile(fn): - continue - - if fn[-4:].lower() != '.msg': - continue - - generate_output_from_file( - format_idx, fn, outputdir, package, templatedir, includepath) - return True - - -def copy_changed(inputdir, outputdir, prefix='', quiet=False): - """ - Copies files from inputdir to outputdir if they don't exist in - outputdir or if their content changed - """ - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - for input_file in os.listdir(inputdir): - fni = os.path.join(inputdir, input_file) - if os.path.isfile(fni): - # Check if input_file exists in outpoutdir, copy the file if not - fno = os.path.join(outputdir, prefix + input_file) - if not os.path.isfile(fno): - shutil.copy(fni, fno) - if not quiet: - print("{0}: new header file".format(fno)) - continue - - if os.path.getmtime(fni) > os.path.getmtime(fno): - # The file exists in inputdir and outputdir - # only copy if contents do not match - if not filecmp.cmp(fni, fno): - shutil.copy(fni, fno) - if not quiet: - print("{0}: updated".format(input_file)) - continue - - if not quiet: - print("{0}: unchanged".format(input_file)) - - -def convert_dir_save(format_idx, inputdir, outputdir, package, templatedir, temporarydir, prefix, quiet=False): - """ - Converts all .msg files in inputdir to uORB header files - Unchanged existing files are not overwritten. - """ - # Create new headers in temporary output directory - convert_dir(format_idx, inputdir, temporarydir, package, templatedir) - if generate_idx == 1: - generate_topics_list_file(inputdir, temporarydir, TOPICS_LIST_TEMPLATE_FILE[1], templatedir) - # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir, prefix, quiet) - - -def generate_topics_list_file(msgdir, outputdir, template_filename, templatedir): - # generate cpp file with topics list - msgs = get_msgs_list(msgdir) - topics = [] - for msg in msgs: - msg_filename = os.path.join(msgdir, msg) - topics.extend(get_topics(msg_filename, msg)) - tl_globals = {"msgs": msgs, "topics": topics} - tl_template_file = os.path.join(templatedir, template_filename) - tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) - generate_by_template(tl_out_file, tl_template_file, tl_globals) - - -def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir): - # Get message file names ending with .msg only - msg_filenames = [p for p in files if os.path.basename(p).endswith(".msg")] - - # Get topics used in messages - topics = [] - for msg_filename in msg_filenames: - msg_name = os.path.basename(msg_filename).replace('.msg', '') - topics.extend(get_topics(msg_filename, msg_name)) - - # Get only the message file name for "msgs" component - msg_basenames = [os.path.basename(p) for p in msg_filenames] - - # Set the Template dictionary settings - tl_globals = {"msgs": msg_basenames, "topics": topics} - tl_template_file = os.path.join(templatedir, template_filename) - tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) - generate_by_template(tl_out_file, tl_template_file, tl_globals) - - -def append_to_include_path(path_to_append, curr_include, package): - for p in path_to_append: - curr_include.append('%s:%s' % (package, p)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description='Convert msg files to uorb headers/sources') - parser.add_argument('--headers', help='Generate header files', - action='store_true') - parser.add_argument('--sources', help='Generate source files', - action='store_true') - parser.add_argument('-d', dest='dir', help='directory with msg files') - parser.add_argument('-f', dest='file', - help="files to convert (use only without -d)", - nargs="+") - parser.add_argument('-i', dest="include_paths", - help='Additional Include Paths', nargs="*", - default=None) - parser.add_argument('-e', dest='templatedir', - help='directory with template files',) - parser.add_argument('-k', dest='package', default=PACKAGE, - help='package name') - parser.add_argument('-o', dest='outputdir', - help='output directory for header files') - parser.add_argument('-t', dest='temporarydir', - help='temporary directory') - parser.add_argument('-p', dest='prefix', default='', - help='string added as prefix to the output file ' - ' name when converting directories') - parser.add_argument('-q', dest='quiet', default=False, action='store_true', - help='string added as prefix to the output file ' - ' name when converting directories') - parser.add_argument('--constrained-flash', dest='constrained_flash', default=False, action='store_true', - help='set to save flash space') - args = parser.parse_args() - - if args.include_paths: - append_to_include_path(args.include_paths, INCL_DEFAULT, args.package) - - CONSTRAINED_FLASH = args.constrained_flash - - if args.headers: - generate_idx = 0 - elif args.sources: - generate_idx = 1 - else: - print('Error: either --headers or --sources must be specified') - exit(-1) - if args.file is not None: - for f in args.file: - generate_output_from_file( - generate_idx, f, args.temporarydir, args.package, args.templatedir, INCL_DEFAULT) - - # Generate topics list header and source file - if os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])): - generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir) - - copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet) - elif args.dir is not None: - convert_dir_save( - generate_idx, - args.dir, - args.outputdir, - args.package, - args.templatedir, - args.temporarydir, - args.prefix, - args.quiet) diff --git a/msg/tools/uorb_rtps_classifier.py b/msg/tools/uorb_rtps_classifier.py deleted file mode 100644 index 13a79ffa32..0000000000 --- a/msg/tools/uorb_rtps_classifier.py +++ /dev/null @@ -1,186 +0,0 @@ -#!/usr/bin/env python3 -################################################################################ -# -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -################################################################################ - -import argparse -import difflib -import errno -import os -from typing import Dict, List, Tuple -import yaml - - -class Classifier(): - """Class to classify RTPS msgs as to send, receive and set their IDs.""" - - def __init__(self, yaml_file, msg_folder) -> None: - self.msg_folder = msg_folder - self.msg_map = self.parse_yaml_msgs_file(yaml_file) - - # Check if base types are defined correctly - self.check_base_type() - - # Get messages to send and to receive - self.msgs_to_send: Dict[str, int] = dict() - self.msgs_to_receive: Dict[str, int] = dict() - self.alias_msgs_to_send: List[Tuple[str, str]] = [] - self.alias_msgs_to_receive: List[Tuple[str, str]] = [] - self.msg_list: List[str] = [] - - # Create message map - self.setup_msg_map() - - self.msg_files_send = self.set_msg_files_send() - self.msg_files_receive = self.set_msg_files_receive() - - def setup_msg_map(self) -> None: - """Setup dictionary with an ID map for the messages.""" - for topic in self.msg_map['rtps']: - if 'send' in list(topic.keys()): - if 'base' in list(topic.keys()): - self.alias_msgs_to_send.append( - (topic['msg'], topic['base'])) - else: - self.msgs_to_send.update({topic['msg']: 0}) - if 'receive' in list(topic.keys()): - if 'base' in list(topic.keys()): - self.alias_msgs_to_receive.append( - (topic['msg'], topic['base'])) - else: - self.msgs_to_receive.update({topic['msg']: 0}) - self.msg_list.append(topic['msg']) - - def set_msg_files_send(self) -> list: - """ - Append the path to the files which messages are marked to - be sent. - """ - return [os.path.join(self.msg_folder, msg + ".msg") - for msg in list(self.msgs_to_send.keys())] - - def set_msg_files_receive(self) -> list: - """ - Append the path to the files which messages are marked to - be received. - """ - return [os.path.join(self.msg_folder, msg + ".msg") - for msg in list(self.msgs_to_receive.keys())] - - def check_base_type(self) -> None: - """Check if alias message has correct base type.""" - registered_alias_msgs = list( - topic['base'] for topic in self.msg_map['rtps'] if 'base' in list(topic.keys())) - - base_types = [] - for topic in self.msg_map['rtps']: - if 'base' not in list(topic.keys()): - base_types.append(topic['msg']) - - incorrect_base_types = list( - set(registered_alias_msgs) - set(base_types)) - - base_types_suggestion = {} - for incorrect in incorrect_base_types: - base_types_suggestion.update({incorrect: difflib.get_close_matches( - incorrect, base_types, n=1, cutoff=0.6)}) - - if len(base_types_suggestion) > 0: - raise AssertionError( - ('\n' + '\n'.join('\t- The multi-topic message base type \'{}\' does not exist.{}'.format(k, (' Did you mean \'' + v[0] + '\'?' if v else '')) for k, v in list(base_types_suggestion.items())))) - - @staticmethod - def parse_yaml_msgs_file(yaml_file) -> dict: - """Parses a yaml file into a dict.""" - try: - with open(yaml_file, 'r') as file: - return yaml.safe_load(file) - except OSError as err: - if err.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror( - errno.ENOENT), yaml_file) - raise - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - - parser.add_argument("-s", "--send", dest='send', - action="store_true", help="Get topics to be sent") - parser.add_argument("-a", "--alias", dest='alias', - action="store_true", help="Get alias topics") - parser.add_argument("-r", "--receive", dest='receive', - action="store_true", help="Get topics to be received") - parser.add_argument("-i", "--ignore", dest='ignore', - action="store_true", help="Get topics to be ignored") - parser.add_argument("-p", "--path", dest='path', - action="store_true", help="Get msgs and its paths") - parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, - help="Topics message dir, by default msg/", default="msg") - parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str, - help="RTPS msg IDs definition file absolute path, by default use relative path to msg, tools/urtps_bridge_topics.yaml", - default='tools/urtps_bridge_topics.yaml') - - # Parse arguments - args = parser.parse_args() - - msg_dir = args.msgdir - if args.msgdir == 'msg': - msg_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) - else: - msg_dir = os.path.abspath(args.msgdir) - classifier = (Classifier(os.path.abspath(args.yaml_file), msg_dir) if os.path.isabs(args.yaml_file) - else Classifier(os.path.join(msg_dir, args.yaml_file), msg_dir)) - - if args.send: - if args.path: - print(('send files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msg_files_send) + '\n')) - else: - if args.alias: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_send)) + (' alias ' + ', '.join(msg[0] - for msg in classifier.alias_msgs_to_send) if len(classifier.alias_msgs_to_send) > 0 else '') + '\n')) - else: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_send)))) - if args.receive: - if args.path: - print(('receive files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msg_files_receive) + '\n')) - else: - if args.alias: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_receive)) + (' alias ' + ', '.join(msg[0] - for msg in classifier.alias_msgs_to_receive) if len(classifier.alias_msgs_to_receive) > 0 else '') + '\n')) - else: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_receive)))) diff --git a/msg/tools/uorb_to_ros_msgs.py b/msg/tools/uorb_to_ros_msgs.py deleted file mode 100755 index a0d2aa9fb3..0000000000 --- a/msg/tools/uorb_to_ros_msgs.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python3 -""" -Script to parse uORB message format to ROS msg format -Adapted from https://github.com/eProsima/px4_to_ros/blob/master/px4_to_ros2_PoC/px4_msgs/scripts/copy_and_rename.py -""" - -import os -import re -import sys -from shutil import copyfile - -__author__ = 'PX4 Development Team' -__copyright__ = \ - ''' - ' - ' Copyright (c) 2018 PX4 Development Team. All rights reserved. - ' - ' Redistribution and use in source and binary forms, or without - ' modification, permitted provided that the following conditions - ' are met: - ' - ' 1. Redistributions of source code must retain the above copyright - ' notice, list of conditions and the following disclaimer. - ' 2. Redistributions in binary form must reproduce the above copyright - ' notice, list of conditions and the following disclaimer in - ' the documentation and/or other materials provided with the - ' distribution. - ' 3. Neither the name PX4 nor the names of its contributors may be - ' used to endorse or promote products derived from self software - ' without specific prior written permission. - ' - ' THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - ' "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, NOT - ' LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - ' FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - ' COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - ' INCIDENTAL, SPECIAL, EXEMPLARY, CONSEQUENTIAL DAMAGES (INCLUDING, - ' BUT NOT LIMITED TO, OF SUBSTITUTE GOODS OR SERVICES; LOSS - ' OF USE, DATA, PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - ' AND ON ANY THEORY OF LIABILITY, IN CONTRACT, STRICT - ' LIABILITY, TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ' ANY WAY OUT OF THE USE OF THIS SOFTWARE, IF ADVISED OF THE - ' POSSIBILITY OF SUCH DAMAGE. - ' - ''' -__credits__ = ['Nuno Marques '] -__license__ = 'BSD-3-Clause' -__version__ = '0.1.0' -__maintainer__ = 'Nuno Marques' -__email__ = 'nuno.marques@dronesolution.io' -__status__ = 'Development' - -input_dir = sys.argv[1] -output_dir = sys.argv[2] - - -def main(): - print("----------------------- \033[1mmicroRTPS bridge uORB to ROS messages\033[0m -----------------------") - print("-------------------------------------------------------------------------------------------------------") - - if not os.path.exists(os.path.abspath(output_dir)): - os.mkdir(os.path.abspath(output_dir)) - else: - ros_msg_dir = os.path.abspath(output_dir) - msg_files = os.listdir(ros_msg_dir) - for msg in msg_files: - if msg.endswith(".msg"): - os.remove(os.path.join(ros_msg_dir, msg)) - - msg_list = list() - - for filename in os.listdir(input_dir): - if '.msg' in filename: - msg_list.append(filename.rstrip('.msg')) - input_file = input_dir + filename - - output_file = output_dir + \ - filename.partition(".")[0].title().replace('_', '') + ".msg" - copyfile(input_file, output_file) - - for filename in os.listdir(output_dir): - if '.msg' in filename: - input_file = output_dir + filename - - fileUpdated = False - - with open(input_file, 'r') as f: - lines = f.readlines() - newlines = [] - alias_msgs = [] - alias_msg_files = [] - - for line in lines: - for msg_type in msg_list: - if ('px4/' + msg_type + ' ') in line: - fileUpdated = True - line = line.replace(('px4/' + msg_type), - msg_type.partition(".")[0].title().replace('_', '')) - - if re.findall('^' + msg_type + '[\s\[]', line.partition('#')[0]): - fileUpdated = True - line = line.replace(msg_type, - msg_type.partition(".")[0].title().replace('_', '')) - if '# TOPICS' in line: - fileUpdated = True - alias_msgs += line.split() - alias_msgs.remove('#') - alias_msgs.remove('TOPICS') - line = line.replace(line, '') - newlines.append(line) - - for msg_file in alias_msgs: - with open(output_dir + msg_file.partition(".")[0].title().replace('_', '') + ".msg", 'w+') as f: - for line in newlines: - f.write(line) - - if fileUpdated: - with open(input_file, 'w+') as f: - for line in newlines: - f.write(line) - - print("--\t\t- Generated {} ROS message files in '{}'".format(len(os.listdir(output_dir)), os.path.abspath(output_dir))) - print("-------------------------------------------------------------------------------------------------------") - - -if __name__ == '__main__': - main() diff --git a/msg/tools/uorb_to_ros_urtps_topics.py b/msg/tools/uorb_to_ros_urtps_topics.py deleted file mode 100755 index 8ff5f0e59d..0000000000 --- a/msg/tools/uorb_to_ros_urtps_topics.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python3 -""" -Script to read an yaml file containing the microRTPS topics and update the naming convention to PascalCase -""" - -import argparse -import errno -from pathlib import Path -import os -import six -import yaml - -__author__ = 'PX4 Development Team' -__copyright__ = \ - ''' - ' - ' Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - ' - ' Redistribution and use in source and binary forms, or without - ' modification, permitted provided that the following conditions - ' are met: - ' - ' 1. Redistributions of source code must retain the above copyright - ' notice, list of conditions and the following disclaimer. - ' 2. Redistributions in binary form must reproduce the above copyright - ' notice, list of conditions and the following disclaimer in - ' the documentation and/or other materials provided with the - ' distribution. - ' 3. Neither the name PX4 nor the names of its contributors may be - ' used to endorse or promote products derived from self software - ' without specific prior written permission. - ' - ' THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - ' "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, NOT - ' LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - ' FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - ' COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - ' INCIDENTAL, SPECIAL, EXEMPLARY, CONSEQUENTIAL DAMAGES (INCLUDING, - ' BUT NOT LIMITED TO, OF SUBSTITUTE GOODS OR SERVICES; LOSS - ' OF USE, DATA, PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - ' AND ON ANY THEORY OF LIABILITY, IN CONTRACT, STRICT - ' LIABILITY, TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ' ANY WAY OUT OF THE USE OF THIS SOFTWARE, IF ADVISED OF THE - ' POSSIBILITY OF SUCH DAMAGE. - ' - ''' -__credits__ = ['Nuno Marques '] -__license__ = 'BSD-3-Clause' -__version__ = '0.1.0' -__maintainer__ = 'Nuno Marques' -__email__ = 'nuno.marques@dronesolution.io' -__status__ = 'Development' - -verbose = False - - -class IndenterDumper(yaml.Dumper): - """ Custom dumper for yaml files that apply the correct indentation """ - - def increase_indent(self, flow=False, indentless=False): - return super(IndenterDumper, self).increase_indent(flow, False) - - -def load_yaml_file(file): - """ - Open yaml file and parse the data into a list of dict - - :param file: the yaml file to load - :returns: the list of dictionaries that represent the topics to send and receive - :raises IOError: raises and error when the file is not found - """ - try: - with open(file, 'r') as f: - if verbose: - print(("--\t\t- '%s' file loaded" % file)) - return yaml.safe_load(f) - except OSError as e: - if e.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), file) - else: - raise - - -def update_dict(list): - """ - Update the message naming on the dictionary to fit the PascalCase convention - - :param file: the list of dicts to be updated - """ - if verbose: - num_of_msgs = 0 - for i, dictionary in enumerate(list["rtps"]): - list["rtps"][i] = {k: v.title().replace('_', '') if isinstance( - v, six.string_types) else v for k, v in six.iteritems(dictionary)} - if verbose: - num_of_msgs += 1 - if verbose: - print(("--\t\t- %d ROS message file names updated" % num_of_msgs)) - - -def update_yaml_file(list, in_file, out_file): - """ - Open the yaml file to dump the new list of dict toself. - - :param list: the list of updated dicts - :param file: the yaml file to load and write the new data - :raises IOError: raises and error when the file is not found - """ - try: - with open(out_file, 'w') as f: - f.write("# AUTOGENERATED-FILE! DO NOT MODIFY IT DIRECTLY.\n#" - " Edit instead the same file under PX4-Autopilot/msg/tools and" - " use the \n# PX4-Autopilot/msg/tools/uorb_to_ros_urtps_topics.py" - " to regenerate this file.\n") - yaml.dump(list, f, Dumper=IndenterDumper, default_flow_style=False) - if verbose: - if in_file == out_file: - print(("--\t\t- '%s' updated" % in_file)) - else: - print(("--\t\t- '%s' created" % out_file)) - - except OSError as err: - if err.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), out_file) - raise - - -def main(): - parser = argparse.ArgumentParser( - description='Read an yaml file containing the microRTPS topics and update the naming convention to PascalCase') - optional = parser._action_groups.pop() - required = parser.add_argument_group('Required') - required.add_argument("-i", "--input-file", dest="input_file", - help="Yaml file to read", metavar="INFILE") - optional.add_argument("-o", "--output-file", dest="output_file", - help="Yaml file to dump. If not set, it is the same as the input", - metavar="OUTFILE", default="") - optional.add_argument("-q", "--quiet", action="store_false", dest="verbose", - default=True, help="Don't print status messages to stdout") - - args = parser.parse_args() - global verbose - verbose = args.verbose - in_file = Path(args.input_file) - out_file = Path(args.output_file) if ( - Path(args.output_file) != in_file and Path(args.output_file) != "") else in_file - - if verbose: - print("---------------------- \033[1mmicroRTPS bridge yaml PX4 to ROS topics\033[0m ----------------------") - print("-------------------------------------------------------------------------------------------------------") - - list = load_yaml_file(in_file) - update_dict(list) - update_yaml_file(list, in_file, out_file) - if verbose: - print("-------------------------------------------------------------------------------------------------------") - - -if __name__ == "__main__": - main() diff --git a/msg/tools/urtps_bridge_topics.yaml b/msg/tools/urtps_bridge_topics.yaml deleted file mode 100644 index 747a86c8e3..0000000000 --- a/msg/tools/urtps_bridge_topics.yaml +++ /dev/null @@ -1,94 +0,0 @@ -##### -# -# This file maps all the topics that are to be used on the microRTPS bridge. -# When one wants to add a new topic to the bridge, it should add it to this file -# and mark it to be sent or received from the link. -# For alias/multi-topic messages (i.e. the ones found on the '#TOPICS' of the -# uORB messages), these can be also added, requiring an extra entry ('base') to -# define the base message. -# -# IMPORTANT NOTICE: The IDs of the messages sent on the bridge get generated -# according to the order of the messages in this file. To keep consistency and -# backwards compatibility, it is recommended that any new message that one wants -# to be streamed in the bridge gets added to the end of the list. Any changes -# in the middle of the list (additions, removals, replacements) will change also -# the current message IDS, which might result with incompatibilities with -# previous PX4 versions (where the list with this format got introduced or -# subsisted). -# -# Any updates to this file should be mirrored in both sides of the bridge (i.e., -# PX4 and px4_ros_com), when using it with ROS2. That can be easily done using -# the 'msg/tools/uorb_to_ros_urtps_topics.py' script to regenerate this same -# file under 'px4_ros_com/templates/''. The same is not applicable/required if -# using this bridge with "raw" RTPS/DDS applications, since the microRTPS agent -# to be used and stored in 'build//src/modules/micrortps_bridge/'' -# gets generated using this same list. -# -##### -rtps: - # topic ID 1 - - msg: debug_array - receive: true - # topic ID 2 - - msg: debug_key_value - receive: true - # topic ID 3 - - msg: debug_value - receive: true - # ... - - msg: debug_vect - receive: true - - msg: offboard_control_mode - receive: true - - msg: sensor_optical_flow - receive: true - - msg: position_setpoint - receive: true - - msg: position_setpoint_triplet - receive: true - - msg: telemetry_status - receive: true - - msg: timesync - receive: true - send: true - - msg: trajectory_waypoint - send: true - - msg: vehicle_command - receive: true - - msg: vehicle_control_mode - send: true - - msg: vehicle_local_position_setpoint - receive: true - - msg: vehicle_attitude_setpoint - receive: true - - msg: vehicle_rates_setpoint - receive: true - - msg: trajectory_setpoint - receive: true - - msg: vehicle_odometry - send: true - - msg: vehicle_mocap_odometry # multi-topic / alias of vehicle_odometry - base: vehicle_odometry - receive: true - - msg: vehicle_visual_odometry # multi-topic / alias of vehicle_odometry - base: vehicle_odometry - receive: true - - msg: vehicle_status - send: true - - msg: vehicle_trajectory_waypoint - receive: true - - msg: vehicle_trajectory_waypoint_desired # multi-topic / alias of vehicle_trajectory_waypoint - base: vehicle_trajectory_waypoint - send: true - - msg: collision_constraints - send: true - - msg: onboard_computer_status - receive: true - - msg: trajectory_bezier - receive: true - - msg: vehicle_trajectory_bezier - receive: true - - msg: timesync_status - send: true - - msg: sensor_combined - send: true diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 3b56d65e6d..71c8809ad2 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -181,6 +181,8 @@ public: unsigned get_last_generation() const { return _last_generation; } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } + ORB_ID orb_id() const { return _orb_id; } + protected: friend class SubscriptionCallback; diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index c0fa973251..bf701cbbd1 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -139,7 +139,7 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts) hrt_abstime result; result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; + result += (hrt_abstime)(ts->tv_nsec / 1000); return result; } @@ -149,9 +149,9 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts) */ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + ts->tv_sec = (time_t)abstime / 1000000; + abstime -= (hrt_abstime)(ts->tv_sec * 1000000); + ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000); } /** @@ -249,8 +249,8 @@ __EXPORT extern void px4_lockstep_wait_for_components(void); #else static inline int px4_lockstep_register_component(void) { return 0; } -static inline void px4_lockstep_unregister_component(int component) { } -static inline void px4_lockstep_progress(int component) { } +static inline void px4_lockstep_unregister_component(int component) { (void)component; } +static inline void px4_lockstep_progress(int component) {(void)component; } static inline void px4_lockstep_wait_for_components(void) { } #endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */ diff --git a/src/drivers/protocol_splitter/Kconfig b/src/drivers/protocol_splitter/Kconfig deleted file mode 100644 index f251603986..0000000000 --- a/src/drivers/protocol_splitter/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_PROTOCOL_SPLITTER - bool "protocol_splitter" - default n - ---help--- - Enable support for protocol_splitter \ No newline at end of file diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp deleted file mode 100644 index dacf0bb4c5..0000000000 --- a/src/drivers/protocol_splitter/protocol_splitter.cpp +++ /dev/null @@ -1,845 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file protocol_splitter.cpp - * - * NuttX Driver to multiplex MAVLink and RTPS on a single serial port. - * Makes sure the two protocols can be read & written simultaneously by two - * processes. - * - * It will create two devices: - * /dev/mavlink - * /dev/rtps - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - - -class Mavlink2Dev; -class RtpsDev; -class ReadBuffer; - -extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); - -/* - MessageType is in MSB of header[1] - | - v - Mavlink 0000 0000b - Rtps 1000 0000b -*/ -enum class MessageType : uint8_t {Mavlink = 0x00, Rtps = 0x01}; - -constexpr char Sp2HeaderMagic = 'S'; -constexpr int Sp2HeaderSize = 4; - -/* - Header Structure: - - bits: 1 2 3 4 5 6 7 8 - header[0] - | Magic | - header[1] - |T| LenH | - header[2] - | LenL | - header[3] - | Checksum | -*/ -union __attribute__((packed)) Sp2Header { - uint8_t bytes[4]; - struct { - char magic; // 'S' - uint8_t len_h: 7, // Length MSB - type: 1; // 0=MAVLINK, 1=RTPS - uint8_t len_l; // Length LSB - uint8_t checksum; // XOR of the three bytes above - } fields; -}; - -struct StaticData { - Mavlink2Dev *mavlink2; - RtpsDev *rtps; - sem_t r_lock; - sem_t w_lock; - char device_name[16]; - ReadBuffer *read_buffer; -}; - -namespace -{ -static StaticData *objects = nullptr; -} - -// Perf counters -perf_counter_t bytes_received_count; -perf_counter_t header_bytes_received_count; -perf_counter_t bytes_lost_count; -perf_counter_t mavlink_messages_parsed_count; -perf_counter_t mavlink_bytes_parsed_count; -perf_counter_t rtps_messages_parsed_count; -perf_counter_t rtps_bytes_parsed_count; -perf_counter_t buffer_drops; - -class ReadBuffer -{ -public: - int read(int fd); - void copy(void *dest, size_t pos, size_t n); - void remove(size_t pos, size_t n); - - void print_stats(); - void update_lost_stats(); - - uint8_t buffer[1024] = {}; - size_t buf_size = 0; - - static const size_t BUFFER_THRESHOLD = sizeof(buffer) * 0.8; - - // We keep track of the first Mavlink and Rtps packet in the buffer. - // If start and end are equal there is no packet. - size_t start_mavlink = 0; - size_t end_mavlink = 0; - size_t start_rtps = 0; - size_t end_rtps = 0; - // Just for stats. - size_t mavlink_parsed = 0; - size_t rtps_parsed = 0; - size_t bytes_received = 0; - size_t bytes_lost = 0; - size_t header_bytes_received = 0; -}; - -int ReadBuffer::read(int fd) -{ - if (buf_size > BUFFER_THRESHOLD) { - // Drop the buffer if it's too full. This is not expected to happen, but it might, if one of the readers - // is too slow. In that case it's best to make space for new data, otherwise the faster reader might - // busy-loop w/o getting new data. - perf_count(buffer_drops); - - PX4_DEBUG("Buffer full, dropping: %zu %zu %zu %zu", start_mavlink, end_mavlink, start_rtps, end_rtps); - - // Drop only as much as we have to - size_t num_remove = math::max(start_mavlink, start_rtps); - - if (num_remove == 0) { - num_remove = buf_size; - } - - remove(0, num_remove); - start_mavlink -= math::min(num_remove, start_mavlink); - end_mavlink -= math::min(num_remove, end_mavlink); - start_rtps -= math::min(num_remove, start_rtps); - end_rtps -= math::min(num_remove, end_rtps); - } - - int bytes_available = 0; - int err = ::ioctl(fd, FIONREAD, (unsigned long)&bytes_available); - ssize_t r = 0; - - if (err != 0 || bytes_available > 0) { - r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size); - } - - if (r <= 0) { - return r; - } - - buf_size += r; - bytes_received += r; - - // Update the lost/unused bytes count - update_lost_stats(); - - perf_set_count(bytes_received_count, bytes_received); - - return r; -} - -void ReadBuffer::copy(void *dest, size_t pos, size_t n) -{ - ASSERT(pos < buf_size); - ASSERT(pos + n <= buf_size); - - if (dest) { - memcpy(dest, buffer + pos, n); - } -} - -void ReadBuffer::remove(size_t pos, size_t n) -{ - ASSERT(pos < buf_size); - ASSERT(pos + n <= buf_size); - - memmove(buffer + pos, buffer + (pos + n), buf_size - pos - n); - buf_size -= n; -} - -void ReadBuffer::update_lost_stats() -{ - bytes_lost = bytes_received - mavlink_parsed - rtps_parsed - header_bytes_received; - - if (end_mavlink > start_mavlink) { - bytes_lost -= end_mavlink - start_mavlink; - } - - if (end_rtps > start_rtps) { - bytes_lost -= end_rtps - start_rtps; - } - - perf_set_count(bytes_lost_count, bytes_lost); -} - -void ReadBuffer::print_stats() -{ - PX4_INFO_RAW("\tReceived:\n"); - PX4_INFO_RAW("\tTotal: %9zu bytes\n", - bytes_received); - PX4_INFO_RAW("\tHeaders: %9zu bytes (%5.1f %%)\n", - header_bytes_received, - static_cast(static_cast(header_bytes_received) - / static_cast(bytes_received) - * 100.f)); - PX4_INFO_RAW("\tMAVLink: %9zu bytes (%5.1f %%)\n", - mavlink_parsed, - static_cast(static_cast(mavlink_parsed) - / static_cast(bytes_received - header_bytes_received) - * 100.f)); - PX4_INFO_RAW("\tRTPS: %9zu bytes (%5.1f %%)\n", - rtps_parsed, - static_cast(static_cast(rtps_parsed) - / static_cast(bytes_received - header_bytes_received) - * 100.f)); - - PX4_INFO_RAW("\tUnused: %9zu bytes (%5.1f %%)\n", bytes_lost, - static_cast(static_cast(bytes_lost) - / static_cast(bytes_received) - * 100.f)); -} - - -class DevCommon : public cdev::CDev -{ -public: - DevCommon(const char *device_path, ReadBuffer *read_buffer); - virtual ~DevCommon(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int open(file *filp); - virtual int close(file *filp); - - enum Operation {Read, Write}; - -protected: - - virtual pollevent_t poll_state(struct file *filp); - - int try_to_copy_data(char *buffer, size_t buflen, MessageType message_type); - void scan_for_packets(); - void cleanup(); - - void lock(enum Operation op) - { - sem_t *this_lock = op == Read ? &objects->r_lock : &objects->w_lock; - - while (sem_wait(this_lock) != 0) { - /* The only case that an error should occur here is if - * the wait was awakened by a signal. - */ - ASSERT(get_errno() == EINTR); - } - } - - void unlock(enum Operation op) - { - sem_t *this_lock = op == Read ? &objects->r_lock : &objects->w_lock; - sem_post(this_lock); - } - - int _fd = -1; - - Sp2Header _header; - - ReadBuffer *_read_buffer; -}; - -DevCommon::DevCommon(const char *device_path, ReadBuffer *read_buffer) - : CDev(device_path) - , _read_buffer(read_buffer) -{ -} - -DevCommon::~DevCommon() -{ - if (_fd >= 0) { - // discard all pending data, as close() might block otherwise on NuttX with flow control enabled - tcflush(_fd, TCIOFLUSH); - ::close(_fd); - } -} - -int DevCommon::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - if (cmd == FIONSPACE) { - int ret = ::ioctl(_fd, FIONSPACE, arg); - - if (ret == 0) { - int *buf_free = (int *)arg; - *buf_free -= sizeof(_header); - - if (*buf_free < 0) { - *buf_free = 0; - } - } - - return ret; - } - - return ::ioctl(_fd, cmd, arg); -} - -int DevCommon::open(file *filp) -{ - _fd = ::open(objects->device_name, O_RDWR | O_NOCTTY); - - if (_fd < 0) { - PX4_ERR("open failed: %s", strerror(errno)); - return -1; - } - - CDev::open(filp); - - - return 0; -} - -int DevCommon::close(file *filp) -{ - //int ret = ::close(_fd); // FIXME: calling this results in a dead-lock, because DevCommon::close() - // is called from within another close(), and NuttX seems to hold a semaphore at this point - _fd = -1; - CDev::close(filp); - return 0; -} - -pollevent_t DevCommon::poll_state(struct file *filp) -{ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - /* Here we should just check the poll state (which is called before an actual poll waiting). - * Instead we poll on the fd with some timeout, and then pretend that there is data. - * This will let the calling poll return immediately (there's still no busy loop since - * we do actually poll here). - * We do this because there is no simple way with the given interface to poll on - * the _fd in here or by overriding some other method. - */ - - ::poll(fds, sizeof(fds) / sizeof(fds[0]), 10); - - return POLLIN; -} - -int DevCommon::try_to_copy_data(char *buffer, size_t buflen, MessageType message_type) -{ - if (buflen == 0) { - return 0; - } - - switch (message_type) { - case MessageType::Mavlink: - if (_read_buffer->start_mavlink < _read_buffer->end_mavlink) { - // We have Mavlink data ready to send. - const size_t len_available = _read_buffer->end_mavlink - _read_buffer->start_mavlink; - // We can only send what fits in the callers buffer. - const size_t len_to_copy = math::min(len_available, buflen); - - // Copy it to the callers buffer and remove it from our buffer. - _read_buffer->copy(buffer, _read_buffer->start_mavlink, len_to_copy); - - // Shift the markers accordingly. - _read_buffer->start_mavlink += len_to_copy; - - // Keep track for stats. - _read_buffer->mavlink_parsed += len_to_copy; - - // Update the lost/unused bytes count - _read_buffer->update_lost_stats(); - - // Update the number of MAVLink bytes parsed - perf_set_count(mavlink_bytes_parsed_count, _read_buffer->mavlink_parsed); - - // Update the number of MAVLink messages parsed - perf_count(mavlink_messages_parsed_count); - - return len_to_copy; - - } else { - return 0; - } - - case MessageType::Rtps: - if (_read_buffer->start_rtps < _read_buffer->end_rtps) { - // We have Rtps data ready to send - const size_t len_available = _read_buffer->end_rtps - _read_buffer->start_rtps; - // We can only send what fits in the callers buffer. - const size_t len_to_copy = math::min(len_available, buflen); - - // Copy it to the callers buffer and remove it from our buffer. - _read_buffer->copy(buffer, _read_buffer->start_rtps, len_to_copy); - - // Shift the markers accordingly. - _read_buffer->start_rtps += len_to_copy; - - // Keep track for stats. - _read_buffer->rtps_parsed += len_to_copy; - - // Update the lost/unused bytes count - _read_buffer->update_lost_stats(); - - // Update the number of RTPS bytes parsed - perf_set_count(rtps_bytes_parsed_count, _read_buffer->rtps_parsed); - - // Update the number of RTPS messages parsed - perf_count(rtps_messages_parsed_count); - - return len_to_copy; - - } else { - return 0; - } - - break; - - - default: - return 0; - } -} - -void DevCommon::scan_for_packets() -{ - if (_read_buffer->buf_size < Sp2HeaderSize) { - // We have not even one header in the buffer, no need to scan yet. - return; - } - - bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink); - bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps); - - if (mavlink_available && rtps_available) { - // We still have data for both, no need to scan yet. - return; - } - - const size_t begin = math::min(_read_buffer->end_mavlink, _read_buffer->end_rtps); - - for (size_t i = begin; i < _read_buffer->buf_size - Sp2HeaderSize; /* ++i */) { - - const Sp2Header *header = reinterpret_cast(&_read_buffer->buffer[i]); - - if (header->fields.magic != Sp2HeaderMagic) { - // Not the magic byte that we're looking for. - ++i; - continue; - } - - const uint8_t checksum = (_read_buffer->buffer[i] ^ _read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]); - - if (header->fields.checksum != checksum) { - // Checksum failed. - ++i; - continue; - } - - if (header->fields.type != static_cast(MessageType::Mavlink) && - header->fields.type != static_cast(MessageType::Rtps)) { - // Ignore unknown protocols - ++i; - continue; - } - - const size_t payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.len_l; - - if (payload_len > sizeof(_read_buffer->buffer)) { - // This can happen if by accident data matches the header including checksum. - // Given we skip most data using the last payload_len, we should not see this too often, - // unless the link is very lossy and we often have to re-sync. - PX4_DEBUG("payload size %zu > buffer size %zu: %d, protocol: %s", - payload_len, sizeof(_read_buffer->buffer), - (header->fields.type == static_cast(MessageType::Mavlink)) ? "Mavlink" : "Rtps"); - ++i; - continue; - } - - if (i + Sp2HeaderSize + payload_len > _read_buffer->buf_size) { - // We don't have a enough data in the buffer yet, try again later. - break; - } - - if (header->fields.type == static_cast(MessageType::Mavlink) && !mavlink_available) { - _read_buffer->start_mavlink = i + Sp2HeaderSize; - _read_buffer->end_mavlink = _read_buffer->start_mavlink + payload_len; - mavlink_available = true; - - // Overwrite header magic byte, so we don't parse them again. - _read_buffer->buffer[i] = 0; - _read_buffer->header_bytes_received += Sp2HeaderSize; - - } else if (header->fields.type == static_cast(MessageType::Rtps) && !rtps_available) { - _read_buffer->start_rtps = i + Sp2HeaderSize; - _read_buffer->end_rtps = _read_buffer->start_rtps + payload_len; - rtps_available = true; - - // Overwrite header magic byte, so we don't parse them again. - _read_buffer->buffer[i] = 0; - _read_buffer->header_bytes_received += Sp2HeaderSize; - } - - if (mavlink_available && rtps_available) { - // Both have at least one message ready, we can stop now. - break; - } - - // Update the lost/unused bytes count - _read_buffer->update_lost_stats(); - - perf_set_count(header_bytes_received_count, _read_buffer->header_bytes_received); - - i += Sp2HeaderSize + payload_len; - - } -} - - -void DevCommon::cleanup() -{ - const bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink); - const bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps); - - // Clean up garbage bytes and accumulated headers - - size_t garbage_end = 0; - - if (!mavlink_available && !rtps_available) { - garbage_end = math::max(_read_buffer->start_mavlink, _read_buffer->start_rtps); - - } else { - garbage_end = math::min(_read_buffer->start_mavlink, _read_buffer->start_rtps); - } - - if (garbage_end > 0) { - _read_buffer->remove(0, garbage_end); - - _read_buffer->start_mavlink -= math::min(garbage_end, _read_buffer->start_mavlink); - _read_buffer->end_mavlink -= math::min(garbage_end, _read_buffer->end_mavlink); - _read_buffer->start_rtps -= math::min(garbage_end, _read_buffer->start_rtps); - _read_buffer->end_rtps -= math::min(garbage_end, _read_buffer->end_rtps); - } -} - - -class Mavlink2Dev : public DevCommon -{ -public: - Mavlink2Dev(ReadBuffer *read_buffer); - virtual ~Mavlink2Dev() {} - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); -}; - -Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) - : DevCommon("/dev/mavlink", read_buffer) -{ - _header.fields.magic = Sp2HeaderMagic; - _header.fields.len_h = 0; - _header.fields.len_l = 0; - _header.fields.checksum = 0; - _header.fields.type = static_cast(MessageType::Mavlink); -} - -ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) -{ - lock(Read); - - // The cleanup needs to be right after a scan, so we don't clean up - // something that we haven't found yet. - scan_for_packets(); - cleanup(); - - // If we have already a packet ready in the current buffer, we don't have - // to read and can grab data straightaway. - int ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink); - - if (ret > 0) { - unlock(Read); - return ret; - } - - // Otherwise, we have to do a read. - ret = _read_buffer->read(_fd); - - if (ret <= 0) { - unlock(Read); - return ret; - } - - // Now we need to check again if there is data available. - scan_for_packets(); - - // And try to copy it out. - ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink); - - unlock(Read); - return ret; -} - -ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) -{ - _header.fields.len_h = (buflen >> 8) & 0x7f; - _header.fields.len_l = buflen & 0xff; - _header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2]; - lock(Write); - int buf_free = 0; - ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); - ssize_t ret = -1; - - if ((size_t)buf_free >= sizeof(_header) + buflen) { - ret = ::write(_fd, _header.bytes, sizeof(_header)); - - if (ret == sizeof(_header)) { - ret = ::write(_fd, buffer, buflen); - } - } - - unlock(Write); - - return ret; -} - -class RtpsDev : public DevCommon -{ -public: - RtpsDev(ReadBuffer *read_buffer); - virtual ~RtpsDev() {} - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - -protected: - static const uint8_t HEADER_SIZE = 10; -}; - -RtpsDev::RtpsDev(ReadBuffer *read_buffer) - : DevCommon("/dev/rtps", read_buffer) -{ - _header.fields.magic = Sp2HeaderMagic; - _header.fields.len_h = 0; - _header.fields.len_l = 0; - _header.fields.checksum = 0; - _header.fields.type = static_cast(MessageType::Rtps); -} - -ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) -{ - lock(Read); - - scan_for_packets(); - cleanup(); - - // If we have already a packet ready in the current buffer, we don't have to read. - int ret = try_to_copy_data(buffer, buflen, MessageType::Rtps); - - if (ret > 0) { - unlock(Read); - return ret; - } - - // Otherwise, we have to do a read. - ret = _read_buffer->read(_fd); - - if (ret <= 0) { - unlock(Read); - return ret; - } - - scan_for_packets(); - - // And check again. - ret = try_to_copy_data(buffer, buflen, MessageType::Rtps); - - unlock(Read); - return ret; -} - -ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen) -{ - _header.fields.len_h = (buflen >> 8) & 0x7f; - _header.fields.len_l = buflen & 0xff; - _header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2]; - lock(Write); - int buf_free = 0; - ssize_t ret = -1; - ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); - - if ((size_t)buf_free >= sizeof(_header) + buflen) { - ret = ::write(_fd, _header.bytes, sizeof(_header)); - - if (ret == sizeof(_header)) { - ret = ::write(_fd, buffer, buflen); - } - } - - unlock(Write); - - return ret; -} - -int protocol_splitter_main(int argc, char *argv[]) -{ - if (argc < 2) { - goto out; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) { - if (objects) { - PX4_WARN("already running"); - return 1; - } - - if (argc != 3) { - goto out; - } - - objects = new StaticData(); - - if (!objects) { - PX4_ERR("alloc failed"); - return -1; - } - - bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes received"); - bytes_lost_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes unused/lost"); - header_bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: header bytes received"); - mavlink_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs parsed"); - mavlink_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs bytes parsed"); - rtps_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs parsed"); - rtps_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs bytes parsed"); - buffer_drops = perf_alloc(PC_COUNT, "protocol_splitter: buffer drops"); - - strncpy(objects->device_name, argv[2], sizeof(objects->device_name)); - sem_init(&objects->r_lock, 1, 1); - sem_init(&objects->w_lock, 1, 1); - objects->read_buffer = new ReadBuffer(); - objects->mavlink2 = new Mavlink2Dev(objects->read_buffer); - objects->rtps = new RtpsDev(objects->read_buffer); - - if (!objects->mavlink2 || !objects->rtps) { - delete objects->mavlink2; - delete objects->rtps; - delete objects->read_buffer; - sem_destroy(&objects->r_lock); - sem_destroy(&objects->w_lock); - delete objects; - objects = nullptr; - PX4_ERR("alloc failed"); - return -1; - - } else { - objects->mavlink2->init(); - objects->rtps->init(); - } - } - - if (!strcmp(argv[1], "stop")) { - if (objects) { - delete objects->mavlink2; - delete objects->rtps; - delete objects->read_buffer; - sem_destroy(&objects->r_lock); - sem_destroy(&objects->w_lock); - delete objects; - objects = nullptr; - - perf_free(bytes_received_count); - perf_free(header_bytes_received_count); - perf_free(bytes_lost_count); - perf_free(mavlink_messages_parsed_count); - perf_free(mavlink_bytes_parsed_count); - perf_free(rtps_messages_parsed_count); - perf_free(rtps_bytes_parsed_count); - perf_free(buffer_drops); - } - } - - /* - * Print driver status. - */ - if (!strcmp(argv[1], "status")) { - if (objects) { - PX4_INFO("running"); - - if (sem_wait(&objects->r_lock) != 0) { - return -1; - } - - objects->read_buffer->print_stats(); - sem_post(&objects->r_lock); - - } else { - PX4_INFO("not running"); - } - } - - return 0; - -out: - PX4_ERR("unrecognized command, try 'start ', 'stop', 'status'"); - return 1; -} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1601c4d4e6..14b4dcf9f6 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -65,6 +65,7 @@ add_subdirectory(systemlib) add_subdirectory(system_identification) add_subdirectory(tecs) add_subdirectory(terrain_estimation) +add_subdirectory(timesync) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(weather_vane) diff --git a/src/drivers/protocol_splitter/CMakeLists.txt b/src/lib/timesync/CMakeLists.txt similarity index 90% rename from src/drivers/protocol_splitter/CMakeLists.txt rename to src/lib/timesync/CMakeLists.txt index 8e09f1b106..dc150e0e79 100644 --- a/src/drivers/protocol_splitter/CMakeLists.txt +++ b/src/lib/timesync/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,10 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ -px4_add_module( - MODULE drivers__protocol_splitter - MAIN protocol_splitter - SRCS - protocol_splitter.cpp - DEPENDS - ) + +px4_add_library(timesync + Timesync.cpp + Timesync.hpp +) +target_link_libraries(timesync) diff --git a/src/lib/timesync/Timesync.cpp b/src/lib/timesync/Timesync.cpp new file mode 100644 index 0000000000..84961b128a --- /dev/null +++ b/src/lib/timesync/Timesync.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file timesync.cpp + * timesync implementation. + * + * @author Mohammed Kabir + */ + +#include "Timesync.hpp" + +#include + +void Timesync::update(const uint64_t now_us, const int64_t remote_timestamp_ns, const int64_t originate_timestamp_ns) +{ + // Message originating from this system, compute time offset from it + if (remote_timestamp_ns > 0) { + // Calculate time offset between this system and the remote system, assuming RTT for + // the timesync packet is roughly equal both ways. + int64_t offset_us = (int64_t)((originate_timestamp_ns / 1000ULL) + now_us - (remote_timestamp_ns / 1000ULL) * 2) / 2 ; + + // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system + uint64_t rtt_us = now_us - (originate_timestamp_ns / 1000ULL); + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs((int64_t)_time_offset - offset_us); + + if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT + + if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { + + // Increment the counter if we have a good estimate and are getting samples far from the estimate + _high_deviation_count++; + + // We reset the filter if we received 5 consecutive samples which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { + PX4_ERR("Time jump detected. Resetting time synchroniser."); + // Reset the filter + reset_filter(); + } + + } else { + + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; + double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress))); + _filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL; + _filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL; + + } else { + _filter_alpha = ALPHA_GAIN_FINAL; + _filter_beta = BETA_GAIN_FINAL; + } + + // Perform filter update + add_sample(offset_us); + + // Increment sequence counter after filter update + _sequence++; + + // Reset high deviation count after filter update + _high_deviation_count = 0; + + // Reset high RTT count after filter update + _high_rtt_count = 0; + } + + } else { + // Increment counter if round trip time is too high for accurate timesync + _high_rtt_count++; + + if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { + PX4_WARN("RTT too high for timesync: %llu ms", rtt_us / 1000ULL); + // Reset counter to rate-limit warnings + _high_rtt_count = 0; + } + + } + + // Publish status message + timesync_status_s tsync_status{}; + tsync_status.source_protocol = _source; + tsync_status.remote_timestamp = remote_timestamp_ns / 1000ULL; + tsync_status.observed_offset = offset_us; + tsync_status.estimated_offset = (int64_t)_time_offset; + tsync_status.round_trip_time = rtt_us; + tsync_status.timestamp = hrt_absolute_time(); + + _timesync_status_pub.publish(tsync_status); + } +} + +uint64_t Timesync::sync_stamp(uint64_t usec) +{ + // Only return synchronised stamp if we have converged to a good value + if (sync_converged()) { + return usec + (int64_t)_time_offset; + + } else { + return hrt_absolute_time(); + } +} + +void Timesync::add_sample(int64_t offset_us) +{ + // Online exponential smoothing filter. The derivative of the estimate is also + // estimated in order to produce an estimate without steady state lag: + // https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + double time_offset_prev = _time_offset; + + if (_sequence == 0) { + // First offset sample + _time_offset = offset_us; + + } else { + // Update the clock offset estimate + _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); + + // Update the clock skew estimate + _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; + } +} + +void Timesync::reset_filter() +{ + // Do a full reset of all statistics and parameters + _sequence = 0; + _time_offset = 0.0; + _time_skew = 0.0; + _filter_alpha = ALPHA_GAIN_INITIAL; + _filter_beta = BETA_GAIN_INITIAL; + _high_deviation_count = 0; + _high_rtt_count = 0; +} diff --git a/src/lib/timesync/Timesync.hpp b/src/lib/timesync/Timesync.hpp new file mode 100644 index 0000000000..b17766b09a --- /dev/null +++ b/src/lib/timesync/Timesync.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Timesync.hpp + * time synchroniser definition. + * + * @author Mohammed Kabir + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +using namespace time_literals; + +static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL; + +// Filter gains +// +// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead +// to a smoother estimate, but track time drift more slowly, introducing a bias +// in the estimate. Larger values will cause low-amplitude oscillations. +// +// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a +// tighter estimation of the skew (derivative), but will negatively affect how fast the +// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). +// Larger values will cause large-amplitude oscillations. +static constexpr double ALPHA_GAIN_INITIAL = 0.05; +static constexpr double BETA_GAIN_INITIAL = 0.05; +static constexpr double ALPHA_GAIN_FINAL = 0.003; +static constexpr double BETA_GAIN_FINAL = 0.003; + +// Filter gain scheduling +// +// The filter interpolates between the INITIAL and FINAL gains while the number of +// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will +// allow the timesync to converge faster, but with potentially less accurate initial +// offset and skew estimates. +static constexpr uint32_t CONVERGENCE_WINDOW = 500; + +// Outlier rejection and filter reset +// +// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. +// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning +// but not reset the filter. +// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current +// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number +// of such events in a row will reset the filter. This usually happens only due to a time jump +// on the remote system. +// TODO : automatically determine these using ping statistics? +static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms; +static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5; + +class Timesync +{ +public: + Timesync(uint8_t source = timesync_status_s::SOURCE_PROTOCOL_UNKNOWN) : _source(source) {}; + ~Timesync() = default; + + void update(const uint64_t now_us, const int64_t remote_timestamp_ns, int64_t originate_timestamp_ns); + + /** + * Convert remote timestamp to local hrt time (usec) + * Use synchronised time if available, monotonic boot time otherwise + */ + uint64_t sync_stamp(uint64_t usec); + + int64_t offset() const { return (int64_t)_time_offset; } + +private: + + /** + * Online exponential filter to smooth time offset + */ + void add_sample(int64_t offset_us); + + /** + * Return true if the timesync algorithm converged to a good estimate, + * return false otherwise + */ + bool sync_converged() const { return _sequence >= CONVERGENCE_WINDOW; } + + /** + * Reset the exponential filter and its states + */ + void reset_filter(); + + uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; + + uint32_t _sequence{0}; + + // Timesync statistics + double _time_offset{0}; + double _time_skew{0}; + + // Filter parameters + double _filter_alpha{ALPHA_GAIN_INITIAL}; + double _filter_beta{BETA_GAIN_INITIAL}; + + // Outlier rejection and filter reset + uint32_t _high_deviation_count{0}; + uint32_t _high_rtt_count{0}; + + uint8_t _source{}; +}; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index e1b7d9972f..e76cdf582c 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -190,7 +190,7 @@ int AirshipAttitudeControl::print_status() perf_print_counter(_loop_perf); - print_message(ORB_ID(actuator_controls), _actuator_controls); + print_message(ORB_ID(actuator_controls_0), _actuator_controls); return 0; } diff --git a/src/modules/commander/failsafe/CMakeLists.txt b/src/modules/commander/failsafe/CMakeLists.txt index 23363265b1..0d7b3bd366 100644 --- a/src/modules/commander/failsafe/CMakeLists.txt +++ b/src/modules/commander/failsafe/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ # Extract information from failsafe msg file -set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/failsafe_flags.msg) +set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/FailsafeFlags.msg) set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_struct_field_mapping.h) set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html) set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html) diff --git a/src/modules/commander/failsafe/parse_flags_from_msg.py b/src/modules/commander/failsafe/parse_flags_from_msg.py index c7cace101e..6b1ddf24b9 100755 --- a/src/modules/commander/failsafe/parse_flags_from_msg.py +++ b/src/modules/commander/failsafe/parse_flags_from_msg.py @@ -12,7 +12,7 @@ import re if __name__ == "__main__": parser = argparse.ArgumentParser(description='Extract fields from .msg files') - parser.add_argument('msg_file', help='failsafe_flags.msg file') + parser.add_argument('msg_file', help='FailsafeFlags.msg file') parser.add_argument('header_file', help='generated_uorb_struct_field_mapping.h') parser.add_argument('html_template', help='HTML template input file') parser.add_argument('html_output', help='HTML output file') @@ -81,7 +81,7 @@ if __name__ == "__main__": macro_lines = '' for group in groups: for field_type, field_name, comment in group.fields: - macro_lines += ' .property("{0}", &{1}_s::{0}) \\\n'.format(field_name, msg_name) + macro_lines += ' .property("{0}", &px4::msg::{1}::{0}) \\\n'.format(field_name, msg_name) cpp_emscription_macro = '#define UORB_STRUCT_FIELD_MAPPING \\\n{}\n'.format(macro_lines) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 0e89a350ae..261ee9a338 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -49,7 +49,7 @@ #include -void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const +void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const { // reset flags resetEstimatorAidStatus(airspeed); @@ -75,7 +75,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so setEstimatorAidStatusTestRatio(airspeed, innov_gate); } -void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed) +void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed) { if (airspeed.innovation_rejected) { return; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 14ab85e479..93a8bec694 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -49,9 +49,9 @@ #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" -#include -#include -#include +#include +#include +#include enum class Likelihood { LOW, MEDIUM, HIGH }; @@ -551,30 +551,30 @@ private: uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive - estimator_aid_source_1d_s _aid_src_baro_hgt{}; - estimator_aid_source_1d_s _aid_src_rng_hgt{}; - estimator_aid_source_1d_s _aid_src_airspeed{}; - estimator_aid_source_1d_s _aid_src_sideslip{}; + estimator_aid_source1d_s _aid_src_baro_hgt{}; + estimator_aid_source1d_s _aid_src_rng_hgt{}; + estimator_aid_source1d_s _aid_src_airspeed{}; + estimator_aid_source1d_s _aid_src_sideslip{}; - estimator_aid_source_2d_s _aid_src_fake_pos{}; - estimator_aid_source_1d_s _aid_src_fake_hgt{}; + estimator_aid_source2d_s _aid_src_fake_pos{}; + estimator_aid_source1d_s _aid_src_fake_hgt{}; - estimator_aid_source_1d_s _aid_src_ev_hgt{}; - estimator_aid_source_2d_s _aid_src_ev_pos{}; - estimator_aid_source_3d_s _aid_src_ev_vel{}; - estimator_aid_source_1d_s _aid_src_ev_yaw{}; + estimator_aid_source1d_s _aid_src_ev_hgt{}; + estimator_aid_source2d_s _aid_src_ev_pos{}; + estimator_aid_source3d_s _aid_src_ev_vel{}; + estimator_aid_source1d_s _aid_src_ev_yaw{}; - estimator_aid_source_1d_s _aid_src_gnss_hgt{}; - estimator_aid_source_2d_s _aid_src_gnss_pos{}; - estimator_aid_source_3d_s _aid_src_gnss_vel{}; - estimator_aid_source_1d_s _aid_src_gnss_yaw{}; + estimator_aid_source1d_s _aid_src_gnss_hgt{}; + estimator_aid_source2d_s _aid_src_gnss_pos{}; + estimator_aid_source3d_s _aid_src_gnss_vel{}; + estimator_aid_source1d_s _aid_src_gnss_yaw{}; - estimator_aid_source_1d_s _aid_src_mag_heading{}; - estimator_aid_source_3d_s _aid_src_mag{}; + estimator_aid_source1d_s _aid_src_mag_heading{}; + estimator_aid_source3d_s _aid_src_mag{}; - estimator_aid_source_2d_s _aid_src_aux_vel{}; + estimator_aid_source2d_s _aid_src_aux_vel{}; - estimator_aid_source_2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_optical_flow{}; // output predictor states Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) @@ -662,12 +662,12 @@ private: void predictCovariance(); // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true); + bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance - bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s &aid_src_status); + bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(const gpsSample &gps_sample); @@ -683,12 +683,12 @@ private: // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); - void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const; - void fuseAirspeed(estimator_aid_source_1d_s &airspeed); + void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const; + void fuseAirspeed(estimator_aid_source1d_s &airspeed); // fuse synthetic zero sideslip measurement - void updateSideslip(estimator_aid_source_1d_s &_aid_src_sideslip) const; - void fuseSideslip(estimator_aid_source_1d_s &_aid_src_sideslip); + void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; + void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(const dragSample &drag_sample); @@ -718,24 +718,24 @@ private: void resetVerticalVelocityToZero(); // fuse optical flow line of sight rate measurements - void updateOptFlow(estimator_aid_source_2d_s &aid_src); + void updateOptFlow(estimator_aid_source2d_s &aid_src); void fuseOptFlow(); // horizontal and vertical position aid source - void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const; - void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source_1d_s &aid_src) const; + void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; // 2d & 3d velocity aid source - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const; - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source_3d_s &aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; // horizontal and vertical position fusion - void fuseHorizontalPosition(estimator_aid_source_2d_s &pos_aid_src); - void fuseVerticalPosition(estimator_aid_source_1d_s &hgt_aid_src); + void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); + void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); // 2d & 3d velocity fusion - void fuseVelocity(estimator_aid_source_2d_s &vel_aid_src); - void fuseVelocity(estimator_aid_source_3d_s &vel_aid_src); + void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); + void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); void updateGpsYaw(const gpsSample &gps_sample); @@ -1085,7 +1085,7 @@ private: void resetGpsDriftCheckFilters(); - void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const + void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const { // only bother resetting if timestamp_sample is set if (status.timestamp_sample != 0) { @@ -1130,7 +1130,7 @@ private: } } - void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const + void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const { if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) { status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index dbc7440466..7bf93699f8 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -50,7 +50,7 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states) +bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states) { // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f)); @@ -223,7 +223,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b } // update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status) +bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status) { aid_src_status.innovation = innovation; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index fc3b210175..26a199c18f 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -47,7 +47,7 @@ #include #include "utils.hpp" -void Ekf::updateOptFlow(estimator_aid_source_2d_s &aid_src) +void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) { resetEstimatorAidStatus(aid_src); aid_src.timestamp_sample = _flow_sample_delayed.time_us; diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 8a6b281a77..fae55e0c4a 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -47,7 +47,7 @@ #include -void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const +void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const { // reset flags resetEstimatorAidStatus(sideslip); @@ -71,7 +71,7 @@ void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const setEstimatorAidStatusTestRatio(sideslip, innov_gate); } -void Ekf::fuseSideslip(estimator_aid_source_1d_s &sideslip) +void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) { if (sideslip.innovation_rejected) { return; diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index e3959326f9..e7cf2db341 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -44,7 +44,7 @@ #include "ekf.h" void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source_2d_s &aid_src) const + const float innov_gate, estimator_aid_source2d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -62,7 +62,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob } void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, - const float innov_gate, estimator_aid_source_3d_s &aid_src) const + const float innov_gate, estimator_aid_source3d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -88,7 +88,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob } void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, - const float innov_gate, estimator_aid_source_1d_s &aid_src) const + const float innov_gate, estimator_aid_source1d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -112,7 +112,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa } void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source_2d_s &aid_src) const + const float innov_gate, estimator_aid_source2d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -129,7 +129,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve aid_src.timestamp_sample = time_us; } -void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src) +void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) { if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { // vx, vy @@ -145,7 +145,7 @@ void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src) } } -void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src) +void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { // vx, vy, vz @@ -162,7 +162,7 @@ void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src) } } -void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src) +void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { @@ -178,7 +178,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src) } } -void Ekf::fuseVerticalPosition(estimator_aid_source_1d_s &aid_src) +void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 140875b9d6..2beea63e9f 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -47,7 +47,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() // fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low float innovation = 0.f; float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f; - estimator_aid_source_1d_s unused; + estimator_aid_source1d_s unused; fuseYaw(innovation, obs_var, unused); _last_static_yaw = NAN; @@ -62,7 +62,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { float innovation = wrap_pi(euler_yaw - _last_static_yaw); float obs_var = 0.01f; - estimator_aid_source_1d_s unused; + estimator_aid_source1d_s unused; fuseYaw(innovation, obs_var, unused); } @@ -78,7 +78,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { float innovation = 0.f; float obs_var = 0.01f; - estimator_aid_source_1d_s unused; + estimator_aid_source1d_s unused; fuseYaw(innovation, obs_var, unused); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index f582d7fb2f..652e3f6b6f 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -353,30 +353,30 @@ private: uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; - uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; - uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; - uORB::PublicationMulti _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)}; + uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; + uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; + uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; + uORB::PublicationMulti _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)}; - uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; - uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; + uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; + uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; - uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)}; - uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; - uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; - uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; + uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)}; + uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; + uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; + uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; - uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; - uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; - uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; - uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)}; + uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; + uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; + uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; + uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)}; - uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; - uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; + uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; + uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; - uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; + uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index b6163ee203..5d02efce66 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -100,6 +100,7 @@ px4_add_module( sensor_calibration geo mavlink_c + timesync tunes version UNITY_BUILD diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index ee0d2e3272..7bbe8f8dfa 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -72,82 +72,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) } else if (tsync.tc1 > 0) { // Message originating from this system, compute time offset from it - // Calculate time offset between this system and the remote system, assuming RTT for - // the timesync packet is roughly equal both ways. - int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ; - - // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system - uint64_t rtt_us = now - (tsync.ts1 / 1000ULL); - - // Calculate the difference of this sample from the current estimate - uint64_t deviation = llabs((int64_t)_time_offset - offset_us); - - if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT - - if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { - - // Increment the counter if we have a good estimate and are getting samples far from the estimate - _high_deviation_count++; - - // We reset the filter if we received 5 consecutive samples which violate our present estimate. - // This is most likely due to a time jump on the offboard system. - if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { - PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser."); - // Reset the filter - reset_filter(); - } - - } else { - - // Filter gain scheduling - if (!sync_converged()) { - // Interpolate with a sigmoid function - double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; - double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress))); - _filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL; - _filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL; - - } else { - _filter_alpha = ALPHA_GAIN_FINAL; - _filter_beta = BETA_GAIN_FINAL; - } - - // Perform filter update - add_sample(offset_us); - - // Increment sequence counter after filter update - _sequence++; - - // Reset high deviation count after filter update - _high_deviation_count = 0; - - // Reset high RTT count after filter update - _high_rtt_count = 0; - } - - } else { - // Increment counter if round trip time is too high for accurate timesync - _high_rtt_count++; - - if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { - PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid); - // Reset counter to rate-limit warnings - _high_rtt_count = 0; - } - - } - - // Publish status message - timesync_status_s tsync_status{}; - - tsync_status.timestamp = hrt_absolute_time(); - tsync_status.source_protocol = timesync_status_s::SOURCE_PROTOCOL_MAVLINK; - tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; - tsync_status.observed_offset = offset_us; - tsync_status.estimated_offset = (int64_t)_time_offset; - tsync_status.round_trip_time = rtt_us; - - _timesync_status_pub.publish(tsync_status); + _timesync.update(now, tsync.tc1, tsync.ts1); } break; @@ -181,58 +106,3 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) break; } } - -uint64_t -MavlinkTimesync::sync_stamp(uint64_t usec) -{ - // Only return synchronised stamp if we have converged to a good value - if (sync_converged()) { - return usec + (int64_t)_time_offset; - - } else { - return hrt_absolute_time(); - } -} - -bool -MavlinkTimesync::sync_converged() -{ - return _sequence >= CONVERGENCE_WINDOW; -} - -void -MavlinkTimesync::add_sample(int64_t offset_us) -{ - /* Online exponential smoothing filter. The derivative of the estimate is also - * estimated in order to produce an estimate without steady state lag: - * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing - */ - - double time_offset_prev = _time_offset; - - if (_sequence == 0) { // First offset sample - _time_offset = offset_us; - - } else { - // Update the clock offset estimate - _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); - - // Update the clock skew estimate - _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; - } - -} - -void -MavlinkTimesync::reset_filter() -{ - // Do a full reset of all statistics and parameters - _sequence = 0; - _time_offset = 0.0; - _time_skew = 0.0; - _filter_alpha = ALPHA_GAIN_INITIAL; - _filter_beta = BETA_GAIN_INITIAL; - _high_deviation_count = 0; - _high_rtt_count = 0; - -} diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/modules/mavlink/mavlink_timesync.h index 3204696f42..a3e8295ae0 100644 --- a/src/modules/mavlink/mavlink_timesync.h +++ b/src/modules/mavlink/mavlink_timesync.h @@ -42,55 +42,7 @@ #include "mavlink_bridge_header.h" -#include -#include - -#include - -#include -#include - -using namespace time_literals; - -static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL; - -// Filter gains -// -// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead -// to a smoother estimate, but track time drift more slowly, introducing a bias -// in the estimate. Larger values will cause low-amplitude oscillations. -// -// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a -// tighter estimation of the skew (derivative), but will negatively affect how fast the -// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). -// Larger values will cause large-amplitude oscillations. -static constexpr double ALPHA_GAIN_INITIAL = 0.05; -static constexpr double BETA_GAIN_INITIAL = 0.05; -static constexpr double ALPHA_GAIN_FINAL = 0.003; -static constexpr double BETA_GAIN_FINAL = 0.003; - -// Filter gain scheduling -// -// The filter interpolates between the INITIAL and FINAL gains while the number of -// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will -// allow the timesync to converge faster, but with potentially less accurate initial -// offset and skew estimates. -static constexpr uint32_t CONVERGENCE_WINDOW = 500; - -// Outlier rejection and filter reset -// -// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. -// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning -// but not reset the filter. -// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current -// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number -// of such events in a row will reset the filter. This usually happens only due to a time jump -// on the remote system. -// TODO : automatically determine these using ping statistics? -static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms; -static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms; -static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5; -static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5; +#include class Mavlink; @@ -106,47 +58,9 @@ public: * Convert remote timestamp to local hrt time (usec) * Use synchronised time if available, monotonic boot time otherwise */ - uint64_t sync_stamp(uint64_t usec); + uint64_t sync_stamp(uint64_t usec) { return _timesync.sync_stamp(usec); } private: - - /* do not allow top copying this class */ - MavlinkTimesync(MavlinkTimesync &); - MavlinkTimesync &operator = (const MavlinkTimesync &); - -protected: - - /** - * Online exponential filter to smooth time offset - */ - void add_sample(int64_t offset_us); - - /** - * Return true if the timesync algorithm converged to a good estimate, - * return false otherwise - */ - bool sync_converged(); - - /** - * Reset the exponential filter and its states - */ - void reset_filter(); - - uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; - - uint32_t _sequence{0}; - - // Timesync statistics - double _time_offset{0}; - double _time_skew{0}; - - // Filter parameters - double _filter_alpha{ALPHA_GAIN_INITIAL}; - double _filter_beta{BETA_GAIN_INITIAL}; - - // Outlier rejection and filter reset - uint32_t _high_deviation_count{0}; - uint32_t _high_rtt_count{0}; - Mavlink *const _mavlink; + Timesync _timesync{}; }; diff --git a/src/modules/microdds_client/CMakeLists.txt b/src/modules/microdds_client/CMakeLists.txt index a3bad1a701..8860267f79 100644 --- a/src/modules/microdds_client/CMakeLists.txt +++ b/src/modules/microdds_client/CMakeLists.txt @@ -31,89 +31,125 @@ # ############################################################################ -px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client") +if(${CMAKE_VERSION} VERSION_LESS "3.11") + message(WARNING "skipping microdds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}") -# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient -if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$") - set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE}) -elseif(CMAKE_TOOLCHAIN_FILE) - set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake) -endif() +else() + px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client") -# Include NuttX headers -get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) -set(c_flags_with_includes "${CMAKE_C_FLAGS}") -set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}") -foreach(dir ${include_dirs}) - set(c_flags_with_includes "${c_flags_with_includes} -I${dir}") - set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}") -endforeach() + # If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient + if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$") + set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE}) + elseif(CMAKE_TOOLCHAIN_FILE) + set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake) + endif() -set(lib_dir ${CMAKE_INSTALL_LIBDIR}) -if("${lib_dir}" STREQUAL "") - set(lib_dir "lib") -endif() + # Include NuttX headers + get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) + set(c_flags_with_includes "${CMAKE_C_FLAGS}") + set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}") + foreach(dir ${include_dirs}) + set(c_flags_with_includes "${c_flags_with_includes} -I${dir}") + set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}") + endforeach() -set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client) -set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR}) + set(lib_dir ${CMAKE_INSTALL_LIBDIR}) + if("${lib_dir}" STREQUAL "") + set(lib_dir "lib") + endif() -include(ExternalProject) + set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client) + set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR}) -ExternalProject_Add( - libmicroxrceddsclient_project - PREFIX ${microxrceddsclient_build_dir} - SOURCE_DIR ${microxrceddsclient_src_dir} - INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR} - CMAKE_CACHE_ARGS - -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} - -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} - -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} - -DCMAKE_C_FLAGS:STRING=${c_flags_with_includes} - -DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes} - -DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS} - -DUCLIENT_PIC:BOOL=OFF - -DUCLIENT_PROFILE_TCP:BOOL=OFF - -DUCLIENT_PROFILE_UDP:BOOL=ON - -DUCLIENT_PROFILE_SERIAL:BOOL=ON - -DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF - -DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=ON - -DUCLIENT_PLATFORM_POSIX:BOOL=ON - -DUCLIENT_BUILD_MICROCDR:BOOL=ON - -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR} - -DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR} - -DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain} - BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a - BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a - DEPENDS prebuild_targets git_micro_xrce_dds_client - ) + include(ExternalProject) -add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL) -set_target_properties(libmicroxrceddsclient - PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a -) - -add_library(libmicrocdr STATIC IMPORTED GLOBAL) -set_target_properties(libmicrocdr - PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a -) - -add_library(microxrceddsclient INTERFACE) -add_dependencies(microxrceddsclient libmicroxrceddsclient) -add_dependencies(microxrceddsclient libmicrocdr) -add_dependencies(microxrceddsclient libmicroxrceddsclient_project) -target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include) - -px4_add_module( - MODULE modules__microdds_client - MAIN microdds_client - STACK_MAIN 8000 - SRCS - microdds_client.cpp - DEPENDS - microxrceddsclient - libmicroxrceddsclient - libmicrocdr - uorb_ucdr_headers + ExternalProject_Add( + libmicroxrceddsclient_project + PREFIX ${microxrceddsclient_build_dir} + SOURCE_DIR ${microxrceddsclient_src_dir} + INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR} + CMAKE_CACHE_ARGS + -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} + -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} + -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} + -DCMAKE_C_FLAGS:STRING=${c_flags_with_includes} + -DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes} + -DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS} + -DUCLIENT_PIC:BOOL=OFF + -DUCLIENT_PROFILE_TCP:BOOL=OFF + -DUCLIENT_PROFILE_UDP:BOOL=ON + -DUCLIENT_PROFILE_SERIAL:BOOL=ON + -DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF + -DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=OFF + -DUCLIENT_PROFILE_MULTITHREAD:BOOL=OFF + -DUCLIENT_PROFILE_SHARED_MEMORY:BOOL=OFF + -DUCLIENT_PLATFORM_POSIX:BOOL=ON + -DUCLIENT_BUILD_MICROCDR:BOOL=ON + -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR} + -DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR} + -DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain} + BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a + BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a + DEPENDS prebuild_targets git_micro_xrce_dds_client + LOG_CONFIGURE true + LOG_BUILD true + LOG_INSTALL true + LOG_OUTPUT_ON_FAILURE true ) -add_dependencies(modules__microdds_client topic_bridge_files) + add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL) + set_target_properties(libmicroxrceddsclient + PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a + ) + + add_library(libmicrocdr STATIC IMPORTED GLOBAL) + set_target_properties(libmicrocdr + PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a + ) + + add_library(microxrceddsclient INTERFACE) + add_dependencies(microxrceddsclient libmicroxrceddsclient) + add_dependencies(microxrceddsclient libmicrocdr) + add_dependencies(microxrceddsclient libmicroxrceddsclient_project) + target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include) + + + + add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py + --topic-msg-dir ${PX4_SOURCE_DIR}/msg + --client-outdir ${CMAKE_CURRENT_BINARY_DIR} + --dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml + --template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em + DEPENDS + ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py + ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml + ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em + COMMENT "Generating XRCE-DDS topic bridge" + ) + add_custom_target(topic_bridge_files DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h) + + px4_add_module( + MODULE modules__microdds_client + MAIN microdds_client + STACK_MAIN 9000 + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} + COMPILE_FLAGS + #-O0 + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h + microdds_client.cpp + microdds_client.h + DEPENDS + microxrceddsclient + libmicroxrceddsclient + libmicrocdr + timesync + topic_bridge_files + uorb_ucdr_headers + MODULE_CONFIG + module.yaml + ) +endif() diff --git a/src/modules/microdds_client/Micro-XRCE-DDS-Client b/src/modules/microdds_client/Micro-XRCE-DDS-Client index b5187a9f39..4248559f3b 160000 --- a/src/modules/microdds_client/Micro-XRCE-DDS-Client +++ b/src/modules/microdds_client/Micro-XRCE-DDS-Client @@ -1 +1 @@ -Subproject commit b5187a9f399e34cda7d2cdce8823295f83d9f3cc +Subproject commit 4248559f3b111155c783e524e461ccc83e768103 diff --git a/src/modules/microdds_client/dds_topics.h.em b/src/modules/microdds_client/dds_topics.h.em new file mode 100644 index 0000000000..bde8b8d8cb --- /dev/null +++ b/src/modules/microdds_client/dds_topics.h.em @@ -0,0 +1,127 @@ +@############################################### +@# +@# EmPy template +@# +@############################################### +@# Generates publications and subscriptions for XRCE +@# +@# Context: +@# - msgs (List) list of all RTPS messages +@# - topics (List) list of all topic names +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@############################################### +@{ +import os + + +}@ + +#include + +#include +#include + +#include +#include +@[for include in type_includes]@ +#include +@[end for]@ + +// Subscribers for messages to send +struct SendTopicsSubs { +@[ for pub in publications]@ + uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))}; + uxrObjectId @(pub['topic_simple'])_data_writer{}; +@[ end for]@ + + uint32_t num_payload_sent{}; + + void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace); +}; + +void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace) +{ + int64_t time_offset_us = session->time_offset / 1000; // ns -> us +@[ for idx, pub in enumerate(publications)]@ + + { + @(pub['simple_base_type'])_s data; + + if (@(pub['topic_simple'])_sub.update(&data)) { + + if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { + // data writer not created yet + create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); + } + + if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { + + ucdrBuffer ub; + uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); + if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { + ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); + // TODO: fill up the MTU and then flush, which reduces the packet overhead + uxr_flash_output_streams(session); + num_payload_sent += topic_size; + + } else { + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); + } + + } else { + //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); + } + + } + } +@[ end for]@ +} + +// Publishers for received messages +struct RcvTopicsPubs { +@[ for sub in subscriptions]@ + uORB::Publication<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))}; +@[ end for]@ + + uint32_t num_payload_received{}; + + bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace); +}; + +static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, + struct ucdrBuffer *ub, uint16_t length, void *args) +{ + RcvTopicsPubs *pubs = (RcvTopicsPubs *)args; + const int64_t time_offset_us = session->time_offset / 1000; // ns -> us + pubs->num_payload_received += length; + + switch (object_id.id) { +@[ for idx, sub in enumerate(subscriptions)]@ + case @(idx)+1000: { + @(sub['simple_base_type'])_s data; + + if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) { + //print_message(ORB_ID(@(sub['simple_base_type'])), data); + pubs->@(sub['topic_simple'])_pub.publish(data); + } + } + break; + +@[ end for]@ + + default: + PX4_ERR("unknown object id: %i", object_id.id); + break; + } +} + +bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace) +{ +@[ for idx, sub in enumerate(subscriptions)]@ + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])"); +@[ end for]@ + + uxr_set_topic_callback(session, on_topic_update, this); + + return true; +} diff --git a/src/modules/microdds_client/dds_topics.yaml b/src/modules/microdds_client/dds_topics.yaml new file mode 100644 index 0000000000..3a871848bb --- /dev/null +++ b/src/modules/microdds_client/dds_topics.yaml @@ -0,0 +1,86 @@ +##### +# +# This file maps all the topics that are to be used on the micro DDS client. +# +##### +publications: + + - topic: /fmu/out/collision_constraints + type: px4_msgs::msg::CollisionConstraints + + - topic: /fmu/out/failsafe_flags + type: px4_msgs::msg::FailsafeFlags + + - topic: /fmu/out/sensor_combined + type: px4_msgs::msg::SensorCombined + + - topic: /fmu/out/timesync_status + type: px4_msgs::msg::TimesyncStatus + + # - topic: /fmu/out/vehicle_angular_velocity + # type: px4_msgs::msg::VehicleAngularVelocity + + - topic: /fmu/out/vehicle_attitude + type: px4_msgs::msg::VehicleAttitude + + - topic: /fmu/out/vehicle_control_mode + type: px4_msgs::msg::VehicleControlMode + + - topic: /fmu/out/vehicle_global_position + type: px4_msgs::msg::VehicleGlobalPosition + + - topic: /fmu/out/vehicle_gps_position + type: px4_msgs::msg::SensorGps + + - topic: /fmu/out/vehicle_local_position + type: px4_msgs::msg::VehicleLocalPosition + + - topic: /fmu/out/vehicle_odometry + type: px4_msgs::msg::VehicleOdometry + + - topic: /fmu/out/vehicle_status + type: px4_msgs::msg::VehicleStatus + + - topic: /fmu/out/vehicle_trajectory_waypoint_desired + type: px4_msgs::msg::VehicleTrajectoryWaypoint + +subscriptions: + + - topic: /fmu/in/offboard_control_mode + type: px4_msgs::msg::OffboardControlMode + + - topic: /fmu/in/onboard_computer_status + type: px4_msgs::msg::OnboardComputerStatus + + - topic: /fmu/in/obstacle_distance + type: px4_msgs::msg::ObstacleDistance + + - topic: /fmu/in/sensor_optical_flow + type: px4_msgs::msg::SensorOpticalFlow + + - topic: /fmu/in/telemetry_status + type: px4_msgs::msg::TelemetryStatus + + - topic: /fmu/in/trajectory_setpoint + type: px4_msgs::msg::TrajectorySetpoint + + - topic: /fmu/in/vehicle_attitude_setpoint + type: px4_msgs::msg::VehicleAttitudeSetpoint + + - topic: /fmu/in/vehicle_mocap_odometry + type: px4_msgs::msg::VehicleOdometry + + - topic: /fmu/in/vehicle_rates_setpoint + type: px4_msgs::msg::VehicleRatesSetpoint + + - topic: /fmu/in/vehicle_visual_odometry + type: px4_msgs::msg::VehicleOdometry + + - topic: /fmu/in/vehicle_command + type: px4_msgs::msg::VehicleCommand + + - topic: /fmu/in/vehicle_trajectory_bezier + type: px4_msgs::msg::VehicleTrajectoryBezier + + - topic: /fmu/in/vehicle_trajectory_waypoint + type: px4_msgs::msg::VehicleTrajectoryWaypoint diff --git a/src/modules/microdds_client/generate_dds_topics.py b/src/modules/microdds_client/generate_dds_topics.py new file mode 100644 index 0000000000..8018fe22d0 --- /dev/null +++ b/src/modules/microdds_client/generate_dds_topics.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +################################################################################ +# +# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +################################################################################ + +# This script can generate the client code based on a set of topics +# to sent and set to receive. + +import sys +import os +import argparse +import re +import em +import yaml + +parser = argparse.ArgumentParser() +parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, + help="Topics message, by default using relative path 'msg/'", default="msg") + +parser.add_argument("-y", "--dds-topics-file", dest='yaml_file', type=str, + help="Setup topics file path, by default using 'dds_topics.yaml'") + +parser.add_argument("-t", "--template_file", dest='template_file', type=str, + help="DDS topics template file") + +parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str, + help="Client output dir, by default using relative path 'src/modules/microdds_client'", default=None) + +if len(sys.argv) <= 1: + parser.print_usage() + exit(-1) + +# Parse arguments +args = parser.parse_args() + +# Client files output path +client_out_dir = os.path.abspath(args.clientdir) +template_file = os.path.join(args.template_file) + +# Make sure output directory exists: +if not os.path.isdir(client_out_dir): + os.makedirs(client_out_dir) + +output_file = os.path.join(client_out_dir, os.path.basename(template_file).replace(".em", "")) +folder_name = os.path.dirname(output_file) + +if not os.path.exists(folder_name): + os.makedirs(folder_name) + + + +# open yaml file to load dictionary of publications and subscriptions +with open(args.yaml_file, 'r') as file: + msg_map = yaml.safe_load(file) + +merged_em_globals = {} +all_type_includes = [] + +for p in msg_map['publications']: + # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint + simple_base_type = p['type'].split('::')[-1] + + # eg TrajectoryWaypoint -> trajectory_waypoint + base_type_name_snake_case = re.sub(r'(? trajectory_waypoint + base_type_name_snake_case = re.sub(r'(? #include -#include #include "microdds_client.h" @@ -46,27 +45,68 @@ #include #include +#if defined(CONFIG_NET) || defined(__PX4_POSIX) +# define MICRODDS_CLIENT_UDP 1 +#endif + #define STREAM_HISTORY 4 #define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default using namespace time_literals; +void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp, + int64_t originate_timestamp, void *args) +{ + // latest round trip time (RTT) + int64_t rtt = current_time - originate_timestamp; + + // HRT to AGENT + int64_t offset_1 = (received_timestamp - originate_timestamp) - (rtt / 2); + int64_t offset_2 = (transmit_timestamp - current_time) - (rtt / 2); + + session->time_offset = (offset_1 + offset_2) / 2; + + if (args) { + Timesync *timesync = static_cast(args); + timesync->update(current_time / 1000, transmit_timestamp, originate_timestamp); + + //fprintf(stderr, "time_offset: %ld, timesync: %ld, diff: %ld\n", session->time_offset/1000, timesync->offset(), session->time_offset/1000 + timesync->offset()); + + session->time_offset = -timesync->offset() * 1000; // us -> ns + } +} + MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, - const char *port, bool localhost_only) - : _localhost_only(localhost_only) + const char *port, bool localhost_only, const char *client_namespace) : + ModuleParams(nullptr), + _localhost_only(localhost_only), + _client_namespace(client_namespace) { if (transport == Transport::Serial) { - int fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + int fd = -1; - if (fd < 0) { - PX4_ERR("open %s failed (%i)", device, errno); + for (int attempt = 0; attempt < 3; attempt++) { + fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (fd < 0) { + PX4_ERR("open %s failed (%i)", device, errno); + // sleep before trying again + usleep(1'000'000); + + } else { + break; + } } _transport_serial = new uxrSerialTransport(); if (fd >= 0 && setBaudrate(fd, baudrate) == 0 && _transport_serial) { - if (uxr_init_serial_transport(_transport_serial, fd, 0, 1)) { + // TODO: + uint8_t remote_addr = 0; // Identifier of the Agent in the connection + uint8_t local_addr = 1; // Identifier of the Client in the serial connection + + if (uxr_init_serial_transport(_transport_serial, fd, remote_addr, local_addr)) { _comm = &_transport_serial->comm; _fd = fd; @@ -77,7 +117,7 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud } else if (transport == Transport::Udp) { -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MICRODDS_CLIENT_UDP) _transport_udp = new uxrUDPTransport(); if (_transport_udp) { @@ -127,8 +167,6 @@ void MicroddsClient::run() return; } - int polling_topic_sub = orb_subscribe(ORB_ID(vehicle_imu)); - while (!should_exit()) { bool got_response = false; @@ -142,29 +180,48 @@ void MicroddsClient::run() } // Session + // The key identifier of the Client. All Clients connected to an Agent must have a different key. + const uint32_t key = 0xAAAABBBB; uxrSession session; - uxr_init_session(&session, _comm, 0xAAAABBBB); + uxr_init_session(&session, _comm, key); + // void uxr_create_session_retries(uxrSession* session, size_t retries); if (!uxr_create_session(&session)) { PX4_ERR("uxr_create_session failed"); return; } + // TODO: uxr_set_status_callback + // Streams // Reliable for setup, afterwards best-effort to send the data (important: need to create all 4 streams) uint8_t output_reliable_stream_buffer[BUFFER_SIZE] {}; - uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE, - STREAM_HISTORY); - uint8_t output_data_stream_buffer[1024] {}; - uxrStreamId data_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer, - sizeof(output_data_stream_buffer)); + uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, + sizeof(output_reliable_stream_buffer), STREAM_HISTORY); + + uint8_t output_data_stream_buffer[2048] {}; + uxrStreamId best_effort_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer, + sizeof(output_data_stream_buffer)); uint8_t input_reliable_stream_buffer[BUFFER_SIZE] {}; - uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY); - uxrStreamId input_stream = uxr_create_input_best_effort_stream(&session); + uxrStreamId reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, + sizeof(input_reliable_stream_buffer), + STREAM_HISTORY); + (void)reliable_in; + + uxrStreamId best_effort_in = uxr_create_input_best_effort_stream(&session); + (void)best_effort_in; // Create entities uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID); + + uint16_t domain_id = _param_xrce_dds_dom_id.get(); + + // const char *participant_name = "px4_micro_xrce_dds"; + // uint16_t participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, domain_id, + // participant_name, UXR_REPLACE); + + // TODO: configurable participant name with client namespace? const char *participant_xml = _localhost_only ? "" "" @@ -178,7 +235,7 @@ void MicroddsClient::run() "" "" "" - "default_xrce_participant" + "px4_micro_xrce_dds" "false" "udp_localhost" "" @@ -188,11 +245,11 @@ void MicroddsClient::run() "" "" "" - "default_xrce_participant" + "px4_micro_xrce_dds" "" "" "" ; - uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0, + uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id, participant_xml, UXR_REPLACE); uint8_t request_status; @@ -202,69 +259,60 @@ void MicroddsClient::run() return; } - if (!_subs->init(&session, reliable_out, participant_id)) { - PX4_ERR("subs init failed"); - return; - } - - if (!_pubs->init(&session, reliable_out, input_stream, participant_id)) { + if (!_pubs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) { PX4_ERR("pubs init failed"); return; } _connected = true; + // Set time-callback. + uxr_set_time_callback(&session, on_time, &_timesync); + + // Synchronize with the Agent + bool synchronized = false; + + while (!synchronized) { + synchronized = uxr_sync_session(&session, 1000); + + if (synchronized) { + PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000); + //sleep(1); + + } else { + usleep(10000); + } + } + + hrt_abstime last_sync_session = 0; hrt_abstime last_status_update = hrt_absolute_time(); hrt_abstime last_ping = hrt_absolute_time(); int num_pings_missed = 0; bool had_ping_reply = false; uint32_t last_num_payload_sent{}; uint32_t last_num_payload_received{}; - bool error_printed = false; - hrt_abstime last_read = hrt_absolute_time(); while (!should_exit() && _connected) { - px4_pollfd_struct_t fds[1]; - fds[0].fd = polling_topic_sub; - fds[0].events = POLLIN; - // we could poll on the uart/udp fd as well (on nuttx) - int pret = px4_poll(fds, 1, 20); - if (pret < 0) { - if (!error_printed) { - PX4_ERR("poll failed (%i)", pret); - error_printed = true; - } + _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); - } else if (pret != 0) { - if (fds[0].revents & POLLIN) { - vehicle_imu_s data; - orb_copy(ORB_ID(vehicle_imu), polling_topic_sub, &data); + uxr_run_session_timeout(&session, 0); + + // time sync session + if (hrt_elapsed_time(&last_sync_session) > 1_s) { + if (uxr_sync_session(&session, 100)) { + //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); + last_sync_session = hrt_absolute_time(); } } - _subs->update(data_out); - - hrt_abstime read_start = hrt_absolute_time(); - - if (read_start - last_read > 5_ms) { - last_read = read_start; - - // Read as long as there's data or until a timeout - pollfd fd_read; - fd_read.fd = _fd; - fd_read.events = POLLIN; - - do { - uxr_run_session_timeout(&session, 0); - - if (session.on_pong_flag == 1 /* PONG_IN_SESSION_STATUS */) { // Check for a ping response - had_ping_reply = true; - } - } while (poll(&fd_read, 1, 0) > 0 && hrt_absolute_time() - read_start < 2_ms); + // Check for a ping response + /* PONG_IN_SESSION_STATUS */ + if (session.on_pong_flag == 1) { + had_ping_reply = true; } - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); if (now - last_status_update > 1_s) { float dt = (now - last_status_update) / 1e6f; @@ -287,6 +335,7 @@ void MicroddsClient::run() } uxr_ping_agent_session(&session, 0, 1); + had_ping_reply = false; } @@ -294,14 +343,14 @@ void MicroddsClient::run() PX4_INFO("No ping response, disconnecting"); _connected = false; } + + px4_usleep(1000); } uxr_delete_session_retries(&session, _connected ? 1 : 0); _last_payload_tx_rate = 0; _last_payload_tx_rate = 0; } - - orb_unsubscribe(polling_topic_sub); } int MicroddsClient::setBaudrate(int fd, unsigned baud) @@ -333,6 +382,48 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud) case 921600: speed = B921600; break; +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + +#ifndef B2000000 +#define B2000000 2000000 +#endif + + case 2000000: speed = B2000000; break; + +#ifndef B2500000 +#define B2500000 2500000 +#endif + + case 2500000: speed = B2500000; break; + +#ifndef B3000000 +#define B3000000 3000000 +#endif + + case 3000000: speed = B3000000; break; + +#ifndef B3500000 +#define B3500000 3500000 +#endif + + case 3500000: speed = B3500000; break; + +#ifndef B4000000 +#define B4000000 4000000 +#endif + + case 4000000: speed = B4000000; break; + default: PX4_ERR("ERR: unknown baudrate: %d", baud); return -EINVAL; @@ -399,12 +490,6 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud) return 0; } - -int microdds_client_main(int argc, char *argv[]) -{ - return MicroddsClient::main(argc, argv); -} - int MicroddsClient::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); @@ -414,8 +499,8 @@ int MicroddsClient::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("microdds_client", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 4, - PX4_STACK_ADJUSTED(8000), + SCHED_PRIORITY_DEFAULT, + PX4_STACK_ADJUSTED(10000), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -442,14 +527,22 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; +#if defined(MICRODDS_CLIENT_UDP) Transport transport = Transport::Udp; - const char *device = nullptr; - const char *ip = "127.0.0.1"; - int baudrate = 921600; - const char *port = "15555"; - bool localhost_only = false; - while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l", &myoptind, &myoptarg)) != EOF) { +#else + Transport transport = Transport::Serial; +#endif + const char *device = nullptr; + int baudrate = 921600; + + const char *port = "8888"; + bool localhost_only = false; + const char *ip = "127.0.0.1"; + + const char *client_namespace = nullptr;//"px4"; + + while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 't': if (!strcmp(myoptarg, "serial")) { @@ -477,6 +570,8 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) break; +#if defined(MICRODDS_CLIENT_UDP) + case 'h': ip = myoptarg; break; @@ -488,6 +583,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) case 'l': localhost_only = true; break; +#endif // MICRODDS_CLIENT_UDP + + case 'n': + client_namespace = myoptarg; + break; case '?': error_flag = true; @@ -511,7 +611,7 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) } } - return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only); + return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, client_namespace); } int MicroddsClient::print_usage(const char *reason) @@ -536,9 +636,15 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555 PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "serial device", true); PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:)", true); PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "", "Host IP", true); - PRINT_MODULE_USAGE_PARAM_INT('p', 15555, 0, 3000000, "Remote Port", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true); PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true); + PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } + +extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]) +{ + return MicroddsClient::main(argc, argv); +} diff --git a/src/modules/microdds_client/microdds_client.h b/src/modules/microdds_client/microdds_client.h index 5a770c37ee..7801ddb9d7 100644 --- a/src/modules/microdds_client/microdds_client.h +++ b/src/modules/microdds_client/microdds_client.h @@ -34,13 +34,13 @@ #pragma once #include +#include -#include +#include -extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]); +#include - -class MicroddsClient : public ModuleBase +class MicroddsClient : public ModuleBase, public ModuleParams { public: enum class Transport { @@ -49,7 +49,7 @@ public: }; MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port, - bool localhost_only); + bool localhost_only, const char *client_namespace); ~MicroddsClient(); @@ -75,6 +75,7 @@ private: int setBaudrate(int fd, unsigned baud); const bool _localhost_only; + const char *_client_namespace; SendTopicsSubs *_subs{nullptr}; RcvTopicsPubs *_pubs{nullptr}; @@ -87,5 +88,11 @@ private: int _last_payload_tx_rate{}; ///< in B/s int _last_payload_rx_rate{}; ///< in B/s bool _connected{false}; + + Timesync _timesync{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_xrce_dds_dom_id + ) }; diff --git a/src/modules/microdds_client/module.yaml b/src/modules/microdds_client/module.yaml new file mode 100644 index 0000000000..1c5bf77856 --- /dev/null +++ b/src/modules/microdds_client/module.yaml @@ -0,0 +1,56 @@ + + +# parameters to auto start +# mode (normal, minimal) +# UDP port +# max rate +# DDS DOMAIN ID +# + + +# multiple instances? + + +module_name: Micro XRCE-DDS +serial_config: + - command: | + if [ $SERIAL_DEV != ethernet ]; then + set XRCE_DDS_ARGS "-t serial -d ${SERIAL_DEV} -b p:${BAUD_PARAM}" + else + set XRCE_DDS_ARGS "-t udp" + fi + microdds_client start ${XRCE_DDS_ARGS} + + port_config_param: + name: XRCE_DDS_${i}_CFG + group: Micro XRCE-DDS + # MAVLink instances: + # 0: Telem1 Port (Telemetry Link) + # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage + # 2: Board-specific / no fixed function or port + #default: [TEL1, "", ""] + supports_networking: true + +parameters: + - group: Micro XRCE-DDS + definitions: + + XRCE_DDS_DOM_ID: + description: + short: XRCE DDS domain ID + long: XRCE DDS domain ID + category: System + type: int32 + reboot_required: true + default: 0 + + XRCE_DDS_UDP_PRT: + description: + short: Micro DDS UDP Port + long: | + If ethernet enabled and selected as configuration for micro DDS, + selected udp port will be set and used. + type: int32 + reboot_required: true + default: 8888 + requires_ethernet: true diff --git a/src/modules/microdds_client/utilities.hpp b/src/modules/microdds_client/utilities.hpp new file mode 100644 index 0000000000..6244eb1adf --- /dev/null +++ b/src/modules/microdds_client/utilities.hpp @@ -0,0 +1,138 @@ + +#pragma once + +#include +#include + +#include + +#define TOPIC_NAME_SIZE 128 + +uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) +{ + if (orb_id != ORB_ID::INVALID) { + uint16_t id = static_cast(orb_id) + (instance * UINT8_MAX); + uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID); + return topic_id; + } + + return uxrObjectId{}; +} + +static bool generate_topic_name(char *topic, const char *client_namespace, const char *direction, const char *name) +{ + if (client_namespace != nullptr) { + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); + } + + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); +} + +static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id, + ORB_ID orb_id, const char *client_namespace, const char *topic_name_simple, const char *type_name, + uxrObjectId &datawriter_id) +{ + // topic + char topic_name[TOPIC_NAME_SIZE]; + + if (!generate_topic_name(topic_name, client_namespace, "out", topic_name_simple)) { + PX4_ERR("topic path too long"); + return false; + } + + uxrObjectId topic_id = topic_id_from_orb(orb_id); + uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name, + type_name, UXR_REPLACE); + + + // publisher + uxrObjectId publisher_id = uxr_object_id(topic_id.id, UXR_PUBLISHER_ID); + uint16_t publisher_req = uxr_buffer_create_publisher_bin(session, reliable_out_stream_id, publisher_id, participant_id, + UXR_REPLACE); + + + // data writer + datawriter_id = uxr_object_id(topic_id.id, UXR_DATAWRITER_ID); + + uxrQoS_t qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 0, + }; + + uint16_t datawriter_req = uxr_buffer_create_datawriter_bin(session, reliable_out_stream_id, datawriter_id, publisher_id, + topic_id, qos, UXR_REPLACE); + + // Send create entities message and wait its status + uint16_t requests[3] {topic_req, publisher_req, datawriter_req}; + uint8_t status[3]; + + if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", + topic_name, status[0], status[1], status[2]); + return false; + + } else { + PX4_INFO("successfully created %s data writer, topic id: %d", topic_name, topic_id.id); + } + + return true; +} + +static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, + uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic_name_simple, + const char *type_name) +{ + // topic + char topic_name[TOPIC_NAME_SIZE]; + + if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) { + PX4_ERR("topic path too long"); + return false; + } + + uint16_t id = index + 1000; + + + uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID); + uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name, + type_name, UXR_REPLACE); + + + // subscriber + uxrObjectId subscriber_id = uxr_object_id(id, UXR_SUBSCRIBER_ID); + uint16_t subscriber_req = uxr_buffer_create_subscriber_bin(session, reliable_out_stream_id, subscriber_id, + participant_id, UXR_REPLACE); + + + // data reader + uxrObjectId datareader_id = uxr_object_id(id, UXR_DATAREADER_ID); + + uxrQoS_t qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 0, + }; + + uint16_t datareader_req = uxr_buffer_create_datareader_bin(session, reliable_out_stream_id, datareader_id, + subscriber_id, topic_id, qos, UXR_REPLACE); + + uint16_t requests[3] {topic_req, subscriber_req, datareader_req}; + uint8_t status[3]; + + if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + PX4_ERR("create entities failed: %s %i %i %i", topic_name, + status[0], status[1], status[2]); + return false; + } + + uxrDeliveryControl delivery_control{}; + delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED; + uxr_buffer_request_data(session, reliable_out_stream_id, datareader_id, input_stream_id, &delivery_control); + + return true; +} diff --git a/src/modules/micrortps_bridge/CMakeLists.txt b/src/modules/micrortps_bridge/CMakeLists.txt deleted file mode 100644 index 8af919eb9f..0000000000 --- a/src/modules/micrortps_bridge/CMakeLists.txt +++ /dev/null @@ -1,193 +0,0 @@ -############################################################################ -# -# Copyright (c) 2017 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -#============================================================================= -# RTPS and micro-cdr -# - -find_program(FASTRTPSGEN fastrtpsgen PATHS $ENV{FASTRTPSGEN_DIR}) -if(NOT FASTRTPSGEN) - message(FATAL_ERROR "Unable to find fastrtpsgen") -else() - execute_process( - COMMAND $ENV{FASTRTPSGEN_DIR}fastrtpsgen -version - OUTPUT_VARIABLE FASTRTPSGEN_VERSION - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET - ) - message(STATUS "${FASTRTPSGEN_VERSION}") -endif() - -set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS - ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml -) - -if (EXISTS "${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml") - set(config_rtps_send_topics) - execute_process( - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -sa - OUTPUT_VARIABLE config_rtps_send_topics - ) - set(config_rtps_send_alias_topics "") - string(FIND ${config_rtps_send_topics} "alias" found_send_alias) - if (NOT ${found_send_alias} EQUAL "-1") - STRING(REGEX REPLACE ".*alias " "" config_rtps_send_alias_topics "${config_rtps_send_topics}") - STRING(REPLACE ", " ";" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}") - STRING(REPLACE "\n" "" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}") - STRING(REGEX REPLACE " alias.*" "" config_rtps_send_topics "${config_rtps_send_topics}") - endif() - STRING(REGEX REPLACE ", " ";" config_rtps_send_topics "${config_rtps_send_topics}") - STRING(REGEX REPLACE "\n" "" config_rtps_send_topics "${config_rtps_send_topics}") - - set(config_rtps_receive_topics) - execute_process( - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -ra - OUTPUT_VARIABLE config_rtps_receive_topics - ) - set(config_rtps_receive_alias_topics "") - string(FIND ${config_rtps_receive_topics} "alias" found_receive_alias) - if (NOT ${found_receive_alias} EQUAL "-1") - STRING(REGEX REPLACE ".*alias " "" config_rtps_receive_alias_topics "${config_rtps_receive_topics}") - STRING(REPLACE ", " ";" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}") - STRING(REPLACE "\n" "" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}") - STRING(REGEX REPLACE " alias.*" "" config_rtps_receive_topics "${config_rtps_receive_topics}") - endif() - STRING(REPLACE ", " ";" config_rtps_receive_topics "${config_rtps_receive_topics}") - STRING(REPLACE "\n" "" config_rtps_receive_topics "${config_rtps_receive_topics}") -endif() - -if (FASTRTPSGEN AND (config_rtps_send_topics OR config_rtps_receive_topics)) - option(GENERATE_RTPS_BRIDGE "enable RTPS and microCDR" ON) -endif() - -if (GENERATE_RTPS_BRIDGE) - add_subdirectory(micrortps_client) - - ############################################################################### - # micro-cdr serialization - ############################################################################### - include(px4_git) - px4_add_git_submodule(TARGET git_micro_cdr PATH micro-CDR) - - set(UCDR_SUPERBUILD CACHE BOOL "Disable micro-CDR superbuild compilation.") - add_subdirectory(micro-CDR) - - set(msg_out_path_microcdr ${PX4_BINARY_DIR}/uORB_microcdr/topics) - set(msg_source_out_path_microcdr ${CMAKE_CURRENT_BINARY_DIR}/topics_microcdr_sources) - - set(uorb_headers_microcdr) - set(uorb_sources_microcdr) - - # send topic files - STRING(REGEX REPLACE ";" ", " send_list "${config_rtps_send_topics};${config_rtps_send_alias_topics}") - message(STATUS "microRTPS bridge:") - message(STATUS " Publish to the bridge from: ${send_list}") - set(send_topic_files) - foreach(topic ${config_rtps_send_topics}) - list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h) - list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp) - endforeach() - - # receive topic files - STRING(REGEX REPLACE ";" ", " rcv_list "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}") - message(STATUS " Subscribe from the bridge to: ${rcv_list}") - set(receive_topic_files) - foreach(topic ${config_rtps_receive_topics}) - list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h) - list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp) - endforeach() - - list(REMOVE_DUPLICATES uorb_headers_microcdr) - list(REMOVE_DUPLICATES uorb_sources_microcdr) - - # Generate uORB serialization headers - add_custom_command(OUTPUT ${uorb_headers_microcdr} - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - --headers - -f ${send_topic_files} ${receive_topic_files} - -i ${PX4_SOURCE_DIR}/msg/ - -o ${msg_out_path_microcdr} - -e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers_microcdr - -q - DEPENDS - ${receive_topic_files} - ${send_topic_files} - ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml - COMMENT "Generating uORB microcdr topic headers" - VERBATIM - ) - add_custom_target(uorb_headers_microcdr_gen DEPENDS ${uorb_headers_microcdr}) - - # Generate uORB serialization sources - add_custom_command(OUTPUT ${uorb_sources_microcdr} - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - --sources - -f ${send_topic_files} ${receive_topic_files} - -i ${PX4_SOURCE_DIR}/msg/ - -o ${msg_source_out_path_microcdr} - -e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources_microcdr - -q - DEPENDS - ${receive_topic_files} - ${send_topic_files} - ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml - COMMENT "Generating uORB microcdr topic sources" - VERBATIM - ) - add_custom_target(uorb_sources_microcdr_gen DEPENDS ${uorb_sources_microcdr}) - - px4_add_library(uorb_msgs_microcdr ${uorb_sources_microcdr} ${uorb_headers_microcdr}) - add_dependencies(uorb_msgs_microcdr - uorb_headers_microcdr_gen - uorb_sources_microcdr_gen - git_micro_cdr - microcdr - ) - add_dependencies(microcdr prebuild_targets) - - # microCDR - target_include_directories(uorb_msgs_microcdr - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/micro-CDR/include - ${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include - ${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include/microcdr - ) - - target_link_libraries(uorb_msgs_microcdr PRIVATE microcdr) -endif() diff --git a/src/modules/micrortps_bridge/Kconfig b/src/modules/micrortps_bridge/Kconfig deleted file mode 100644 index 2262e81f33..0000000000 --- a/src/modules/micrortps_bridge/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_MICRORTPS_BRIDGE - bool "micrortps_bridge" - default n - ---help--- - Enable support for micrortps_bridge - -menuconfig USER_MICRORTPS_BRIDGE - bool "micrortps_bridge running as userspace module" - default y - depends on BOARD_PROTECTED && MODULES_MICRORTPS_BRIDGE - ---help--- - Put micrortps_bridge in userspace memory diff --git a/src/modules/micrortps_bridge/README.md b/src/modules/micrortps_bridge/README.md deleted file mode 100644 index 27750f054c..0000000000 --- a/src/modules/micrortps_bridge/README.md +++ /dev/null @@ -1 +0,0 @@ -For see a complete documentation, please follow this [link](https://docs.px4.io/main/en/middleware/micrortps.html) diff --git a/src/modules/micrortps_bridge/micro-CDR b/src/modules/micrortps_bridge/micro-CDR deleted file mode 160000 index 21d3cfe3ae..0000000000 --- a/src/modules/micrortps_bridge/micro-CDR +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 21d3cfe3ae570d1674da0105ab23b80958e0449a diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt deleted file mode 100644 index 17e2608d91..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt +++ /dev/null @@ -1,120 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -set(msg_out_path ${CMAKE_CURRENT_BINARY_DIR}) -get_filename_component(micrortps_bridge_path ${msg_out_path} PATH) - -option(BUILD_MICRORTPS_AGENT "enable building the micrortps_agent after its generation" OFF) - -if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_topics}" STREQUAL "") - set(send_topic_files) - foreach(topic ${config_rtps_send_topics}) - list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - endforeach() - - set(receive_topic_files) - foreach(topic ${config_rtps_receive_topics}) - list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - endforeach() - - if (NOT "${config_rtps_send_alias_topics}" STREQUAL "") - set(config_rtps_send_topics "${config_rtps_send_topics};${config_rtps_send_alias_topics}") - endif() - - if (NOT "${config_rtps_receive_alias_topics}" STREQUAL "") - set(config_rtps_receive_topics "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}") - endif() - - foreach(topic ${config_rtps_send_topics}) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Publisher.cpp) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Publisher.h) - endforeach() - - foreach(topic ${config_rtps_receive_topics}) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Subscriber.cpp) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Subscriber.h) - endforeach() - - list(APPEND topic_bridge_files_out - ${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.h - ${micrortps_bridge_path}/micrortps_client/dds_topics.h - ) - - add_custom_command(OUTPUT ${topic_bridge_files_out} - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/generate_microRTPS_bridge.py - --fastrtpsgen-dir $ENV{FASTRTPSGEN_DIR} - --generate-idl - --mkdir-build - --generate-cmakelists - --topic-msg-dir ${PX4_SOURCE_DIR}/msg - --uorb-templates-dir templates/uorb_microcdr - --urtps-templates-dir templates/urtps - --agent-outdir ${micrortps_bridge_path}/micrortps_agent/src - --client-outdir ${micrortps_bridge_path}/micrortps_client - --idl-dir ${micrortps_bridge_path}/micrortps_agent/idl - >micrortps_bridge.log 2>&1 || cat micrortps_bridge.log - DEPENDS ${send_topic_files} ${receive_topic_files} ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml - ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr/dds_topics.h.em - COMMENT "Generating RTPS topic bridge" - ) - add_custom_target(topic_bridge_files DEPENDS ${topic_bridge_files_out}) - - px4_add_module( - MODULE modules__micrortps_bridge__micrortps_client - MAIN micrortps_client - STACK_MAIN 4096 - INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR} - ${micrortps_bridge_path}/micrortps_client - SRCS - microRTPS_client_main.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp - MODULE_CONFIG - module.yaml - DEPENDS - topic_bridge_files - ) - target_link_libraries(modules__micrortps_bridge__micrortps_client PRIVATE uorb_msgs_microcdr) - - if (BUILD_MICRORTPS_AGENT) - add_custom_command(TARGET modules__micrortps_bridge__micrortps_client POST_BUILD - COMMAND ${PX4_SOURCE_DIR}/Tools/build_micrortps_agent.sh - WORKING_DIRECTORY ${PX4_SOURCE_DIR} - COMMENT "Building micrortps_agent..." - ) - # add_subdirectory(${micrortps_bridge_path}/micrortps_agent) - endif() -endif() diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h deleted file mode 100644 index 029bb2331d..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#define LOOPS -1 -#define SLEEP_US 1000 -#define MAX_SLEEP_US 1000000 -#define BAUDRATE 460800 -#define MAX_DATA_RATE 10000000 -#define DEVICE "/dev/ttyACM0" -#define POLL_MS 1 -#define MAX_POLL_MS 1000 -#define DEFAULT_IP "127.0.0.1" -#define DEFAULT_RECV_PORT 2019 -#define DEFAULT_SEND_PORT 2020 -#define MIN_TX_INTERVAL_US 1000.f -#define MAX_TX_INTERVAL_US 1000000.f - - -void *send(void *args); -void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd, - uint64_t &total_sent, uint64_t &sent_last_sec, - uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop); - -struct baudtype { - speed_t code; - uint32_t val; -}; - -struct options { - enum class eTransports { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - char ip[16] = DEFAULT_IP; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; - uint32_t sleep_us = SLEEP_US; - uint32_t baudrate = BAUDRATE; - uint32_t datarate = 0; - uint32_t poll_ms = POLL_MS; - int loops = LOOPS; - bool sw_flow_control = false; - bool hw_flow_control = false; - bool verbose_debug = false; -}; - -extern struct options _options; -extern bool _should_exit_task; -extern Transport_node *transport_node; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp deleted file mode 100644 index 1ff41c4682..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ /dev/null @@ -1,330 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); - -static int _rtps_task = -1; -bool _should_exit_task = false; -Transport_node *transport_node = nullptr; -struct options _options; - -struct timespec begin; -struct timespec end; - -uint64_t total_rcvd{0}; -uint64_t total_sent{0}; -uint64_t rcvd_last_sec{0}; -uint64_t sent_last_sec{0}; -uint64_t received{0}; -uint64_t sent{0}; -int rcv_loop{0}; -int send_loop{0}; - -static void usage(const char *name) -{ - PRINT_MODULE_USAGE_NAME("micrortps_client", "communication"); - PRINT_MODULE_USAGE_COMMAND("start"); - - PRINT_MODULE_USAGE_PARAM_STRING('t', "UART", "UART|UDP", "Transport protocol", true); - PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyACM0", "", "Select Serial Device", true); - PRINT_MODULE_USAGE_PARAM_INT('b', 460800, 9600, 3000000, "Baudrate (can also be p:)", true); - PRINT_MODULE_USAGE_PARAM_INT('m', 0, 0, MAX_DATA_RATE, "Maximum sending data rate in B/s (0=not limited)", true); - PRINT_MODULE_USAGE_PARAM_INT('p', 1, 1, MAX_POLL_MS, "Poll timeout for UART in milliseconds", true); - PRINT_MODULE_USAGE_PARAM_INT('l', -1, -1, INT32_MAX, "Limit number of iterations until the program exits (-1=infinite)", - true); - PRINT_MODULE_USAGE_PARAM_INT('w', 1000, 0, MAX_SLEEP_US, - "Iteration time for data publishing to the uORB side, in microseconds", true); - PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true); - PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true); - PRINT_MODULE_USAGE_PARAM_STRING('i', "127.0.0.1", "", "Select IP address (remote)", true); - PRINT_MODULE_USAGE_PARAM_FLAG('f', "Activate UART link SW flow control", true); - PRINT_MODULE_USAGE_PARAM_FLAG('h', "Activate UART link HW flow control", true); - PRINT_MODULE_USAGE_PARAM_FLAG('v', "Add more verbosity", true); - - PRINT_MODULE_USAGE_COMMAND("stop"); - PRINT_MODULE_USAGE_COMMAND("status"); -} - -static int parse_options(int argc, char *argv[]) -{ - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:m:p:r:s:i:fhv", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? - options::eTransports::UDP - : options::eTransports::UART; break; - - case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; - - case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; - - case 'w': _options.sleep_us = strtoul(myoptarg, nullptr, 10); break; - - case 'b': { - int baudrate = 0; - - if (px4_get_parameter_value(myoptarg, baudrate) != 0) { - PX4_ERR("baudrate parsing failed"); - } - - _options.baudrate = baudrate; - - break; - } - - case 'm': { - int datarate = 0; - - if (px4_get_parameter_value(myoptarg, datarate) != 0) { - PX4_ERR("datarate parsing failed"); - } - - _options.datarate = datarate; - - break; - } - - case 'p': _options.poll_ms = strtoul(myoptarg, nullptr, 10); break; - - case 'r': _options.recv_port = strtoul(myoptarg, nullptr, 10); break; - - case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break; - - case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break; - - case 'f': _options.sw_flow_control = true; break; - - case 'h': _options.hw_flow_control = true; break; - - case 'v': _options.verbose_debug = true; break; - - default: - usage(argv[1]); - return -1; - } - } - - if (_options.datarate > MAX_DATA_RATE) { - _options.datarate = MAX_DATA_RATE; - PX4_WARN("Data rate too high. Using max datarate of %d B/s instead", MAX_DATA_RATE); - } - - if (_options.poll_ms < POLL_MS) { - _options.poll_ms = POLL_MS; - PX4_WARN("Poll timeout too low. Using %d ms instead", POLL_MS); - - } else if (_options.poll_ms > MAX_POLL_MS) { - _options.poll_ms = MAX_POLL_MS; - PX4_WARN("Poll timeout too high. Using %d ms instead", MAX_POLL_MS); - } - - if (_options.sleep_us > MAX_SLEEP_US) { - _options.sleep_us = MAX_SLEEP_US; - PX4_WARN("Publishing iteration cycle too slow. Using %d us instead", MAX_SLEEP_US); - } - - if (_options.hw_flow_control && _options.sw_flow_control) { - PX4_ERR("HW and SW flow control set. Please set only one or another"); - return -1; - } - - return 0; -} - -static int micrortps_start(int argc, char *argv[]) -{ - if (0 > parse_options(argc, argv)) { - PX4_INFO("EXITING..."); - _rtps_task = -1; - return -1; - } - - // Set the system ID to FMU, in order to identify the client side - const uint8_t sys_id = static_cast(MicroRtps::System::FMU); - - switch (_options.transport) { - case options::eTransports::UART: { - transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control, _options.hw_flow_control, sys_id, - _options.verbose_debug); - PX4_INFO("UART transport: device: %s; baudrate: %" PRIu32 "; poll: %" PRIu32 "ms; flow_control: %s", - _options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); - } - break; - - case options::eTransports::UDP: { - transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, - sys_id, _options.verbose_debug); - PX4_INFO("UDP transport: ip address: %s; recv port: %" PRIu16 "; send port: %" PRIu16, - _options.ip, _options.recv_port, _options.send_port); - - } - break; - - default: - _rtps_task = -1; - PX4_INFO("EXITING..."); - return -1; - } - - if (0 > transport_node->init()) { - PX4_INFO("EXITING..."); - _rtps_task = -1; - return -1; - } - - micrortps_start_topics(_options.datarate, begin, total_rcvd, total_sent, sent_last_sec, rcvd_last_sec, received, sent, - rcv_loop, - send_loop); - - px4_clock_gettime(CLOCK_REALTIME, &end); - - const double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); - - PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s", - received, rcv_loop, total_rcvd, elapsed_secs, static_cast(total_rcvd / (1e3 * elapsed_secs))); - PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s", - sent, send_loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs)); - - delete transport_node; - - transport_node = nullptr; - - PX4_INFO("Stopped!"); - - fflush(stdout); - - _rtps_task = -1; - - return 0; -} - -int micrortps_client_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(argv[0]); - return -1; - } - - if (!strcmp(argv[1], "start")) { - if (_rtps_task != -1) { - PX4_INFO("Already running"); - return -1; - } - - _rtps_task = px4_task_spawn_cmd("micrortps_client", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - PX4_STACK_ADJUSTED(2900), - (px4_main_t) micrortps_start, - (char *const *)argv); - - if (_rtps_task < 0) { - PX4_WARN("Could not start task"); - _rtps_task = -1; - return -1; - } - - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (_rtps_task == -1) { - PX4_INFO("Not running"); - - } else { - px4_clock_gettime(CLOCK_REALTIME, &end); - - const double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); - - printf("\tup and running for %.03f seconds\n", elapsed_secs); - printf("\tnr. of messages received: %" PRIu64 "\n", received); - printf("\tnr. of messages sent: %" PRIu64 "\n", sent); - printf("\ttotal data read: %" PRIu64 " bytes\n", total_rcvd); - printf("\ttotal data sent: %" PRIu64 " bytes\n", total_sent); - printf("\trates:\n"); - printf("\t rx: %.3f kB/s\n", rcvd_last_sec / 1e3); - printf("\t tx: %.3f kB/s\n", sent_last_sec / 1e3); - printf("\t avg rx: %.3f kB/s\n", total_rcvd / (1e3 * elapsed_secs)); - printf("\t avg tx: %.3f kB/s\n", total_sent / (1e3 * elapsed_secs)); - printf("\t tx rate max:"); - - if (_options.datarate != 0) { - printf(" %.1f kB/s\n", _options.datarate / 1e3); - - } else { - printf(" Unlimited\n"); - } - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (_rtps_task == -1) { - PX4_INFO("Not running"); - return -1; - } - - _should_exit_task = true; - - if (nullptr != transport_node) { transport_node->close(); } - - _rtps_task = -1; - return 0; - } - - usage(argv[0]); - return -1; -} diff --git a/src/modules/micrortps_bridge/micrortps_client/module.yaml b/src/modules/micrortps_bridge/micrortps_client/module.yaml deleted file mode 100644 index 10049eac32..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/module.yaml +++ /dev/null @@ -1,38 +0,0 @@ -module_name: RTPS -serial_config: - - command: | - protocol_splitter start ${SERIAL_DEV} - mavlink start -d /dev/mavlink -b p:${BAUD_PARAM} -m onboard -r 5000 -x - micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1 - port_config_param: - name: RTPS_MAV_CONFIG - group: RTPS - label: MAVLink + FastRTPS - - command: | - micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1 - port_config_param: - name: RTPS_CONFIG - group: RTPS - label: FastRTPS - -parameters: - - group: RTPS - definitions: - RTPS_RATE: - description: - short: Maximum RTPS data rate - long: | - Configure the maximum sending rate for the RTPS streams in Bytes/sec. - If the configured streams exceed the maximum rate, the sending rate of - each stream is automatically decreased. - - 0 is unlimited. Note this can cause reliability issues - if enough RTPS topics are selected that exceed the - serial bus limit. - - type: int32 - min: 0 - unit: B/s - reboot_required: true - default: 0 - diff --git a/src/modules/micrortps_bridge/res/basic_example_flow.png b/src/modules/micrortps_bridge/res/basic_example_flow.png deleted file mode 100644 index a48972b7ad..0000000000 Binary files a/src/modules/micrortps_bridge/res/basic_example_flow.png and /dev/null differ diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake index 56d34b1d5b..37acf693f0 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake @@ -29,8 +29,8 @@ if(parallel_jobs LESS 1) set(parallel_jobs 1) endif() -message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available. - Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}") +# message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available. +# Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}") # project to build sitl_gazebo if necessary px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_gazebo") diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 0054bd702c..fad0874f1e 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -110,7 +110,7 @@ def is_everything_ready(config: Dict[str, str], build_dir: str) -> bool: print("PX4 SITL is not built\n" "run `DONT_RUN=1 " "make px4_sitl gazebo mavsdk_tests` or " - "`DONT_RUN=1 make px4_sitl_rtps gazebo mavsdk_tests`") + "`DONT_RUN=1 make px4_sitl_default gazebo mavsdk_tests`") result = False if config['simulator'] == 'gazebo': if is_running('gzserver'): @@ -127,7 +127,7 @@ def is_everything_ready(config: Dict[str, str], build_dir: str) -> bool: print("Test runner is not built\n" "run `DONT_RUN=1 " "make px4_sitl gazebo mavsdk_tests` or " - "`DONT_RUN=1 make px4_sitl_rtps gazebo mavsdk_tests`") + "`DONT_RUN=1 make px4_sitl_default gazebo mavsdk_tests`") result = False return result