Message versioning and translation for ROS

.github/workflows/ros_translation_node.yml

@@ -0,0 +1,45 @@
+name: ROS translation node tests
+on:
+  push:
+    branches:
+    - 'main'
+  pull_request:
+    branches:
+    - '*'
+defaults:
+  run:
+    shell: bash
+jobs:
+  build_and_test:
+    name: Build and test
+    runs-on: ubuntu-latest
+    container:
+      image: rostooling/setup-ros-docker:ubuntu-jammy-latest
+    steps:
+      - name: Setup ROS 2
+        uses: ros-tooling/[email protected]
+        with:
+          required-ros-distributions: humble
+      - name: Checkout repository
+        uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      # Workaround for https://github.com/actions/runner/issues/2033
+      - name: ownership workaround
+        run: git config --system --add safe.directory '*'
+
+      - name: Check .msg file versioning
+        if: github.event_name == 'pull_request'
+        run: |
+          ./Tools/ci/check_msg_versioning.sh ${{ github.event.pull_request.base.sha }} ${{github.event.pull_request.head.sha}}
+
+      - name: Build and test
+        run: |
+          ros_ws=/ros_ws
+          mkdir -p $ros_ws/src
+          ./Tools/copy_to_ros_ws.sh $ros_ws
+          cd $ros_ws
+          source /opt/ros/humble/setup.sh
+          colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --event-handlers=console_cohesion+
+          ./build/translation_node/translation_node_unit_tests

Tools/astyle/files_to_check_code_style.sh

@@ -8,6 +8,7 @@ if [ $# -gt 0 ]; then
 fi
 
 exec find boards msg src platforms test \
+    -path msg/translation_node -prune -o \
     -path platforms/nuttx/NuttX -prune -o \
     -path platforms/qurt/dspal -prune -o \
     -path src/drivers/ins/vectornav/libvnc -prune -o \

Tools/ci/check_msg_versioning.sh

@@ -0,0 +1,67 @@
+#! /bin/bash
+# Copy a git diff between two commits if msg versioning is added
+
+DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+
+PX4_SRC_DIR="$DIR/.."
+
+BASE_COMMIT="$1"
+HEAD_COMMIT="$2"
+if [ -z "${BASE_COMMIT}" ] || [ -z "${HEAD_COMMIT}" ]
+then
+  echo "Usage: $0 <base_commit> <head_commit>"
+  exit 1
+fi
+
+failed=0
+
+# Iterate git diff files between BASE_COMMIT and HEAD_COMMIT
+modified_files="$(git --no-pager diff --no-color --name-only --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}")"
+all_files="$(     git --no-pager diff --no-color --name-only                 "${BASE_COMMIT}"..."${HEAD_COMMIT}")"
+for file in ${modified_files}
+do
+  if [[ "$file" == msg/versioned/*.msg ]]; then
+    echo "Modified msg file: ${file}"
+    # A modified versioned .msg file requires:
+    # - Incrementing the version
+    # - An old .msg version exists
+    # - A translation header exists and is included
+    diff=$(git --no-pager diff --no-color --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}" -- "${file}")
+    old_version=$(echo "${diff}" | sed -n 's/^-uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p')
+    new_version=$(echo "${diff}" | sed -n 's/^+uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p')
+
+    # Check that the version is incremented
+    if [ -z "${new_version}" ] || [ -z "${old_version}" ]; then
+      echo "ERROR: Missing version update for ${file}"
+      failed=1
+    else
+      if [ $((new_version-old_version)) -ne 1 ]; then
+        echo "ERROR: Version not incremented by +1 for ${file}"
+        failed=1
+      fi
+    fi
+
+    # Check that an old version exists
+    filename=$(basename -- "$file")
+    filename="${filename%.*}"
+    old_version_file="px4_msgs_old/msg/${filename}V${old_version}.msg"
+    if [[ "${all_files}" != *"${old_version_file}"* ]]; then
+      echo "ERROR: Old message version does not exist for ${file} (missing ${old_version_file})"
+      failed=1
+    fi
+
+    # Check that a translation header got added by checking for a modification to all_translations.h
+    # If it is changed, we assume a new header got added too, so we don't explicitly check for that
+    if [[ "${modified_files}" != *"translations/all_translations.h"* ]]; then
+      echo "ERROR: missing modification to translations/all_translations.h"
+      failed=1
+    fi
+  fi
+done
+
+if [ ${failed} -ne 0 ]; then
+  echo ""
+  echo "ERROR: missing message versioning due to changed .msg file(s) (see above)"
+  echo "Check the documentation under msg/translation_node/README.md for how to add a new version"
+  exit 1
+fi

Tools/copy_to_ros_ws.sh

@@ -0,0 +1,34 @@
+#! /bin/bash
+# Copy msgs and the message translation node into a ROS workspace directory
+
+DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+
+PX4_SRC_DIR="$DIR/.."
+
+WS_DIR="$1"
+if [ ! -e "${WS_DIR}" ]
+then
+	echo "Usage: $0 <ros_ws_dir>"
+	exit 1
+fi
+WS_DIR="$WS_DIR"/src
+if [ ! -e "${WS_DIR}" ]
+then
+	echo "'src' directory not found inside ROS workspace (${WS_DIR})"
+	exit 1
+fi
+
+cp -ar "${PX4_SRC_DIR}"/msg/translation_node "${WS_DIR}"
+cp -ar "${PX4_SRC_DIR}"/msg/px4_msgs_old "${WS_DIR}"
+PX4_MSGS_DIR="${WS_DIR}"/px4_msgs
+if [ ! -e "${PX4_MSGS_DIR}" ]
+then
+  git clone https://github.com/PX4/px4_msgs.git "${PX4_MSGS_DIR}"
+  rm -rf "${PX4_MSGS_DIR}"/msg/*.msg
+  rm -rf "${PX4_MSGS_DIR}"/msg/versioned/*.msg
+  rm -rf "${PX4_MSGS_DIR}"/srv/*.srv
+fi
+cp -ar "${PX4_SRC_DIR}"/msg/*.msg "${PX4_MSGS_DIR}"/msg
+mkdir -p "${PX4_MSGS_DIR}"/msg/versioned
+cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg/versioned
+cp -ar "${PX4_SRC_DIR}"/srv/*.srv "${PX4_MSGS_DIR}"/srv

msg/px4_msgs_old/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(px4_msgs_old)
+
+list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+	add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# ##############################################################################
+# Generate ROS messages, ROS2 interfaces and IDL files #
+# ##############################################################################
+
+# get all msg files
+set(MSGS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/msg")
+file(GLOB PX4_MSGS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${MSGS_DIR}/*.msg")
+
+# get all srv files
+set(SRVS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/srv")
+file(GLOB PX4_SRVS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${SRVS_DIR}/*.srv")
+
+
+
+# For the versioned topics, replace the namespace (px4_msgs_old -> px4_msgs) and message type name (<msg>Vx -> <msg>),
+# so that DDS does not reject the subscription/publication due to mismatching type
+# rosidl_typesupport_fastrtps_cpp
+set(rosidl_typesupport_fastrtps_cpp_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp_wrapper.py)
+file(TOUCH ${rosidl_typesupport_fastrtps_cpp_BIN})
+
+# rosidl_typesupport_fastrtps_c
+set(rosidl_typesupport_fastrtps_c_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_c_wrapper.py)
+file(TOUCH ${rosidl_typesupport_fastrtps_c_BIN})
+
+# rosidl_typesupport_introspection_cpp (for cyclonedds)
+set(rosidl_typesupport_introspection_cpp_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_introspection_cpp_wrapper.py)
+file(TOUCH ${rosidl_typesupport_introspection_cpp_BIN})
+
+# Generate introspection typesupport for C and C++ and IDL files
+if(PX4_MSGS)
+	rosidl_generate_interfaces(${PROJECT_NAME}
+			${PX4_MSGS}
+			${PX4_SRVS}
+			DEPENDENCIES builtin_interfaces
+			ADD_LINTER_TESTS
+	)
+endif()
+
+# rosidl_typesupport_fastrtps_cpp
+set(rosidl_typesupport_fastrtps_cpp_orig ${rosidl_typesupport_fastrtps_cpp_DIR})
+string(REPLACE "share/rosidl_typesupport_fastrtps_cpp/cmake" "lib/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp"
+		rosidl_typesupport_fastrtps_cpp_orig ${rosidl_typesupport_fastrtps_cpp_DIR})
+set(original_script_path ${rosidl_typesupport_fastrtps_cpp_orig})
+configure_file(rename_msg_type.py.in ${rosidl_typesupport_fastrtps_cpp_BIN} @ONLY)
+
+# rosidl_typesupport_fastrtps_c
+set(rosidl_typesupport_fastrtps_c_orig ${rosidl_typesupport_fastrtps_c_DIR})
+string(REPLACE "share/rosidl_typesupport_fastrtps_c/cmake" "lib/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c"
+		rosidl_typesupport_fastrtps_c_orig ${rosidl_typesupport_fastrtps_c_DIR})
+set(original_script_path ${rosidl_typesupport_fastrtps_c_orig})
+configure_file(rename_msg_type.py.in ${rosidl_typesupport_fastrtps_c_BIN} @ONLY)
+
+# rosidl_typesupport_introspection_cpp
+set(rosidl_typesupport_introspection_cpp_orig ${rosidl_typesupport_introspection_cpp_DIR})
+string(REPLACE "share/rosidl_typesupport_introspection_cpp/cmake" "lib/rosidl_typesupport_introspection_cpp/rosidl_typesupport_introspection_cpp"
+		rosidl_typesupport_introspection_cpp_orig ${rosidl_typesupport_introspection_cpp_DIR})
+set(original_script_path ${rosidl_typesupport_introspection_cpp_orig})
+configure_file(rename_msg_type.py.in ${rosidl_typesupport_introspection_cpp_BIN} @ONLY)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

msg/px4_msgs_old/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>px4_msgs_old</name>
+  <version>2.0.1</version>
+  <description>Package with the ROS-equivalent of PX4 uORB msgs (old message definitions)</description>
+  <maintainer email="[email protected]">PX4</maintainer>
+  <license>BSD 3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>builtin_interfaces</depend>
+  <depend>ros_environment</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

msg/px4_msgs_old/rename_msg_type.py.in

@@ -0,0 +1,39 @@
+#! /bin/python
+import sys
+import subprocess
+import json
+import os
+import re
+
+original_script = "@original_script_path@"
+args = sys.argv[1:]
+
+json_file = [arg for arg in args if arg.endswith('.json')][0]
+
+proc = subprocess.run(['python3', original_script] + args)
+proc.check_returncode()
+
+def replace_namespace_and_type(content: str):
+    # Replace namespace type
+    content = content.replace('"px4_msgs_old"', '"px4_msgs"')
+    content = content.replace('"px4_msgs_old::msg"', '"px4_msgs::msg"')
+    # Replace versioned type with non-versioned one
+    content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+"', '\\1"', content)
+    # Services
+    content = content.replace('"px4_msgs_old::srv"', '"px4_msgs::srv"')
+    content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Request"', '\\1_Request"', content)
+    content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Response"', '\\1_Response"', content)
+    return content
+
+with open(json_file, 'r') as f:
+    data = json.load(f)
+    output_dir = data['output_dir']
+
+    # Iterate files recursively
+    for root, dirs, files in os.walk(output_dir):
+        for file in files:
+            with open(os.path.join(root, file), 'r+') as f:
+                content = f.read()
+                f.seek(0)
+                f.write(replace_namespace_and_type(content))
+                f.truncate()

msg/translation_node/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.8)
+project(translation_node)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Werror)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(px4_msgs REQUIRED)
+find_package(px4_msgs_old REQUIRED)
+
+if(DEFINED ENV{ROS_DISTRO})
+  set(ROS_DISTRO $ENV{ROS_DISTRO})
+else()
+  set(ROS_DISTRO "rolling")
+endif()
+
+
+add_library(${PROJECT_NAME}_lib
+        src/monitor.cpp
+        src/pub_sub_graph.cpp
+        src/service_graph.cpp
+        src/translations.cpp
+)
+ament_target_dependencies(${PROJECT_NAME}_lib rclcpp px4_msgs px4_msgs_old)
+add_executable(${PROJECT_NAME}_bin
+        src/main.cpp
+)
+target_link_libraries(${PROJECT_NAME}_bin ${PROJECT_NAME}_lib)
+target_include_directories(${PROJECT_NAME}_bin PUBLIC src)
+ament_target_dependencies(${PROJECT_NAME}_bin rclcpp px4_msgs px4_msgs_old)
+install(TARGETS
+        ${PROJECT_NAME}_bin
+        DESTINATION lib/${PROJECT_NAME})
+
+option(DISABLE_SERVICES "Disable services" OFF)
+if(${ROS_DISTRO} STREQUAL "humble")
+  message(WARNING "Disabling services for ROS humble (API is not supported)")
+  target_compile_definitions(${PROJECT_NAME}_lib PRIVATE DISABLE_SERVICES)
+  set(DISABLE_SERVICES ON)
+endif()
+
+if(BUILD_TESTING)
+  find_package(std_msgs REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(rosidl_default_generators REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  set(SRV_FILES
+          test/srv/TestV0.srv
+          test/srv/TestV1.srv
+          test/srv/TestV2.srv
+  )
+  rosidl_generate_interfaces(${PROJECT_NAME} ${SRV_FILES})
+
+  # Unit tests
+  set(TEST_SRC
+          test/graph.cpp
+          test/main.cpp
+          test/pub_sub.cpp
+  )
+  if (NOT DISABLE_SERVICES)
+    list(APPEND TEST_SRC test/services.cpp)
+  endif()
+  ament_add_gtest(${PROJECT_NAME}_unit_tests
+          ${TEST_SRC}
+  )
+  target_include_directories(${PROJECT_NAME}_unit_tests PRIVATE ${CMAKE_CURRENT_LIST_DIR})
+  target_compile_options(${PROJECT_NAME}_unit_tests PRIVATE -Wno-error=sign-compare) # There is a warning from gtest internal
+  target_link_libraries(${PROJECT_NAME}_unit_tests ${PROJECT_NAME}_lib)
+  rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  target_link_libraries(${PROJECT_NAME}_unit_tests "${cpp_typesupport_target}")
+  ament_target_dependencies(${PROJECT_NAME}_unit_tests
+          std_msgs
+          rclcpp
+  )
+endif()
+
+ament_package()