From 5c82573a23ceb7f76792282f7fd00a46924f8e45 Mon Sep 17 00:00:00 2001 From: sokhealy Date: Thu, 30 Jun 2022 16:32:55 -0700 Subject: [PATCH] Initial release --- .clang-format | 118 ++ .clang-tidy | 8 + .github/workflows/build.yml | 46 + .github/workflows/sonar-project.properties | 12 + .gitignore | 41 + CHANGELOG.rst | 14 + CMakeLists.txt | 163 +++ Dockerfile | 56 + LICENSE | 21 + README.md | 416 ++++++ code_coverage.sh | 46 + config/settings.yaml | 47 + docs/build-in-docker.md | 30 + docs/images/launch-example.png | Bin 0 -> 29656 bytes docs/images/piksi-multi-configuration.png | Bin 0 -> 34624 bytes include/data_sources/sbp_data_source.h | 40 + include/data_sources/sbp_data_sources.h | 42 + include/data_sources/sbp_file_datasource.h | 65 + include/data_sources/sbp_serial_datasource.h | 70 + include/data_sources/sbp_tcp_datasource.h | 71 + include/data_sources/serial.h | 114 ++ include/data_sources/tcp.h | 138 ++ include/logging/issue_logger.h | 120 ++ include/logging/ros_logger.h | 54 + include/logging/sbp_file_logger.h | 62 + include/logging/sbp_to_ros2_logger.h | 55 + include/publishers/base_publisher.h | 25 + include/publishers/baseline_publisher.h | 44 + include/publishers/gpsfix_publisher.h | 92 ++ include/publishers/imu_publisher.h | 75 + include/publishers/navsatfix_publisher.h | 90 ++ include/publishers/publisher_factory.h | 37 + include/publishers/publisher_manager.h | 24 + include/publishers/sbp2ros2_publisher.h | 78 + include/publishers/timereference_publisher.h | 48 + .../twistwithcovariancestamped_publisher.h | 79 + include/test/mocked_logger.h | 45 + include/test/test_utils.h | 35 + include/utils/config.h | 92 ++ include/utils/utils.h | 80 + .../sbpros2_driver.cpython-310.pyc | Bin 0 -> 692 bytes launch/start.py | 32 + msg/Baseline.msg | 41 + package.xml | 32 + src/data_sources/sbp_data_sources.cpp | 48 + src/data_sources/sbp_file_datasource.cpp | 48 + src/data_sources/sbp_serial_datasource.cpp | 48 + src/data_sources/sbp_tcp_datasource.cpp | 71 + src/data_sources/serial.cpp | 334 +++++ src/data_sources/tcp.cpp | 260 ++++ src/logging/ros_logger.cpp | 67 + src/logging/sbp_file_logger.cpp | 81 ++ src/logging/sbp_to_ros2_logger.cpp | 52 + src/publishers/baseline_publisher.cpp | 124 ++ src/publishers/gpsfix_publisher.cpp | 298 ++++ src/publishers/imu_publisher.cpp | 225 +++ src/publishers/navsatfix_publisher.cpp | 249 ++++ src/publishers/publisher_factory.cpp | 99 ++ src/publishers/timereference_publisher.cpp | 83 ++ .../twistwithcovariancestamped_publisher.cpp | 104 ++ src/sbp-to-ros.cpp | 125 ++ src/utils/config.cpp | 65 + src/utils/utils.cpp | 110 ++ test/mocked_logger.cpp | 57 + test/publishers/test_custom_publishers.cpp | 171 +++ test/publishers/test_gps_fix_publisher.cpp | 1285 +++++++++++++++++ .../publishers/test_nav_sat_fix_publisher.cpp | 456 ++++++ .../test_time_reference_publisher.cpp | 72 + test/test_main.cpp | 18 + test/test_network.cpp | 134 ++ test/test_serial.cpp | 152 ++ 71 files changed, 7634 insertions(+) create mode 100644 .clang-format create mode 100644 .clang-tidy create mode 100644 .github/workflows/build.yml create mode 100644 .github/workflows/sonar-project.properties create mode 100644 .gitignore create mode 100644 CHANGELOG.rst create mode 100644 CMakeLists.txt create mode 100644 Dockerfile create mode 100644 LICENSE create mode 100644 README.md create mode 100755 code_coverage.sh create mode 100644 config/settings.yaml create mode 100644 docs/build-in-docker.md create mode 100644 docs/images/launch-example.png create mode 100644 docs/images/piksi-multi-configuration.png create mode 100644 include/data_sources/sbp_data_source.h create mode 100644 include/data_sources/sbp_data_sources.h create mode 100644 include/data_sources/sbp_file_datasource.h create mode 100644 include/data_sources/sbp_serial_datasource.h create mode 100644 include/data_sources/sbp_tcp_datasource.h create mode 100644 include/data_sources/serial.h create mode 100644 include/data_sources/tcp.h create mode 100644 include/logging/issue_logger.h create mode 100644 include/logging/ros_logger.h create mode 100644 include/logging/sbp_file_logger.h create mode 100644 include/logging/sbp_to_ros2_logger.h create mode 100644 include/publishers/base_publisher.h create mode 100644 include/publishers/baseline_publisher.h create mode 100644 include/publishers/gpsfix_publisher.h create mode 100644 include/publishers/imu_publisher.h create mode 100644 include/publishers/navsatfix_publisher.h create mode 100644 include/publishers/publisher_factory.h create mode 100644 include/publishers/publisher_manager.h create mode 100644 include/publishers/sbp2ros2_publisher.h create mode 100644 include/publishers/timereference_publisher.h create mode 100644 include/publishers/twistwithcovariancestamped_publisher.h create mode 100644 include/test/mocked_logger.h create mode 100644 include/test/test_utils.h create mode 100644 include/utils/config.h create mode 100644 include/utils/utils.h create mode 100644 launch/__pycache__/sbpros2_driver.cpython-310.pyc create mode 100644 launch/start.py create mode 100644 msg/Baseline.msg create mode 100644 package.xml create mode 100644 src/data_sources/sbp_data_sources.cpp create mode 100644 src/data_sources/sbp_file_datasource.cpp create mode 100644 src/data_sources/sbp_serial_datasource.cpp create mode 100644 src/data_sources/sbp_tcp_datasource.cpp create mode 100644 src/data_sources/serial.cpp create mode 100644 src/data_sources/tcp.cpp create mode 100644 src/logging/ros_logger.cpp create mode 100644 src/logging/sbp_file_logger.cpp create mode 100644 src/logging/sbp_to_ros2_logger.cpp create mode 100644 src/publishers/baseline_publisher.cpp create mode 100644 src/publishers/gpsfix_publisher.cpp create mode 100644 src/publishers/imu_publisher.cpp create mode 100644 src/publishers/navsatfix_publisher.cpp create mode 100644 src/publishers/publisher_factory.cpp create mode 100644 src/publishers/timereference_publisher.cpp create mode 100644 src/publishers/twistwithcovariancestamped_publisher.cpp create mode 100644 src/sbp-to-ros.cpp create mode 100644 src/utils/config.cpp create mode 100644 src/utils/utils.cpp create mode 100644 test/mocked_logger.cpp create mode 100644 test/publishers/test_custom_publishers.cpp create mode 100644 test/publishers/test_gps_fix_publisher.cpp create mode 100644 test/publishers/test_nav_sat_fix_publisher.cpp create mode 100644 test/publishers/test_time_reference_publisher.cpp create mode 100644 test/test_main.cpp create mode 100644 test/test_network.cpp create mode 100644 test/test_serial.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..37c648e8 --- /dev/null +++ b/.clang-format @@ -0,0 +1,118 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BreakBeforeBraces: Custom +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: true + AfterStruct: true + AfterUnion: true + AfterExternBlock: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^' + Priority: 2 + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Delimiters: + - pb + Language: TextProto + BasedOnStyle: google +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 2 +UseTab: Never +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..0a75e748 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,8 @@ +# See: http://clang.llvm.org/extra/clang-tidy/ +# clang-tidy-4.0 src/pvt_engine/*.cc -- -std=c++14 -Irefactor/common/ -Irefactor/common/libswiftnav -Iinclude/ -Iinclude/libswiftnav/ -isystem -third_party/ -isystem./libfec/include/ -Ithird_party/Optional -isystem./third_party/json/src/ -isystem./third_party/eigen/ +# + +Checks: "-*,cert-*,google-*,misc-*,readability-*,clang-analyzer-*,modernize-*,performance-*,-clang-analyzer-alpha*,cppcoreguidelines-*,cert-*,-cppcoreguidelines-pro-bounds-constant-array-index,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-pro-bounds-pointer-arithmetic,-cppcoreguidelines-pro-type-vararg,-modernize-pass-by-value,-modernize-deprecated-headers,-modernize-use-default-member-init,-modernize-redundant-void-arg,-modernize-use-using,-modernize-use-equals-delete,-modernize-use-equals-default,-modernize-use-bool-literals,-modernize-use-auto,-modernize-use-emplace,-cppcoreguidelines-special-member-functions,-cppcoreguidelines-pro-type-member-init,-readability-avoid-const-params-in-decls,-readability-non-const-parameter,-readability-redundant-member-init,-readability-redundant-declaration,-cert-err34-c,-cert-err58-cpp,-performance-unnecessary-value-param,-google-runtime-references,-clang-analyzer-optin.cplusplus.VirtualCall,-clang-analyzer-core.CallAndMessage,-clang-analyzer-core.UndefinedBinaryOperatorResult,-clang-analyzer-core.uninitialized.Assign,-cppcoreguidelines-owning-memory,-clang-analyzer-core.uninitialized.UndefReturn,-cert-dcl21-cpp,-modernize-return-braced-init-list,-cert-dcl03-c,-misc-static-assert,-cppcoreguidelines-pro-type-static-cast-downcast,-clang-analyzer-optin.performance.Padding,-cppcoreguidelines-pro-type-union-access" +HeaderFilterRegex: '.*' +AnalyzeTemporaryDtors: true +... diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 00000000..3ffe4dfe --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,46 @@ +name: Build +on: + push: + branches: + - master + pull_request: + types: [opened, synchronize, reopened] +env: + SONAR_SCANNER_VERSION: 4.7.0.2747 + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + SONAR_TOKEN: ${{ secrets.SONAR_TOKEN }} + PARALLEL_THREADS: 2 +jobs: + build-master: + name: Build master + runs-on: ubuntu-latest + if: github.ref == 'refs/heads/master' + steps: + - uses: actions/checkout@v2 + with: + fetch-depth: 0 # Shallow clones should be disabled for a better relevancy of analysis + - name: Code coverage + run: | + docker build -t swiftnav-ros2 --build-arg UID=$UID --build-arg SONAR_SCANNER_VERSION=$SONAR_SCANNER_VERSION - < Dockerfile + docker run --rm -v $PWD:/mnt/workspace/src/swiftnav-ros2 swiftnav-ros2:latest /bin/bash ./code_coverage.sh $GITHUB_TOKEN \ + $SONAR_TOKEN \ + $PARALLEL_THREADS + build-pr: + name: Build pull request + runs-on: ubuntu-latest + if: github.ref != 'refs/heads/master' + steps: + - uses: actions/checkout@v2 + with: + fetch-depth: 0 # Shallow clones should be disabled for a better relevancy of analysis + - name: Code coverage + env: + PR_BRANCH_NAME: ${{ github.head_ref }} + run: | + PR_NUMBER=$(jq --raw-output .pull_request.number "$GITHUB_EVENT_PATH") + docker build -t swiftnav-ros2 --build-arg UID=$UID --build-arg SONAR_SCANNER_VERSION=$SONAR_SCANNER_VERSION - < Dockerfile + docker run --rm -v $PWD:/mnt/workspace/src/swiftnav-ros2 swiftnav-ros2:latest /bin/bash ./code_coverage.sh $GITHUB_TOKEN \ + $SONAR_TOKEN \ + $PARALLEL_THREADS \ + $PR_BRANCH_NAME \ + $PR_NUMBER diff --git a/.github/workflows/sonar-project.properties b/.github/workflows/sonar-project.properties new file mode 100644 index 00000000..f8cad753 --- /dev/null +++ b/.github/workflows/sonar-project.properties @@ -0,0 +1,12 @@ +sonar.projectKey=swift-nav_swiftnav-ros2 +sonar.organization=swift-nav + +# This is the name and version displayed in the SonarCloud UI. +sonar.projectName=swiftnav-ros2 +sonar.projectVersion=1.0 + +# Path is relative to the sonar-project.properties file. Replace "\" by "/" on Windows. +sonar.sources=src + +# Encoding of the source code. Default is default system encoding +sonar.sourceEncoding=UTF-8 diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..954d30ec --- /dev/null +++ b/.gitignore @@ -0,0 +1,41 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Code coverage +*.gcov + +# Folders +build/ +install/ +log/ +.vscode/ \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 00000000..c2ee9acf --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,14 @@ +CHANGE LOG +========= + +Version 1.0.0 (2023-04-04) +------------- + +Changes + +- Initial release. + +Known Issues + +- SBP recording saves all SBP messages with sender ID equal zero. +- Driver builds on ROS 2 Foxy with compilation warnings. diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..9d89ecd9 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,163 @@ +cmake_minimum_required(VERSION 3.8) +project(swiftnav_ros2_driver) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################################ +# Check the ROS2 version + +set(ROS2_FOUND FALSE) +if(DEFINED ENV{ROS_DISTRO}) + set(FOUND_ROS2_DISTRO $ENV{ROS_DISTRO}) + set(ROS2_FOUND TRUE) +else() + set(ROS2_DISTROS "ardent;crystal;dashing;eloquent;foxy;galactic;humble;rolling") + set(ROS2_FOUND FALSE) + foreach(distro ${ROS2_DISTROS}) + if(NOT ROS2_FOUND) + find_path(RCLCPP_H rclcpp.hpp PATHS /opt/ros/${distro}/include/rclcpp) + if(RCLCPP_H) + set(FOUND_ROS2_DISTRO ${distro}) + set(ROS2_FOUND TRUE) + endif() + endif() + endforeach() +endif() + +if(${FOUND_ROS2_DISTRO} STREQUAL "foxy") + add_definitions(-DFOUND_FOXY) +elseif((${FOUND_ROS2_DISTRO} STREQUAL "galactic") OR (${FOUND_ROS2_DISTRO} STREQUAL "humble")) + add_definitions(-DFOUND_NEWER) +endif() +################################################ + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(gps_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(tf2 REQUIRED) + +link_directories("/usr/local/lib/") +include_directories("/usr/local/include/") + +add_executable(sbp-to-ros + src/sbp-to-ros.cpp + src/data_sources/sbp_file_datasource.cpp + src/data_sources/sbp_serial_datasource.cpp + src/data_sources/sbp_tcp_datasource.cpp + src/data_sources/sbp_data_sources.cpp + src/data_sources/serial.cpp + src/data_sources/tcp.cpp + src/utils/utils.cpp + src/utils/config.cpp + src/logging/ros_logger.cpp + src/logging/sbp_to_ros2_logger.cpp + src/logging/sbp_file_logger.cpp + src/publishers/navsatfix_publisher.cpp + src/publishers/twistwithcovariancestamped_publisher.cpp + src/publishers/timereference_publisher.cpp + src/publishers/gpsfix_publisher.cpp + src/publishers/baseline_publisher.cpp + src/publishers/imu_publisher.cpp + src/publishers/publisher_factory.cpp + ) + + rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Baseline.msg" + DEPENDENCIES std_msgs + ) + +if(${FOUND_ROS2_DISTRO} STREQUAL "foxy") + rosidl_target_interfaces(sbp-to-ros ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +endif() + +target_link_libraries(sbp-to-ros sbp serialport "${cpp_typesupport_target}") +target_include_directories(sbp-to-ros PUBLIC + $) +target_compile_features(sbp-to-ros PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(sbp-to-ros rclcpp sensor_msgs geometry_msgs nav_msgs gps_msgs tf2) +ament_export_dependencies(rosidl_default_runtime) + +install(TARGETS + sbp-to-ros + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(rclcpp REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(nav_msgs REQUIRED) + find_package(gps_msgs REQUIRED) + find_package(rosidl_default_generators REQUIRED) + find_package(tf2 REQUIRED) + link_directories("/usr/local/lib/") + include_directories("/usr/local/include/") + + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + ament_add_gmock(${PROJECT_NAME}_test + src/data_sources/sbp_serial_datasource.cpp + src/data_sources/sbp_tcp_datasource.cpp + src/data_sources/serial.cpp + src/data_sources/tcp.cpp + src/utils/utils.cpp + src/utils/config.cpp + src/publishers/baseline_publisher.cpp + src/publishers/gpsfix_publisher.cpp + src/publishers/imu_publisher.cpp + src/publishers/navsatfix_publisher.cpp + src/publishers/twistwithcovariancestamped_publisher.cpp + src/publishers/publisher_factory.cpp + src/publishers/timereference_publisher.cpp + test/mocked_logger.cpp + test/test_network.cpp + test/test_serial.cpp + test/publishers/test_nav_sat_fix_publisher.cpp + test/publishers/test_time_reference_publisher.cpp + test/publishers/test_gps_fix_publisher.cpp + test/publishers/test_custom_publishers.cpp + test/test_main.cpp + ) + + target_include_directories(${PROJECT_NAME}_test PUBLIC + $ + ) + + if(${FOUND_ROS2_DISTRO} STREQUAL "foxy") + rosidl_target_interfaces(${PROJECT_NAME}_test ${PROJECT_NAME} "rosidl_typesupport_cpp") + else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + endif() + + target_compile_features(${PROJECT_NAME}_test PRIVATE c_std_99 cxx_std_17) # Require C99 and C++17 + target_link_libraries(${PROJECT_NAME}_test sbp serialport "${cpp_typesupport_target}") + ament_target_dependencies(${PROJECT_NAME}_test rclcpp sensor_msgs geometry_msgs nav_msgs gps_msgs tf2) + ament_export_dependencies(rosidl_default_runtime) +endif() + +ament_package() diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..b049124a --- /dev/null +++ b/Dockerfile @@ -0,0 +1,56 @@ +FROM osrf/ros:humble-desktop + +ARG SONAR_SCANNER_VERSION=4.7.0.2747 + +ARG DEBIAN_FRONTEND=noninteractive + +ENV CC=gcc-11 +ENV CXX=g++-11 +ENV HOME /home/dockerdev + +ARG UID=1000 + +RUN apt-get update && apt-get install --yes \ + build-essential \ + pkg-config \ + cmake \ + doxygen \ + check \ + clang-format-13 \ + libserialport-dev \ + ros-humble-gps-msgs + +# Add a "dockerdev" user with sudo capabilities +# 1000 is the first user ID issued on Ubuntu; might +# be different for Mac users. Might need to add more. +RUN \ + useradd -u ${UID} -ms /bin/bash -G sudo dockerdev \ + && echo '%sudo ALL=(ALL) NOPASSWD:ALL' >>/etc/sudoers + +RUN chown -R dockerdev:dockerdev $HOME/ +USER dockerdev + +WORKDIR $HOME/ +RUN git clone https://github.com/swift-nav/libsbp.git && cd libsbp && git checkout v4.11.0 +WORKDIR $HOME/libsbp/c +RUN git submodule update --init --recursive +RUN mkdir build && \ + cd build && \ + cmake DCMAKE_CXX_STANDARD=17 -DCMAKE_CXX_STANDARD_REQUIRED=ON -DCMAKE_CXX_EXTENSIONS=OFF ../ && \ + make && \ + sudo make install + +# Install code coverage tool +RUN sudo apt-get -y install gcovr + +# Download and set up sonar-scanner +RUN sudo apt-get -y install unzip +RUN mkdir -p $HOME/.sonar +RUN curl -sSLo $HOME/.sonar/sonar-scanner.zip https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${SONAR_SCANNER_VERSION}-linux.zip +RUN unzip -o $HOME/.sonar/sonar-scanner.zip -d $HOME/.sonar/ +ENV PATH="${PATH}:/home/dockerdev/.sonar/sonar-scanner-${SONAR_SCANNER_VERSION}-linux/bin" + +WORKDIR /mnt/workspace/src/swiftnav-ros2 +RUN sudo chown -R dockerdev:dockerdev /mnt/workspace/ + +#CMD ["make", "all"] diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..7e5100db --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Swift Navigation + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 00000000..9a0a28f3 --- /dev/null +++ b/README.md @@ -0,0 +1,416 @@ +# **swiftnav-ros2** +ROS 2 driver for Swift Navigation's GNSS/INS receivers and Starling Positioning Engine software. + +# **Table of Contents** +- [Features](#features) +- [ROS Topics](#ros-topics) +- [Building Driver](#building-driver) +- [Launching Driver](#launching-driver) +- [Driver Configuration](#driver-configuration) +- [GNSS Receiver Configuration](#gnss-receiver-configuration) +- [Technical Support](#technical-support) + +# Features +- Designed for ROS 2 Humble but also works with ROS 2 Foxy +- Developed and tested on Ubuntu 22.04 (ROS 2 Humble) and Ubuntu 20.04 (ROS 2 Foxy) platforms +- Supports Swift Navigation receivers and Starling Positioning Engine in Swift Binary Protocol (SBP) +- TCP Client and Serial communication interfaces +- SBP file playback +- SBP data logging +- Publishes ROS 2 standard and Swift Navigation proprietary topics +- Configurable time stamping +- Written in C++ + +# ROS Topics +The driver receives Swift binary (SBP) messages (see [GNSS Receiver Configuration](#gnss-receiver-configuration) for setting up the receiver) and publishes the following ROS topics: + - [`GpsFix`](#gpsfix) + - [`NavSatFix`](#navsatfix) + - [`TwistWithCovarianceStamped`](#twistwithcovariancestamped) + - [`Baseline` *(proprietary)*](#baseline) + - [`TimeReference`](#timereference) + - [`Imu`](#imu) + +Topic publication details are described below. + +## GpsFix + +`gps_msgs/msg/GPSFix` + +### SBP Messages Used +- `UTC TIME` (ID: 259, *required/optional*) - UTC time of reported position. Required when `timestamp_source_gnss` is `True`. +- `GPS TIME` (ID: 258, *required*) - GPS time of reported position. +- `POS LLH COV` (ID: 529, *required*) - GNSS position with covariance. +- `VEL NED COV` (ID: 530, *required*) - GNSS velocity with covariance. +- `ORIENT EULER` (ID: 545, *optional*) - GNSS/INS orientation with estimated errors. +- `DOPS` (ID: 520, *optional*) - GNSS DOP (Dilution Of Precision) data. + +### Topic Publication +Topic publication depends on `timestamp_source_gnss` setting flag in the configuration file: +- `True`: The topic is published upon receiving SBP `UTC TIME`, `GPS TIME`, `POS LLH COV`, `VEL NED COV` and, if present, `ORIENT EULER` messages with the same TOW. The topic timestamp contains the UTC time reported by the GNSS receiver. If the UTC time is not available the current platform time is reported. +- `False`: The topic is published upon receiving SBP `GPS TIME`, `POS LLH COV`, `VEL NED COV` and, if present, `ORIENT EULER` messages with the same TOW. The topic timestamp contains the current platform time. + +### Topic Fields +| ROS2 Message Field | SBP Message Data Source | Notes | +| :--- | :---: | :--- | +|`header.stamp`|`UTC TIME`|See Topic Publication for time stamping details| +|`header.frame_id`|--|Text from `frame_name` field in the `settings.yaml` configuration file | +|`status.satellites_used`|`POS LLH COV`|| +|`status.satellite_used_prn[]`|--|Not populated| +|`status.satellites_visible`
`status.satellite_visible_prn[]`
`status.satellite_visible_z[]`
`status.satellite_visible_azimuth[]`
`status.satellite_visible_snr[]`|--|Not populated| +|`status.status`|`POS LLH COV`|Dead Reckoning (DR) position is reported as `STATUS_FIX` (0)| +|`status.motion_source`|`VEL NED COV`|| +|`status.orientation_source`|`POS LLH COV`|| +|`status.position_source`|`POS LLH COV`|| +|`latitude`
`longitude`
`altitude`
|`POS LLH COV`|Zeros when the fix is invalid. If position is valid altitude is always present (i.e. never NaN).| +|`track`|`VEL NED COV`
or
`ORIENT EULER`|If `ORIENT EULER` message is present and `yaw` data valid, reports `yaw`. If `yaw` is invalid reports computed Course Over Ground from `VEL NED COV` message. `VEL NED COV` updates `track` only if horizontal speed is above the `track_update_min_speed_mps` setting in the settings file. When the track becomes invalid the last valid track is reported. | +|`speed`|`POS LLH COV`|Computed horizontal (2D) speed| +|`climb`|`POS LLH COV`|| +|`pitch`
`roll`|`ORIENT EULER`|| +|`dip`|--|Not populated| +|`time`|`GPS TIME`|GPS time in seconds since 1980-01-06 | +|`gdop`
`pdop`
`hdop`
`vdop`
`tdop`|`DOPS`|DOPs are published if the most recent SBP `DOPS` message is not older than 2 seconds.| +|`err`
`err_horz`
`err_vert`|`POS LLH COV`|| +|`err_track`|`VEL NED COV`
or
`ORIENT EULER`|| +|`err_speed`
`err_climb`|`VEL NED COV`|| +|`err_time`|--|Not populated| +|`err_pitch`
`err_roll`|`ORIENT EULER`|| +|`err_dip`|--|Not populated| +|`position_covariance`
`position_covariance_type`|`POS LLH COV`|Covariance, if valid, is always `TYPE_KNOWN` (full matrix).| + + +## NavSatFix + +`sensor_msgs/msg/NavSatFix` + +### SBP Messages Used +- `UTC TIME` (ID: 259, *required/optional*) - UTC time of reported position. Required when `timestamp_source_gnss` is `True`. +- `POS LLH COV` (ID: 529, *required*) - GNSS position data with covariance. +- `MEASUREMENT STATE` (ID: 97, *optional*) - GNSS constellations data. + +### Topic Publication +Topic publication depends on `timestamp_source_gnss` setting flag in the configuration file: +- `True`: the topic is published upon receiving SBP `UTC TIME` and `POS LLH COV` messages with the same TOW. The topic timestamp contains the UTC time reported by the GNSS receiver. If the UTC time is not available the current platform time is reported. +- `False`: the topic is published upon receiving SBP `POS LLH COV` message. The topic timestamp contains the current platform time. + +### Topic Fields +| ROS2 Message Field | SBP Message Data Source | Notes | +| :--- | :---: | :--- | +|`header.stamp`|`UTC TIME`|See Topic Publication for time stamping details| +|`header.frame_id`|--|Text from `frame_name` field in the `settings.yaml` configuration file | +|`status.status`|`POS LLH COV`|Dead Reckoning (DR) position is reported as `STATUS_FIX` (0)| +|`status.service`|`MEASUREMENT STATE`|GNSS constellations from the last `MEASUREMENT STATE` message. Reports zero when message is not present.| +|`latitude`
`longitude`
`altitude`
`position_covariance`
`position_covariance_type`|`POS LLH COV`|Zeros when the fix is invalid. If position is valid altitude is always present (i.e. never NaN). Covariance, if valid, is always `TYPE_KNOWN` (full matrix).| + + + ## TwistWithCovarianceStamped + +`geometry_msgs/msg/TwistWithCovarianceStamped` + +### SBP Messages Used +- `UTC TIME` (ID: 259, *required/optional*) - UTC time of reported velocity. Required when `timestamp_source_gnss` is `True`. +- `VEL NED COV` (ID: 530, *required*) - GNSS velocity data with covariance. + +### Topic Publication +Topic publication depends on `timestamp_source_gnss` setting flag in the configuration file: +- `True`: the topic is published upon receiving SBP `UTC TIME` and `VEL NED COV` messages with the same TOW. The topic timestamp contains the UTC time reported by the GNSS receiver. If the UTC time is not available the current platform time is reported. +- `False`: the topic is published upon receiving SBP `VEL NED COV` message. The topic timestamp contains the current platform time. + +### Topic Fields +| ROS2 Message Field | SBP Message Data Source | Notes | +| :--- | :---: | :--- | +|`header.stamp`|`UTC TIME`|See Topic Publication for time stamping details| +|`header.frame_id`|--|Text from `frame_name` field in the `settings.yaml` configuration file | +|`linear.x`
`linear.y`
`linear.z`|`VEL NED COV`|Conversion from NED frame:
`x` = `east`
`y` = `north`
`z` = `-down`
Zeros when velocity is invalid.| +|`angular.x`
`angular.y`
`angular.z`|--|Not populated. Always zero.| +|`covariance`|`VEL NED COV`|If velocity is valid, linear velocity covariance is full matrix. `covariance[0]` is set to -1 when linear velocity is invalid. `covariance[21]` is always -1.| + + +## Baseline + +`swiftnav-ros2/msg/Baseline` *Proprietary message* + +### SBP Messages Used +- `UTC TIME` (ID: 259, *required/optional*) - UTC time of reported baseline. Required when `timestamp_source_gnss` is `True`. +- `BASELINE NED` (ID: 524, *required*) - RTK baseline NED vector. + +### Topic Publication +Topic publication depends on `timestamp_source_gnss` setting flag in the configuration file: +- `True`: the topic is published upon receiving SBP `UTC TIME` and `BASELINE NED` messages with the same TOW. The topic timestamp contains the UTC time reported by the GNSS receiver. If the UTC time is not available the current platform time is reported. +- `False`: the topic is published upon receiving SBP `BASELINE NED` message. The topic timestamp contains the current platform time. + +### Topic Fields +| ROS2 Message Field | SBP Message Data Source | Notes | +| :--- | :---: | :--- | +|`header.stamp`|`UTC TIME`|See Topic Publication for time stamping details| +|`header.frame_id`|--|Text from `frame_name` field in the `settings.yaml` configuration file | +|`mode`|`BASELINE NED`|Solution mode:
`0` - Invalid
`3` - Float RTK
`4` - Fixed RTK| +|`satellites_used`|`BASELINE NED`|Number of satellites used in the solution| +|`baseline_n_m`
`baseline_e_m`
`baseline_d_m`|`BASELINE NED`|Baseline NED vectors in [m]. Zeros when invalid. Vectors origin is at the base location.| +|`baseline_err_h_m`|`BASELINE NED`|Estimated (95%) horizontal error of baseline in [m]. Zero when invalid.| +|`baseline_err_v_m`|`BASELINE NED`|Estimated (95%) vertical error of baseline in [m]. Zero when invalid.| +|`baseline_length_m`|`BASELINE NED`|Computed 3D baseline length. Zero when invalid.| +|`baseline_length_h_m`|`BASELINE NED`|Computed horizontal baseline length. Zero when invalid.| +|`baseline_orientation_valid`|`BASELINE NED`|`True` when baseline orientation (dir and dip) is valid. `False` when invalid.| +|`baseline_dir_deg`|`BASELINE NED`|Computed horizontal angle (bearing/heading) from base to rover in [degrees]. Valid only in RTK fixed mode. Range [0..360) from true north. Zero when invalid.| +|`baseline_dir_err_deg`|`BASELINE NED`|Estimated (95%) error of `baseline_dir_deg` in [degrees]. Range [0..180]. Zero when invalid.| +|`baseline_dip_deg`|`BASELINE NED`|Computed vertical angle from base to rover in [degrees]. Valid only in RTK fixed mode. Range [-90..90]. Zero when invalid.| +|`baseline_dip_err_deg`|`BASELINE NED`|Estimated (95%) error of `baseline_dip_deg` in [degrees]. Range [0..90]. Zero when invalid.| + + +## TimeReference + +`sensor_msgs/msg/TimeReference` + +### SBP Messages Used +- `UTC TIME` (ID: 259, *required/optional*) - UTC time. Required when `timestamp_source_gnss` is `True`. +- `GPS TIME` (ID: 258, *required*) - GPS time. + +### Topic Publication +Topic publication depends on `timestamp_source_gnss` setting flag in the configuration file: +- `True`: the topic is published upon receiving SBP `UTC TIME` and `GPS TIME` messages with the same TOW. The topic timestamp contains the UTC time reported by the GNSS receiver. If the UTC time is not available the current platform time is reported. +- `False`: the topic is published upon receiving SBP `GPS TIME` message. The topic timestamp contains the current platform time. + +### Topic Fields +| ROS2 Message Field | SBP Message Data Source | Notes | +| :--- | :---: | :--- | +|`header.stamp`|`UTC TIME`|See Topic Publication for time stamping details| +|`header.frame_id`|--|Not used| +|`time_ref`|`GPS TIME`|GPS time in seconds since 1980-01-06. `sec` value is set to -1 if the GPS time is not available.| +|`source`|--|Text from `frame_name` field in the `settings.yaml` configuration file | + + +## Imu + +`sensor_msgs/msg/Imu` + +### SBP Messages Used +- `UTC TIME` (ID: 259, *required/optional*) - UTC time. Required when `timestamp_source_gnss` is `True`. +- `GPS TIME` (ID: 258, *required*) - GPS time +- `GNSS TIME OFFSET` (ID: 65287, *required/optional*) - Offset of the IMU local time with respect to GNSS time. Required when the original IMU time source is a local time. +- `IMU AUX` (ID: 2305, *required*) - Auxiliary IMU data +- `IMU RAW` (ID: 2304, *required*) - Raw IMU data + +### Topic Publication +Topic is published upon receiving `IMU RAW` SBP message. +Time stamp depends on `timestamp_source_gnss` setting flag in the configuration file: +- `True`: The topic timestamp contains the UTC time of the measurement computed from `UTC TIME`, `GPS TIME`, `GNSS TIME OFFSET` and `IMU RAW` SBP messages depending on original IMU time stamping source. If the UTC time is not available the current platform time is reported. +- `False`: The topic timestamp contains the current platform time. + +### Topic Fields +| ROS2 Message Field | SBP Message Data Source | Notes | +| :--- | :---: | :--- | +|`header.stamp`|`UTC TIME`
`GPS TIME`
`GNSS TIME OFFSET`|See Topic Publication for time stamping details| +|`header.frame_id`|--|Text from `frame_name` field in the `settings.yaml` configuration file | +|`orientation`
`orientation_covariance`|--|Not populated. Always zero. `orientation_covariance[0]` is always -1.| +|`angular_velocity`|`IMU RAW`
`IMU AUX`|Reported in sensor frame. Zeros when invalid.| +|`angular_velocity_covariance`|--|Not populated. `angular_velocity_covariance[0]` is set to -1 when angular velocity is not valid or when the time stamping source has changed| +|`linear_acceleration`|`IMU RAW`
`IMU AUX`|Reported in sensor frame. Zeros when invalid.| +|`linear_acceleration_covariance`|--|Not populated. `linear_acceleration_covariance[0]` is set to -1 when linear acceleration is not valid or when the time stamping source has changed | + + +# Building Driver + +[Click here if building driver in a docker](docs/build-in-docker.md) + +### Dependencies: +- `libsbp` - Swift Binary Protocol library +- `libserialport` - Serial Port communication library + + +## Step 1 (Install ROS 2 Humble): + Follow [instructions to install ROS 2 Humble](https://docs.ros.org/en/humble/Installation.html) + +## Step 2 (Install libspb): + In any directory you wish, clone libsp v4.11.0, init the repo, make the lib and install it + + ``` + git clone https://github.com/swift-nav/libsbp.git + cd libsbp + git checkout v4.11.0 + cd c + git submodule update --init --recursive + mkdir build + cd build + cmake DCMAKE_CXX_STANDARD=17 -DCMAKE_CXX_STANDARD_REQUIRED=ON -DCMAKE_CXX_EXTENSIONS=OFF ../ + make + sudo make install + ``` + +## Step 3 (Download Driver Code) + Navigate to workspace directory (e.g.: `~/workspace`) and download driver source files + + ``` + cd ~/workspace + mkdir src + cd src + git clone https://github.com/swift-nav/swiftnav-ros2.git + ``` + +## Step 4 (Install Dependencies) + Initialize environment and install dependencies + + ``` + cd ~/workspace + source /opt/ros/humble/setup.bash + sudo apt-get update + sudo apt-get install libserialport-dev + rosdep install --from-paths src --ignore-src -r -y + ``` + +## Step 5 (Edit Configuration) + Edit configuration file as required. See [ROS 2 driver configuration](#driver-configuration) for setting details. + + ``` + nano ~/workspace/src/swiftnav-ros2/config/settings.yaml + ``` + +## Step 6 (Build) + Initialize environment and build the driver + + ``` + cd ~/workspace + source /opt/ros/humble/setup.bash + colcon build + ``` + +# Launching Driver + +## Launching + Source installed driver and start it + + ``` + source install/setup.bash + ros2 launch swiftnav_ros2_driver start.py + ``` + +![Driver Launch Example](docs/images/launch-example.png) + + +## Viewing Topics + Swift specific SBP messages are not a part of the ROS 2 standard library, therefore the following command must be run in any terminal that is used for interfacing with this driver (e.g.: echoing the `baseline` message in a new terminal) + + ``` + source install/setup.bash + ros2 topic echo /baseline + ``` + +## Changing Configuration + Changing the configuration file can be done in the driver source (`config/settings.yaml`), but the driver will need to be rebuilt. Alternatively, the configuration file can be changed in the installed directory: + + ``` + nano install/swiftnav_ros2_driver/share/swiftnav_ros2_driver/config/settings.yaml + ``` + + +# Driver Configuration +The driver configuration is stored in the `config/settings.yaml` file. The following settings are available: + +| Parameter | Accepted Values | Description | +| :--- | :--- | :--- | +| `interface` | `1`, `2`, `3` | SwiftNav GNSS receiver communication interface:
`1` - TCP Client
`2` - Serial port
`3` - File (playback) | +| `host_ip`| E.g.: `192.168.0.222` | IP address of the GNSS receiver. Only used if `interface` is `1`. | +| `host_port`| E.g.: `55556` | TCP port used. Only used if `interface` is `1`. | +| `read_timeout`
`write_timeout` | E.g.: `10000` | A timeout for read/write operations in milliseconds. Used for `interface` `1` and `2`. | +| `device_name` | E.g.: `/dev/ttyS0` (Linux), `COM1` (Windows) | Serial device name. Only used if `interface` is `2`. | +| `connection_str` | E.g.: `115200\|N\|8\|1\|N` (See [Connection String Description](#connection-string-description)) | A connection string that describes the parameters needed for the serial communication. Only used if `interface` is `2`. | +| `sbp_file` | E.g.: `/logs/sbp-file.sbp` | SBP file name for playback. Absolute path is required. Only used if `interface` is `3`. Playback is done at file reading rate, not a real-time. | +| `frame_name`|string|ROS topics frame name | +| `timestamp_source_gnss`|`True`, `False`|Topic publication header time stamp source. `True`: use GNSS receiver reported time, `False`: use current platfrom time. | +| `baseline_dir_offset_deg`| -180.0 .. 180.0 | RTK Baseline direction offset in [deg]. Floating point value is required. | +| `baseline_dip_offset_deg`| -90.0 .. 90.0 | RTK Baseline dip offset in [deg]. Floating point value is required. | +| `track_update_min_speed_mps`| E.g.: `1.0`| Mininal horizontal speed for `track` updates from SBP message `VEL NED COV` in [m/s]. `track` and `err_track` outputs are 'frozen' below this threshold. Floating point value is required. | +| `enabled_publishers[]`|`gpsfix`
`navsatfix`
`twistwithcovariancestamped`
`baseline`
`timereference`
`imu`| List of enabled publishers. Delete (comment out) the line to disable publisher. +| log_sbp_messages | `True`, `False` | Enable/disable SBP raw data recording. | +| log_sbp_filepath | E.g.: `/logs/sbp-files/` | Absolute path (without a file name) for SBP log file location. File name is created automatically with the current date and time, e.g.: `swiftnav-20230404-160720.sbp`. | + + +## Connection String Description +The connection string for the serial interface has the form: +`BAUD RATE`|`PARITY`|`DATA BITS`|`STOP BITS`|`FLOW CONTROL` + +### Baud Rates +`1200`, `2400`, `4800`, `9600`, `19200`, `38400`, `57600`, `115200`, `230400`, `460800`, `921600`. + +### Parity +| Value | Description | +|:--- | :--- | +| `N` | No parity | +| `E` | Even parity | +| `O` | Odd parity | +| `M` | Mark parity *(Not available in some Linux distributions)* | +| `S` | Space parity *(Not available in some Linux distributions)* | + +### Data Bits +`7` or `8` + +### Stop Bits +`1` or `2` + +### Flow Control +| Value | Description | +|:--- | :--- | +| `N` | No flow control | +| `X` | Xon/Xoff | +| `R` | RTS/CTS | +| `D` | DTR/DSR | + + +# GNSS Receiver Configuration + +The ROS 2 driver works with Swift Navigation receivers and Starling Position Engine software using data in SBP protocol. Refer to the receiver-specific manual to configure your receiver: + +- [Piksi Multi](https://support.swiftnav.com/support/solutions/folders/44001200455) +- [Duro](https://support.swiftnav.com/support/solutions/folders/44001200456) +- [PGM EVK](https://support.swiftnav.com/support/solutions/articles/44002129828-pgm-evaluation-kit) +- [Starling Positioning Engine](https://support.swiftnav.com/support/solutions/folders/44001223202) + +It's recommended to dedicate one output port for ROS and output on that port only messages required by the driver. This will minimize the latency and jitter of the incoming messages, and decrease CPU load. + +The driver uses the following SBP messages: + +| Message Name | Message ID (decimal) | Description | +| :--- | :---: | :--- | +| `MEASUREMENT STATE` | 97 | Satellite tracking data | +| `GPS TIME` | 258 | GPS time | +| `UTC TIME` | 259 | UTC time | +| `DOPS` | 520 | Dillution Of Precision | +| `BASELINE NED` | 524 | Baseline vectors in NED frame | +| `POS LLH COV` | 529 | Position (latitude, longitude, altitude) with covariance | +| `VEL NED COV` | 530 | Velocity vectors in NED frame with covariance | +| `ORIENT EULER` | 545 | Orientation (roll, pitch, yaw)
*Note: message is available only in products with inertial fusion enabled* | +| `IMU RAW` | 2304 | Raw IMU data | +| `IMU AUX` | 2305 | IMU temperature and senor ranges | +| `GNSS TIME OFFSET` | 65287 | Offset of the local time with respect to GNSS time | + +Download [Swift Binary Protocol Specification](https://support.swiftnav.com/support/solutions/articles/44001850782-swift-binary-protocol) + + +### Piksi Multi / Duro Configuration Example + +Piksi Multi and Duro configuration can be changed using Swift Console program. `TCP Server 1` settings example: + +![Piksi Multi Configuration Example](docs/images/piksi-multi-configuration.png) + +*Note: Click SAVE TO DEVICE button to memorize settings over the power cycle.* + + +### Starling Configuration Example + +Starling configuration is saved in the yaml configuration file. `TCP server` output example: + ``` + ... + outputs: + - name: sbp-ros2 + protocol: sbp + type: tcp-server + port: 55556 + max-conns: 4 + sbp: + enabled-messages: [ 97,258,259,520,524,529,530,545,2304,2305,65287 ] + ... + ``` + + +# Technical Support + +Support requests can be made by filling the Support Request Form on the [Swift Navigation Support page](https://support.swiftnav.com/) (Support Request Form button is at the bottom of the page). A simple login is required to confirm your email address. diff --git a/code_coverage.sh b/code_coverage.sh new file mode 100755 index 00000000..0fd45e3c --- /dev/null +++ b/code_coverage.sh @@ -0,0 +1,46 @@ +#!/bin/bash + +# arguments: +# 1 - github token +# 2 - sonar token +# 3 - number of threads to use in parallel +# 4 - pull request branch name +# 5 - pull request number + +# set -e + +export GITHUB_TOKEN=$1 +export SONAR_TOKEN=$2 + +mkdir -p build +cd build +cmake -DCMAKE_C_FLAGS=--coverage -DCMAKE_CXX_FLAGS=--coverage .. + +make -j$3 all +make -j$3 test + +cd .. +gcovr -j $3 --gcov-executable gcov --sonarqube ./build/code_coverage.xml --root . ./build + +if [ -n "$4" ] && [ -n "$5" ]; then + # pull request build + sonar-scanner -X -Dproject.settings=.github/workflows/sonar-project.properties \ + -Dsonar.cfamily.cache.enabled=false \ + -Dsonar.cfamily.compile-commands=./build/compile_commands.json \ + -Dsonar.coverageReportPaths=./build/code_coverage.xml \ + -Dsonar.organization=swift-nav \ + -Dsonar.projectKey=swift-nav_swiftnav-ros2 \ + -Dsonar.host.url="https://sonarcloud.io" \ + -Dsonar.pullrequest.branch=$4 \ + -Dsonar.pullrequest.key=$5 +else + # master build + sonar-scanner -X -Dproject.settings=.github/workflows/sonar-project.properties \ + -Dsonar.cfamily.cache.enabled=false \ + -Dsonar.cfamily.compile-commands=./build/compile_commands.json \ + -Dsonar.coverageReportPaths=./build/code_coverage.xml \ + -Dsonar.organization=swift-nav \ + -Dsonar.projectKey=swift-nav_swiftnav-ros2 \ + -Dsonar.host.url="https://sonarcloud.io" \ + -Dsonar.branch.name=master +fi diff --git a/config/settings.yaml b/config/settings.yaml new file mode 100644 index 00000000..6078e7f4 --- /dev/null +++ b/config/settings.yaml @@ -0,0 +1,47 @@ +swiftnav_ros2_driver: + ros__parameters: + + # SwiftNav GNSS Receiver Interface + interface: 1 # 1: TCP Client, 2: Serial, 3: File + + # TCP Client (interface: 1) + host_ip: "192.168.0.222" + host_port: 55556 + read_timeout: 2000 # [ms] + write_timeout: 2000 # [ms] + + # Serial (interface: 2) + device_name: "/dev/ttyUSB0" + connection_str: "115200|N|8|1|N" + + # File (interface: 3) + sbp_file: "/home/swiftnav/ros2logs/sample-drive.sbp" + + # ROS2 Frame Name + frame_name: "swiftnav-gnss" + + # Topic Publication Time Stamp Source + timestamp_source_gnss: True # True: use GNSS receiver reported time (default), False: use current platfrom time + + # RTK Baseline Direction Offsets (Baseline Topic) + baseline_dir_offset_deg: 0.0 # [deg], range [-180 to 180] + baseline_dip_offset_deg: 0.0 # [deg], range [-90 to 90] + + # Mininal Speed For Track Updates From SBP Message VEL NED COV (GPSFix Topic) + track_update_min_speed_mps: 0.2 # [m/s], Track and track error outputs are 'frozen' below this threshold. + + # Publishers + enabled_publishers: + [ + "gpsfix", + "navsatfix", + "twistwithcovariancestamped", + "baseline", + "timereference", + "imu" + ] + + # SBP Logging + log_sbp_messages: False + log_sbp_filepath: "/home/swiftnav/ros2logs" + diff --git a/docs/build-in-docker.md b/docs/build-in-docker.md new file mode 100644 index 00000000..b311e024 --- /dev/null +++ b/docs/build-in-docker.md @@ -0,0 +1,30 @@ +# Building Driver In Docker + +## Step 1 (clone and build docker image) + - Clone the repo, build Docker image, run docker image. + ``` + git clone https://github.com/swift-nav/swiftnav-ros2.git + cd swiftnav-ros2 + docker build -t swiftnav-ros2 . + docker run -it -v :/mnt/workspace/src/swiftnav-ros2 swiftnav-ros2:latest /bin/bash + ``` + +## Step 2 (edit configuration) + - Edit configuration file, if required. + ``` + nano config/params.yaml + ``` + +## Step 3 (build) + - Build driver inside docker image. + ``` + cd /mnt/workspace/ + colcon build + ``` + +## Step 4 (launch) + - Launching the driver inside the docker image may require access to serial device or TCP ports inside the docker. + ``` + source install/setup.bash + ros2 launch swiftnav_ros2_driver start.py + ``` diff --git a/docs/images/launch-example.png b/docs/images/launch-example.png new file mode 100644 index 0000000000000000000000000000000000000000..eaee9a6fd16e791210b0ddd54821673229e22a1a GIT binary patch literal 29656 zcmcG$1yqz>+cu1WhzKYsh%{1?(lLZINJ=--2uL@~AfcqBq=ZQ4NH?R@&_fRm(%nPM zFz^j}-_QHp>wTa9UF-k;f3L-2u8Td_wevjA<2cVvn3{?l{=KL7u&}W3738JWv9NB7 zVqx9jxO)?G=jQfhGv?O~S9Li_tg-=$4a~)DYYAluEUfZqybDuo%r&l)yq+r-7D3zf z&kd+!fdv+p>pKN$2~98Koo2--HbC4doLFM2mGbSk*I#*wX}-QD#=R-_Rh|8=jRa|~ z!mZ&Qbvbv10)}Jy)#eYCVW~3`r#EeJnsIj);+y54D)1nmiE=-KonNV)er_DH;q(zy zi0%r7oFDz5zWTN2e--WFm+XImo=2m_FPz0d)O+_7Xo)$JPoMbxL>m5ZN4%v_+J8Ti z#J==nXhMGe)dDn|n%s=&NhyGukfkghZa(8}C2UJZpFMUXUpb_ClYX7HS3zyB^-C#K zkGW(YHGj&m5a}=8!7sbjs~IH)w{?0g30u##b}-taZqJntfn0?kdl2=jx69R_RogO% z;YXM;@l(EB-Mie~yX3)KT$d)6CdaI+?fZYOu#5?t=C8*WQ@q^wL3n>YE{y$bxgJqW z+234nHT(1Nz0O)6irt^s^xdCpEGNE@Q}!d0mH8|5{MCxdCAIWU~T(La{w?;B@+HWDpAK8xLUIfX+m$5bfjMJT&dfDzm3_9K!Y@MP@; z6mq#Uf3X9(457GMA+#vXFv9jNNaMM@pt$>sqA;MSazW_F~f%SY?mxW6Cp!`s+;Uh;n9XHN(H zP$LvcPvy5SiAZrtffvD5Ez}VFIxEGOjU?rgSFjsb>=OAFsEdIWIs_PZ9E&u!p@>pW=qZJ zF|mXmUMl02d(KN|(;0IGNSdUK~`LhD3u}Zs0l82A<&X8R% zMEubuz=*W!PvLS2a$a0J+Cy%(igw;f@1r9M*lW=ZzECd8r`5i*9tAxnmp^X0Sda%_ zox#QBrDB^TV~B#(mitwP9E5L1&{aRMWd%;C6yf~tObbiZRAqLU%*fH%R=%G~iG%+7 z_B!?ARY*x~Z(PdwWP@)0lK6!Tc8ONf0*JlEy)VxlfA0RqYk;-o_;Z8X!U?X!PQ9Kr z$0-s7UVvQxN{z75n+1gyvI_dCzn{O*7?|ko!CP+QL*&16%F?!h>otD}ySjjRkVq(O z%6^F9Bs3@3F}SP$Vhfo{I1>WC+{c+#s$=k~I)+6RN7|2RpGbI2FV0lyhQt6vsdO8E zqQlFh2`fU7kF<7kZB=ZfZO178OiQe835--3wg4XDV0g3}a>C|+lrN6%9-w1Zn62uw zUxRJg=CTcXd+&_zz6&!-nx1W#zVGD735MDS{t|up?y+g@vo}2b>+jEL#02CCPZ{2A ztX73HFX7DY^y5oKXM&plnu9-nu#1>K*2Am047v0s|Q*V-jZ$V9OJ-gq|qEi2LfhG-+P-(SO6 z363VmbJeS@D^u&g?(F+u>YQHfM|<4)>kb%a&){++x)|d4XYXOXhkZC@znZ_39|_+1FZ=TMOR>NNC+vUK(}A9_pBTjb{Vod|>ccC_V9a+<#jhY%f8EB) z=<5ddQeTDw|F%j&7^`x=l6h(IcbgWv(`ITfqQ$RNtroUieSQ5<3FjwXjVbp|GJoig9=S5VqjGR^#%Bmgq`WQhDhrq3&d?q3xDujH%5HE)-mi?{z0_& zV<-Pr^&(%K_ld$V-4A}U#+-7+JJ_f;w9^K{sOro8HZjPo)&Xq{RJHMG3v}yfrjGQ? zthx={rnQ7HpH0F%(eHT$uor@!hD;9K#tb#;>X$#)t*Xtpg#3^pmuTt3vpUhRD!GY< zNAelgq|OovZ@W#`J`NPHT%OoOO-rLzTkF5nJ+;o?l}_b-;48fK4LRlgq2Ro+c67M< zp4Y*v%=)liqhg9z?~;rqDzlo{_Qt0__13E2Wqbk-Ky$^czKAkKyElzm;n(#nId9)n z=IJcU*F0uF~tvOxVlBo2JX5$f8Ma zz5~IHcMTsFQUY4Fa9|(kNy#l{^crV-Xg_H9dQcTgy&e3at8oe5480^BV3q2dk0?xp zp?_u6X|xjJ3679{1LAdjWhL2c{i#)1HD|W#=*Sm~&#bDSt8SIt7z%cC@{VC!Z~W` zsA0p77j6JXD5*91taPxz0vL0{!U?>&I!EB8CSuq}qR-R;9>K200>nt63el(>uL}`n zu8Cn&UH!pwwmTo|PZZZW6yvD2xNh6*u3ssYR*b4RyZ|KQlDBW7erV<)Dg%^{;r?(XGoMZL-dYH=ZMb9{A@QJ9&sbI+r2(LmFd- z%F6yzcOZAaz3KNSg-v!dIIdC>H?2Q1M=8HxNb)Do%o!vv32@z-t*$ zW^MTOO_RlrzVQ7a&IKP|xHr0(vHLk>s`zMyDE5QJ(65*Mnt7SA%e_FzF``>~!*do) z?YXhhcZPoXB$^1$(~~6qYb&CA2S$I+J!Vc0M%2-RZUJQXeN5)5*FRq&Ej>LjJ^7L9 zJ#QxW!E3(t3YV8>OnhV|8heZ^e3i{0D*;BpRb@~9-8+gDNF{*o7M5f_nXLJt-;pE4 zs?2MC!m9Ampz&>OXo;Q1<)Jr&Q`Chu)T(%8r8SFE_FXpu7vDM4JYaDqUvzvV>64De z%@C_rizeH#dj@kO&PrP*y|OBMGhdz-aeMAUhoGFe-<=AnkJ61Z&C|rvtfK(#2a~U! zG`V*#Dr^->pML~{Wm>p4>Ilgmd2@5HLb50&eBHS?_6VoUd6+QWc*FT?eg*tXw0Hi> z@v4#R3^|JZP`Qy22|Dnt6r~t5kt${`F~d4@n5YvUj9P7-#`p&BsWXUCBy*J)=P-Z% zTMJu*8Ip3Dc&S1~gji?Jx--->KW2KbjHh%tsj)4_je} z*J$xR;>UkEu>Y#-<$hiFcWDxysKvqpNjtzn-KP#>y?d4C##`sjQ5Xl-Po`|Lb}-%O zvbB14u8o>sBa0=0WBk!Ye+jz2Ld8vpp2H6#ABbJqnPZ{@l}LX+32 zE520()atdu$IH;9h%|`6rH-T-gnPflA<69Zv|f8klb8cVcRc#Z#Ua8=sDJNmm$Yss zA=zmp-l4F}UeW;XRc~1)E!aYMv3UL9)0dCRDtZ3Cb`bI!*75es*MLHhnK;8JWOi-> zbiOk^+w~ng$(}J&;m~&LzFBnEuCkmZYS#OCO*H-R#Scc%P9U|X5Qy30zEF!#U zRh8JuPNTcGZ+&E=KlU|!5%ip+Tt!acxo2q!1lA&2Br-x;>CqaI*>6LPlH18ahM_-K zw>Zwq2Bm^Wl&E4>W$*zFI=aor2qT{Qmx0d*O2oouJV{(-)^5G=&-9o-dqc7$kLre^ zclaMK7Et(MF`cqQ&SyETUUnJ3txPkiISn!59A-0<1%E431iu{Q&x4GbLC8`-G+Y#dr|(7Hl)wAj0}*ZgaJuOOUP{agMOQy#cg!}vlWXdD(Q+|+Bty!ymn$$n-<-uC6|qHM zPh}?Pboyh^V^jL6zmk5fjUlfJzQZ13MzrkP(x-;>Ub{8-TySR5+j>Vr28o7QO=YTh z_|mq{g|2(f9T!_8{!Y8&=}AubFBVR~+tODLNj3FwhO=ImbqX|O6gzuts#IywY?PH& zoG=1b^XmyZb?$C1g`#(xK*8L`j})9t^jA9IN?h&W;G#8him*BpYo_gT2s8@1wO773 zIk=Z9Kx4EkO4hynrO9(%eRQL4s(V@biM4j7(r#Gpvw0{mTdMyEImkPU&)WblJ0|w) z-l)?G>LJrcV}|=KX1K-CtDaUb*UPY=Y;bsKvrq2i)1KlePRKCBi|OWF4G0-(iC%A3 zVT5(SWNXL8{bx*?NvDhKB1`^KjczvFSRo40y_o~1ccz-Eb;{W+VoiyeP+cLb1Tii6 zeo8MltXx@(dKan_>JHeQq!ynwniA?bdJ**P?XB!Kx{&9Sg2M_P>A`TRJ0=7kb9|S55`To>OSd22F*J^ouwz<0Yg|tm{{GWG^XQXtbfHN-md)X? zF<*JMcUU{it4(dyIF6&#ofp@jZ=*;vyNpx4wU4_iAf9p~t5KM;GVS9R>?xWneJStsGmZegEyvsU6uHqa z=cud)8RnV^rdivtH=;d%&9?i|#uZz7_#XAy z(3=~qAl>+_l&9+)-(JNZy#Q0}scd{RO?zcXO^X1GGgXRf+TUk`MHG+nxf|hBw&%=$ z)%#w0)wT2|^gG2c*&OUS*T< z=4d;0pA%>Df;%00NB8U25UVAK#{;SW+FZJiTWz#;u*DyX((#b9l1o_>>N{&iX!Y(; zBLJJ8dR&Pq>nWk~#lt+uE*8!zE+-ZtWC9uMHdQ;kj1g!1?s2h?T;wiz)Ug5D4K~S7 zUz1p~Q4^(5DyQ`--p83`0ZLyGUdf%t8?#&Ra=1|yA^O0{gRVf!$1)Dq-qP!}Lna}M zp;y1I@|w&v_=+;VGc&yTSrM1L?8CVI`Sa=R%CGgT_cQ1c&fOdK0sNLHGB9cBJ-$j+ z)0pb4EW^8!dmN*CPg+BDCmgrGkNi6Job_@!N6BuZqF-l%zyA=hjdp1V#TEIbnC{0%(iUwkk%MJXp!Q zVuXhaB%Sw9kTYe9ANR|lX*@oa*dFRARN)Ks8XOzNsf0K(Td|Djbb3XiGl|LBJmx~% zK`gLj1R!AMC8DHzgC9U(9;jQplz~3bxa}rYO^ow=y!v)Xz1FA~#iWT(I&XZNjr2r^ zrjoN89c28@#;1lP}0e-5*5RrNRp2jS@P zdr6k^I~i+_Q3>J6xA;x8621EornZ`*7*B9;1!(PEHms{vou#jwRi2ySe$+VZdsEWr}VQO%0S6Y*0lFIX%+wV2ruZNNLoC-mR*V@HS&%Q1G6H&zR+EaXni`PmDdM? zD4~lJ2b}LQ4)FS@i|w5}TF7E=^Vt znV>3msp91Jr_nm=N%G(}L2#8ALy#o<7s7%GlVyW1ov4f-r32itD+~p1=S?H$L&%~W zwKLlt^)vg#JYRE>F?}-QsWB;t~Jy`iP5@MunqYjCGG2$Wd-HY*d96AMMWi8c;o2RZgD+GRxE0) z?af*Ng-GUF%lx5z6cSw`7=5U#dx(G6igmm9XC7}LZVz#a$*Jh+O|19%K`0_D@Jk@W@FrARF={ht z@1xz{3J2D{h{~=3Xxg8U`;WhgB@!KvO3d{CiTYauA-tJM{FiR>#~J-y(U|hT^pXLL zpT>!9vAhKRItYncW&&vzyvmK>s|HsUPk?WpXNn)rYj_Wx(j>hUKlBaZi(C##E<2-I z);2Uf-J=^*SEMGUS}O7Zb=AehM^l_;%j1{b-=92h@e<|CG0ga|wdZ*K#wS7;8KYly z_B3pWp0Z5GN=SM5!aU!HuHQ%A4f0q~+48xX&g=!Cq`AJ&vGY-hAD{3x_dCwt-%NXwl(cAbLAnYs$%N0%U%z6Ze$~}%63Lwg-#ZQI zUl?kdbKiJ;7xI*e`?pu-pUu2wuI8#>>sEH752g(f)hDDj_a$B;pX3!EU2*0lGz9<| z!;Tom9~Ga_Et!vlVAN+DJW7otS1?lkme<6!~>eL^8`Ae^sZ2LcNWW@ZW6^Cs$|4t_qm@ zr_nb)4lq%Qu`Mtnce?_w%KR!lDSou`vHM#8>XaqIaETyjS&7 zXSp!5=|u}v#R`!A9$w+m=~BA-Nw`{(Ii9r=`1)R_U1+GnQ%*ZuGkvz|A@UuqiI!kx#sY_P3>);J~cm|7^fDsjdb%;X{# z7ZlqU71!a71?i?a{=BWP@;IN~c&E^On0UkWoxCe-L&t?HJ^Qi8J%{Z&41~7$WCSC%SF&k9v}=hj<>o zUg}={AT%)0<^Z6z3jgdI;UP$z@(FKHru(*$W0gG!iee8YOHW$~KKaC)@i_gm$N|-+ zaT6_yh)g~zrJP)D{Ig;9{ia%E2>cOS$xi5>cx#8Fh$W1u1*1m(qCf`)J=GhduMc)? zmY@5!7w0B?aZb#Wu)%6B(m4qWD>ngTJ(~*c`W5U|ZT^7)y7@KuKqsZ?L**787G@Rv z0A}QOpFj7EV7gucaluO!HSs^tIE2MWfahb@ve5cbhK2!qlMrOyM{*DIfuw?pw}y33 zF5pi!k|O_1dnQ3;!^#0P7-_UnmSMN*XQV3Zw}-~Rp79Lp9RnM&-I(W%_muTW5Pm=6 z8~FO|-lSaRnj$j>ohWPG3{$t2G~OH@b#<94?XwfCnql_OG)ky<<M?SBGtKjgJ~2+ZQnA2spUykyINI>nUY3=rAecUjhUl&IH3%c zkU;#Jo6M$Ys8?0I{PS-9J0OZS;laf^Q0e;-lTlY8ju}TK8oKJt_V~ChD4gASxxtJG zz1FggP;=PMvhpiUO!@`f4O&A8?!SjisMM>S!E(z(JSfz!|l-EIw z1-?3r)mFVP?i6cIC_q?$5h)n$!pQk@RBo!xbxR5Kxp>=PMQ{w)Z+ zUkmt`&iwDuum3C_Tn{UjaXQB4AD!Yg?!A6IvsY25zG0?}C^3Z?CZRc|i6J<8N>{oj zS3Zr+2e9XN#3z7x0}Hpxi^m33Bp6R3!7YB(vCsvxo_eK-FyFSebpR2!mvgYQZ)=!@5#F;;$vs&beCNeFo#@9w6Qic z8K*6K-iaPR4_Pca0ESRf@dKKoUSu3LTs%2h4of4*i#Zz+rf z`jB+xr>-{L77;@DA2scy^qOPRbWE&*d_p3=MyciF1@G+12POGjSUvWIOa}exQ%Qv1 zQJ_&sKixG5)tZf3W_b4cDKFQUzetgAFLk1$c1SitZ=DJLqm;h)-2JJW*otC1Tp=xq z^OnLnP+l}_V@vt|65C|GD{W*5uXnplQeoMyXk7J~KRG7_CM^OS1C|yA(M2S$|Cj~?*?=vq?bto=ryp_ zKy*_a#Gv&y){(5@4gu}ejG{b3VQbbAG6e-SytI(egQ?S%{%a9%qS7WVn=GW%O1{s` zj65vumYIni`(*F`?h%qB(NQqX@I^vI}^fw|uN~ugry$fBbr~F9&mA;b0Q0y8%7NbA+$`C#9;L%g+Up(2 zcHDN58~!-OSWX`z)3V`Cv^r)NF^M3`yfys9rdAKfH4-Yv3)P4>%N^!xc<~efZ9W%G zO?qIIxr;o|Vl0XviGI-Q3Ml_G0}&YCcK%|Ql!wM|`~o<-hg!PeB4sHc7lcA1Q9AWk z*WFwYO#?z!)(-Xzj>DF5R2EOVPGWjJJN2jpDsK=2uhI&Seh}&j>^m=AYVC?&f5D5; z`l{u_<=?SI!e7fRAFlp&bpv&+{A0?G;-LS{aAWk`?F%Y(6j?p>Mue1Ot-$5a%7tl@Wwgy#Sxbgv9&SI>WN2wURKD}4as=i3#s=r!)Uk8sTRxf zwkU2W&*-~#q7Lf#j+E)>5QY)h_8KB=E3I>+jH#9MICX3J#R~R50kPe{GM`2(+i}uj zWucUkw_nK`Jis_x`@{KZ1jcmvJ?5CM-A;pZUS!pvNK*sf{(6wx3~_jsC*IpldsWx) zTbDJOEl?P+F_OWt4fLOH7Vy+cGo`e-+OLDm`fx$Dcoic9Yxhr{fZx{*5D{3eRJ#I9 zMOuzxj0zw>A6Z*v67)uoRo1bvFPbh2-*(v~9~Y|LOJEF?Pox-w)%B=!=C{|g9Mev0 zErpFK)UjQ%#`G8nUc~%L(ZQ(b@7J6$anmoEAnsd|ywpca_TEi0cxL)i(+zrg(I*Rv zNfsFAXL{CsB3ZjD)0SlB{>H_43X5E3&@~KvvEu9=#%$cymHA%?7M@Q%lIUj&helIVjj3yZ$xDm zzu8ZfX6;eY#4mp{Wvhk})7QksZ$E54~AJS!3m5{_AZO~NK}hcWR^AkOMg zDJ(5OjMJ(D#^RGQ7~5#94tOi_VQ!!X)b+^P{JTjkO^{d?$VxmWp1Hu){JUy?94pVt zmyYCzTXJAI4p;X)P7%0cP5g%SX$k_()VaA0FRp>!<^7(rDGWLu$U;r|o%O5~CvWy0;z~T)qUmrY|HLm+?_&H*vqt+p zar;hg;M`MGM;8+0D0Hy2Vn8F+@l; zy{&qv^5FQ@Hu=m?0_A7O@k$n{db`LUN+1 z3dEW0;X;<~Kd1RI>rw2(HJqR?BFG+CSh*1s!-%7iYkBT6eZ+T^Bbw$mmT!}LExmP# z;f&F9A5YR#@0jK(m0orl(X(gjv-`RJ)#PQLy8+Ku2(D(<^AYa@gIB8e6a9ps$z{|w2={bo;N2`r#;B02ra)}fBCtav)fT4bZVo=33&1F*%1o5c)jZNu>s%c+nITV zApE5537Lyp}(9M3*7D8w+eU z6t`ZefbVI!m|g)fsXk&6y(jDdN4Lhuwcb00kdSe<*HfsiJPTxH8pUC>;#L^%7DfX7 zl)4~{A=r;oX24os0yPpS$$ zgh2A5;RI}PjZ2+<%6v3YY3{KzO$B(HiqkUOFDM&HrZd^?1pz*{D0h11n^|??&c+d% z?~*kiP3;+H_!!Nln6gy7JmYd9hRIx_9ymH-cO$Q20?d2izuU6XUdE{(|H^9VefxN4 zSJ+-j_L@`eYHwe}u@dL4oAce93$ztL^r@u5?;QA~9tra9Sz$Uvss1(f6K_QFP+0NS zFzW+N67fb%&_W$uPat9-tcqQ+Ap1>k>%JEQLlI9mn`wU9l)~bH+eeRe9BF6mk0q^V zYxc*?@r3vpgDqUp`o1K4w!_qv-*Ecje}q#xhLX}>yG$!$Qk#BAn}d>-NYz?A@uDwJ zU3jI0u}Y@xOD+x>)Pp^D$sZSe-&cG$b`gcDkvw7aOi5ox7REnJdglh1X6jxtu2|oU zLAn$4_>~vkdT6Ax?R7u8UIn!JXJ9H%*y+mGPI=7-HEBaCq-n;c)EvIiuWgLmTL{4a zP`2n1eJs_ni`Cm0&UP#RFL&K_b{sRwhLouW7@dpqW|qe_9GN_}R>O%C>FFCVe`EHR z#_J801LZi``jHtUs^nV|E@k}|Lp27D)_3f;LX@2OeNcKA7FkVEXKj@465_iH8{50; z1T|1S6l>fjHHj;ae`Bx5bl^DsPGmC1;!Oe><5?Aq8&BEKojgCHP)h8a3dQP-`C9pu z4=vVA;jY>^bWa`42Lup`AuI_3u^)|Tv*t?(>{bPN0N#`imV*k`6uor>ISdu$1X@Gq zkMUM@F*#51aPi67?`lqkhKy@BDIw7}uzrLbHJuu)%Q6c{G~hR&7QV_;B2Ln}HdMq1 zcUOM`A*rmNaV;$KS@_feJ~oVo#)_!+ZOPEhnrokHRgHYv#E#kh$mnJz@CSxeYWf%5 zdaGU@zV87^TAgmzjv=tiA9!C^Zf;p2eqZTb|8~y7Bl(|%J9(hKN7k0#GdolZ>_`p+ zv)y}^@z=~vGbhE3Z&q7*e`rTaIO|%3Jd97*^j9lM=vwX>YLcz^MKvSXQP$xey+S{B zU{|26k7IVclYFF%_CawBU}A6eTnfu(!_!Mkg1M!pr|Ui ziQRx5ZpLWTyH5l{ndmo)L+grvJ$W#&E~7*(-~cDRMuiK#!e@E<&IpNRBNhy z@9bsvq&cqEKBqcM(_IiKZc7y!=uLu6&Mz#+~>~N2X*Okc& z`Js&&lKR$_Zn=bER(Qlg6RTCuZun* zDzTvy4rQS`!QSjoD34*ys|}?oh_>5BgP%8=&e(IA?kKjTnJdAe8(V6k)=TTMAHLp+ z2as+FM`J&f1SnAuN_iv7?-sT+?D&){Mec+gB$37%km9B^6|coOz43p%Rv~82m868s zHqiM*0%@YDh8OJl1S2>8Z1YV~it_gkSgE&|I8Mo2@`?D3)N`=7>pfV8Dru7>3~|1> z+e|r&CV`BGx<;agqA!RfC|wo>KVZfoB=u=DF16hqrp*T^qdrmv%Da3vYuvt;O4!y`*(26{x<$OIJoPb+~@>!8zt2 z1twb`{;$~5g16z35Z50s0OJw-PU$#~C;RKhd!`^5W?p`m|KKC6r2=~g_IMJ&kI>~U zYR4YFOPS_)AO&RV-X}iR$LHnTkBhFQ&>dD2))SP~rS+hn61+VH9#DUvDjq3%0jo$+ zTA)^0O5BCgji0bbP4Qm`Hg}{5-Gpnr1=qjtQ;W-ll28{Jvj`8O;_$`>0IgHXD||^ZBhPD>egm8%?ar9Ou>DyBpdgYlFi3!Y&pgmWh0+7AvQ5AKy!RRiV?)Gl zn~s|_Q9Y-3FsH3{N7F$9KpvTsO;_~Zv-A@Mytfo?V0(%`KP6LN8~8T`$aAJ8X|He7gbb`EO9t=^z^o zppUL_S>k$8&ig>*1z{~{^Zv0nS3FRAs5dxj1`#)UirCH5X{58&*yEcGOO$gwzzDCVK2wI|eT`#= z0f7%HSpiywvHw(FAE#Wh355Jk$|?M^N0hvCi{MdW4pM_sK0aJ4xE9sSJHJ~U=%x#B zztjGSk~JP}`o`AIis@7S%X=Uj0fc#;Agk{V6N0u1?{pDA`YrpWho%W_O0>p98PXn- za*!GY$)st!1=ASyBRi55L!nf-d`#?%I_Gesh%KsKg(9g5wbCkr_Vq7a+xHLRr(6Uv z{dmuWv8kA(-CiqDU%vI)I6mz`({%4gTmh^9d&)a)2jPj)U;5}$Rr00WM4Vq@_!>7s zF)FE9+>;6joQ_mkMxCftK1?#r1W%o_vnM;p7wG{xmlrYP0Z)KDVd+1W5(%!y+9IRc zkyhgLXCSxC;Vn`11d{Vadb@=cy-T4_F`Mxxg!7>4XaTo??@Mk|4$}JnTt}|*A%?D) zM3kvv;cCW{a{naNhAL^2tYRJogY=EM6BkF0n3H6MHxL?xv#3X&1!BcekQ28sABViEhDu^yJ`e^{*w>?kOO+egdW$0ZS8!!+tA2yg0*mNuN6!rBSKrdK+(eFc!f zh8^D17n9ptxlO(xeoC=|XK|T^uq4rT7L50lmnNva6ZRLN9RVw-Uj4rYG!ZlLLRD|) zdUW-}HD67W(4|M`B~6%^Uhh&VY4g{{%llrk0-@Y;qYFkX8`s0r>g%E%hnH- zx80@|8zFAL?b*V4Ch;`15#LWCYJ3OnE(X;JBlhC}`=?tFzylV1u1{ka2Vkfdl6iM# z*_i6tr5bF1+j}D`uDNm`{|}kPGh~_sT6tdy#bYvTF+le7|DKRu>vS9WnSJlSV`%Bl zS`R4aH!Yppjdolj93+POIhN=Zc114~swKQ4wX-!j*Ds&%KN4qKGjCuVXd>v_*-=rJ zBD|9FWF$TdIj+k^DSWC1_L_xIT1HtHOb;@Ii!(+X1*6ZbZ+)Vfmlh}UFt4~)cLfYx z(GcJs)Ga1m(@}<9K^L?&#-$!rV!xZ|l=P;h;Pw*H3ai@J zv3HG*M_n_D2tUd=Bxf0H5OW&py`A@Xiwr$~@8F}Co<2gK3nMW2R=*dYmXA~Uv$A{D z;>jk)9-*C)_ZI5qsQ}l~FNM`V-1EPs-}MXqhTB%C_uz1zf?EnyzZ!y{-la;Gt95X? zgKSFf7As!2l$6``QP{ySE+4=&l2#+_Q~JruRJ>h+c#einkzPv*NGe>dKi^|r)6CRQ zMAFgxi>|njs0&#U5Aat#s4`tiyb*>;_wL5PneUJNh``SFgcG2Dou3?|G@d z3?We5=&$T`TUOR0&ACVVA4)CY){@$59@2F#v7mYf1qE3xf&?K>%fyKGZ%3!^;*RdV zlF|vOx>P@X+r*8+EQzueZ_L_dqlr(MT~ZciR+mc;B@$A7DEe;h-s*;JnVQv)g!H1( z|L>Jy^htG?f%h+)^a{T)wfTjRj&uvte6DtuKsIoUt~fJ6#P$obKdNfbwW?}IPwimk zz~HZa_6iqsz#$){>X}4_)5ZmzXVAU%K=dK*)_3erRR!-%uJqEEyau8a>6x zGeAjXqDXqnuHJBg=QORnr^%&k2{nDnve6%*Tbbip=zbb|=^+xoY!^c5HaQMOn#V*q zr!MUw$FV*N3KR{fMN7f`wbukQDwpEAQ!ZQ5&sLrZh3YbS+ZbbJ%P9tv<$)24B`%boPKNtHZR+K|%}r80W&oy)Z3sP`CZfMV+42XSiNjPrkUG8`_Tt=U;I9o zjkiPumC0`(KH#{hyL|Q8(ux|R_*ehI-V;5Y_NwFsJ$WlePNs<;`FA|4|gBVN(xbn7^j@=6~DxJ4U zZ;ZZiSu-{e&!&E6DuAr*r3m9^!4>OKRUcVjpt4;QJ@ zAzr4+*UBW5`@}`&>-*D4t4p60g{|#RU3l?^iZ(g(F2-vKy-TQ*3IbEK9Lh?x{8~kJ z$+!|K2R2RrNLsd;oikZ;{i6mW;R>rxlwHx@q9dSRhb8`9l9-r6UF0=Xf8Cf#)1>nK z!ydC6{{wsER4(nj08Ih(#;>{I#A@o_p;jyr!hey;e?QpyKWoSTcA6ZMaWi=i%sWi@ zCO+=DH0R)WarEvdr>eb93mUaiZBTVB*D-XYqKkxfxl!-jat}MRoGwBb2Qi)JJ zTskaH9&%IHt%B2RcZJ?ZzXj)eSL@4Hou)?vXNbdflWI<7RAJ9$#{u0E<_P@!4NUrt z$6S&ps{`?6%~^ybvHs_i%ZBF|E5HGHe;@9452_QQGwP(g4S zN(`37%nod(cJQ!JEGd_-s=txjy%wjbK|K*_JHK1j)@dPksad(O%D+}y*5z_sPJ`_z zQLV>aNsKXS75YNLVC#QZloMXUb9HD6*g*9$=l$}CHQ{}hX>XPoYyqNved`~r&tIo2 zSm6RzMpcNS^LeVI#A(7SE1;KYpD3*k`l?g+QHdpDz{1Yg84Pw-=#H&e$wht=bptr( zA^AEcUfGXQje{!(&a&7AM1J3VaXdS%GgCtgzv*1pe^-x06yByEY8LVvxXmow)ljxo z1P?H%y#O-C-6ZMY9Q8)mhv-mTYlxFY%xmmfbB1fj0 z<~d%PP&?pu$~^3lsG%E=HjYly#@9FTDx0@w8iDQp_R)o_98JHNEv~-WYFlnps?W4= zF^>wNch^;b)Kq+X<@k-rVkmYji}K;}*U9{H79_<+MnlIz$f;JG>xbN|-mPDAlV1r4 zbu!nH-CdiN1yVj&N&JA_*hDdcgHGA4YpBe&vG5S%B8gza^&h@0s${c0AMbh0}U_3DE_YlQUxS^dkP5JgI zQ3xe*<6-!c#T`3>w<(eh&0)FCKQY#iCgbXqUBLLkP`@6m;+!UvN&RL*;dI$^`~Q1h z46x4Z@Nv_G1PPORYiABDSQy<8_U^z)<%0@TAG%eF#F@wYxa(bb!6~L!_8pY1kN(k{ z?C`!fb*Z0rGD{5@t&u3(#iZzE8Z)`nkL{+=_T8rB^Gk7E*WmNRX}+pcGSBM_8SU#C ztU?HJ5sgllU!7sG7q1(G_aH|&hOcgM)*iy%25V;W`d>@Q{Dau7QTZEYjKe*c1h988 zw}}R<@%|AZ<_#HdKXa99t}kAU+V8KTVb(@$P8T|=m#YE_33D{ zM|isX_&H)Ub{q7IX5?q3^)$A;3{NHgR&PSD2<)3Q>^x%I#zc zRnHAA{0;Uhb-%%$Q}~z^7@{$qgFOsqDwZN)dZst&3`+wGYA9M*`Wj+Jh8gAq`QUJj zfJyh_r|u5=wa#yk_TuvY@COO^kaqcrZS``}o{|6V~IO5jZ z7xk{?WkZyIr|wZ6H{&VblBR@U}9b>~imdfR~hI{_DX zFC3v&!OCyj=c11j(flgjn5bN2pw?$0{+s$u4~5qnqr?G2@V)i z&Fdt2SKk~uOQgyMR<_WUse;nmPp^bKF^iCIhP>!<$Cv}1)n&{{1F`Z`5guDi%JJ{ea@x-S99rJWEtu@z%O+sk z+MnfyuR%*N8nz#9*?HO%%-CO-|KJ>~ZMUqf&8K3l&+p>J{cn+IKb61!WyJm_W?A<^ zk{nIQ>o2}3pT8QS-fDEdPTCTxTp*1FW5Ugq>ap{HKgV)MTNW$L#A8?e*9V|(%4yz| zPtJ39%0nXLp7EaHVAtoOK79CZq@JfXb3DMyfA;f>OI?VOYOtqMzq?EF;Z3iQFh1+U zROZ)28#-IzDkn01&1urAy{A|tdQarL1?}g^AjVW9T32WdlU7a0FTRs|OBfj|vx_g2 zI2+e$4eeID$p6bR0>aGuQ0DC(_)8o{$aU!wrz>R|E(wv=5wMNp$#~cc79XMWNT$N@ z!jfC1(bT842Ar#P;s2+$^Ny#w|ND5?)sPfL4_6+(oJkQL&{%sL^CWMz}= zmA%Qz?8sgrdpq_zWFF&K_xBjB`|7&7@85lY|4rxNdz|z6zCN$l^Y#Add)!~TI!ro# z|%}T@_HWsV;IM7{1)??h85NDIO(rt zL32}^ZV<5>{P}b50q%*1s=M(_oHu$AG~ zP~7I}l8fmBP_4VUK^TRS2*2Zi)JJ4e9ncq9xFq5b^u{cJPj5q z=c^btTOZ-e3!DUYg7xZ!T!02tcAt6>o_WjtZOj-ZVLdXl*$Y+*SMD4wgFH%IwwR(}}+eKksC^oj}mWwwkV_yrhP6c)p$KOk=N zSgQ{9k^pXa0RLPjcfji9&Wr2UFuTAqT7~uZV z3O|Ff!j(^mDKfsVqEIle!U`*6t?M|gUWy08%Ddz|YRLWG@x5t4q<4Ez{pJ?sx|DoJ zCT}6QJ)?RZ+ip4Erp&q@+8>-MOX(|IC^EDgCX%LGCwPBI0!>T0P6w1Gv_g-g=dF&n zD#{J7@}uLi&fh<4LPO@0Q-zyVqBuXOUE% zO$!6+zLd}eRmlZ~Q+lgxbT!Ed^NB&sdqGiqpKjSnLfhGsYRwiuN2d%h0cq>ACIGXb zPAR;BT+O{Zl1b>JbTkaW6p~;wzv#COp0T0&BdxK;Px56YC4nOS@ym(CYKzQ@j zbYD58hQHklkkuLwYdk)8g(YORSgAj6dlaJEPRfBjPgorXB1nzmCDmmG;MdjGOM2$O za{4ITe$cK4rU0x2v}n$E`4>BM;W9}Ee#Q9Y17<5;diJgt87;TNCFFmKxHJD-RZW^GY~- z6k&PdTJ@ub5KHe_G_M0FZQ7V*Tj?ZM=wPu*k1gY?i~ZVTHh(py`-w@Qx=ea>L6FD7 znN>8_y01409LyY9MNJmVWNKH6&WCH+-ZIR-k3gM8b!tH zWcOH}@{@Kc%HX=K2YB}Kig!OU)2B)Ji-|wPatx?^jIri)av;yfdW=aR<`ju1w_HNUNz1rhBQXc~`cK=-|zc~szzj74LW_@X$>O+Y;z^NvJUa(xn8sW!K-(|8Io2B}rW+CO*^Siyii>yTt zJ3a@IwLr%kz3cl~mS$hlc!APX$&R^WDeC7qfTXd}Xc}5+V?Rw%D(C1--=eT}$Ck|; zE|6sCu`ZM7%4yPTHfS^e(-9BArRfJ9f!|!~1(Bmy44=k>ppREtAsV3@JwA-y${Sq* z164}vB49GPnCuoD#Tjd@a$q6~pdd!;s&_svC_lW|LDa8V$Zj2SwIo)8&er%4T7w=y zYb_H8BBH+PqNazc+yZIHK|G+ENfOn=v8&pGOE1@aCtU2)j6>NWv*wuBdaNQA|4Bu# z*4R=aSXn6F$Z9wvq3^6JaSK{dTC7MGO}`26zljFAdyHL!e2u0=vbE*Y(;>9~&0cU= z0n`1RzfirQ#N4kjC?(R=gqKa6wS)HfTuqXe*-jMenn3~b+`qBy0W$GG2EsMfBlR>H zglba30o)I0L=Xn*+bf4jPeGSqP*%LV&sl)RX zwm-nVS-8!aoNCUvPQ!9iAs>y&mXyrTSd%xzOP~D)nM?f=GEaVmI}tG%)X*SjJGAWH zzjNO7)pARhIrxK*iO%i}s$MUvri(Bd$ph@y^0q&r_W~!Z4G@*wHPy`*Qt<*8*)J!N zKn@HE)C$#Zf1~G($&C(ln%HA&1MyRQ2J4eTagd)KiP)OUDOFWnbS1E>=XIN{`q-Iu z^&NMrY1TbH`Wm>Q0s^jIe7-tRmJ1csuR2kiFBsFU%%c8->bFi(v8%NTo@ZyGGp zoF3D)!jH8~2gR*3(`F>6=D}fub8N#kdKxMk)hfv1je4Yl~5-((A+@EcEHTl37az z+hs%7_?^Ig^&w|nMQI_uPM}=M?D&pvk%#aVy03s;69%x;Hy{CbqAw;DjXGn>#It{9 z4Y~UgD?L6_)n#nFZ#=}#*MU$bD7UA^x&&@tTcaS>aRz2$%#tDl-`txay_@?LEk;wO=cH%@01@OjKB?3x@WwN$_Mkie?C3arbw<;}IzM>;ObK zf=#!D>9`XV7Xr&JfPb<~EWcVNxPNb%kg&i#6$V6nYFeotyNsM!?}S)W!^ZHpMGfe( zWr^eW(9hgdzYHxs$-W8k<1gxZ#kL0ZUY>^f*2;>H7UenbqFToSind-1dZoC9d|-ZU zL;WWqE=@e$i5DBIKx&K#wtYy~XcYS8j6hSvfna(k?24SgnSik(n}#Hhyp}Ie!U62n zhpha)4;?f9m#D*pIcbwJ_A2F{wzh4>TN*2uwDsyuv!g0YvjcMkn4=$%{Vbb0x8?FY z9y~5+BP=NOlKhp=qRuSwE1xAH-Kr_XKAoXIsyi3trBz%l#5SPXLsz!DLfQ?PHpW{_$F@{PUW9Iu_W`4Ne!T{ zI?%=(fHQOd1oN3}oD-Vvn6SoRbkL|&V0$w*TC%qz>K9o}NSr=TxgY;Kz>N{UZliHx zN&f$)-3$h>DHv=v9Tq!t%@nX1~B(d2nQ( zq$rL-&R3gmI zIhO8-#d7I>m(h%W_%5Tlmd^;;+Hihud63bpvKgDOPeQc-3Wvc`7yh$d9RZy(Fd@s=&biBoMmL>ON%iNit;v^H|neO zBpNhu2ZimdsuE7^T2tY4C_!mt;ZMCGS}_3WG`<9UNrYS8i{C4Adv6#TIhinBeQ0W8Mim2Zx6*&xqAI?il?wiBT-u= zdjZQ}yEhGjQJ(l)G}n-*t3l)|G54fr0RF85~Jyu;)$1|ev1mO=Co4t7z&~1ch*&-IlR1`O^8+PsOfR`D5c2cq^J-b3Hz^h~f+Aq3zo0x+Bf zS74-BoxU?99c3@#!mmMY>{!PXoqQ2y`SJtem-?s0G4Z>S-K9+8BBv22TPq|!E$;7MQD?mnZ{wO_by%;q}>r-CAMrR6R_K?A&`D#Gz~88A4wHz~BJI2_Ho#Vn%TXim#BWg{WaBHW&x znUT^Sk#b7a>h~lN`^u(S0ac$-J9UeaKsUoHstgeUUnzjNzWy*z>Y_2yYTg*;< zLL)b|CDVkvoH$ce#B53tQmYG^3=-}I3Xq$Fy*ccCWM`7z>qFaZ&LxbvLluPeJp)VJ zwutgz+OOO{`}5uymMf^0&iVum9YQJAOI(Ejl|1UEZ-PUfZ#Hv)I>WJmTjjL~~D7p@V|evn~RlU(p!UVF*|o)4Y$i0U*O zbbg!)1OCXo8r2ojQ2E_kIkX2cQ}g+4!O~4P)|v}sk5?)ItKsQ_kkw!cCC5?B6pTSG z!Vi#(-T-|Xj~KUfcqTioo|QTC_$3@L@+qwruqh6}3`(^wIXl5%9>TZM?Nic6_? zaU35daxSf~>_^^_K+*fv&YP?;UxW_2U%@{{{2Qk7ya9>|>~2-h)k8p;!6e&B!?s6q zAG809=l5sM9Q%#--YcCd+RtSN zV#wzp>%$J|0gG{HwaBh;&y(n>VYvh9KH00+*2r@w3%QhwTR$f!IG21rYIhTs^~wB3 zp1NJQ3jwqK!&5FzUbx9#MI)Wu9^=!Sq)Ia^?-_y5w4s?6sAU4!4IwaovA<*WhQ)u&zB@$2+p}I|0&gJ!!t|C9XDny8O-F&ldf|bn(qmpZPON*Nq_e;lAdq!>Gz*o6yd8gH5VSMNe zoLU^YzQ-Rko?eYD6w*^J!|sl}3HL*Wbt59aw^HKpUbZybPO=dYv$k#c3Ce;MbDwo@ zUDl$JSt_=7LQ&21-op-=maKd5$6bIL8C2Uf=;~xZWly za3sRPWdoEoj?$VRhW-C{Jj2qgf<@^V&oDa1GrFFLIbvsS$AZ5aDQR`2%%nb_N60Qu z1b+#k{h8_)XkpaatuPI0t(`8z--UuMI=C0%=0ug@_NddjvlT9~m}%Hf&gdSdFDk!EU#yPO7xv%vO~NLT*VdV|HKT!}7xHm^E6nt| zTPT6~dq8piT+rV*C)}xfE=te-LVq#gHXZOca^fA-cVYC3MrtzwlNKw%8$S4Kx%?>( zTo18c$redH70y0o?KMU?_A`vr;(SNyZw z@DT(%^Z>aGGx~g_1cdZ1AM#trc1X~md&9FZ3*m-|S`s|Txp%K&CVFB)!=g06qLD5F zrmVk}C#!p9ur-KJ218l0caOdn2|h+N0*(+3RaWl;h<9MT_8$v5oN3`pZ8i_kKyx2Q zxi94WOS`+2iCD8C40@9QIW6Q2RHH|q``2xQMV{S`?!qok4!4RvI}ayo+6o@*mU59)n^dhpg=28IY z&fzmv{}|oKn))}Dz!vzuN&vhLa4qj-B;L#>4 z^^|nTfE=L&>6i(`Afk^4ED7S?Ib;1uBC=bUvOC68WC^9}!jol5*akpL(^3)m%uHN9 z-zf?EsKcoB0>vYvObduhb3VCT+A zAV*j%iXYX#aRru%ab8PXdo7Kru5+T}YM8(V{W_~Xebeh5i99t?tPLNLY|7$d>uuyy zr(J8osvfDUz_fMytF3@`6B;%dk+R4)UCdtPgWruDv9#SqQd<_`PHY!NZ!P#|GQzz? zf#}4(5CaVrmTv=y2xm6(WSFI;=JWbaVcPwyoM{dByX!6dF=GcZh2?3~*~k0~<|6y^tKv}pR~GaD^g!rn#ry<% z6c>(!q#c1CWTeNS$6UTnxl%VwD`Owtc`#R?p2fsZP+ef5r}ONd6Q}7c?1_#EBDYst zJZ${h9JiInm|w%V^oq&2xD___GF??DuxFnWAxW*`>S@sa^qbjDce-tW9T0W(PGIJU z>^=8?^0LnsfygYnw-PbLf}MFSK7XU<9#CxK2j(&78I)y0duX^BV9SGisU$^)Zqm! zFO=ou_ya%TLmN}0s7YlZ2JWzrmm$GsJtMs-&aA!OK%XtPL)ZG^nWl(OXs;fsng$cb zg!frm6Fi>*!ZBhahB`aqRH;(Z%PTe*wZZzIs152<%dhIw=nFy=a`RFE7U_lGD6oVE z^fKO2I6pKpDVKCdKJ$W}CIz3YFveZ**>WpAb!abCS1R}mB++(72tERu;TSTGy~pGp zFn_5lwm3-N=rhGd!|%csD)kMylNBddXS!)xZpILD^g*tCke;d6U`dVQjJ|CKQ4@ab zsUUg_J}+a;(E=lK6mw}2H;yTYLmN+X&EUkhG96?qqn+eyuG8pdDElrjssj%6FhTu= z@9wdgTZ&Bozt9f;CLj&@+w!8y1gB)O3cj2ARC@tBfs zf=IK3+E97=-tHfAU#!c|eW1O=q>Tw&?a#M92Y9%-+?gD{>V(x(u^l%lr|DCYn~7j& z*h*Ag={cGjWmtC;plhF{i^_u6J^EZPcA1C)Z$on|H)br|+*B#1%W~3{CUp-mxf67* zj4r@fn)4rDzmibZpUOdQI>tF?vo0 zNpDTB7%z@|D?#o&=BeVy?EC6#i^J<+HqHym8&?NL{ZoGk0-XC^RXpV)9uQV5l#scD zBATD_>Sd9a>G&?MCRQz_o2M7*?6kgDuZ6DCgVG zKNGDtQ*PqOX_0|@PBO4wS7(>JjCHK`u%JnFP|!T@qwM~W>^BL|lez*HEm)I!52_@_ zfF*kA7G{Z_dIm(@+GUGP_78ew_Qp@{y!BR06oY5h6WN5lag?D-Ku@^#XRqft0Tx1; zu8v1OJ6IJMhu{Q^z6k`pplN^H;wih^E(M9$CCBwh?u);{bb4NhaPgC3xU!llQ+v`&D`#?5QzG-IzL|nMY;YSy;!K8+uyR)%{hQwKmN#~tF z%uV`Y>Xh$Y7Gh!xdHR~+kb2qEo8Q%O2l)+Z-D*g-mK%vsc^Bu`+aUYjV zF>6*`;0qhwig~6^OH83o{trluxYoBA`yhwt&_3w;&+LPZ1+_iP6%1#Lxr%VV)jcqC zsb$fE^;OWsJzyJtaAl1H&quBwI6mF*gaFyMoB8{>N0Y8adRfd$v(_Q2e{?4&qR$T> o55z4yI3edZU)@q4u`(^ZzrVf<9xX7@KTv~`Vltvxw;#RyFKwr7`2YX_ literal 0 HcmV?d00001 diff --git a/docs/images/piksi-multi-configuration.png b/docs/images/piksi-multi-configuration.png new file mode 100644 index 0000000000000000000000000000000000000000..0968e559bfe119c47564572eea30864191d1e4b5 GIT binary patch literal 34624 zcmaHSWmFtpvo5Xy5;RQE0KozzxI2Rc8{7%Oo!}0^5@c|97~I_@xVw9BclSHw{mysZ z`{Q2LVy1h#d+*w%&#tNtl$RC9Km(w`z`$TgN{A}Jz`(6SkDpQCpr509X~@toSUUxA zVVII(qFv|-g0YZ{5DZLt82W=ABJ>>9Mnc^V1_ra^AE2`4Wab8Rw%EI;^c>SgOtcU=0Ocjs~>SSKCb{}<}9w5nc>q6?W8w1rOT`% zqLvM85vcp}-%5Yi^m#voyAh(*EBrcDagy3xMq3a`%uZ$^rO4v*Pj0fLEF%$)KK+80 zHBs9Kyh~zG)Q!I)jplIT2MvNxJKvMJdiq?BF4V19w6=zl`Bq$N8V885X6jQbw_Sex zaIiwSgE@st1p0SJm=5}TuoLDDgnc}NY*l?@Y1Q%#7T2oT zR%cr_BM5|?`Pz{$$OETO?Lp#=;s`*;Q@Bnvi`;dZQ>iY#6azVd5;1Yq`DxWuM+6M) zuy2B~?bZ}2Uz)n!qbM(k^{4&-jGf#1yG_XUZ2S0y;Dso-9X9|tp%ecx7NJRMT>IK% zW6m;nU4_8xuS7S~QwuEJ*b+GADHkrr*U{1O>GH83)q7%&^?cwA#nGMZtoR~9Y4gii zoMfy4;g>311|hLAK@iTUDv9*HAZlkL*S3z|U-c2vGRAGk(P-G#k;wNkkDj&oDj z7yRdNURTkdem$?D)9;W<+%M=eHntq@3th=@>17LRjP6WdzjJ1&=G3-+^YPP00lAu< zUkC0tOldbq=8rAHr+n?Z5`LjPbQ%h;Rwhgx%N;KgkbTXL$#N9^DkVR{If&Uud!q1J z@WEA&?AQ6N9_=ZiWt)g+;y`D+iLpO^q)AS6x9Z}Ml~vQ!_(aVX9_3nnxPa>DXTDF- z$nTn2f5B zdCYHe_DfOBq8uPOm5b%~Q;v+GqSmM~D~gk3B1~p#ef~y2Oz1 z=85k^cOaWWCwVjor*TQDCb78cCOrv>cMPaV}^H*p(fnseSNpTJuiPLD*sr& zgOS`v&e-&i)o3~i=O0s+e2ZGB?xtpPi=Cgd?=RD&@J3E`?lTRrDxClPYcCKKbN)`W zc;7vz{s_<{VL2#Hf_L`cje3R(rq` zuQy_~aTN6it3G4otwc{FuASq5`H`Axf-7suz{B3K?8aVOKe7_}zRELP4{$)PN=lwb zmA$@+Oy2wZx;oCAPk8RO`gCFq>N%*`L_WH?wLPFjM1F2QM==RA3!~8NJ5IETmzMsC z2#c-dr#ANfk$}?LDTsg`x7+{y4OB9qI!2=}d7Obm9rgbGt1|;~jJRSxm|Y7L85U4V z{>+T?fyY%8+8YQIzFCdkojPcatNzFT?U*;@f;idOrWR^ctrk1;>3SGnXovA86|6nJ zoV>lYQ~&7x_ZOHG{hd{o%72apaN9dN;F16OX_z|<8{+@BqyGPS^DLkX#@_))N66UF z2zKPsD}6*>e)!}^&W47(BS!}QpL1C7_%RIlCB(y^C_t)UA5pVFHh=Xl;8Qy2j`%-; zLC7pxC0*bLD;nktZNFa!Oww5V>pVRxBU1a~$I`eT77q-oR4Y8|3yk>GYid+Znpo4B z^0iX4OdLB4XbO9WBLVsI;tpY6Da;;EmzO@ag)S{AsQPGs3kPHq{);pT)BNFYx{oQu zH*N#)9b0=^2Y878J2b9|3xBZ3CC7M4n2{I%sFv;ziW}$vsQ((k*;KZ5poPxYuzzYr z<0y|E&&$#xuV+j~=D+LmCKvKB{1Oiqgpipx^EYgEHEsEraHmevfCataOmT@`*y7zL zW5~|qoLnZIQG8o(u6RnI0AaldX&CQ5TkK9WbLWiDx6Wg*`Z!`*RMHtLyK`CgU*^u5 zKDqjk%aQl>1QC#JFQBLS`Ez5^_P_m^Z zS-)D3W3qVeZLVl62jpgkyF%&sAMR!dYRjyv>YnBHz5-X+ZuYzG(l&ST3o&jtd3aR; zwXCGQrrtVvdt9yLU)~7J>v(ncy>_{$kvYLyG+!PX6OX@Qh*j1d>2GG(;Qieq5o0Hs z^r-NMcRKk3<1f!5gWbW{er9cduH%)WIz6jAg8d9WjP$;L}H5WYo%(8uIH+Hqn5g^au zeU;C#t)!581SRCZ+T58}oL^FyFRY@AyTSlg=yJ^$(*ATqe{ary6%k#@U-E4wsiUQ} zf8NSge#y-;1l=c5wy?T`j9<;2>C< zxVtHIwls-N1xJjN$&*n1QPjU#y5Ql7 zk#Cks!2W~l1GZ zmo`oQx_NZYBzAw4>az{&?weVwgxDJ*x}Y%De=}Jx9H{Q+yOdo}#(t`H9ux6UTlnZt z9OVspt%mb}v#n>+D8CbuRtPFU-%F#hae6GR87%|Qvi19xu%gLqLr3~gAo{@?WUO3D zlYiR~?@AQ&=a!*_zH6DT1!(H_RtF4K{lPW*u)FsMd)_4q_y{L#{V`Vsh=9!}_=7>7 z4*oN&rVf0JmC|#z{?$Y&?vg5Y;TU%#$Ln8PH$?r4AnlWE_I*Xm>H7v_Zl!2xJ1urR zstC*gF|=@C8~IDfdfWb*KCV$&N^*>4dOb#Fl|Cs`xS%}vL-kX)ZeQ0;`e*uFxO--& zb2ce8JVysvg=Rq)+;6JL_A$)QTIjlvDpl~pd6?xj=}8J#wO&+R<5N9@0fjDvsGxhO zG|$r<_?Udtsl@pRf}( zpBKlGd|s9WuLPG_!abyCr zHUF2VLQf^R!WxnLKKbbI_aE3k80eE`9^mYwQ2{UkuRRI#c{wpYQ8*Ka}FzzK-Nlt~u zBPms=yQdF6ql!ASH~v=yAb(ZfzSN*)Mz=;#6QN3gvaAR0-npMr|E`>Wy?5L{HuBS! ziL`{SgLYuTsl3m_`#JycuBIU#T{;+C(7B61If?jf6OL}cLqW$zdx=gp77}%d8)GZiKoo$W_N(2p@uhUQD4ddf~OEYL7iYv#4={l*#AP1uaW+X?O@0xzzVPa^+Y=q*rEIj*!le% zv_b(hD}-zw|x!&|0P%ci`mDo0b(7KR%3}W z^z0VKv+c_7CPOUW2D+Knz3kng*qHlH2hvGJxmwZn>-HicqAo6A;O|!8>*!byK&)#N z4G(~QWpI=k^UCdkx1zWh9MKX7y#7m)cbm91TYt!Hu+QZL`Q4v1bU=L)(+rS%vNx;~)iCe~oa){%FCswMDF&jVy7_#;&Wzdyyx#cy z@6JH+oe6y8D>^DY1M}+j9|Ksp7gZwjR;#zA(qBEMH`l8i(P^mhSb3uaj##n>TBN;Q zLJmw%C?R=E_PbAbQYAK4d1}>{{MA3CCICU7tLAqq<(9HM#8aVsM-EHXL3st7dOOQK zCd+V6TgY*bGJY|>!!Lm_WbXj~bI*FF_T!2tOML)0oy%^cYsaRX@?@L**RLr>M+zrv zorzP>VOBdb^AwlqvHZ6Gi12f9EKU)RP_9D$ch2&Z-R@Nz-PYhYz0uT6I^zzV-5wA= zZhTDLRw`T0GSe3e*flHU;1!; zBus^IoZB@ibi1(B9mJ@rkPlB@;j^93l`Q*P{Kb4;x#Lr;x`8jZp4wmZB0?#PdFR9cnKCND_9sMz3Z7SGVLrlEs_-mDbE zddk@5)D&KVwXYlrFW@ymRrCY>)5D3B{0@Z26FE%FIaMJZ;Ig@+x57hup3huB&zxDB zHtTe9;LC)lTxFd8sZ3Voy02+B-{|1H0T+>8fV4a^jU+ro)=1d-cZM%@iCA_94tQKE zEqCwFb8!RRfp7!*-KtbMa~7=wgA-Xp9DsXC>NjQ75m& zq|u>bFpM|;k|+5(4g%ShXP+O|GYMl5_o%V8?k2s3=d-9 zLDPcQqiYYM(59p7?>A_w$ehj$XO_Jk@MUyGi`v*wl5QUM%!f>1qVDNhuqv#EMOS2Q z`Fs%?sIHFlj{%h@7tvR;4LY?H5=iCb7_u85Pq?$kq|hE&0~8FvEg=xrCJo;I^sX-DUUBX=7S2 zeNd)TVjd%EUw$u^X13Lz^PH?q-sKrRpUn)pY&@#cnqAW(Po;2242JL2>;3foAc?*s z1T+jHr0Kpr)V^(Fy7{yRjYRu4vAM0;?TYB0T&!CGDIx9F)r?T0O+3??t%CV`Pcc$| z{b?x@4rY3~pj2yi=HRgdrbf*M@~}dxBYDzOP~TAfP8RRE6eCB+h>F84WSm*c-Iio$ezOlJXXGced z^&DdGxz(<1heYGA4A73CYvvo3tvh^oak2d^jVS)?cXl>@ z?FLqk3o!8FH)G&Ymq`*;)N6WTXCCv`dolIX!zxJs-~FzOlSZ?c2z<1}psw3nRq|J;f-Anm6r4$#I7#-$Z;I@g>P znfrHfY~?lTFlBi()okd;YAiGD?QAN_8yWtJJDc<-Vvcuf139dANcAttaeZGyAW;OF zfPKDEOUIX)&y-m8%92e2tWbLjB}rGA9f$3s$|Q#s7an-)_vFtvsXUm5ss3$ zJGx6q6Ya@w!R0dTf1GWYa2>ZFKayBtTDNXE9aXJCcNF)n(oW|-o`$I))2t_^wgbn0PrRmu;A@x?KRx=wOqSE7-QC>Zquk*yD`TjG$MD z#OA%VLUEBxnTwZ0RquL{@W>d=)YO#mNGjhl?9LU)^sN*JZDH>lWx?k+CfKIcMq@G^+? zzzVMBsq+Vau=~SI?mZ{9>^k;=?z1bu&2wnrz&`Ex`1oNYeC_#-0a^qy&5mmk+(c(7 z~Ry*-Tif2B}L6#4Ojal@O)`jcZUG~ z&%F);X5pw@>I~iOEBfbZM=7xp{%Dq#mX9a5`%6E=9!7Mz+;%l2X7%rc(SxLu2=hrRf7y98&DXgQS*B>g*W%5WxNC`j` z8o*7?HHQj5^s%pYgk4jb>k7Pd;uzf0HFm>v54JZ2pRph(d`hJ3f!ehxW9k4S;*yk< zWMNUkmV%O!3KeB8f);8bzco1&@?$s&V?*6F2`8=xufgZOTle=4;g=>U!zLb8u~f)R zlW#dZBa;hKd;@(Z5^mN$NQ#U&~ZUNr8e zQtp8}bGWN6FVu*!Xn!r6v&%YlL}{Io9Zz}g2kL*2PRug5F4QkW<%(h&0j1ck;RM|~ zI<)lflvm(v41EjM$fKbD~R&v2=CFDCIlO1}-ih9v<$8 z0oi0n{+|t#+RwHrHJ5>9QS2J#y@h|OfDv~(a!bK%G|8P_-~2Can`3t~Jnu!RNmM{@ z0f^;#9Z2@$-&2RvTN+u8E>GyvT=w@FxocL`hb)|4Wn=akO!r_XF3cXNJb@{#rq$i1 z{IPmT?c_JKY?O3uNc)5?@N_4(Hm=!qG`G4E3MSx~PrhQ3fPLiU<$t?00M#HS3(3+} z(wqh*t(RlXo7}Plb{hQ0(>(zHkVkc2>Ac>8&-hRLtQa_&tF;Z6_5W<}#NdBDBo7WA zEW)`fg(oG%Xi5Z}F-Kvy7;kPG8XIkGKspXW4tSX}$vDud$R`LC_?a2bjHo;w6XSn` zAz{KF7+m7oYwT8vgW{90pV_FWnW!nExFZg!iBq;OtaYPU>?*(GW`5$hT$rhgws;aZ zI5{h1Mf3P22#XsGr_hTlJ8$a7wvC=YiD%_Fjc6Y3nVFdEEZca5>*%HgE zMBGZYYhVbt7cAWOj%?%1bUzOG!RqFZeqPPnmCu0R05Ns-+x{4OPEhTeDVi2qBC9KZ zQ*I!z;mGNWvjqs;J{kPtDh=&ayW0i4U;SegFi0pi6)>=m;t;ZMX+l2Ni(2cI9TC(j zgGDW?j{=gCB8oCrJ_VI4s`#Z3ZNkN-+boDIocotR;;6^((TOIv6+I?ZEhFL&f9Uf; z2GsN!{{F&a&i9&66y>ZKrR*kXtZ26)+jdroQJc|IxYV-ax~?phnyn$X^-xbDA|}?g zyi9_@jGS~R@eQ0Rjp-ap@A(sOFK&hwtnvw{%t^u z-=(Q$EmyN>gSx+vPA)Z__40S4S-|2KKz zt(y&+oSX~=rNd~z>=>XRBO`lxdAX0m7GuP8`BP2FR7L-(Vx8nb(_JAW{`gS%jj=I5 zBak6}X(4g%lmv$I5`$q-ljETKu3IIdE4{crr>ZWQYVW}7kKSQW_LsPv+}w5Ely+0S zRlQ6T6N|h;NC!vk1FW86M^H?YU&CaQ#7od?*kVS)h{pe5bPsIP2X_& zK8DG%261wR;ie1aAZK|PXe8aFX=A?gD9cAz7qAOQt_1eM|07)ibqE7nBE zFaS7q-g3@G#rClp_a9G>%C?2T*%zSC>-$Uu_C`usZ3FgF_yN4dMGcKV6N?kp% zOoQez;~c*sM3&&v5tRfS>1LX&V#;qL(tK2ih%Sgrb3Mz-wq59NEcj-))uNjLNsQ{!1pu|xI8qdf2lMRK{3kFC~A5R(u!dOU%`7iu&0?X7s zW+8s(Mk6AJt2ZZGCU_0b@>!nW_753)jyxS!#CAeEaGwx@U8`2Avev9?>m5sb*nLf! zQH-{0NpT#>qalzL9ynR>7$d1?P{%EWQTXj7Ta{ip&?%m%B{8l>B_Vxe$%! zX<)a!5Y7{ZP4|begf!I8Fu$64cWx-%t(0Z|W_?AKn(BC`z1=@vZ66lK2pJj_s59_?Ayo$^Z`G9XCa)k z1b_Q3pAKsBMOWiWgD_gCAK@qxJ^$Iu-Z|(D{-S)beFSFs;%v-HEJa&I*U*KX~!8DLQ0p_rR(&G96usG#ciF4CYB4|9Vt z_L~`+`JA8FcaL#l8GR$Sd#@gZO&-ZH;I$zqy1gp6yyA98;PuY3Mw%~N?J@*hDeVnV1;MbT1YVII897z7!o}Ez5wtG4y>;jq{@Tmn_i(ZTv$IRRXdjcO5p=%%ggu^ifJ>TtyvpbDH%N0bFt5IjyQo zPu)7Z=w{U|AP@PJBlhW2*^`lgA_V1_*2K~h4EHJ=M=S$I%5iIuFu7RqEr9B@E@pMQ z7Gnd@x5y^%ZYR{%u8zBr$(-cLm`!Y!acHsM=800x>4;Gbs%Sj-jJ7(glt{rfAK7Fz ze^{RSXFV*e{d-e|j}H%>USlG@=fC7h%Yk3KwwPVDR1VOF;_7a7fAS!{=OeG#{ln4r zU-4PLn^1^&oX9pf_3%`^vxip9Clb1f-k z0?VuB9*a(fka*CixZ>`$AfL8`Ue;$JAP!MdRi#L<)j|sw%rC%Rv_F0Y0xP^}kT5qB zMEYqTw8Z~~##vMmIN9Fx-o@;r+|2|Iq)`OoD}l1>H^7s|T6m`f~9f)6spAvSm3t z`xctG$K#A-tQuov9F5yL%c7YuPUA*YobrHUt9!r}kq5dUCRiFMEvD&}8KIYGoK}VI zPp`)UAkeB5Zy57V24jRoz$t;$yJ7b9^tcZ3DVoXX^RLw{i9IR8j5=3-GZNTaTRuMi zQL2yj*@-3B=eD+}M!AU6stK-)IOxPi_8)ns^NTmPg_>gwuZV(O9|W@2BNX$2&5v$Wv} zlhA9-aY;%)Y>g^qqoM&4>7bc6MieRY-Lmkfkf^Ar&`?YrN*+NK6-Q|1A|_^Op}7<) zE`zcAySm zC#x(`^gV)zjFreQ0q0+LM#JF%<5Lem?mdHCK}je{;qt+wHU7>!l6&&Nt(u6* zp4K!IT!UBud!_VAHi0R4j~Qo`3zNYU2Ij$bt-<#s?bA~s63kbLaW7UWIVeO&S)Ice z358xYE4vHFyeO8s!6JONgjVw|-c|t+<-jm77w|7vvOs>%vAY76(EfcgUnvO62Q~v| zGX$7)Qtzps_MceMnD4NNV7xPs%D;Uw!^Q1Zq=12Wgel_B@|~s$X*GFy%FkzU#m2_F zBYyZ~lCw@y&Xv^GJtj_00?rXe@EM^HYxlJYF2edT0LGgfF@xPUTuWSU(9qco_x;wm znQ`teiFj(xw;sW*`yTlfw$Q}&fPvnSQ7sn&R1p(JRL?6U@O%D<)%=JLmyeHjxuHen zLIAN5yA|5!{fNoT)Vs~ODT;+A)#5qZ8#7TYz^;^w({Xa?@m!Kz8Zzlzlj~*9V_d4| zuEa!^I|`rE*Zl@CkDOLVz3KQG#3$6^68e(ErNkd}t%HR|W4V;GW_y;K$?3}lIf2`o zy)8mr-PUvhHLcEo)a%PjzUveH2Bo$?H#^0@6jUmz?JjGBkU&oZFUuV)9S zR7A-x*nau|0~gId0|Q1-ij}XiSt3&pQkY#h9s7D8GMKle?ajKc#gj0^!IM?hphuC{ ztN1565Q4JZKbZn)twZHvVhBris*aZzW}V0zbEKmQOIkLJc*kjw zB;JRxXxr`X?)N4&=Fo7aU@PeeM@J~GY0 z!r1E^&5th8Sh>csqHFpCkYlCA?JwsWKf~DPPFW+GLf^CWZ7x#xKR@HzK*yTyy$pfHia~<_|;M*AcS6B*62=Y+G2(ml=T)rZm01BlT|=QVwv_LC`&u z3h6lP%X=k&cF`6lw*I{#eg*~xhsz=yV@5T(G>FC5z(J#Kt!H#q^`ZcLb~k9{EaYSJ z=fyx0ljz)q5MaM_{1JgK#jX4Z1jRJERCNi@Ai%t+DNcg4HVXe;MUR;A0}5f5Qn$$v z-`&MKS`T+=@d!*kpxMX9Q+1zS373W?=Sj|mN4;9r{RbhoY|S}IsE>e0_^;A>xRmyE z*%4yX>?X_s9Vbk~63a1|xh>coRIEH^ z9X(lPp98$LA2y5}L`iWN+^31J>a4B39-1KLkT$9-*q)lL%4^1l75)1{z$!+npO=PRnR7Tlh$=INz*wah$TiyD7O7LW98o>`Cv0F<{2MS zv}@bi7KH`m&$B+I`glBV0 zJr|XYO9Pky#J4nGFUkLQu&3{`fNk&Alreu68gKq;hnkJ5lFA&{2n1r0vSeny-{Sd2 zR1aO8K#5*TTKp2)VnNnv{F#>x?VM}$ON_g_f#39=TOeV+HGGqO_sNRQ+0UlK*hbyc zbDE9~>7we>d!>NBL~FFb9x9a6%_~I)nS7tuG<;9~bM90}>_`V4`IZfwi=A!~EGHPY$c< zqoJwECVePhmyvOLdM>d#O9JhYhz3wmP;9QRn{N*z@;l>+>k>%bV!Lcdw(1Jn+%zKG zu~|NBgWM_JA0gb_AuYO&t(qILjALh#>hP_*-HjjSl8TY2N`l=8Z8tAyS1Mt}W8v zt2Xu@iR3xysY}!IW}W+AaZB}#lb5D{;$wN|4?f7Tt6vxv zrCu5k=;#UxdRZ}2rgi0v$c-O^!N9C2VRevQ+tMJxyL^KxaUtu&bKza)*eX1}1ukt( zAsrzdkkj;2UL-r+9Fo5HGwxHS`lEBqYREa^O`h&}w7biX2|V3R>pNeXLO5PjUZGuC8*#qB9AB20an1LWs*kx z5netJj>~7zV(;S}A@2MZqedY1ClcjdFz7!X1O-M=00xQ@-_ig!=ja&rJJJgG3+T`I zQxYIXWeg2JR7uM<-fLQ~rQZm&b)<@ZEXn^TI`On3wT+K}T$!bOm3;WOpH&cG0$D@H9t!jvP;M zRh6ZaXF4vD`b{#usdCKRh@+Z${R)GXbE8<#*X>**=8Btctr)_%^WHZ74YX6Gw_Gk) zmvFdZ?0rYL<21#A)CU($R$Hu<``EHvbd5-OXXv@IAWKEsO2!JB^swD8;7n5^RTZ6x z*a)gq=D?r~!*8}ye8Gla_d9>>m9NS|(Sr9A1_OVV0rb_1cLE;?=tP>LiQ#NbFxs3H zez|20^vYaUgHF+Cvw0VbdZ;m1ZrTAhfNX}<^uVjTxQ(vxL^&bxk>-!ld7!$4+{3D7 zihWvY?DyOyLWsONeGD8+h~(K0$F*}ZDMkWJb#)As2UMK#N4yq%3 zs)1JMdt=Wz^I`3X9f5`s{wq0X)Ne|IZQ(tnbj@%L8nMy@{Yg&w22Ln=Y!*qyZjrQ% zZrt>h2qd5`nw4Sg_2}`BD7GD;3K6M06^N3GN{h$+6|`=#J)GipwjrraT)lCDaj_X* zS`BuX@+W=No*cUFVg~`*C(XBY590l)SGJ6V&7!AE-7vt{ zJriq?&#u7{9SQc)*9+f~f5KaTd)TT=sVf`yd>mMOE%%rmR*ulS&#_?R1i zcsu?+^Bn-fu{(QbY-65UI-URsA1V&hu->ux0Cukv7iM&*J+GZk8g7ana&7e;r#WD9 zd-dwt-7rc_&rj2H!_zFnSzC!YooQDeocYZpAvtk+bkyW%=ly~B&yOl1vN|mRfyHI5 zaREHA07GwhYV%Q%eJs-*daw`L9JEAJ_kA6R_Sx}Zb|{HMRau#hgM&%4=HPU_htokW z&#gtn!w2rx-lwUkJ(@Vow%2+iE1*ox1zYmH=&f+$4}93Ke*R2O~cbLg=wIl2K{^$enYHb`&g3tE=uSZ2PC zQ=Su-hPRNZy5<%e?_QDWj0OV9nQ#ajiFsC!%I@u!8NzmZtd3b~1yX3U5HxTw)zye3 zEzCoCxTl^>h%}`kH{?t5cr8jjm#$Ym!~wBAk&-Etv~hRI>4V1vMw-PfF2?%C0K{6} zKeh&r<$ev`EjeLqXcfEX_%&s7AnRpR9$jGTwhgCMuqrB#n{^#dItYo=rUHpC`XM}s ze~bnZ5iv0_Q7!&g1_ZKxa1a956GqxY=qkKashr48-CBD(&&)Kel!6~F#5WPd>gVUi zcj#pNTETE5`h%!g%Sh*pN5OlJx9_ZitqfwXqK+=f9^2yGr~u=iIpImIT*eD~vD7#C zh-8nd=Kyxrv7^jzE1y?a_E)orYA2U(5^~!QXv=i_tp~fNLqJps;XgaL|NnZEFjkm>7)q zk%`Ica&Ibxki|qm=9n$0SQkJ`0N}rQTfl%x-=nZDS}}@)6UinuLs*zrGC8Vw-IRQ{ z`h94h;X{Dt*x7?QE1G^w%&!erwhw1 z@?C2*CJel^&R~)9F7WNfpN2z!h%SRvJFZye^Rgc;iSWbEX!PGc&pRZ<;4* zMpR7~ZuRTs|EWIe>puopYeeBjo4%AXkw1tRjg?>GJZQWnH|$>Q-+`Ak!`Ymys*3(V zA76@2Z$Xj$N<9(!zD>@qO$jM9SC;$9zB(c~IWeH0(xq@|jBR85sD$eo^EqKo+sH8s zxcYrS*P>b}xhRX*Wzk@NY3#vRHmIbLPJ`$RWhraS%4Il5771yaje^|VPLZ4Bb4mQn z>*lG+w&NvYENBLTh9h{C77%=ni7f+vY`IGfR0YZkCnV#-_(X&`@M+oQNv~;iqEW}A%Hia%X zq04O5%gM^zXljN7TKZ%t;wA^oYDE~}Di~4fa}8BzBO-1Vsd%cs&Cciuz>lzcLV0^$ zEXpm}VVTdsKv%q3TUlIBa*bKtispKvP7=rt-JXKZWYBO8*-)PZhY?2EUou4@5SRuN5s~3l~lr*tTPNQa&gDLYYXc7 zum({-&h6P%I(;uKF0?i?b9B@l{Iy5lk#h4Lko~lYk50WJnt@*X8%wag&x{uAX3MhU zC@Rc#d+4W`dU4hj{Oi}RUR6@X{ybG;6fRxSmfK`yc34uQtqU4py22X}Y>4w<=g@4Rb$ zzge8Bu2WTK@BPSLP15nSsf9(IuflhzrsZz#etPhlAQ~CEapC!t(i&?)K`M#+WRw0g z?DN?#`v)Lur)EK@%BfASuWV$ZqG-r&gm!9S8liWC9hJNE=Sg8a|Vk0Cm)OHR?RMwc;+^kq*tJ(U2e6y>XW z58vVFNzR9PQmKz=!s$iNdCuS8h z0snR-&@&Ts!*p8d4+G|Cp_Q25;h<_OSHF)H#y*c16uvAYjyJpZS^O5&q@O}-69WLb z=`$c8b)Dh_XH%k0zdyHT4&?D2S*(6>barNAUrF$KhK7c=IaOcJy}Qj5?5F7tpGZ`3 zL-2SJvKXbW8|z8%fs>Wm!hMT6&vY}`Vvzb|y%SLv3o7(cDcbW-KiKWFE+pBhwwb^!@`mdJboy<65 z408J6eW5tl7#-cq6>AGBKRN8fo9B2Omfv^nD2$mA@c1dym*Kw|N5Xz(@I%FW0#tRZ zOsilA?y@$6yQrg(v;X0&A7&Hwru<%c3Nqk8r z@V}6!FuK{_$xhp{g1sh+_^l!*H(nl`S-|^H@|`vuQW1UzPKi^YS1jG4^-s(zRAC-4 zz|u@9_`Z==8Sc-+d=`m;v5IMHK;)v>jmfZz;?A!U`t{G*fU>Z-Z5;4_)ZuG*mAWbA zuBn`2k0Y%h@T#XLT~X9xennA2TF9+`WgKi%NP;xe?Cx2SBGo@EA4|(nq+7ZR=Db@2 z=Ntc?Jkd!Jxf%wD9~odq9-;k{FV;GRJ^8-`SSr#Ji^tUPlw+(WG;Bma+8L!^cU#k` zHbs^*pR4p{W7D6ch;#!KYF4`nMZ zSlkMv;Lu-;<;dFR7Zov_i#;nis?6;rikb+U?Ua`vFP<{n%F36I_KTU2SBy&MRx$yx zugJWO>Ac_H_^I9WAStRNYi57>k}S%p3SoE|tPgN*bvF`P9=@=-%^0ybl?Ixi2*Rdv z=@A)n^4|CQb216GJKoH4>W`1x^>cWZMP{l0WQ*r_8Z!Mu5-(b<%^TVMn%?{W0l>HV zHDCR`<$t;%w^Y^9LfIPAFNiT_xV6E!W95;}D*ZG*%V;#!gJ%dt))PTi0J=I>Un3 zP8ipfQhO)z#vQMQVg#-^d-k;LP4Ltb_YNLU_4PStPr1=3DH)l#xHu$kczF2xBH`@x zG~+7bb7!ar=@`n}3<%+9a;3K*CGL)9X*}Q6yG^q!_CaAeU%Fw-j|sx+9mUL+NjlmB z2g;o*(ngE|~p zz`;S7XAf7n#oaS;EJ8tLCvDTAYn_LuZ?zCj=8BW(5*cH_9_LZ7DB^xk;^%TgWbCkMc%pGa%%9*oNg7CWxWEYePS* zh3bJjW7G_q#@5n9E;tPOQdvmvz5F~nf4l0{BQjMe|DRnNn;cJ$V5 zcxvI@zR&>>R-I|3WYkxVE@zaTV)ofL6p5g>!1T7!?UK0XlXg$#a}4zJH1bE2vYFkc zQe6QW9W=;<20KGwZ*>qQodIEfonR+`C0UK*}J@=h;en5>s^K zq9|^RG2PvhoBKWxoodr5=|Nxti5|fkLc#9*7)L4@Il@GJ`U*=ZNhvI&C%(U0n@h|q zJH_JpT+OOkAPWt5oa?dNnSHq`XEX1@`)$VdK<)6AjFdc|Mj=2`T!^-skVql0;s#_x9EU_7}p>96yWh?Q@1;IY1GS|D#W$q27G}G+Bl-G~BZe%`%Q93B8 zQPU?_QVBHwViro()7@=c5R_)I`ym&&8vtO}o~lvLFMo2ZYDfh&KE`oEVNsydE^qKi zr*}g_xTG9hV!yc5Fr=)!*sOK74|h_(zW-XZ=k@IFmi9-7-Wdv;F`=?|wHBIHJucmi z5qJ&ypdk6lNAhVZB>tqS-ZeqSv&~BK`bN=0Hm{mnN`9s;Te@aUex~DlXzHXFVaIC3 zff%I`-NX6nL;;m%#ENFqt%86$Uruj`I|_#Ffu0!VJ=xQ1giJ7hF->^dzD5)3HLst~ z>gz1pqcy~EATVr-VH=M58g?Q-R%grI^!yH2#pAOe0P%ZmZyTqp3~gh*d-o0<-RrHD zkfML?tMGFdp0gtMEn}A^;+1WU{NWE=q>j8$fXj<@O+QB2rEz4Lx}Sd%8Zxu((v;oR zu$?;+2hMq|p3cfj>?O$AZazBJq?TpoM!K60oruRiwW$Fv9dnqe)h^eAi|oP*8Lxpx zj5uxjB3b<_t%b+b*4%%ee@g125C&Y)UmR>~pNEdl%tYJGNSVy{z7&;z-jocO9W@}K zW(Y%O=kev=n6U)YvSPi+ilUmUKK*UuG_clgUk@Abqc z628p`(YY z(;?YZa@BC`>$xgK;zGSz1S2%KhzDyCnrbRcg?$snJYm1EdNi;rq@))=P1-I_Vrd*f zInY=Uu3UMms5hF8-B?4peAIes)T)gGJ1)5T%m_>+s?su|1P=R5&qkw+YZp%Uy>`pL zZM~Nr)YDFF1y)0FpWl#V%Ik~SdX%de>__O7MP*^yCQNO)c3&WW=J3m!jAZsCN(GBvI+T;KWgS2f=NZ zWeGY9V=!V@%-gEly^eISsY&`Gtd6(;!Z7mb+r)f+@;F&X zaG8U>zuUUU7&xt43=MIj1z~&Bs*Ss>$w2*y0N#H&!A4$~2%jWEGB!UyUtnL{pn~*6 zTqIGttbHlkg2or+N0RQgu|;nT4;Xgl#(PXOglULTPTYwfpUc9}FDR=wLOu`2Z$w9c zHFB_o;akACe!yQHG~@NfmWppKl%BO`MUPVn_IZKfgK-@+{Cj+*Zwq;pk)L)KI-LgV z;m%ei7*Z9lXRHU){r%gOvT?T+a^NyO{7UV|qN#;MhnM7tVWeJ~@+EG&(?(~#D$OU@ z8{yx&NY0OpvpXT!p-RHGvViI!i4f1~fKC7H4UJhpYeu^O-a`O6m>MtVrgbjb*SSOb z8-&UFdGoceeyZEnj3HHKjEgzAJ>U?u-1Q zfXS8D?O0InASI=SYEG&JD>Lzu$uCkB*hl=^NcT=v*mEe8DhncjL;ySWRnZ5TA`0L|?|jPwq;mu^>wTK1#F3z+0x21jE$O38mu)xA4T0xo=EC?-YW}Ek z|MV#oQ1b)Ko*~e4{Ey0BgupHI{Ey1{jXod*|HSu{8hPuFM?nA2#*osWCwM3?ftsZ1 z9+nZ(<1Yn-)Zgx$y?|sNN+QI`_y`d_D#*`wkbnhY_eI%i`=hi5MC4ON?7;IE-U+0m z19Xok-=4_8e*gXicE$42vv`67Y~S1CL4RBAM?VpiC%75R zc^AZ{l&J8$dxYmQEmpKg`X+TE-_0m0gGpRt0J7GKFGQU$!(C1AF zQmigC+#aFiuhY?#MM~n98b^`K8CNJ9ROp=O@Y2cx;!&V&1r5$MEbUfH0pW=3Ero^s za;I_27KwmEeCbUSv&yj;gVEJk2mg`R;I(9th6 zW1OZzjBSq=W#V6M_YsMXa+w{KnkInrJ=7nJO829jK64wQ<4dOo(MobgV`t!up7ciy%Z2PJkBx=RqY^K%T^o8PHz% z@o8!0ns@s_+bG=&0|LgIgEsm53m2nf`w=v-*wKhe4C*>lTM*p~2Oh=)_T@AOmu32s zk^2LC?>!{}MR6gRZJgpYTTCDE7$v&XCiA-F$2;)c^bnM7w8Jp9jisE?`!m(cKPS(7 zrH>=n$KwgkzBdbQyWeX$(nvql>n7BWYoT%a?`!;?Ka(*AW zq+y&5%f_Us9F7c<%@GxJmqVQ zmQO+1>qH7gq-ik_$r&NqXcGE-61eCGN9`3_QfuBY5mC{B_bpPsj^zQr^C{Hasmu!w zzzc6xNuy=z%ik*sNJe*ox3ylCTp(-bDY74jce9}cfzfWDLac!pP8k-=oRj^L#OA7! z5_d^*?ju!kEMP>6%%%UH=}#Xy2K8uf=a~#H+C^Lk2~UtM&y`yIui=4=+y|VKAiWt( zxm<~EX5z2hgx074z(oUYY{2Fo`13Ai{7AEYLPZ`s`z2-p(UYHw(akomdpKE z)=3LYiAo26pHv%SzAc5ENZ5C;GEb%JI)AXyDe2jldSu8Nc&ZE4YQIlA3_Oe@>?*{3 zgVmzyJGD8S9An!0nbaO{xb%DMF_reD0Mq0-YFnW&+RU%~F4@(?JgPD;%SEgR554F$W6QNS z$O44jZCY*K5Fr-aRrNMm9s!p(j5Zir7E)slR2Ecps6{>yl{n^9dJzob4XNd+ciII% zr)5@!my~Q;h9mt;0D+Ip>YhfzB)VUDz!sUm5u4CT;eJKjbW6yr?|-i$;>cA4byfbv>f@X+GmUg&UIi^h7YYFLnA*MmxK61OFfl15{G2TTNF{uZe4g< z+DW0Lhc9|i&6#ITDywXG7%$jKQ7UYxth8xs-2JOOA*d%`^1b3@UrVxXb6v1$w|TiqJ@(v?iVod(>z_nC`aD#`9@{-KoVb)tl#&>M?bkSe-w!K=52l9Qy zl*}GVj#d4m)M7)aW!(E?p7f>^WwQISEUEzrQInn@R$f4g{{YUnBIg|>%Wq@pp`&OS&ryf@W5?=?ZyGM) zftCkhmX#=>w6DFIP}tUm;*?5SaMyfEb5vg7a1Dc&%|31U&}at`P6XP0AlP~@DW-VD z8pxuD0C?qgjDhN2pF1x%k{z`rS>l1YfAhH=%Y+s)k;Mzkj#N#lWyxG{{kTGr|SPaz2JE?k09jw_xNG19W z7&ULBVq;GT_PB9|)v3Z-54ZZ>j!@|SWCI+#V)Pn?H6yRgc4*KiLFE*w2b(=YjBeEd zO78{nm3ucC;mRQEBZG=8Ba`qx*ZEuUjQ*r!P~ z(_9&D^jeLMA66WLE_*LF1wuJrd0ns= zFeIsq+pyF&ChFb!Lc8U|a=U)2fqpxJ5c*x|BK*9l_YL{6r>qwAd#2-L6Z*OFDzXq# zg+G3YwkY)2p@rX5i7DU*fs{~cKypeII+S$bC0JNE%@0nSAbmDi>@fDT;T?1gq#iBG zt2Q@DSqM4SM`0uL4`}sIIDMX8=f521-Q!3ag(ls#y^e|t3oyhMxU%prd&cNjx)$=n zfXYfSs$RW9BKgOI7jAcd+eDbRCwGZ1Y3he$SHc=Rr*;*A4au2i3FZCwspaC<-s?gw z;?YA2WEr2>kB>V^bPyUNMhfa>D&y*+Eg!hFG9?_LRJL*f&(f{a&6(%9m$1_6gIR1B zyfND%n^0b@qtbX#txHq5g3)BnxFV?kgoU4~2lmD-LV2QHQn+16*!E`^U|H|}I<&)} zP1R#8Yh?s$N|~XV`XECM@%mHp_e1ejasF0RGSmil#>@8-%PR=WFs`ubr&*#JLUXbF zowKAIctdy%Qg?V55LCB7VMo+FH3IldTRaI@2Vh_n}gqC5%XZX)|V%>sIRex=FN6 z-+{4-<)u?bRby{23Te{2zn*Hd!SI##mgj@s^Ik6G7S*X;yIFhYH23T!sW+}r8K8vU z^MC*?jqpzaR|iu_P@xzIX}-B;Hv5`HTTJ-wpWuIp9TyHKocf|Q1@LMBK54+UJYY%n zvJs54{7;r3V;m_C*l|6*mVgn*naH1gIn?<$W6D981}qJhwg~?~%t24mz0mG< z(?}MuC`+c zsW*QTHFtmDN&2QI+h%WaiOIM+TZZ)lF4j*Jy&MR+?fdwiw7w z-hVFOiY3@f2xPM_(z(-LJwAkmPYhUG_L6{*EAMR(J+(0I+F9K@!C8Q*TZ89%t^waj zV9V9kJWlrhnt)8P>hEWalD>U=Vn$`78WM!?d(5Gk>7q^(+i^rwc6gT=N^$1a1#RsvXZ^ zJyL+E1iS;3M>Yun$h`cM&R*i+!o`L72&xI7f39=-h&WG$q?DU5D#2f4N{SwbHD(g44>zyAq^OF6N$Ee6$(%MCpy18__PXp31c zeL=R4yna?X=0t@yRp_itR8nN5`glu5NPMn%Pi|fib7_w-q@x&RNHotv8-96{0Vdyxl#VNlFSPRX=wLl`s>%5#jNk&sdG z;c_%VM4E4Pgzz`^BR=}hA2)-Q!g74hr!#F!KZbjbZE)vMJdF3&css@EZN&ULnshc5 zJ`4PZy3SE&S5RE7Wz{03+1-rKXH*9KC)a@_=@459dw1Q`Zl)1cL6emP-x+^aT+W1r zpM1l<4zs6Y3fu)?Twz(acGxkSHLHnZ1%L1DZ}#fIW9F~Mky)@?buNL|hBpQ)8gB2$EWn80}s7@M;PF%8U^(8CvzlI5k)PL`gD z7kD(FW{CcLAf$#`ljj8SN&S@mY78(pQa@&;qb5~K*9Avnb7ybHy-KpBgR(LVsSF>Q z=M|o3vL8lX~49k-<-J zpdkj>^R_MS=Rx1O*=Z-HUW5A!YIqk?QWrO!&C->8=y(eGZKT4{T=IbWsdDF$F ziRj_HaXG;AG|b7)$AW%u(QS8tJ%6WdmP{?_k_1-vTH(F%gH%&-jC7H@Q?gWuXpG*o zy9knONT5aEc&rVK_z}URxKq#KHBL(I&pG%BamSQ^j!cY<38?ok%lj~lTXCm!?}cSC z>L(O77b;oXf$G=OmaM9RgI~DAJp#i|T?m97nmHApUvRZQ16+ALJdZz0z8Z8==vjH= zw!nU<#4u?T94%iF%P}hCUjDtbwy+^6siQhT3Y&?e{#c}UtTT7I-=R!~5Z1s}>N|OU z=Y6JBfhnC+1Zk1;9eh}6ib{-O_&{_;mDDO)P=B38*m`f`9iK7Fy@L9n=%mQ45+;7| zvG-Ql?L5~<-{W?AiqET6hXm&+Ynao1KbdXi;Y|ANWRMR_ppxg@+zHmhkwwJryg|bI z+r33{Lk#l);k>3>^VOObXos)juzEV{&Aql;O{^lGjnQD(4YeuD ztXcBy1xISd>0T^S%8>2kwOQ8kqW)YCpyE4vcJi$6KFxs%eSE&TA=x)Z9Q;P}us%fb zYwO38~(dX&{{*(g{`OLM{A(mI*W6{~L{$QVaCqFNX zT*PTy78barR!DdEByJ_2dwqiI7oG2TwlYU(VEZRu#(h?z(*EsNvDVtVt+d| z4C|1t`vIh&Dx)#lua?J?Rig4p(Cd zE~p77X<*F+cqIww=OAR82I{ z0c9A=;r4EqMUl8^&OL=Wdp5!tbCTbj-qSTUHe+c}FL+9n^}IgjX? zs6<$0QA-3($tm)Js49o`e4GfP{8--l~SmY!)UxCP=f~q#Xl8N{5_u7lof*g{w zqw(32wLS+Txp{}H9e=pN?pJ&!yxvXxG$qwwYQ*Gg?26~q_)=?S?K&W+0Pn-m?Cy;o zzPbG7+@ddC@K^Z#Gkj(o@d<%)H6yUVf{&~S%lnT{#GpjMp*6;9)}k*w6d z7EvgZes>TqKoZ-}ZwgVuZ@-7UWS|Gz}XS0Og5~*p~S2F2NXo}E5b2lp1H7g#_ zorc-0o7>HZ>xM1!xyI)+AW9ld{eZ=l&jdd=Cdry0)j7_omMDDPu2Sl-t{H z9}YYoVm$84wvO7c{#1QdS=~9l9xguIGCUk@lyDk*bOBN3$jBGz6apbPS-c`jmo6)8_<%i#s)+H@rV4Qou`s3oJ`>VzKGYhQ7Zqv807h{CCVwhS`(u>)-Viov3 z)PlkQ06b9EKsSC!c{p3VzhTfB$Mncbj@v7h;I97E1oOLamNU?l>Zx041c7R}nU6I- z>)O8`0g1gFT~7>_D9917sTvnr)V}ZE{T_KaRn*nEFvBJu4z^mjVmz2Y-~x+*Gm_rz zzkt)Be+2xV0^pvPA{8|dTKh~+%mEEcPMV&j!queu)ZPzID9I}{8*8zTSeTfZX_sVU zmyT@BI<5w(;VIo>*I38r1|sw-6E2aEK9=FBd;aZ13jQ|0c5hO0d@t zKt6^>AJgg=L?LJJ6-ruv?NH{e0m0|i39sF`Hp?>wKlsh9BU^^)5(9K4nV#JDK$@l% z1>UzARdwg{lUM!{9;Fy!R2bvy&&L43 zbouL7=Kf#KxzUX7rSk19(=7x1y~gn&@LLczD;6uu5ezj%M*9y|wv4|ZZ)-CZ#+sr} zn{@5Va%{m%kt2X`66L!n(*I=g{68TtJbG>6fnpCo(7Ne5VM*#WN$liTW4up~h7OO6 zK11j;k1oLA`(*-{|Mj4`;nDLazhk^2{Z3y$z^^nJ1GWd`USD9u-WwmeMVsh+&JCCwU1{}Zvq3`E7$WTRB!l`_inexdK zK6F|^^5#p#45SAfzy+Y3A6Jh=XNl<3qm>vptNl;8U%!4`XOWD{(44+y2S#N|2RAo2 z!zq9;f<;l@yvZlRbyJ4>qs5MI_8<^`*OB=8<;(M4Bt&$Q$0Elk=5iLXgR9?sI^X*~ z!_1^*yJ2mw7C1e+x@LCW!()$lwzEfdV8V>XCZgXvJ1$qIZjsN%0(o1l|(bnPx5%F}dDkTFl6 z?cOzD=~yD*#G$I>?Yq6!Nau9*o+(v@m!lR%p4_@8A!ENFl?T`l&;HRne*^QozOjso z!J*_^IfR93F2WVDy`x|@?+IhyT5M(CCRt)qvkJT@2~xBWaSwSoJU=3#I@TDhRkh%C z6}`1JDy;AOd<%OH-i0*9% z!y*8~^QS=cuzmYxgjr(A62bL9*lee3m3&f>Ge`z)br4(N@376R77mNb5( z9Tv_vC!mrIbt*&?jNGB4x1C!-z)m~-#~)+V0Jj}5r46T4qi5$}-8=Ux4w^IvwAgf`RS zHj%-7hb6}&I--y&ZYlH*$<{p|`+TqDjaX`liw5VZiIqdo|LF>b95W{oSDEbotIFJ4v%e zUV)%LS3)Jai&OIQhqxpCyn(rDckPP+K8e6m*`^l4; z$36A30Bn?#2P(=pSRs4zq&B?@7pPc1K_dlyYkM^U>YXriI$oxbp$a8Sdse6+;tbb~ z9SNLtOu@XFncDFLSMRb5z{h~xGYd=--r_JLfiKlGubZEo5tbjkI~D<0q?J1u=+mrB z!h**vll8bCEf`vh!pRLIg9kk|a>>5b0*Cm3g47M&$=3IG|NB-drGwWQi%|gQ2L&+6 z==?PbnwzV`+o(HmQV}Jpq}0^luow=gIy)svnW%OH>+Ib!*8rVMzZC+lBb?6I0JmQh zjG+%DlD%C5VibVu9u3u?&DBisZenEl8%gH61`+9D2d4$M;Ik%6`5#pmf8D)-;b?eP z8);3889!nUGy!2ek_(pL`~rVERXq|C=Z*{K$7Pe-8-pb+tx!$3dqSc+7H(SZ<>!2; zl0BWBlLJYJu7~eK8o!1|AD%B^KFGSq>^BDciG!XH!vo5HohKS1OX(?i~k+&zG7%mRQE+5h0U(sdwbMJ-dmeFFo@Lacb#Kwoam7I!*9q51;zTIn@oXN5{7$-sbA3 zqEp%>PWdW5?tIOpn}<7HPnJH=e|>4(Kus(lX& za7lknn1Q{c%fUy2FhRm&)Eq_fkGvv#0s7-RBOy{ZRp znj&jtVU!GI6wlgZ#6Sg1)@3>M-*bC(;pzlaKt>{Dk{W>%7#@BB#xyAtzjeGH$tX59 z$5$Nj2)VfLBckx7-Ob2HKhNvqhGE(N0N4ABOw#Z^#zw z#@Ny)E!UYMy+u6doCVRS4{WqL@XU-;*Wh1@x%8Igy5RG;dI^sWTmb;6bAlN3e&KSoAw5Fx}QN*C|tn4r|~kx0>9zw>am| z1ZK%syYfO0sbS|VId4Ow6|1rV&g^P9>~dC$q}Vjh2Px_#`P_PfD(^Ue5ofOOOAesV zQY1#7GLMdN??*2sj;v;kM)9hdW@hBjr9o;3XxtL=0Qr6~XZTQy$-RA>iq7dUoryU5S13{i8uP(}2L?lIf7q&{CZ?oF9sOOX`Nic<-Wvfai|G!)>r^q(3mYQ0fy~>A5eC}RIw5g0Dv)yZ9;(> z(zAbzu|hn7YSCyGPX~g{cSO7e^WivduIlXde^ultnt1HfAVYHuMU9kcnFSO#EpL?f z=nlFp>eb$gxNo!Q7^ zF}q`~<~Bd)cy(7X7-(#quo;z2ca`n4J_9;6tKAK$i$58E)l3}{$Q z@4n>JM@w^^ow>bOoX_YIuT9XtP?|1M-uX$B4@vK$u}g5uzrAG0(OX?BYWbyXd}1DT z44~q0Fdwwh&&H|cXJYR>lcOnX%r?nTmvh#fDH=-abD!R1yweJkj?DdyE=+oT6&bx? zi~-o(`{s`Q4s`O8lyBH>!LbMq)zR|;%l!?bH){F_t{U{2{AXx zE&Lv7QlURt(z{s_pLd@Ho!q^v$qeho55RDODB2YEvuYmwhBjq`!$>VIX4I07w~yv! z#rvT#erZ~JQVH!UQ&zKv-p*8#4FthYQud=J_-9p`2tJHgluSaRQ&=D#?OvqhCh6~% z-ts{RJb1aXs&L_yHzWMNw`dPy?${G@hmiY6UtwHpcV{NuY<&aRFlnp_fX$PIk2iw# zS-`*d;nQ4*qa9G5H?Cnw&i!zvMj4}sU7?`!ecp3(xpY$Xv~~ofY(UU~XX=!QauK)e z5-`O4H0_6M!}t5+AY_Jul0@!>YoKKKVrJwv%a9(vvxychjnlXx`8<$n4EAg{ya;)} zzR%3Cuc_{{u#TB7K@r`>0F$iXO-&)2Z{;j3wuY;J79vhQCO{^qGOg}#rYu0Cm;SZk zfRmw!a%A>vPtIxwMkaWR^KqrF9XDJJOb8_F)QfU!#{o`@{TLhW(gI0seiaP$=a#t< zL3shAb6IqKuTS}U<;YEtT=2QOQJB6r*R@bZ#+B&3tGzJLNrwQ@FF&!qYtrJZfB-<> zl_gUmE_qm~ioW%m&;KrqL7n}-TT32xXq@a~5{k#%Jiq|w##8i0Jw1EVjw|aDmQt3M z4c79Q*7?}sy4V?h$|cSs5_aKiOs0?*S~i2OE~AP=kWwWiI@8J;SZ`&6-w%mtpa<2DpMX~c;9UJLe~p)=;3auqIt4-<_m7_O zF?pnH@47;8Im8ww7Hkt62eT+W!Alk7Ny(Ia#$$%c>Nc=$@Xd+}AStcF-U%jRwO-9; zM5wG=l#?C&UHOnK@C-9laZ_e+Gn}Q(baHa~GX9~TuvH2vHpko{@}A>W8eRM3@Z1KS zlaj4Yh_A_)r)V<9=LX$bOe#OjM;0X7A+|{|=VU@Eq#P7wmLZrAVpfOvN~d?dnvL0^Bv{T4qCP5K=9%O%zP|+vb;ukHzG4L%BJGs&`*}}COu*%xbb+(`TDnC zUS#FuW)}yK2d+D87iWa^kxLCHoZK`R8292>ENhAtxh)kK{XX$*k{YX@`nqg+eA4NL zwax4{p#D+X>P()nisQaGqem`fha?Mo>-|7&eCra{i$VDb&0UX+i6xG0+};#9J2Hk` z|6w#v3z_H4dV={%Mo2`i!I4=9@U^w$8r|o*Om$pNIlSQ}n>9JLG|4}U*5`X@^b!D} zzcNV0;;rkuYP-PFwbW|;@*%NV{aUwbADV8g<2G>U01fXzzsy)~R>Q5vB&qaoku-LRd?;f4dhmj#C3aHKG*|Ap0r%>^TDYh{N^RvlcdqF&k_>Qq5XFe=s zeV0N39=vtJL?)3^yCPi}fHUeZfJ*dVO2-!6-0fHPJC4bjrU8fNT{JvR?~6X()s$KNYMSO>XPaZ~$9!(8o zZ03Bgb;viP|IhCHo83|g=J(YV1KUf98Yzp5_s$g5;grlX0h9X>-Eck0Wtu2O@!9p6 zB<{ARd5Pt*8bkNKIT)Yo@hCcn4vV#4o_ zv9EV>*Aw1uir?H&w(7{mFOw3{wyD>{PmbiuKAU2Tqv5@DG!PI^T8 zfxw0TGC24ISaP3%)!h(noP3nANrg+<+jmQb<&r=0Ys_;Hd$pSMzfLTI<(p*}ky_Mq zgxAy+cFCIpYWwuFV}?6sC#}4=jFfTC!e_$f0ID zS!^>63cs@}N*$PRkX$CrrTy*qR{{93S7fR!z)l!V;H0lOikY}pq@B1CWKIfgXFG{+ z+Gif0#-AtVhspWUE3nQ#RK#^sBdX>~X@~%Ru7CJ0mChx*^}dRD<$cKRWJPIX!UbfD zujcwpdTY4F&7?krbA9UkJO-+eR%Buc^v|NOuWJ72q>gB_*NrYwDN<$OF&u5D7ahG* vp;qXfM9n#8_58B=u>$;9KK-_Ibnl@^I{xdB<;LW(bEc@Ulu)6-*YE!iFZS*n literal 0 HcmV?d00001 diff --git a/include/data_sources/sbp_data_source.h b/include/data_sources/sbp_data_source.h new file mode 100644 index 00000000..5a6d43b3 --- /dev/null +++ b/include/data_sources/sbp_data_source.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include + +/** + * @brief Base class for Data Sources + */ +class SbpDataSource : public sbp::IReader, public sbp::IWriter { + public: + /** + * @brief Method to read data from the connection + * + * @param buffer Buffer to save the readed data. It must be long enough to + * contain buffer_length bytes + * @param buffer_length Max number of bytes to read + * @return Number of bytes actually readed + */ + s32 read(u8* /*buffer*/, u32 /*buffer_length*/) override { return -1; } + + /** + * @brief Method to write data to the connection + * + * @param buffer Buffer containing the data to write + * @param buffer_length Number of bytes to write + * @return Number of bytes actually written + */ + s32 write(const u8* /*buffer*/, u32 /*buffer_length*/) override { return -1; } +}; diff --git a/include/data_sources/sbp_data_sources.h b/include/data_sources/sbp_data_sources.h new file mode 100644 index 00000000..d6347ca9 --- /dev/null +++ b/include/data_sources/sbp_data_sources.h @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include + +#include + +/** + * @brief Enumeration that lists every type of data source available + */ +enum DataSources { + INVALID_DATA_SOURCE = 0U, + TCP_DATA_SOURCE, + SERIAL_DATA_SOURCE, + FILE_DATA_SOURCE, + MAX_DATA_SOURCE +}; + +/** + * @brief Function that creates an SBP data source + * + * @param config Node configuration + * @param logger Logging facility to use + * @return DataSource corresponding to the type of interface set in the + * configuration + */ +std::shared_ptr dataSourceFactory( + std::shared_ptr& config, const LoggerPtr& logger); diff --git a/include/data_sources/sbp_file_datasource.h b/include/data_sources/sbp_file_datasource.h new file mode 100644 index 00000000..48821c19 --- /dev/null +++ b/include/data_sources/sbp_file_datasource.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include + +class SbpFileDataSource : public SbpDataSource { + public: + SbpFileDataSource() = delete; + + /** + * @brief Construct a new Sbp File Data Source object + * + * @param file_path Path to the SBP file to use + * @param logger Logging facility + */ + SbpFileDataSource(const std::string &file_path, const LoggerPtr &logger); + + /** + * @brief Destroy the Sbp File Data Source object + */ + ~SbpFileDataSource(); + + /** + * @brief Methor to determine if the file is open + * + * @return true File opened + * @return false File closed + */ + bool is_open() const { return file_stream_.is_open(); } + + /** + * @brief Method to determine if the reading operation has reached the EOF + * + * @return true EOF reached + * @return false EOF not reached + */ + bool eof() const; + + /** + * @brief Overriden method to read from the file + * + * @param buffer Buffer where to put the data readed + * @param buffer_length Number of bytes to read + * @return Number of bytes actually readed + */ + s32 read(u8 *buffer, u32 buffer_length) override; + + private: + std::ifstream file_stream_; /** @brief File stream representing the file */ + LoggerPtr logger_; /** @brief Logging facility object */ +}; diff --git a/include/data_sources/sbp_serial_datasource.h b/include/data_sources/sbp_serial_datasource.h new file mode 100644 index 00000000..82bc7611 --- /dev/null +++ b/include/data_sources/sbp_serial_datasource.h @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * @brief Class that implements a Serial Port reader based on the SBP reader + * interface + */ +class SbpSerialDataSource : public SbpDataSource { + public: + /** + * @brief Construct a new SbpSerialDataSource object + * + * @param logger Logger facility to use + * @param serial A serial communications object + */ + SbpSerialDataSource(const LoggerPtr& logger, + const std::shared_ptr& serial) noexcept; + + // Deleted methods + SbpSerialDataSource() = delete; + SbpSerialDataSource(const SbpSerialDataSource& rhs) = delete; + + /** + * @brief Method to read data from the serial connection + * + * @param buffer Buffer to save the readed data. It must be long enough to + * contain buffer_length bytes + * @param buffer_length Max number of bytes to read + * @return Number of bytes actually readed + */ + s32 read(u8* buffer, u32 buffer_length) override; + + /** + * @brief Method to write data to the serial connection + * + * @param buffer Buffer containing the data to write + * @param buffer_length Number of bytes to write + * @return Number of bytes actually written + */ + s32 write(const u8* buffer, u32 buffer_length) override; + + /** + * @brief Determines if the object is valid + * + * @return true Object is valid + * @return false Object isn't valid + */ + bool isValid() const noexcept; + + private: + std::shared_ptr port_; /** @brief Serial port object */ + LoggerPtr logger_; /** @brief Logging facility */ +}; diff --git a/include/data_sources/sbp_tcp_datasource.h b/include/data_sources/sbp_tcp_datasource.h new file mode 100644 index 00000000..546403df --- /dev/null +++ b/include/data_sources/sbp_tcp_datasource.h @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include + +/** + * @brief Class that implements a TCP reader based on the SBP reader interface + * (IReader) + */ +class SbpTCPDataSource : public SbpDataSource { + public: + /** + * @brief Construct a new SbpTCPDataSource object + * + * @param ip IP address to connect to. It could be in IPV4 or IPV6 format + * @param port TCP port to connect to + * @param logger Logger facility to use + * @param read_timeout Timeout in ms for the read operation to start. If 0, + * then the read operation blocks until the requested number of bytes have + * read or an error ocurred + */ + SbpTCPDataSource(const LoggerPtr& logger, + const std::shared_ptr& tcp) noexcept; + + // Deleted methods + SbpTCPDataSource() = delete; + + /** + * @brief Method to read data from the TCP connection + * + * @param buffer Buffer to save the readed data. It must be long enough to + * contain buffer_length bytes + * @param buffer_length Max number of bytes to read + * @return Number of bytes actually readed + */ + s32 read(u8* buffer, u32 buffer_length) override; + + /** + * @brief Method to write data to the TCP connection + * + * @param buffer Buffer containing the data to write + * @param buffer_length Number of bytes to write + * @return Number of bytes actually written + */ + s32 write(const u8* buffer, u32 buffer_length) override; + + /** + * @brief Method to determine if the internal socket is valid or not + * + * @return true Socket is valid + * @return false Socket is not valid + */ + bool isValid() const noexcept; + + private: + std::shared_ptr tcp_; /** @brief TCP/IP data object */ + LoggerPtr logger_; /** @brief Logging facility */ +}; diff --git a/include/data_sources/serial.h b/include/data_sources/serial.h new file mode 100644 index 00000000..ee725599 --- /dev/null +++ b/include/data_sources/serial.h @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * @brief Class of serial port interface + */ +class SerialPort { + public: + SerialPort() = delete; + + /** + * @brief Construct a new Serial Port object + * + * @param device_name String containing the name of the serial port to use + * @param connection_string String containing the data needed to open the + * serial port. The format of the string is: SPEED|DATA BITS|PARITY|STOP + * BITS|FLOW CONTROL, where: \nSPEED: 9600, or 19200, or 115200, etc.\nDATA + * BITS: Number of data bits (8 for example)\nPARITY: Parity control (N: none, + * O: Odd, E: even, M: mark, S: space)\nSTOP BITS: Number of stop bits used + * (1, 2)\nFLOW CONTROL: (N: none, X: Xon/Xoff, R: RTS/CTS, D: DTR/DSR) + * Example: 19200|N|8|1|N + * @param read_timeout Timeout in milliseconds for the read operation to + * complete. If 0 (default) the read operation blocks until the requested + * number of bytes is read or an error occurs + * @param write_timeout Timeout in milliseconds for the write operation to + * complete. If 0 (default) the write operation blocks until the requested + * number of bytes is written or an error occurs + * @param logger Logging facility + */ + SerialPort(const std::string& device_name, + const std::string& connection_string, const uint32_t read_timeout, + const uint32_t write_timeout, const LoggerPtr& logger); + + /** + * @brief Destroy the Serial Port object + */ + virtual ~SerialPort(); + + /** + * @brief Opens the port and configures it with the supplied setting (from + * connection string) + * + * @return true The port is open and functional + * @return false The port can't be used. + */ + virtual bool open() noexcept; + + /** + * @brief Reads bytes from the port + * + * @param buffer Buffer where to store the read bytes + * @param buffer_length Buffer size (max number of bytes to read) + * @return Number of bytes actually read + */ + virtual int32_t read(uint8_t* buffer, const uint32_t buffer_length); + + /** + * @brief Writes bytes to the port + * + * @param buffer Buffer where the bytes are + * @param buffer_length Number of bytes to write + * @return Number of bytes actually written + */ + virtual int32_t write(const uint8_t* buffer, const uint32_t buffer_length); + + /** + * @brief Determines if the object is usable or not + * + * @return true The object is usable + * @return false The object is not usable + */ + virtual bool isValid() const noexcept; + + protected: + /** + * @brief Method to configure the port + * + * @param params Object containing the parameters to set + * @return String containing the error. Empty if OK + */ + std::string setPortSettings( + const class SerialParameterSplitter& params) noexcept; + + /** + * @brief Method to close the port and release the resources + */ + void closePort() noexcept; + + sp_port* port_; /** @brief Pointer to a libserialport structure + representing a port */ + LoggerPtr logger_; /** @brief Logging facility */ + std::string device_name_; /** @brief Name of the serial device */ + std::string connection_string_; /** @brief String containing the serial port + parametrization */ + uint32_t read_timeout_{0U}; /** @brief read timeout in ms */ + uint32_t write_timeout_{0U}; /** @brief write timeout in ms */ +}; diff --git a/include/data_sources/tcp.h b/include/data_sources/tcp.h new file mode 100644 index 00000000..42498d9d --- /dev/null +++ b/include/data_sources/tcp.h @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include + +#if defined(_WIN32) +#include +#include +#endif // _WIN32 + +/** + * @brief Class that isolates the OS TCP implementation from the driver + */ +class TCP { + public: + TCP() = delete; + + /** + * @brief Construct a new TCP object + * + * @param ip Ip address to connect to + * @param port IP port to connect to + * @param logger Logging facility + * @param read_timeout Timeout in ms for the read operation to succeed + * @param write_timeout Timeout in ms for the write operation to succeed + */ + TCP(const std::string& ip, const uint16_t port, const LoggerPtr& logger, + const uint32_t read_timeout, const uint32_t write_timeout); + + /** + * @brief Destroy the TCP object + */ + virtual ~TCP(); + + /** + * @brief Opens the TCP connection + * + * @return true The TCP connection could be opened + * @return false The TCP connection couldn't be opened + */ + virtual bool open() noexcept; + + /** + * @brief Closes the connection + */ + virtual void close() noexcept; + + /** + * @brief Read bytes from the TCP connection + * + * @param buffer Buffer where to put the read bytes + * @param buffer_size Number of bytes to read (up to buffer size) + * @return Number of bytes actually read + */ + virtual int32_t read(uint8_t* buffer, const uint32_t buffer_size); + + /** + * @brief Write bytes to the TCP connection + * + * @param buffer Buffer from where to write the bytes + * @param buffer_size Number of bytes to write (up to buffer size) + * @return Number of bytes actually written + */ + virtual int32_t write(const uint8_t* buffer, const uint32_t buffer_size); + + /** + * @brief Determines if the object is valid (valid TCP connection) or not + * + * @return true The object is valid + * @return false The object isn't valid + */ + virtual bool isValid() const noexcept; + + protected: + /** + * @brief Method to init the socket system. + * + * @return String containig the error. Empty if OK + */ + std::string initSockets() noexcept; + + /** + * @brief Method to clean and free socket system resources + */ + void deinitSockets() noexcept; + + /** + * @brief Closes the open socket in use + */ + void closeSocket() noexcept; + + /** + * @brief Method that opens and connects the socket + */ + bool openSocket() noexcept; + + /** + * @brief Sets the socket to be non-blocking + * + * @return true The socket could be configured + * @return false The configuration failed + */ + bool setNonBlocking() noexcept; + + /** + * @brief Method to connect the socket client to the server + * + * @return true The socket is connected + * @return false The socket failed to connect + */ + bool connectSocket() noexcept; + +#if defined(_WIN32) + SOCKET socket_id_{INVALID_SOCKET}; /** @brief Windows Socket */ +#else + int socket_id_{-1}; /** @brief Linux socket */ +#endif // _WIN32 + + LoggerPtr logger_; /** @brief Logging facility */ + std::string ip_; /** @brief IP to connect to */ + uint16_t port_; /** @brief TCP port to connect to */ + uint32_t read_timeout_; /** @brief Read timeout in ms */ + uint32_t write_timeout_; /** @brief Write timeout in ms */ +}; \ No newline at end of file diff --git a/include/logging/issue_logger.h b/include/logging/issue_logger.h new file mode 100644 index 00000000..deb060b0 --- /dev/null +++ b/include/logging/issue_logger.h @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +/** + * @brief Abstract base class for a logging facility + */ +class IIssueLogger { + public: + /** + * @brief Method used to log Debug information + * + * @param ss String Stream containing the data to log + */ + virtual void logDebug(const std::string_view ss) = 0; + + /** + * @brief Method used to log general information + * + * @param ss String Stream containing the data to log + */ + virtual void logInfo(const std::string_view ss) = 0; + + /** + * @brief Method used to log Warnings + * + * @param ss String Stream containing the data to log + */ + virtual void logWarning(const std::string_view ss) = 0; + + /** + * @brief Method used to log Errors + * + * @param ss String Stream containing the data to log + */ + virtual void logError(const std::string_view ss) = 0; + + /** + * @brief Method used to log fatal conditions or events + * + * @param ss String Stream containing the data to log + */ + virtual void logFatal(const std::string_view ss) = 0; +}; + +/** + * @brief Fantasy name for a shared pointer object to a logger + */ +using LoggerPtr = std::shared_ptr; + +// Logging macros +inline void LOG_FUNC(const LoggerPtr& logger, const int level, + const char* format, ...) { + va_list args; + + if (!logger) return; + + char buffer[1024]; + va_start(args, format); + vsnprintf(buffer, sizeof(buffer) - 1, format, args); + va_end(args); + + switch (level) { + case 0: + logger->logDebug(buffer); + break; + + case 1: + logger->logInfo(buffer); + break; + + case 2: + logger->logWarning(buffer); + break; + + case 3: + logger->logError(buffer); + break; + + case 4: + logger->logFatal(buffer); + break; + + default: + break; + } +} + +#define LOG_DEBUG(logger, ...) LOG_FUNC(logger, 0, __VA_ARGS__) +#define LOG_INFO(logger, ...) LOG_FUNC(logger, 1, __VA_ARGS__) +#define LOG_WARN(logger, ...) LOG_FUNC(logger, 2, __VA_ARGS__) +#define LOG_ERROR(logger, ...) LOG_FUNC(logger, 3, __VA_ARGS__) +#define LOG_FATAL(logger, ...) LOG_FUNC(logger, 4, __VA_ARGS__) + +// Contract checking macros +#define ASSERT_COND(cond, logger, str) \ + do { \ + if (!(cond)) { \ + LOG_FATAL(logger, str); \ + exit(1); \ + } \ + } while (0) diff --git a/include/logging/ros_logger.h b/include/logging/ros_logger.h new file mode 100644 index 00000000..c9742835 --- /dev/null +++ b/include/logging/ros_logger.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include +#include "issue_logger.h" + +class ROSLogger : public IIssueLogger { + public: + ROSLogger() = delete; + explicit ROSLogger(const int64_t log_delay); + void logDebug(const std::string_view ss) override; + void logInfo(const std::string_view ss) override; + void logWarning(const std::string_view ss) override; + void logError(const std::string_view ss) override; + void logFatal(const std::string_view ss) override; + + private: + /** + * @brief Method to determine if we could output a log to ROS or not + * + * @param output_str String to output + * @return true We are allowed to output the string + * @return false We're not allowed to output the string + */ + bool canLog(const std::string_view output_str) const; + + /** + * @brief This method updates the information about the last message sent to + * ROS log + * + * @param output_str Las string sent to ROS + */ + void updateLogStatus(const std::string_view output_str); + + std::string last_output_str_; /** @brief Last string sent to ROS */ + std::chrono::time_point + last_output_time_; /** @brief Time of the last message sent to ROS */ + int64_t timeout_{0LL}; /** @brief Time that must pass before we're allowed to + send the same string */ +}; diff --git a/include/logging/sbp_file_logger.h b/include/logging/sbp_file_logger.h new file mode 100644 index 00000000..92ea0d27 --- /dev/null +++ b/include/logging/sbp_file_logger.h @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * @brief Class that handles the creation of SBP dump files + */ +class SbpFileLogger : public sbp::IWriter { + public: + SbpFileLogger() = delete; + + /** + * @brief Construct a new Sbp File Logger object + * + * @param file_path Path where to create SBP dumps + * @param logger Message logging facility + */ + SbpFileLogger(const std::string& file_path, const LoggerPtr& logger); + + /** + * @brief Destroy the Sbp File Logger object + */ + ~SbpFileLogger(); + + /** + * @brief Method to insert a new SBP message into the file + * + * @param msg_type Type of SBP message + * @param msg SBP message + */ + void insert(const sbp_msg_type_t msg_type, const sbp_msg_t& msg); + + /** + * @brief Overriden method to write data + * + * @param buffer Buffer containing the data to write + * @param buffer_length Number of bytes to write + * @return Number of bytes written. -1 in case of error + */ + s32 write(const u8* buffer, u32 buffer_length) override; + + private: + sbp::State state_; /** @brief SBP State object */ + FILE* file_; /** @brief FILE where to write the data */ + LoggerPtr logger_; /** @brief Message logging facility */ +}; \ No newline at end of file diff --git a/include/logging/sbp_to_ros2_logger.h b/include/logging/sbp_to_ros2_logger.h new file mode 100644 index 00000000..82f5d12a --- /dev/null +++ b/include/logging/sbp_to_ros2_logger.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include + +#include + +/** + * @brief Class to forward SBP messages to ROS 2 logging system. It also creates + * an SBP dump file + */ +class SBPToROS2Logger : private sbp::AllMessageHandler { + public: + SBPToROS2Logger() = delete; + + /** + * @brief Construct a new SBPToROS2Logger object + * + * @param state SBP state object + * @param logger ROS 2 logging object + * @param log_messages Flag that enables/disables SBP file logging + * @param log_path Path where to put the log file + */ + SBPToROS2Logger(sbp::State* state, const LoggerPtr& logger, + const bool log_messages, const std::string& log_path); + + /** + * @brief Callback function for processiing SBP messages + * + * @param sender_id Sender ID + * @param msg_type Type of message + * @param msg General SBP message structure + */ + void handle_sbp_message(uint16_t sender_id, sbp_msg_type_t msg_type, + const sbp_msg_t& msg); + + private: + LoggerPtr ros_logger_; /** @brief ROS logging object */ + std::unique_ptr + file_logger_; /** @brief SBP file logger object */ +}; diff --git a/include/publishers/base_publisher.h b/include/publishers/base_publisher.h new file mode 100644 index 00000000..c2540499 --- /dev/null +++ b/include/publishers/base_publisher.h @@ -0,0 +1,25 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include + +/** + * @brief Class that acts as an empty interface + */ +class BasePublisher { + public: + virtual ~BasePublisher() {} +}; + +using PublisherPtr = std::shared_ptr; diff --git a/include/publishers/baseline_publisher.h b/include/publishers/baseline_publisher.h new file mode 100644 index 00000000..ef18bff8 --- /dev/null +++ b/include/publishers/baseline_publisher.h @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +class BaselinePublisher + : public BasePublisher, + public SBP2ROS2Publisher { + public: + BaselinePublisher() = delete; + BaselinePublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config); + + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_utc_time_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_baseline_ned_t& msg); + + protected: + void publish() override; + + private: + int32_t last_received_utc_time_tow_{-1}; + int32_t last_received_baseline_ned_tow_{-2}; +}; diff --git a/include/publishers/gpsfix_publisher.h b/include/publishers/gpsfix_publisher.h new file mode 100644 index 00000000..cdcd2881 --- /dev/null +++ b/include/publishers/gpsfix_publisher.h @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +/** + * @brief Class publishing a gps_comon::msg::GPSFix ROS2 message. + * + */ +class GPSFixPublisher + : public BasePublisher, + public SBP2ROS2Publisher { + public: + GPSFixPublisher() = delete; + + /** + * @brief Construct a new GPS Fix Publisher object + * + * @param state SBP State object + * @param topic_name Name of the topic to publish a gps_msgs::msg::GPSFix + * message + * @param node ROS 2 node object + */ + GPSFixPublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config); + + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_gps_time_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_utc_time_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_pos_llh_cov_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_vel_ned_cov_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_orient_euler_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_dops_t& msg); + + protected: + /** + * @brief Checks that the ROS2 gps_msgs::msg::GPSFix is complete, if so, + * it publishes it + * + */ + void publish() override; + + private: + uint32_t last_received_gps_time_tow = -1; + uint32_t last_received_utc_time_tow = -2; + uint32_t last_received_pos_llh_cov_tow = -3; + uint32_t last_received_vel_ned_cov_tow = -4; + uint32_t last_received_orient_euler_tow = -5; + + bool orientation_present = false; + + bool vel_ned_track_valid = false; + double vel_ned_track_deg = 0.0; + double vel_ned_err_track_deg = 0.0; + + bool orientation_track_valid = false; + double orientation_track_deg = 0.0; + double orientation_err_track_deg = 0.0; + + bool last_track_valid = false; + double last_track_deg = 0.0; + double last_err_track_deg = 0.0; + + time_t dops_time_s; + double gdop = 0.0; + double pdop = 0.0; + double hdop = 0.0; + double vdop = 0.0; + double tdop = 0.0; +}; diff --git a/include/publishers/imu_publisher.h b/include/publishers/imu_publisher.h new file mode 100644 index 00000000..918bc6f8 --- /dev/null +++ b/include/publishers/imu_publisher.h @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +/** + * @brief Class publishing ROS2 sensor_msgs::msg::Imu message. + * + */ +class ImuPublisher + : public BasePublisher, + public SBP2ROS2Publisher { + public: + ImuPublisher() = delete; + ImuPublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, const std::shared_ptr& config); + + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_utc_time_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_gps_time_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_gnss_time_offset_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_imu_aux_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_imu_raw_t& msg); + + protected: + void publish() override; + + private: + void compute_utc_offset(); + + int32_t last_received_utc_time_tow_{-1}; + int32_t last_received_gps_time_tow_{-2}; + + double linux_stamp_s_{0.0}; + double gps_stamp_s_{0.0}; + + bool gps_week_valid_{false}; + uint32_t gps_week_{0U}; + + bool utc_offset_valid_{false}; + double utc_offset_s_{0.0}; + + bool gps_time_offset_valid_{false}; + double gps_time_offset_s_{0.0}; + + uint32_t last_gps_week_{0U}; + uint32_t last_imu_raw_tow_ms_{0U}; + + enum stamp_source { STAMP_SOURCE_DEFAULT, STAMP_SOURCE_PLATFORM, STAMP_SOURCE_GNSS }; + uint32_t stamp_source_{STAMP_SOURCE_DEFAULT}; + uint32_t last_stamp_source_{STAMP_SOURCE_DEFAULT}; + + double acc_res_mps2_{0.0}; + double gyro_res_rad_{0.0}; +}; diff --git a/include/publishers/navsatfix_publisher.h b/include/publishers/navsatfix_publisher.h new file mode 100644 index 00000000..7f700e1b --- /dev/null +++ b/include/publishers/navsatfix_publisher.h @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +/** + * @brief Class publishing ROS2 sensor_msgs::msg::NavSatFix message. + * + */ +class NavSatFixPublisher + : public BasePublisher, + public SBP2ROS2Publisher { + public: + NavSatFixPublisher() = delete; + + /** + * @brief Construct a new Nav Sat Fix Publisher object + * + * @param state SBP State object + * @param topic_name Name of the topic to publish a + * sensor_msgs::msg::NavSatFix message + * @param node ROS 2 node object + */ + NavSatFixPublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config); + + /** + * @brief Handles a sbp_msg_measurement_state_t message. It gets the + * constellation for each satellite in the measurement states. + * + * @param sender_id Ignored + * @param msg Incoming sbp_msg_measurement_state_t + */ + void handle_sbp_msg(uint16_t sender_id, + const sbp_msg_measurement_state_t& msg); + + /** + * @brief Handles a sbp_msg_utc_time_t message. It gets the + * time stamp. + * + * @param sender_id Ignored + * @param msg Incoming sbp_msg_utc_time_t + */ + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_utc_time_t& msg); + + /** + * @brief Handles a sbp_msg_pos_llh_cov_t message. It gets the position mode, + * latitude, longitude, altitude and covariance matrix. + * + * @param sender_id Ignored + * @param msg Incoming sbp_msg_pos_llh_cov_t + */ + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_pos_llh_cov_t& msg); + + protected: + /** + * @brief Checks that the ROS2 sensor_msgs::msg::NavSatFix is complete, if so, + * it publishes it + * + */ + void publish() override; + + private: + uint32_t last_received_utc_time_tow = -1; + uint32_t last_received_pos_llh_cov_tow = -2; + + uint16_t status_service; +}; diff --git a/include/publishers/publisher_factory.h b/include/publishers/publisher_factory.h new file mode 100644 index 00000000..4929fb14 --- /dev/null +++ b/include/publishers/publisher_factory.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/** + * @brief Function that creates a new publisher + * + * @param pub_type Type of the publisher you want to create + * @param state SBP state object + * @param node ROS2 node + * @param logger Logging facility + * @param frame frame is the frame of reference reported by the satellite + * receiver, usually the location of the antenna. This is a Euclidean frame + * relative to the vehicle, not a reference ellipsoid. + * @return Newly created publisher + */ +PublisherPtr publisherFactory(const std::string& pub_type, sbp::State* state, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config); diff --git a/include/publishers/publisher_manager.h b/include/publishers/publisher_manager.h new file mode 100644 index 00000000..72b4f6fe --- /dev/null +++ b/include/publishers/publisher_manager.h @@ -0,0 +1,24 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +class PublisherManager { + public: + void add(const PublisherPtr& publisher) { publishers_.push_back(publisher); } + + private: + std::list publishers_; +}; diff --git a/include/publishers/sbp2ros2_publisher.h b/include/publishers/sbp2ros2_publisher.h new file mode 100644 index 00000000..e2e4defd --- /dev/null +++ b/include/publishers/sbp2ros2_publisher.h @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include + +#include + +#include +#include + +#include + +#include +#include + +/** + * @brief Template abstract base class for the publishers + * + * @tparam ROS2MsgType Type of ROS 2 message you want to publish + * @tparam SBPMsgTypes Types of the SBP messages you want to merge into the ROS + * 2 message + */ +template +class SBP2ROS2Publisher : private sbp::MessageHandler { + public: + SBP2ROS2Publisher() = delete; + + /** + * @brief Construct a new SBP2ROS2Publisher object + * + * @param state SBP State object + * @param topic_name Name of the topic to publish in ROS + * @param node ROS 2 node object + * @param frame frame is the frame of reference reported by the satellite + * receiver, usually the location of the antenna. This is a Euclidean frame + * relative to the vehicle, not a reference ellipsoid. + * + */ + SBP2ROS2Publisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) + : sbp::MessageHandler(state), + node_(node), + frame_(frame), + logger_(logger), + config_(config) { + publisher_ = + node_->create_publisher(std::string(topic_name), 10); + } + + protected: + /** + * @brief Method to publish the topic + */ + virtual void publish() = 0; + + ROS2MsgType msg_; /** @brief ROS 2 message to publish */ + uint32_t composition_mask_{0U}; /** @brief Bitmask used to know when a ROS + message is completo for publishing */ + std::shared_ptr> + publisher_; /** @brief ROS 2 publisher */ + rclcpp::Node* node_; /** @brief ROS 2 node object */ + std::string frame_; + LoggerPtr logger_; /** @brief Logging facility */ + std::shared_ptr config_; /** Node configuration */ +}; \ No newline at end of file diff --git a/include/publishers/timereference_publisher.h b/include/publishers/timereference_publisher.h new file mode 100644 index 00000000..1f2978e2 --- /dev/null +++ b/include/publishers/timereference_publisher.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +/** + * @brief Class publishing ROS2 sensor_msgs::msg::TimeReference message. + * + */ +class TimeReferencePublisher + : public BasePublisher, + public SBP2ROS2Publisher { + public: + TimeReferencePublisher() = delete; + TimeReferencePublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config); + + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_utc_time_t& msg); + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_gps_time_t& msg); + + protected: + void publish() override; + + private: + int32_t last_received_utc_time_tow_{-1}; + int32_t last_received_gps_time_tow_{-2}; +}; diff --git a/include/publishers/twistwithcovariancestamped_publisher.h b/include/publishers/twistwithcovariancestamped_publisher.h new file mode 100644 index 00000000..5dc9b7a6 --- /dev/null +++ b/include/publishers/twistwithcovariancestamped_publisher.h @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +/** + * @brief Class publishing ROS2 geometry_msgs::msg::TwistWithCovarianceStamped message. + * + */ +class TwistWithCovarianceStampedPublisher + : public BasePublisher, + public SBP2ROS2Publisher { + public: + TwistWithCovarianceStampedPublisher() = delete; + + /** + * @brief Construct a new TwistWithCovarianceStamped Publisher object + * + * @param state SBP State object + * @param topic_name Name of the topic to publish a + * geometry_msgs::msg::TwistWithCovarianceStamped message + * @param node ROS 2 node object + */ + TwistWithCovarianceStampedPublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config); + + /** + * @brief Handles a sbp_msg_utc_time_t message. It gets the + * time stamp. + * + * @param sender_id Ignored + * @param msg Incoming sbp_msg_utc_time_t + */ + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_utc_time_t& msg); + + /** + * @brief Handles a sbp_msg_vel_ned_cov_t message. It gets the velocities, + * and covariance matrix. + * + * @param sender_id Ignored + * @param msg Incoming sbp_msg_vel_ned_cov_t + */ + void handle_sbp_msg(uint16_t sender_id, const sbp_msg_vel_ned_cov_t& msg); + + protected: + /** + * @brief Checks that the ROS2 geometry_msgs::msg::TwistWithCovarianceStamped is complete, if so, + * it publishes it + * + */ + void publish() override; + + private: + int32_t last_received_utc_time_tow_{-1}; + int32_t last_received_vel_ned_cov_tow_{-2}; + +}; diff --git a/include/test/mocked_logger.h b/include/test/mocked_logger.h new file mode 100644 index 00000000..8023bcbf --- /dev/null +++ b/include/test/mocked_logger.h @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include + +// ******************************************* +// Dummy console implementation of a Logger +class MockedLogger : public IIssueLogger { + public: + void logDebug(const std::string_view ss) override; + + void logInfo(const std::string_view ss) override; + + void logWarning(const std::string_view ss) override; + + void logError(const std::string_view ss) override; + + void logFatal(const std::string_view ss) override; + + std::string getLastLoggedDebug(); + std::string getLastLoggedInfo(); + std::string getLastLoggedWarning(); + std::string getLastLoggedError(); + std::string getLastLoggedFatal(); + +private: + std::string last_logged_debug_; + std::string last_logged_info_; + std::string last_logged_warning_; + std::string last_logged_error_; + std::string last_logged_fatal_; + +}; diff --git a/include/test/test_utils.h b/include/test/test_utils.h new file mode 100644 index 00000000..853edcce --- /dev/null +++ b/include/test/test_utils.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include + +#include +#include + +static const int g_max_loops = 50; +static const std::chrono::milliseconds g_sleep_per_loop(10); + +void inline wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} diff --git a/include/utils/config.h b/include/utils/config.h new file mode 100644 index 00000000..17250754 --- /dev/null +++ b/include/utils/config.h @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include + +/** + * @brief Class that manages the ROS node configuration + */ +class Config { + public: + Config() = delete; + + /** + * @brief Construct a new Config object + * + * @param node ROS 2 node + */ + explicit Config(rclcpp::Node* node); + + // Getters + std::string getFrame() const { return frame_; } + bool getLogSBPMessages() const { return log_sbp_messages_; } + std::string getLogPath() const { return log_path_; } + int32_t getInterface() const { return interface_; } + std::string getFile() const { return file_; } + std::string getDevice() const { return device_; } + std::string getConnectionString() const { return connection_str_; } + int32_t getReadTimeout() const { return read_timeout_; } + int32_t getWriteTimeout() const { return write_timeout_; } + std::string getIP() const { return ip_; } + int32_t getPort() const { return port_; } + std::vector getPublishers() const { return enabled_publishers_; } + bool getTimeStampSourceGNSS() const { return timestamp_source_gnss_; } + double getBaseLineDirOffsetDeg() const { return baseline_dir_offset_deg_; } + double getBaseLineDipOffsetDeg() const { return baseline_dip_offset_deg_; } + double getTrackUpdateMinSpeedMps() const { + return track_update_min_speed_mps_; + } + + private: + /** + * @brief Declares the parameters the ROS node will use + * + * @param node ROS 2 node + */ + void declareParameters(rclcpp::Node* node); + + /** + * @brief Loads the declared parameters from the settings.yaml file + * + * @param node ROS 2 node + */ + void loadParameters(rclcpp::Node* node); + + std::string frame_; + bool log_sbp_messages_; /** @brief Flag to enable/disable SBP messages logging + */ + std::string + log_path_; /** @brief Complete path for SBP message logging file */ + int32_t interface_; /** @brief ID of the Data Source type for SBP messages */ + std::string file_; /** @brief Complete path to the file containing SBP + messages to use as input */ + std::string device_; /** @brief Serial device name used as input (in OS native + format e.g. /dev/ttyS0) */ + std::string connection_str_; /** @brief Connection string used to parametrize + serial port */ + int32_t read_timeout_; /** @brief Read timeout in ms */ + int32_t write_timeout_; /** @brief Write timeout in ms */ + std::string ip_; /** @brief IP of the device used as input */ + int32_t port_; /** @brief socket port of the device used as input */ + std::vector + enabled_publishers_; /** @brief Enabled ROS publishers */ + bool timestamp_source_gnss_; /** @brief Flag that indicates from where to take + the time reference */ + double baseline_dir_offset_deg_; + double baseline_dip_offset_deg_; + double track_update_min_speed_mps_; +}; diff --git a/include/utils/utils.h b/include/utils/utils.h new file mode 100644 index 00000000..f5285913 --- /dev/null +++ b/include/utils/utils.h @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#pragma once + +#include +#include +#include +#include + +namespace TimeUtils { +/** + * @brief Converts seconds to milliseconds + * + * @param seconds Number of seconds to convert + * @return Number of milliseconds + */ +uint64_t secondsToMilliseconds(const uint64_t seconds); + +/** + * @brief Converts seconds to nanoseconds + * + * @param seconds Number of seconds to convert + * @return Number of nanoseconds + */ +uint64_t secondsToNanoseconds(const uint64_t seconds); + +/** + * @brief Converts datetime information (year, month, day, hour, min, sec) to + * Linux time + * + * @param utc struct containing the datetime data. + * @return Linux time in seconds + */ +time_t utcToLinuxTime(const struct tm& utc); + +} // namespace TimeUtils + +namespace Covariance { +/** + * @brief Computes estimated horizonal error from covariance matrix + * + * @param cov_n_n Estimated variance of northing [m^2] + * @param cov_n_e Covariance of northing and easting [m^2] + * @param cov_e_e Estimated variance of easting [m^2] + * @return double + */ +double covarianceToEstimatedHorizonatalError( + const double cov_n_n, const double cov_n_e, const double cov_e_e); + +/** + * @brief Computes estimated horizonal direction error from covariance matrix + * + * @return double + */ +double covarianceToEstimatedHorizonatalDirectionError( + const double n, const double e, const double cov_n_n, const double cov_e_e ); +} // namespace Covariance + +namespace Conversions { +constexpr double STANDARD_GRAVITY_MPS2 = 9.80665; + +inline double standardGravityToMPS2(const double x) { return x * STANDARD_GRAVITY_MPS2; } +inline double degreesToRadians(const double x) { return x * M_PI / 180.0; } +inline double radiansToDegrees(const double x) { return x * 180.0 / M_PI; } + +} // namespace Conversions + +namespace FileSystem { +bool createDir(const std::string& dir); +} // namespace FileSystem diff --git a/launch/__pycache__/sbpros2_driver.cpython-310.pyc b/launch/__pycache__/sbpros2_driver.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..184c5e4f1c1d46bfd25cc2a0b8d0956341c27d3e GIT binary patch literal 692 zcmYjPJ#QN!7zU2JbI*>{L>($6f8d6=Ql}14RjC;(FWFcSK8Stk9h?BCab!=ay7fOK zWBw9rr}ihbQq>piN(S-re!$D~Ji;6uRS3pe|7-q@AoL>!hXZox11$Frj6@Q5h{c_B zxF?L@=x1HlQ$~|dySy)00e&uvuZWcrU!l7E3AfR##^H3KJRd~!Su7NHOJNn4tyPUT zb`9m@PhzE;<)w0sZ3o{P4M{pPQZ=!||B-=H!uklyU4ZfE0d0}TH{>z%RN@CLiOim| zE%v#@k^&mK!3!*l@A;N&&||S7PZ*;Ois)wSmHX0Odo6CcHSR5!w!Kw06phi>?c#ns z2rGIwU5mbBlYZ6tc7No)a`WNbn{&8U=b_v)3oo?jm4iryO5Leu<;A>HfyQ*GQak09 zb#-YYmq3gQbP#?S=#6Q$jj~%BE0vIvizZ2tv2mC5;W7%8$2!#@&NH$vA^81zSAGMa z4tPw5b<|xeE4<=|9d;5&CEXdA0#7i3N&gZW_vGW^Pc@?0p1^^6WXG_vQ=p+eZ?#l+ zd|3OX(bGM3m#~Lo_e^5PPWEpCrs + + + swiftnav_ros2_driver + 1.0.0 + ROS 2 driver for Swift Navigation's GNSS/INS receivers and Starling Positioning Engine software. + Swift Navigation + MIT + MIT License File + https://support.swiftnav.com + https://github.com/swift-nav/swiftnav-ros2 + + ament_cmake + + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + gps_msgs + tf2 + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/data_sources/sbp_data_sources.cpp b/src/data_sources/sbp_data_sources.cpp new file mode 100644 index 00000000..351d9f82 --- /dev/null +++ b/src/data_sources/sbp_data_sources.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +std::shared_ptr dataSourceFactory( + std::shared_ptr& config, const LoggerPtr& logger) { + const int32_t interface = config->getInterface(); + + LOG_INFO(logger, "Interface type: %d", interface); + switch (interface) { + case FILE_DATA_SOURCE: { + return std::make_shared(config->getFile(), logger); + } break; + + case SERIAL_DATA_SOURCE: { + auto serial_port = std::make_unique( + config->getDevice(), config->getConnectionString(), + config->getReadTimeout(), config->getWriteTimeout(), logger); + return std::make_shared(logger, + std::move(serial_port)); + + } break; + + case TCP_DATA_SOURCE: { + auto tcp = std::make_unique(config->getIP(), config->getPort(), + logger, config->getReadTimeout(), + config->getWriteTimeout()); + return std::make_shared(logger, std::move(tcp)); + } break; + + default: + LOG_FATAL(logger, "Could not open interface: %d", + interface); + return {}; + break; + } +} diff --git a/src/data_sources/sbp_file_datasource.cpp b/src/data_sources/sbp_file_datasource.cpp new file mode 100644 index 00000000..5405e012 --- /dev/null +++ b/src/data_sources/sbp_file_datasource.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +SbpFileDataSource::SbpFileDataSource(const std::string &file_path, + const LoggerPtr &logger) + : logger_(logger) { + file_stream_ = std::ifstream(file_path, std::ios::binary | std::ios_base::in); + if (file_stream_.is_open()) + LOG_INFO(logger_, "Input file: %s", file_path.c_str()); + else { + LOG_FATAL(logger_, "Cannot open file: %s", file_path.c_str()); + } +} + +SbpFileDataSource::~SbpFileDataSource() { file_stream_.close(); } + +bool SbpFileDataSource::eof() const { + if (file_stream_.is_open()) + return file_stream_.eof(); + else + return true; +} + +s32 SbpFileDataSource::read(u8 *buffer, u32 buffer_length) { + auto start_index = file_stream_.tellg(); + if (start_index == -1) { + return -1; + } + file_stream_.read(reinterpret_cast(buffer), buffer_length); + auto end_index = file_stream_.tellg(); + if (end_index == -1 || file_stream_.fail()) { + LOG_INFO(logger_, "End of input file"); + return -1; + } + + return static_cast(end_index - start_index); +} diff --git a/src/data_sources/sbp_serial_datasource.cpp b/src/data_sources/sbp_serial_datasource.cpp new file mode 100644 index 00000000..cddd7a7d --- /dev/null +++ b/src/data_sources/sbp_serial_datasource.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +SbpSerialDataSource::SbpSerialDataSource( + const LoggerPtr& logger, const std::shared_ptr& serial) noexcept + : port_(serial), logger_(logger) { + if (!port_) { + LOG_FATAL(logger_, "No serial port attached"); + return; + } + + if (!port_->open()) LOG_FATAL(logger_, "Serial port can't be used"); +} + +s32 SbpSerialDataSource::read(u8* buffer, u32 buffer_length) { + if (!isValid()) { + LOG_FATAL(logger_, + "Called read in an uninitialized SbpSerialDataSource"); + return -1; + } else { + return port_->read(buffer, buffer_length); + } +} + +s32 SbpSerialDataSource::write(const u8* buffer, u32 buffer_length) { + if (!isValid()) { + LOG_FATAL(logger_, + "Called write in an uninitialized SbpSerialDataSource"); + return -1; + } else { + return port_->write(buffer, buffer_length); + } +} + +bool SbpSerialDataSource::isValid() const noexcept { + return (port_ && port_->isValid()); +} diff --git a/src/data_sources/sbp_tcp_datasource.cpp b/src/data_sources/sbp_tcp_datasource.cpp new file mode 100644 index 00000000..7786d3cd --- /dev/null +++ b/src/data_sources/sbp_tcp_datasource.cpp @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include + +SbpTCPDataSource::SbpTCPDataSource(const LoggerPtr& logger, + const std::shared_ptr& tcp) noexcept + : tcp_(tcp), logger_(logger) { + if (!tcp_) { + // LOG_FATAL(logger_, "No TCP object attached"); + return; + } + + if (!tcp_->open()) LOG_FATAL(logger_, "Could not establish a TCP connection"); +} + +s32 SbpTCPDataSource::read(u8* buffer, u32 buffer_length) { + int32_t read_bytes = 0; + if (!buffer) { + LOG_ERROR(logger_, "Buffer passed to SbpTCPDataSource::read is NULL"); + return -1; + } + + if (!isValid()) { + LOG_ERROR(logger_, + "Read operation requested on an uninitialized SbpTCPDataSource"); + return -1; + } + + read_bytes = tcp_->read(buffer, buffer_length); + + // Attempt to reconnect on error + if (-1 == read_bytes){ + tcp_->close(); + tcp_->open(); + } + return read_bytes; +} + +s32 SbpTCPDataSource::write(const u8* buffer, u32 buffer_length) { + if (!buffer) { + LOG_ERROR(logger_, "Buffer passed to SbpTCPDataSource::write is NULL"); + return -1; + } + + if (!isValid()) { + LOG_ERROR(logger_, + "Write operation requested on an uninitialized SbpTCPDataSource"); + return -1; + } + + return tcp_->write(buffer, buffer_length); +} + +bool SbpTCPDataSource::isValid() const noexcept { + if (tcp_) + return tcp_->isValid(); + else + return false; +} diff --git a/src/data_sources/serial.cpp b/src/data_sources/serial.cpp new file mode 100644 index 00000000..8cafea0d --- /dev/null +++ b/src/data_sources/serial.cpp @@ -0,0 +1,334 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include + +/** + * @brief Auxiliary class to split the connection string + */ +class SerialParameterSplitter { + public: + uint32_t speed{0U}; /** @brief Baud rate */ + uint32_t data_bits{0U}; /** @brief Number of data bits */ + uint32_t stop_bits{0U}; /** @brief Number of stop bits */ + char parity{'-'}; /** @brief Parity control type */ + char flow_control{'-'}; /** @brief Flow control type */ + + /** + * @brief Construct a new Serial Parameter Splitter object + * + * @param str String to decompose inj tokens + * @param logger Logger facility to use + */ + SerialParameterSplitter(const std::string& str, + const LoggerPtr& logger) noexcept { + split(str); + ASSERT_COND(token_list_.size() == 5U, logger, "Malformed string"); + setValues(); + } + + /** + * @brief Method used to determine if the values in the object are valid + * + * @return true The values are valid + * @return false The values are not valid + */ + bool isValid() const noexcept { + // Test speed + switch (speed) { + case 1200: + case 2400: + case 4800: + case 9600: + case 19200: + case 38400: + case 57600: + case 115200: + case 230400: + case 460800: + case 921600: + break; + + default: + return false; + break; + } + + // Test data bits + if (data_bits < 7 || data_bits > 8) return false; + + // Test stop bits + if (stop_bits < 1 || stop_bits > 2) return false; + + // Test parity + switch (parity) { + case 'N': + case 'E': + case 'O': + case 'M': + case 'S': + break; + + default: + return false; + break; + } + + // Test flow control + switch (flow_control) { + case 'N': + case 'X': + case 'R': + case 'D': + break; + + default: + return false; + break; + } + + return true; + } + + private: + /** + * @brief Method used to split the string into a vector of tokens + * + * @param str String composed by tokens + */ + void split(const std::string& str) noexcept { + try { + std::stringstream ss(str); + std::string token; + + token_list_.clear(); + while (std::getline(ss, token, '|')) { + token_list_.emplace_back(token); + } + } catch (...) { + token_list_.clear(); + } + } + + /** + * @brief Set the Values from the tokens to the corresponding variables + */ + void setValues() noexcept { + try { + speed = static_cast(std::stoul(token_list_[0])); + parity = static_cast(std::toupper(token_list_[1][0])); + data_bits = static_cast(std::stoul(token_list_[2])); + stop_bits = static_cast(std::stoul(token_list_[3])); + flow_control = static_cast(std::toupper(token_list_[4][0])); + } catch (...) { + speed = 0U; + parity = 'N'; + data_bits = 0U; + stop_bits = 0U; + flow_control = 'N'; + } + + token_list_.clear(); + } + + std::vector token_list_; +}; + +SerialPort::SerialPort(const std::string& device_name, + const std::string& connection_string, + const uint32_t read_timeout, + const uint32_t write_timeout, const LoggerPtr& logger) + : logger_(logger), + device_name_(device_name), + connection_string_(connection_string), + read_timeout_(read_timeout), + write_timeout_(write_timeout) {} + +SerialPort::~SerialPort() { closePort(); } + +bool SerialPort::open() noexcept { + if (device_name_.empty()) { + LOG_FATAL(logger_, "The port name should be specified"); + return false; + } + + sp_return result = sp_get_port_by_name(device_name_.c_str(), &port_); + if (result != SP_OK) { + LOG_FATAL(logger_, "No serial port named %s", device_name_.c_str()); + return false; + } + + result = sp_open(port_, SP_MODE_READ_WRITE); + if (result != SP_OK) { + LOG_FATAL(logger_, "Cannot open port : %s. Error: %d", + sp_get_port_name(port_), result); + return false; + } + + SerialParameterSplitter params(connection_string_, logger_); + if (!params.isValid()) { + LOG_FATAL(logger_, "Invalid data in connection string: %s", + connection_string_.c_str()); + return false; + } + + const std::string error = setPortSettings(params); + if (!error.empty()) { + LOG_FATAL(logger_, error.c_str()); + return false; + } + + sp_flush(port_, SP_BUF_BOTH); + LOG_INFO( + logger_, + "Port %s opened with:\nBaud rate: %u\nParity: %c\nData bits: %u\nStop " + "bits: %u\nFlow control: %c", + device_name_.c_str(), params.speed, params.parity, params.data_bits, + params.stop_bits, params.flow_control); + return true; +} + +int32_t SerialPort::read(uint8_t* buffer, const uint32_t buffer_length) { + if (!port_) { + LOG_FATAL(logger_, "Called read in an uninitialized SbpSerialDataSource"); + return -1; + } + + if (!buffer) { + LOG_FATAL(logger_, "Called SerialPort::read with a NULL buffer"); + return -1; + } + + const auto result = sp_nonblocking_read(port_, buffer, buffer_length); + if (result < 0) { + LOG_ERROR(logger_, "Error (%d) while reading the serial port", result); + return -1; + } + + return result; +} + +int32_t SerialPort::write(const uint8_t* buffer, const uint32_t buffer_length) { + if (!port_) { + LOG_ERROR(logger_, "Called write in an uninitialized SbpSerialDataSource"); + return -1; + } + + if (!buffer) { + LOG_ERROR(logger_, "Called SerialPort::write with a NULL buffer"); + return -1; + } + + const auto result = + sp_blocking_write(port_, buffer, buffer_length, write_timeout_); + if (result < 0) { + LOG_ERROR(logger_, "Error (%d) while writing to the serial port", result); + return -1; + } + + return result; +} + +bool SerialPort::isValid() const noexcept { return (port_) ? true : false; } + +std::string SerialPort::setPortSettings( + const SerialParameterSplitter& params) noexcept { + sp_return result; + + sp_flowcontrol flow_control; + switch (params.flow_control) { + case 'N': + flow_control = SP_FLOWCONTROL_NONE; + break; + + case 'X': + flow_control = SP_FLOWCONTROL_XONXOFF; + break; + + case 'R': + flow_control = SP_FLOWCONTROL_RTSCTS; + break; + + case 'D': + flow_control = SP_FLOWCONTROL_DTRDSR; + break; + + default: + flow_control = SP_FLOWCONTROL_NONE; + break; + } + + result = sp_set_flowcontrol(port_, flow_control); + if (result != SP_OK) + return (std::string("Cannot set flow control: ") + std::to_string(result)); + + result = sp_set_bits(port_, params.data_bits); + if (result != SP_OK) + return (std::string("Cannot set data bits: ") + std::to_string(result)); + + sp_parity parity; + switch (params.parity) { + case 'N': + parity = SP_PARITY_NONE; + break; + + case 'O': + parity = SP_PARITY_ODD; + break; + + case 'E': + parity = SP_PARITY_EVEN; + break; + + case 'M': + parity = SP_PARITY_MARK; + break; + + case 'S': + parity = SP_PARITY_SPACE; + break; + + default: + parity = SP_PARITY_INVALID; + break; + } + + result = sp_set_parity(port_, parity); + if (result != SP_OK) + return (std::string("Cannot set parity: ") + std::to_string(result)); + + result = sp_set_stopbits(port_, params.stop_bits); + if (result != SP_OK) + return (std::string("Cannot set stop bits: ") + std::to_string(result)); + + result = sp_set_baudrate(port_, params.speed); + if (result != SP_OK) + return (std::string("Cannot set baud rate: ") + std::to_string(result)); + + return {}; +} + +void SerialPort::closePort() noexcept { + if (port_) { + std::string port_name(sp_get_port_name(port_)); + if (sp_close(port_) != SP_OK) { + LOG_ERROR(logger_, "Could not close %s", port_name.c_str()); + } else { + LOG_INFO(logger_, "%s closed", port_name.c_str()); + } + + sp_free_port(port_); + port_ = nullptr; + } +} diff --git a/src/data_sources/tcp.cpp b/src/data_sources/tcp.cpp new file mode 100644 index 00000000..9142a969 --- /dev/null +++ b/src/data_sources/tcp.cpp @@ -0,0 +1,260 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +#if defined(__linux__) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define GET_SOCKET_ERROR() (errno) +#else +#pragma comment(lib, "ws2_32.lib") + +#define GET_SOCKET_ERROR() WSAGetLastError() + +#endif // __linux__ + +static constexpr uint32_t CONNECT_TIMEOUT = 20; // [s] +static constexpr uint32_t MS_TO_US = 1000; +static constexpr uint32_t S_TO_MS = 1000; + +TCP::TCP(const std::string& ip, const uint16_t port, const LoggerPtr& logger, + const uint32_t read_timeout, const uint32_t write_timeout) + : logger_(logger), + ip_(ip), + port_(port), + read_timeout_(read_timeout), + write_timeout_(write_timeout) {} + +TCP::~TCP() { + closeSocket(); + deinitSockets(); +} + +bool TCP::open() noexcept { + const std::string error = initSockets(); + if (!error.empty()) { + LOG_FATAL(logger_, error.c_str()); + return false; + } else { + return openSocket(); + } +} + +void TCP::close() noexcept { + closeSocket(); +} + +int32_t TCP::read(uint8_t* buffer, const uint32_t buffer_size) { + struct timeval timeout { + (read_timeout_ / S_TO_MS), + (read_timeout_ % S_TO_MS) * MS_TO_US + }; + + fd_set read_set; + FD_ZERO(&read_set); + FD_SET(socket_id_, &read_set); + + int result = select(socket_id_ + 1, &read_set, nullptr, nullptr, &timeout); + if (-1 == result) { + LOG_ERROR(logger_, "Waiting for data error (%u)", + GET_SOCKET_ERROR()); + return -1; + } else if (0 == result) { + LOG_ERROR(logger_, "Receiving data timeout"); + return -1; + } + + result = recv(socket_id_, buffer, buffer_size, 0); + if (result >= 0) { + return result; + } else { + LOG_ERROR(logger_, "Receiving data error (%u)", GET_SOCKET_ERROR()); + return result; + } +} + +int32_t TCP::write(const uint8_t* buffer, const uint32_t buffer_size) { + struct timeval timeout { + (write_timeout_ / S_TO_MS), + (write_timeout_ % S_TO_MS) * MS_TO_US + }; + + fd_set write_set; + FD_ZERO(&write_set); + FD_SET(socket_id_, &write_set); + + int result = select(socket_id_ + 1, nullptr, &write_set, nullptr, &timeout); + if (result == -1) { + LOG_ERROR(logger_, + "Error: %u waiting for the socket to be ready to write data", + GET_SOCKET_ERROR()); + return -1; + } else if (result == 0) { + LOG_WARN(logger_, + "Timeout waiting for the socket to be ready to write data"); + return -1; + } + + result = send(socket_id_, buffer, buffer_size, 0); + if (result > 0) { + return result; + } else { + LOG_ERROR(logger_, "Error (%u) while writing", GET_SOCKET_ERROR()); + return result; + } +} + +std::string TCP::initSockets() noexcept { +#if defined(__linux__) + return {}; +#else + WSADATA d; + if (WSAStartup(MAKEWORD(2, 2), &d)) + return std::string("Failed to initialize sockets"); + else + return {}; +#endif // __linux__ +} + +void TCP::deinitSockets() noexcept { +#if defined(__linux__) + +#else + WSACleanup(); +#endif // __linux__ +} + +void TCP::closeSocket() noexcept { +#if defined(__linux__) + if (socket_id_ != -1) ::close(socket_id_); + socket_id_ = -1; +#else + if (socket_id_ != INVALID_SOCKET) closesocket(socket_id_); + socket_id_ = INVALID_SOCKET; +#endif // __linux__ +} + +bool TCP::isValid() const noexcept { +#if defined(__linux__) + return (socket_id_ != -1); +#else + return (socket_id_ != INVALID_SOCKET); +#endif // __linux__ +} + +bool TCP::openSocket() noexcept { + socket_id_ = socket(AF_INET, SOCK_STREAM, 0); + if (!isValid()) { + LOG_FATAL(logger_, "socket() failed. (%u)", GET_SOCKET_ERROR()); + return false; + } + + if (!setNonBlocking()) { + LOG_FATAL(logger_, "Can't make the socket non-blocking"); + return false; + } + + return connectSocket(); +} + +bool TCP::setNonBlocking() noexcept { +#if defined(_WIN32) + if (socket_id_ == INVALID_SOCKET) return false; + unsigned long mode = 1; + return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false; +#else + int flags = fcntl(socket_id_, F_GETFL, 0); + if (flags == -1) return false; + flags |= O_NONBLOCK; + return (fcntl(socket_id_, F_SETFL, flags) == 0) ? true : false; +#endif +} + +std::string ipFromAddress(const std::string& addr) { + if (addr.empty()) + return {}; + + addrinfo hints; + addrinfo* result = nullptr; + addrinfo* ptr; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_protocol = IPPROTO_TCP; + if (getaddrinfo(addr.c_str(), nullptr, &hints, &result) == 0) + { + sockaddr_in* sockaddr_ipv4; + + for (ptr = result; ptr != nullptr; ptr = ptr->ai_next) { + if (ptr->ai_family == AF_INET) { + char Addr[30]; + + sockaddr_ipv4 = reinterpret_cast(ptr->ai_addr); + return std::string(inet_ntop(AF_INET, &sockaddr_ipv4->sin_addr, Addr, sizeof(Addr))); + } + } + } + + return {}; +} + +bool TCP::connectSocket() noexcept { + struct sockaddr_in server; + + LOG_INFO(logger_, "Connecting to %s:%u",ip_.c_str(),port_); + server.sin_family = AF_INET; + server.sin_addr.s_addr = inet_addr( ipFromAddress(ip_).c_str() ); + server.sin_port = htons(port_); + const int result = + connect(socket_id_, reinterpret_cast(&server), sizeof(server)); + if (result == -1) { +#if defined(_WIN32) + if (WSAGetLastError() != WSAEWOULDBLOCK) return false; +#else + if (errno != EINPROGRESS) return false; +#endif // _WIN32 + + fd_set connect_set; + FD_ZERO(&connect_set); + FD_SET(socket_id_, &connect_set); + + struct timeval timeout { + CONNECT_TIMEOUT, 0 + }; + + switch (select(socket_id_ + 1, nullptr, &connect_set, nullptr, &timeout)) { + case -1: + case 0: + return false; + break; + + default: + LOG_INFO(logger_, "Connected"); + return (FD_ISSET(socket_id_, &connect_set)); + break; + } + } else { + return true; + } +} diff --git a/src/logging/ros_logger.cpp b/src/logging/ros_logger.cpp new file mode 100644 index 00000000..824e7d4c --- /dev/null +++ b/src/logging/ros_logger.cpp @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +void ROSLogger::logDebug(const std::string_view ss) { + if (canLog(ss)) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), ss.data()); + updateLogStatus(ss); + } +} + +void ROSLogger::logInfo(const std::string_view ss) { + if (canLog(ss)) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), ss.data()); + updateLogStatus(ss); + } +} + +void ROSLogger::logWarning(const std::string_view ss) { + if (canLog(ss)) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), ss.data()); + updateLogStatus(ss); + } +} + +void ROSLogger::logError(const std::string_view ss) { + if (canLog(ss)) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), ss.data()); + updateLogStatus(ss); + } +} + +void ROSLogger::logFatal(const std::string_view ss) { + if (canLog(ss)) { + RCLCPP_FATAL(rclcpp::get_logger("rclcpp"), ss.data()); + updateLogStatus(ss); + } +} + +bool ROSLogger::canLog(const std::string_view output_str) const { + if (output_str == last_output_str_) { + const auto now = + std::chrono::time_point::clock::now(); + return ((now - last_output_time_).count() >= timeout_); + } else { + return true; + } +} + +void ROSLogger::updateLogStatus(const std::string_view output_str) { + last_output_str_ = output_str; + last_output_time_ = + std::chrono::time_point::clock::now(); +} + +ROSLogger::ROSLogger(const int64_t log_delay) : timeout_(log_delay) {} diff --git a/src/logging/sbp_file_logger.cpp b/src/logging/sbp_file_logger.cpp new file mode 100644 index 00000000..e9a62899 --- /dev/null +++ b/src/logging/sbp_file_logger.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include +#include +#include +#include +#include + +SbpFileLogger::SbpFileLogger(const std::string& file_path, + const LoggerPtr& logger) + : state_(nullptr, this), logger_(logger) { + std::string file_name(file_path); + time_t now = time(nullptr); + char fname[50] = ""; + + // Create dir + if (!FileSystem::createDir(file_path)) { + LOG_FATAL(logger_, "Unable to create dir: %s", file_path.c_str()); + exit(1); + } + + struct tm local_time; + +#if defined(_WIN32) + localtime_s(&local_time, &now); +#else + localtime_r(&now, &local_time); +#endif // _WIN32 + + std::sprintf(fname, "/swiftnav-%d%02d%02d-%02d%02d%02d.sbp", + local_time.tm_year + 1900, + local_time.tm_mon + 1, + local_time.tm_mday, + local_time.tm_hour, + local_time.tm_min, + local_time.tm_sec); + + file_name += std::string(fname); + + std::filesystem::path path(file_name); + file_name = path.lexically_normal().string(); + +#if defined(_WIN32) + fopen_s(&file_, file_name.c_str(), "wb"); +#else + file_ = fopen(file_name.c_str(), "wb"); +#endif // _WIN32 + if (!file_) { + LOG_FATAL(logger_, "Unable to open the file: %s", file_name.c_str()); + exit(1); + } else + LOG_INFO(logger_, "Logging SBP messages to %s", file_name.c_str()); +} + +SbpFileLogger::~SbpFileLogger() { + if (file_) fclose(file_); +} + +void SbpFileLogger::insert(const sbp_msg_type_t msg_type, + const sbp_msg_t& msg) { + state_.send_message(0, msg_type, msg); +} + +s32 SbpFileLogger::write(const u8* buffer, u32 buffer_length) { + if (file_) + return fwrite(buffer, sizeof(uint8_t), buffer_length, file_); + else + return -1; +} diff --git a/src/logging/sbp_to_ros2_logger.cpp b/src/logging/sbp_to_ros2_logger.cpp new file mode 100644 index 00000000..2d19e03d --- /dev/null +++ b/src/logging/sbp_to_ros2_logger.cpp @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +SBPToROS2Logger::SBPToROS2Logger(sbp::State* state, const LoggerPtr& logger, + const bool log_messages, + const std::string& log_path) + : sbp::AllMessageHandler(state), ros_logger_(logger) { + if (log_messages) + file_logger_ = std::make_unique(log_path, logger); +} + +void SBPToROS2Logger::handle_sbp_message(uint16_t sender_id, + sbp_msg_type_t msg_type, + const sbp_msg_t& msg) { + (void)sender_id; + if (file_logger_) file_logger_->insert(msg_type, msg); + + if (msg_type == SBP_MSG_LOG) { + switch (msg.log.level) { + case SBP_LOG_LOGGING_LEVEL_WARN: + LOG_WARN(ros_logger_, "SBP(WARN): %s", msg.log.text.data); + break; + + case SBP_LOG_LOGGING_LEVEL_EMERG: + LOG_FATAL(ros_logger_, "SBP(EMERG): %s", msg.log.text.data); + break; + + case SBP_LOG_LOGGING_LEVEL_ALERT: + LOG_FATAL(ros_logger_, "SBP(ALERT): %s", msg.log.text.data); + break; + + case SBP_LOG_LOGGING_LEVEL_CRIT: + LOG_ERROR(ros_logger_, "SBP(CRIT): %s", msg.log.text.data); + break; + + case SBP_LOG_LOGGING_LEVEL_ERROR: + LOG_ERROR(ros_logger_, "SBP(ERROR): %s", msg.log.text.data); + break; + } + } +} diff --git a/src/publishers/baseline_publisher.cpp b/src/publishers/baseline_publisher.cpp new file mode 100644 index 00000000..453a0f61 --- /dev/null +++ b/src/publishers/baseline_publisher.cpp @@ -0,0 +1,124 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +BaselinePublisher::BaselinePublisher(sbp::State* state, + const std::string_view topic_name, + rclcpp::Node* node, + const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) + : SBP2ROS2Publisher(state, topic_name, node, logger, + frame, config) {} + +void BaselinePublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_utc_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + if (SBP_UTC_TIME_TIME_SOURCE_NONE != + SBP_UTC_TIME_TIME_SOURCE_GET(msg.flags)) { + struct tm utc; + + utc.tm_year = msg.year; + utc.tm_mon = msg.month; + utc.tm_mday = msg.day; + utc.tm_hour = msg.hours; + utc.tm_min = msg.minutes; + utc.tm_sec = msg.seconds; + msg_.header.stamp.sec = TimeUtils::utcToLinuxTime(utc); + msg_.header.stamp.nanosec = msg.ns; + } + + last_received_utc_time_tow_ = msg.tow; + + publish(); + } +} + +void BaselinePublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_baseline_ned_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + msg_.mode = SBP_BASELINE_NED_FIX_MODE_GET(msg.flags); + + if (SBP_BASELINE_NED_FIX_MODE_INVALID != msg_.mode) { + msg_.satellites_used = msg.n_sats; + + msg_.baseline_n_m = static_cast(msg.n) / 1e3; + msg_.baseline_e_m = static_cast(msg.e) / 1e3; + msg_.baseline_d_m = static_cast(msg.d) / 1e3; + + msg_.baseline_err_h_m = static_cast(msg.h_accuracy) / 1e3; + msg_.baseline_err_v_m = static_cast(msg.v_accuracy) / 1e3; + + double b_m = msg_.baseline_n_m * msg_.baseline_n_m + + msg_.baseline_e_m * msg_.baseline_e_m; + msg_.baseline_length_m = sqrt(b_m + msg_.baseline_d_m * msg_.baseline_d_m); + msg_.baseline_length_h_m = sqrt(b_m); + + if (SBP_BASELINE_NED_FIX_MODE_FIXED_RTK == + SBP_BASELINE_NED_FIX_MODE_GET(msg.flags)) { + // Baseline Direction (bearing or heading) + double dir_rad = atan2(msg_.baseline_e_m, msg_.baseline_n_m); + if (dir_rad < 0.0) { + dir_rad += 2.0 * M_PI; + } + msg_.baseline_dir_deg = + dir_rad * 180.0 / M_PI + config_->getBaseLineDirOffsetDeg(); // [deg] + if (msg_.baseline_dir_deg < 0.0) { + msg_.baseline_dir_deg += 360.0; + } else if (msg_.baseline_dir_deg >= 360.0) { + msg_.baseline_dir_deg -= 360.0; + } + msg_.baseline_dir_err_deg = + atan2(msg_.baseline_err_h_m, msg_.baseline_length_h_m) * 180.0 / + M_PI; + + // Baseline Dip + double dip_rad = atan2(msg_.baseline_d_m, msg_.baseline_length_h_m); + msg_.baseline_dip_deg = + dip_rad * 180.0 / M_PI + config_->getBaseLineDipOffsetDeg(); // [deg] + + msg_.baseline_dip_err_deg = + atan2(msg_.baseline_err_v_m, msg_.baseline_length_h_m) * 180.0 / + M_PI; + + msg_.baseline_orientation_valid = true; + } + } + + last_received_baseline_ned_tow_ = msg.tow; + + publish(); +} + +void BaselinePublisher::publish() { + if ((last_received_baseline_ned_tow_ == last_received_utc_time_tow_) || + !config_->getTimeStampSourceGNSS()) { + if (0 == msg_.header.stamp.sec) { + // Use current platform time if time from the GNSS receiver is not + // available or if a local time source is selected + msg_.header.stamp = node_->now(); + } + + msg_.header.frame_id = frame_; + publisher_->publish(msg_); + + msg_ = swiftnav_ros2_driver::msg::Baseline(); + last_received_utc_time_tow_ = -1; + last_received_baseline_ned_tow_ = -2; + } +} diff --git a/src/publishers/gpsfix_publisher.cpp b/src/publishers/gpsfix_publisher.cpp new file mode 100644 index 00000000..ceac5802 --- /dev/null +++ b/src/publishers/gpsfix_publisher.cpp @@ -0,0 +1,298 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +GPSFixPublisher::GPSFixPublisher(sbp::State* state, + const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) + : SBP2ROS2Publisher(state, topic_name, node, logger, frame, + config) {} + +void GPSFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_gps_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (SBP_GPS_TIME_TIME_SOURCE_NONE != + SBP_GPS_TIME_TIME_SOURCE_GET(msg.flags)) { + msg_.time = (double)(msg.wn) * 604800.0 + (double)msg.tow / 1e3 + + (double)msg.ns_residual / 1e9; // [s] + } + + last_received_gps_time_tow = msg.tow; + + publish(); +} + +void GPSFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_utc_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + // Use GNSS receiver reported time to stamp the data + if (SBP_UTC_TIME_TIME_SOURCE_NONE != + SBP_UTC_TIME_TIME_SOURCE_GET(msg.flags)) { + struct tm utc; + utc.tm_year = msg.year; + utc.tm_mon = msg.month; + utc.tm_mday = msg.day; + utc.tm_hour = msg.hours; + utc.tm_min = msg.minutes; + utc.tm_sec = msg.seconds; + msg_.header.stamp.sec = TimeUtils::utcToLinuxTime(utc); + msg_.header.stamp.nanosec = msg.ns; + } + + last_received_utc_time_tow = msg.tow; + + publish(); + } +} + +void GPSFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_pos_llh_cov_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + switch (SBP_POS_LLH_FIX_MODE_GET(msg.flags)) { + case SBP_POS_LLH_FIX_MODE_SINGLE_POINT_POSITION: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_DIFFERENTIAL_GNSS: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_DGPS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_FLOAT_RTK: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_FIXED_RTK: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_DEAD_RECKONING: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_SBAS_POSITION: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_SBAS_FIX; + break; + default: + msg_.status.status = gps_msgs::msg::GPSStatus::STATUS_NO_FIX; + } // switch() + + if (gps_msgs::msg::GPSStatus::STATUS_NO_FIX != msg_.status.status) { + msg_.status.satellites_used = msg.n_sats; + msg_.status.position_source = 0; + + if (SBP_POS_LLH_FIX_MODE_DEAD_RECKONING != + SBP_POS_LLH_FIX_MODE_GET(msg.flags)) { + msg_.status.position_source |= gps_msgs::msg::GPSStatus::SOURCE_GPS; + } + + if (SBP_POS_LLH_INERTIAL_NAVIGATION_MODE_NONE != + SBP_POS_LLH_INERTIAL_NAVIGATION_MODE_GET(msg.flags)) { + msg_.status.position_source |= gps_msgs::msg::GPSStatus::SOURCE_GYRO | + gps_msgs::msg::GPSStatus::SOURCE_ACCEL; + } + + msg_.latitude = msg.lat; // [deg] + msg_.longitude = msg.lon; // [deg] + msg_.altitude = msg.height; // [m] + + msg_.position_covariance[0] = msg.cov_e_e; // [m] + msg_.position_covariance[1] = msg.cov_n_e; // [m] + msg_.position_covariance[2] = -msg.cov_e_d; // [m] + msg_.position_covariance[3] = msg.cov_n_e; // [m] + msg_.position_covariance[4] = msg.cov_n_n; // [m] + msg_.position_covariance[5] = -msg.cov_n_d; // [m] + msg_.position_covariance[6] = -msg.cov_e_d; // [m] + msg_.position_covariance[7] = -msg.cov_n_d; // [m] + msg_.position_covariance[8] = msg.cov_d_d; // [m] + msg_.position_covariance_type = + gps_msgs::msg::GPSFix::COVARIANCE_TYPE_KNOWN; + + msg_.err_horz = Covariance::covarianceToEstimatedHorizonatalError(msg.cov_n_n, msg.cov_n_e, msg.cov_e_e) * + 2.6926; // [m], scaled to 95% confidence + msg_.err_vert = sqrt(msg.cov_d_d) * 2.0; // [m], scaled to 95% confidence + msg_.err = sqrt( msg_.err_horz*msg_.err_horz + msg_.err_vert*msg_.err_vert ); // [m], 95% confidence + } + else { + msg_.position_covariance[0] = -1.0; // Position is invalid + } + + last_received_pos_llh_cov_tow = msg.tow; + + publish(); +} + +void GPSFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_vel_ned_cov_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (SBP_VEL_NED_VELOCITY_MODE_INVALID != + SBP_VEL_NED_VELOCITY_MODE_GET(msg.flags)) { + msg_.status.motion_source = 0; + + if (SBP_VEL_NED_VELOCITY_MODE_DEAD_RECKONING != + SBP_VEL_NED_VELOCITY_MODE_GET(msg.flags)) { + msg_.status.motion_source |= gps_msgs::msg::GPSStatus::SOURCE_GPS; + } + + if (SBP_VEL_NED_INS_NAVIGATION_MODE_NONE != + SBP_VEL_NED_INS_NAVIGATION_MODE_GET(msg.flags)) { + msg_.status.motion_source |= gps_msgs::msg::GPSStatus::SOURCE_GYRO | + gps_msgs::msg::GPSStatus::SOURCE_ACCEL; + } + + msg_.speed = sqrt((double)(msg.n) * (double)(msg.n) + + (double)(msg.e) * (double)(msg.e)) / + 1e3; // [m/s], horizontal + msg_.climb = (double)msg.d / -1e3; // [m/s], vertical + + msg_.err_speed = + Covariance::covarianceToEstimatedHorizonatalError(msg.cov_n_n, msg.cov_n_e, msg.cov_e_e) * + 2.6926; // [m/s], scaled to 95% confidence + msg_.err_climb = + sqrt(msg.cov_d_d) * 2.0; // [m/s], scaled to 95% confidence + + if (msg_.speed >= config_->getTrackUpdateMinSpeedMps()) { + vel_ned_track_deg = Conversions::radiansToDegrees(atan2((double)msg.e, (double)msg.n)); + if (vel_ned_track_deg < 0.0) { + vel_ned_track_deg += 360.0; + } + vel_ned_err_track_deg = Covariance::covarianceToEstimatedHorizonatalDirectionError( + (double)msg.n/1e3, (double)msg.e/1e3, msg.cov_n_n, msg.cov_e_e ) * + 2.6926; // [deg], scaled to 95% confidence + vel_ned_track_valid = true; + } + } + + last_received_vel_ned_cov_tow = msg.tow; + + publish(); +} + +void GPSFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_orient_euler_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (SBP_ORIENT_EULER_INS_NAVIGATION_MODE_INVALID != + SBP_ORIENT_EULER_INS_NAVIGATION_MODE_GET(msg.flags)) { + msg_.pitch = (double)msg.pitch / 1e6; // [deg] + msg_.roll = (double)msg.roll / 1e6; // [deg] + + msg_.err_pitch = + (double)msg.pitch_accuracy * 2.0; // [deg], scaled to 95% confidence + msg_.err_roll = + (double)msg.roll_accuracy * 2.0; // [deg], scaled to 95% confidence + + orientation_track_deg = + (msg.yaw < 0) ? (double)msg.yaw / 1e6 + 360.0 + : (double)msg.yaw / 1e6; // [deg], in 0 to 360 range + orientation_err_track_deg = + (double)msg.yaw_accuracy * 2.0; // [deg], scaled to 95% confidence + orientation_track_valid = true; + + orientation_present = true; + } + + last_received_orient_euler_tow = msg.tow; + + publish(); +} + +void GPSFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_dops_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (SBP_DOPS_FIX_MODE_INVALID != SBP_DOPS_FIX_MODE_GET(msg.flags)) { + gdop = (double)msg.gdop / 1e2; + pdop = (double)msg.pdop / 1e2; + hdop = (double)msg.hdop / 1e2; + vdop = (double)msg.vdop / 1e2; + tdop = (double)msg.tdop / 1e2; + + time(&dops_time_s); + } +} + +void GPSFixPublisher::publish() { + if (((last_received_gps_time_tow == last_received_utc_time_tow) || + !config_->getTimeStampSourceGNSS()) && + (last_received_gps_time_tow == last_received_pos_llh_cov_tow) && + (last_received_gps_time_tow == last_received_vel_ned_cov_tow) && + ((last_received_gps_time_tow == last_received_orient_euler_tow) || + !orientation_present)) { + if (0 == msg_.header.stamp.sec) { + // Use current platform time if time from the GNSS receiver is not + // available or if a local time source is selected + msg_.header.stamp = node_->now(); + } + + msg_.header.frame_id = frame_; + + if (orientation_track_valid) { + // Use yaw for track if ORIENT EULER message is present and INS solution + // is valid + msg_.track = orientation_track_deg; + msg_.err_track = orientation_err_track_deg; + + last_track_deg = msg_.track; + last_err_track_deg = msg_.err_track; + last_track_valid = true; + + orientation_track_valid = false; + + msg_.status.orientation_source = + msg_.status.position_source; // Orientation is provided by the INS + // fusion only. + } else if (vel_ned_track_valid) { + // Use computed Course Over Ground (COG) for track if VEL NED COV message + // is present and speed is valid + msg_.track = vel_ned_track_deg; + msg_.err_track = vel_ned_err_track_deg; + + last_track_deg = msg_.track; + last_err_track_deg = msg_.err_track; + last_track_valid = true; + + vel_ned_track_valid = false; + } else if (last_track_valid) { + // Use last valid track when there is no valid update + msg_.track = last_track_deg; + msg_.err_track = last_err_track_deg; + } + + time_t current_time_s; + time(¤t_time_s); + + if (difftime(current_time_s, dops_time_s) < + 2.0) { // Publish DOPs if not older than 2 seconds. + msg_.gdop = gdop; + msg_.pdop = pdop; + msg_.hdop = hdop; + msg_.vdop = vdop; + msg_.tdop = tdop; + } + + publisher_->publish(msg_); + + msg_ = gps_msgs::msg::GPSFix(); + last_received_gps_time_tow = -1; + last_received_utc_time_tow = -2; + last_received_pos_llh_cov_tow = -3; + last_received_vel_ned_cov_tow = -4; + last_received_orient_euler_tow = -5; + } +} diff --git a/src/publishers/imu_publisher.cpp b/src/publishers/imu_publisher.cpp new file mode 100644 index 00000000..495e4c47 --- /dev/null +++ b/src/publishers/imu_publisher.cpp @@ -0,0 +1,225 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include + +ImuPublisher::ImuPublisher(sbp::State* state, const std::string_view topic_name, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) + : SBP2ROS2Publisher( + state, topic_name, node, logger, frame, config) {} + +void ImuPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_utc_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + if (SBP_UTC_TIME_TIME_SOURCE_NONE != + SBP_UTC_TIME_TIME_SOURCE_GET(msg.flags)) { + struct tm utc; + + utc.tm_year = msg.year; + utc.tm_mon = msg.month; + utc.tm_mday = msg.day; + utc.tm_hour = msg.hours; + utc.tm_min = msg.minutes; + utc.tm_sec = msg.seconds; + + linux_stamp_s_ = static_cast(TimeUtils::utcToLinuxTime(utc)) + + static_cast(msg.ns) / 1e9; + last_received_utc_time_tow_ = msg.tow; + compute_utc_offset(); + } + } +} + +void ImuPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_gps_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + if (SBP_GPS_TIME_TIME_SOURCE_NONE != + SBP_GPS_TIME_TIME_SOURCE_GET(msg.flags)) { + gps_week_ = msg.wn; + gps_week_valid_ = true; + + gps_stamp_s_ = static_cast(msg.wn * 604800u) + + static_cast(msg.tow) / 1e3 + + static_cast(msg.ns_residual) / 1e9; + last_received_gps_time_tow_ = msg.tow; + compute_utc_offset(); + } + } +} + +void ImuPublisher::compute_utc_offset( void ) { + if (last_received_gps_time_tow_ == last_received_utc_time_tow_) { + utc_offset_s_ = linux_stamp_s_ - gps_stamp_s_; + utc_offset_valid_ = true; + + last_received_utc_time_tow_ = -1; + last_received_gps_time_tow_ = -2; + } +} + + +void ImuPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_gnss_time_offset_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + gps_time_offset_s_ = static_cast(msg.weeks) * 604800.0 + + static_cast(msg.milliseconds) / 1e3 + + static_cast(msg.microseconds) / 1e6; + gps_time_offset_valid_ = true; +} + + +void ImuPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_imu_aux_t& msg) { + const double list_acc_res_mps2[] = {Conversions::standardGravityToMPS2(2.0) / 32768.0, + Conversions::standardGravityToMPS2(4.0) / 32768.0, + Conversions::standardGravityToMPS2(8.0) / 32768.0, + Conversions::standardGravityToMPS2(16.0) / 32768.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + const double list_gyro_res_rad[] = {Conversions::degreesToRadians(2000.0) / 32768.0, + Conversions::degreesToRadians(1000.0) / 32768.0, + Conversions::degreesToRadians(500.0) / 32768.0, + Conversions::degreesToRadians(250.0) / 32768.0, + Conversions::degreesToRadians(125.0) / 32768.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + if (0 == sender_id) return; // Ignore base station data + + acc_res_mps2_ = + list_acc_res_mps2[SBP_IMU_AUX_ACCELEROMETER_RANGE_GET(msg.imu_conf)]; + + gyro_res_rad_ = + list_gyro_res_rad[SBP_IMU_AUX_GYROSCOPE_RANGE_GET(msg.imu_conf)]; +} + + +void ImuPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_imu_raw_t& msg) { + uint32_t imu_raw_tow_ms; + double timestamp_s = 0.0; + + if (0 == sender_id) return; // Ignore base station data + + if ( config_->getTimeStampSourceGNSS() ) { + + switch ( SBP_IMU_RAW_TIME_STATUS_GET(msg.tow) ) { + case SBP_IMU_RAW_TIME_STATUS_REFERENCE_EPOCH_IS_START_OF_CURRENT_GPS_WEEK: + if (gps_week_valid_ && utc_offset_valid_) { + imu_raw_tow_ms = SBP_IMU_RAW_TIME_SINCE_REFERENCE_EPOCH_IN_MILLISECONDS_GET(msg.tow); + + // Check for TOW rollover before the next GPS TIME message arrives + if ( (gps_week_ == last_gps_week_) && (imu_raw_tow_ms < last_imu_raw_tow_ms_) ) { + gps_week_++; + } + last_gps_week_ = gps_week_; + last_imu_raw_tow_ms_ = imu_raw_tow_ms; + + timestamp_s = + static_cast(gps_week_ * 604800u) + + static_cast(imu_raw_tow_ms) / 1e3 + + static_cast(msg.tow_f) / 1e3 / 256.0 + utc_offset_s_; + stamp_source_ = STAMP_SOURCE_GNSS; + } + break; + + case SBP_IMU_RAW_TIME_STATUS_REFERENCE_EPOCH_IS_TIME_OF_SYSTEM_STARTUP: + if (gps_time_offset_valid_ && utc_offset_valid_) { + imu_raw_tow_ms = SBP_IMU_RAW_TIME_SINCE_REFERENCE_EPOCH_IN_MILLISECONDS_GET(msg.tow); + timestamp_s = + gps_time_offset_s_ + + static_cast(imu_raw_tow_ms) / 1e3 + + static_cast(msg.tow_f) / 1e3 / 256.0 + utc_offset_s_; + stamp_source_ = STAMP_SOURCE_GNSS; + } + break; + + } // switch() + + msg_.header.stamp.sec = static_cast(timestamp_s); + msg_.header.stamp.nanosec = static_cast( (timestamp_s - static_cast(msg_.header.stamp.sec)) * 1e9 ); + } + + msg_.orientation_covariance[0] = -1.0; // Orientation is not provided + + if ((acc_res_mps2_ - 0.0) > std::numeric_limits::epsilon()) { + msg_.linear_acceleration.x = static_cast(msg.acc_x) * acc_res_mps2_; + msg_.linear_acceleration.y = static_cast(msg.acc_y) * acc_res_mps2_; + msg_.linear_acceleration.z = static_cast(msg.acc_z) * acc_res_mps2_; + } else { + msg_.linear_acceleration_covariance[0] = -1.0; // Acceleration is not valid + } + + if ((gyro_res_rad_ - 0.0) > std::numeric_limits::epsilon()) { + msg_.angular_velocity.x = static_cast(msg.gyr_x) * gyro_res_rad_; + msg_.angular_velocity.y = static_cast(msg.gyr_y) * gyro_res_rad_; + msg_.angular_velocity.z = static_cast(msg.gyr_z) * gyro_res_rad_; + } else { + msg_.angular_velocity_covariance[0] = -1.0; // Angular velocity is not valid + } + + publish(); +} + + +void ImuPublisher::publish() { + if ( 0 == msg_.header.stamp.sec ) { + // Use current platform time if time from the GNSS receiver is not + // available or if a local time source is selected + msg_.header.stamp = node_->now(); + stamp_source_ = STAMP_SOURCE_PLATFORM; + } + + if ( stamp_source_ != last_stamp_source_ ) { + // Time stamp source has changed - invalidate measurements + msg_.linear_acceleration_covariance[0] = -1.0; + msg_.angular_velocity_covariance[0] = -1.0; + } + last_stamp_source_ = stamp_source_; + + msg_.header.frame_id = frame_; + + publisher_->publish(msg_); + + msg_ = sensor_msgs::msg::Imu(); +} diff --git a/src/publishers/navsatfix_publisher.cpp b/src/publishers/navsatfix_publisher.cpp new file mode 100644 index 00000000..9b24356f --- /dev/null +++ b/src/publishers/navsatfix_publisher.cpp @@ -0,0 +1,249 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +// GNSS Signal Code Identifier +typedef enum { + CODE_GPS_L1CA = 0, + CODE_GPS_L2CM = 1, + CODE_SBAS_L1CA = 2, + CODE_GLO_L1OF = 3, + CODE_GLO_L2OF = 4, + CODE_GPS_L1P = 5, + CODE_GPS_L2P = 6, + CODE_GPS_L2CL = 7, + CODE_GPS_L2CX = 8, + CODE_GPS_L5I = 9, + CODE_GPS_L5Q = 10, + CODE_GPS_L5X = 11, + CODE_BDS2_B1 = 12, + CODE_BDS2_B2 = 13, + CODE_GAL_E1B = 14, + CODE_GAL_E1C = 15, + CODE_GAL_E1X = 16, + CODE_GAL_E6B = 17, + CODE_GAL_E6C = 18, + CODE_GAL_E6X = 19, + CODE_GAL_E7I = 20, + CODE_GAL_E7Q = 21, + CODE_GAL_E7X = 22, + CODE_GAL_E8I = 23, + CODE_GAL_E8Q = 24, + CODE_GAL_E8X = 25, + CODE_GAL_E5I = 26, + CODE_GAL_E5Q = 27, + CODE_GAL_E5X = 28, + CODE_GLO_L1P = 29, + CODE_GLO_L2P = 30, + CODE_BDS3_B1CI = 44, + CODE_BDS3_B1CQ = 45, + CODE_BDS3_B1CX = 46, + CODE_BDS3_B5I = 47, + CODE_BDS3_B5Q = 48, + CODE_BDS3_B5X = 49, + CODE_BDS3_B7I = 50, + CODE_BDS3_B7Q = 51, + CODE_BDS3_B7X = 52, + CODE_BDS3_B3I = 53, + CODE_BDS3_B3Q = 54, + CODE_BDS3_B3X = 55, + CODE_GPS_L1CI = 56, + CODE_GPS_L1CQ = 57, + CODE_GPS_L1CX = 58 +} gnss_signal_code_t; + +NavSatFixPublisher::NavSatFixPublisher(sbp::State* state, + const std::string_view topic_name, + rclcpp::Node* node, + const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) + : SBP2ROS2Publisher(state, topic_name, node, logger, + frame, config) {} + +void NavSatFixPublisher::handle_sbp_msg( + uint16_t sender_id, const sbp_msg_measurement_state_t& msg) { + sbp_measurement_state_t state; + + if (0 == sender_id) return; // Ignore base station data + + status_service = 0; + + for (int i = 0; i < msg.n_states; i++) { + state = msg.states[i]; + + if (state.cn0 > 0) { + switch (state.mesid.code) { + case CODE_GPS_L1CA: + case CODE_GPS_L2CM: + case CODE_GPS_L1P: + case CODE_GPS_L2P: + case CODE_GPS_L2CL: + case CODE_GPS_L2CX: + case CODE_GPS_L5I: + case CODE_GPS_L5Q: + case CODE_GPS_L5X: + case CODE_GPS_L1CI: + case CODE_GPS_L1CQ: + case CODE_GPS_L1CX: + status_service |= sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + break; + + case CODE_GLO_L1OF: + case CODE_GLO_L2OF: + case CODE_GLO_L1P: + case CODE_GLO_L2P: + status_service |= sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS; + break; + + case CODE_GAL_E1B: + case CODE_GAL_E1C: + case CODE_GAL_E1X: + case CODE_GAL_E6B: + case CODE_GAL_E6C: + case CODE_GAL_E6X: + case CODE_GAL_E7I: + case CODE_GAL_E7Q: + case CODE_GAL_E7X: + case CODE_GAL_E8I: + case CODE_GAL_E8Q: + case CODE_GAL_E8X: + case CODE_GAL_E5I: + case CODE_GAL_E5Q: + case CODE_GAL_E5X: + status_service |= sensor_msgs::msg::NavSatStatus::SERVICE_GALILEO; + break; + + case CODE_BDS2_B1: + case CODE_BDS2_B2: + case CODE_BDS3_B1CI: + case CODE_BDS3_B1CQ: + case CODE_BDS3_B1CX: + case CODE_BDS3_B5I: + case CODE_BDS3_B5Q: + case CODE_BDS3_B5X: + case CODE_BDS3_B7I: + case CODE_BDS3_B7Q: + case CODE_BDS3_B7X: + case CODE_BDS3_B3I: + case CODE_BDS3_B3Q: + case CODE_BDS3_B3X: + status_service |= sensor_msgs::msg::NavSatStatus::SERVICE_COMPASS; + break; + + } // switch() + } // if() + } // for() +} + +void NavSatFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_utc_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + if (SBP_UTC_TIME_TIME_SOURCE_NONE != + SBP_UTC_TIME_TIME_SOURCE_GET(msg.flags)) { + struct tm utc; + + utc.tm_year = msg.year; + utc.tm_mon = msg.month; + utc.tm_mday = msg.day; + utc.tm_hour = msg.hours; + utc.tm_min = msg.minutes; + utc.tm_sec = msg.seconds; + msg_.header.stamp.sec = TimeUtils::utcToLinuxTime(utc); + msg_.header.stamp.nanosec = msg.ns; + } + + last_received_utc_time_tow = msg.tow; + + publish(); + } +} + +void NavSatFixPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_pos_llh_cov_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + switch (SBP_POS_LLH_FIX_MODE_GET(msg.flags)) { + case SBP_POS_LLH_FIX_MODE_SINGLE_POINT_POSITION: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_DIFFERENTIAL_GNSS: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_FLOAT_RTK: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_FIXED_RTK: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_DEAD_RECKONING: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + break; + case SBP_POS_LLH_FIX_MODE_SBAS_POSITION: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX; + break; + default: + msg_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } // switch() + + if (sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX != msg_.status.status) { + msg_.status.service = status_service; + + msg_.latitude = msg.lat; // [deg] + msg_.longitude = msg.lon; // [deg] + msg_.altitude = msg.height; // [m] + + msg_.position_covariance[0] = msg.cov_e_e; // [m] + msg_.position_covariance[1] = msg.cov_n_e; // [m] + msg_.position_covariance[2] = -msg.cov_e_d; // [m] + msg_.position_covariance[3] = msg.cov_n_e; // [m] + msg_.position_covariance[4] = msg.cov_n_n; // [m] + msg_.position_covariance[5] = -msg.cov_n_d; // [m] + msg_.position_covariance[6] = -msg.cov_e_d; // [m] + msg_.position_covariance[7] = -msg.cov_n_d; // [m] + msg_.position_covariance[8] = msg.cov_d_d; // [m] + msg_.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN; + } + else { + msg_.position_covariance[0] = -1.0; // Position is invalid + } + + last_received_pos_llh_cov_tow = msg.tow; + + publish(); +} + +void NavSatFixPublisher::publish() { + if ((last_received_pos_llh_cov_tow == last_received_utc_time_tow) || + !config_->getTimeStampSourceGNSS()) { + if (0 == msg_.header.stamp.sec) { + // Use current platform time if time from the GNSS receiver is not + // available or if a local time source is selected + msg_.header.stamp = node_->now(); + } + + msg_.header.frame_id = frame_; + + publisher_->publish(msg_); + + msg_ = sensor_msgs::msg::NavSatFix(); + last_received_utc_time_tow = -1; + last_received_pos_llh_cov_tow = -2; + } +} diff --git a/src/publishers/publisher_factory.cpp b/src/publishers/publisher_factory.cpp new file mode 100644 index 00000000..3637539b --- /dev/null +++ b/src/publishers/publisher_factory.cpp @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +enum class Publishers { + Invalid, + GpsFix, // 1 + NavSatFix, // 2 + TwistWithCovarianceStamped, // 3 + Baseline, // 4 + TimeReference, // 5 + Imu, // 6 +}; + +struct PublisherMap { + Publishers id; + std::string_view name; +}; + +static const PublisherMap publishers[] = { + {Publishers::GpsFix, "gpsfix"}, + {Publishers::NavSatFix, "navsatfix"}, + {Publishers::TwistWithCovarianceStamped, "twistwithcovariancestamped"}, + {Publishers::Baseline, "baseline"}, + {Publishers::TimeReference, "timereference"}, + {Publishers::Imu, "imu"}, +}; + +PublisherPtr publisherFactory(const std::string& pub_type, sbp::State* state, + rclcpp::Node* node, const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) { + PublisherPtr pub; + Publishers pub_id = Publishers::Invalid; + std::string_view topic; + + for (const auto& publisher : publishers) { + if (publisher.name == pub_type) { + pub_id = publisher.id; + topic = publisher.name; + } + } + + switch (pub_id) { + case Publishers::Baseline: + pub = std::make_shared(state, topic, node, logger, + frame, config); + break; + + case Publishers::GpsFix: + pub = std::make_shared(state, topic, node, logger, frame, + config); + break; + + case Publishers::NavSatFix: + pub = std::make_shared(state, topic, node, logger, + frame, config); + break; + + case Publishers::TwistWithCovarianceStamped: + pub = std::make_shared(state, topic, node, logger, + frame, config); + break; + + case Publishers::TimeReference: + pub = std::make_shared(state, topic, node, logger, + frame, config); + break; + + case Publishers::Imu: + pub = std::make_shared(state, topic, node, logger, frame, + config); + break; + + default: + LOG_ERROR(logger, "Publisher %s: isn't valid", pub_type.c_str()); + break; + } + + return pub; +} diff --git a/src/publishers/timereference_publisher.cpp b/src/publishers/timereference_publisher.cpp new file mode 100644 index 00000000..0442c18e --- /dev/null +++ b/src/publishers/timereference_publisher.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +TimeReferencePublisher::TimeReferencePublisher( + sbp::State* state, const std::string_view topic_name, rclcpp::Node* node, + const LoggerPtr& logger, const std::string& frame, + const std::shared_ptr& config) + : SBP2ROS2Publisher(state, topic_name, node, logger, + frame, config) {} + +void TimeReferencePublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_utc_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + if (SBP_UTC_TIME_TIME_SOURCE_NONE != + SBP_UTC_TIME_TIME_SOURCE_GET(msg.flags)) { + struct tm utc; + + utc.tm_year = msg.year; + utc.tm_mon = msg.month; + utc.tm_mday = msg.day; + utc.tm_hour = msg.hours; + utc.tm_min = msg.minutes; + utc.tm_sec = msg.seconds; + msg_.header.stamp.sec = TimeUtils::utcToLinuxTime(utc); + msg_.header.stamp.nanosec = msg.ns; + } + + last_received_utc_time_tow_ = msg.tow; + + publish(); + } +} + +void TimeReferencePublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_gps_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (SBP_GPS_TIME_TIME_SOURCE_NONE != + SBP_GPS_TIME_TIME_SOURCE_GET(msg.flags)) { + msg_.time_ref.sec = msg.wn * 604800u + msg.tow / 1000u; + msg_.time_ref.nanosec = ((msg.tow % 1000u) * 1000000u) + msg.ns_residual; + } else { + msg_.time_ref.sec = -1; + } + + last_received_gps_time_tow_ = msg.tow; + + publish(); +} + +void TimeReferencePublisher::publish() { + if ((last_received_gps_time_tow_ == last_received_utc_time_tow_) || + !config_->getTimeStampSourceGNSS()) { + if (0 == msg_.header.stamp.sec) { + // Use current platform time if time from the GNSS receiver is not + // available or if a local time source is selected + msg_.header.stamp = node_->now(); + } + + msg_.source = frame_; + + publisher_->publish(msg_); + + msg_ = sensor_msgs::msg::TimeReference(); + last_received_utc_time_tow_ = -1; + last_received_gps_time_tow_ = -2; + } +} diff --git a/src/publishers/twistwithcovariancestamped_publisher.cpp b/src/publishers/twistwithcovariancestamped_publisher.cpp new file mode 100644 index 00000000..683706fb --- /dev/null +++ b/src/publishers/twistwithcovariancestamped_publisher.cpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + + +TwistWithCovarianceStampedPublisher::TwistWithCovarianceStampedPublisher(sbp::State* state, + const std::string_view topic_name, + rclcpp::Node* node, + const LoggerPtr& logger, + const std::string& frame, + const std::shared_ptr& config) + : SBP2ROS2Publisher(state, topic_name, node, logger, + frame, config) {} + + +void TwistWithCovarianceStampedPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_utc_time_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (config_->getTimeStampSourceGNSS()) { + if (SBP_UTC_TIME_TIME_SOURCE_NONE != + SBP_UTC_TIME_TIME_SOURCE_GET(msg.flags)) { + struct tm utc; + + utc.tm_year = msg.year; + utc.tm_mon = msg.month; + utc.tm_mday = msg.day; + utc.tm_hour = msg.hours; + utc.tm_min = msg.minutes; + utc.tm_sec = msg.seconds; + msg_.header.stamp.sec = TimeUtils::utcToLinuxTime(utc); + msg_.header.stamp.nanosec = msg.ns; + } + + last_received_utc_time_tow_ = msg.tow; + + publish(); + } +} + +void TwistWithCovarianceStampedPublisher::handle_sbp_msg(uint16_t sender_id, + const sbp_msg_vel_ned_cov_t& msg) { + if (0 == sender_id) return; // Ignore base station data + + if (SBP_VEL_NED_VELOCITY_MODE_INVALID != + SBP_VEL_NED_VELOCITY_MODE_GET(msg.flags)) { + + msg_.twist.twist.linear.x = static_cast(msg.e) / 1e3; // [m/s] + msg_.twist.twist.linear.y = static_cast(msg.n) / 1e3; // [m/s] + msg_.twist.twist.linear.z = -static_cast(msg.d) / 1e3; // [m/s] + + msg_.twist.covariance[0] = msg.cov_e_e; + msg_.twist.covariance[1] = msg.cov_n_e; + msg_.twist.covariance[2] = -msg.cov_e_d; + msg_.twist.covariance[6] = msg.cov_n_e; + msg_.twist.covariance[7] = msg.cov_n_n; + msg_.twist.covariance[8] = -msg.cov_n_d; + msg_.twist.covariance[12] = -msg.cov_e_d; + msg_.twist.covariance[13] = -msg.cov_n_d; + msg_.twist.covariance[14] = msg.cov_d_d; + } + else { + msg_.twist.covariance[0] = -1.0; // Twist is invalid + } + + // Angular velocity is not provided + msg_.twist.covariance[21] = -1.0; + + last_received_vel_ned_cov_tow_ = msg.tow; + + publish(); +} + +void TwistWithCovarianceStampedPublisher::publish() { + if ((last_received_vel_ned_cov_tow_ == last_received_utc_time_tow_) || + !config_->getTimeStampSourceGNSS()) { + if (0 == msg_.header.stamp.sec) { + // Use current platform time if time from the GNSS receiver is not + // available or if a local time source is selected + msg_.header.stamp = node_->now(); + } + + msg_.header.frame_id = frame_; + + publisher_->publish(msg_); + + msg_ = geometry_msgs::msg::TwistWithCovarianceStamped(); + last_received_utc_time_tow_ = -1; + last_received_vel_ned_cov_tow_ = -2; + } +} diff --git a/src/sbp-to-ros.cpp b/src/sbp-to-ros.cpp new file mode 100644 index 00000000..b63efc3e --- /dev/null +++ b/src/sbp-to-ros.cpp @@ -0,0 +1,125 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +static const int64_t LOG_REPUBLISH_DELAY = + TimeUtils::secondsToNanoseconds(2ULL); + +/** + * @brief Class that represents the ROS 2 driver node + */ +class SBPROS2DriverNode : public rclcpp::Node { + public: + /** + * @brief Construct a new SBPROS2DriverNode object + */ + SBPROS2DriverNode() : Node("swiftnav_ros2_driver") { + config_ = std::make_shared(this); + logger_ = std::make_shared(LOG_REPUBLISH_DELAY); + + createDataSources(); + if (!data_source_) exit(EXIT_FAILURE); + state_.set_reader(data_source_.get()); + state_.set_writer(data_source_.get()); + createPublishers(); + + sbptoros2_ = std::make_shared( + &state_, logger_, config_->getLogSBPMessages(), config_->getLogPath()); + + /* SBP Callback processing thread */ + sbp_thread_ = std::thread(&SBPROS2DriverNode::processSBP, this); + } + + // Deleted methods + SBPROS2DriverNode(const SBPROS2DriverNode&) = delete; + SBPROS2DriverNode(SBPROS2DriverNode&&) = delete; + SBPROS2DriverNode& operator=(const SBPROS2DriverNode&) = delete; + SBPROS2DriverNode& operator=(SBPROS2DriverNode&&) = delete; + + /** + * @brief Destroy the SBPROS2DriverNode object + */ + ~SBPROS2DriverNode() { + exit_requested_ = true; + if (sbp_thread_.joinable()) sbp_thread_.join(); + } + + /** + * @brief SBP messages processing thread + */ + void processSBP() { + while (!exit_requested_) { + state_.process(); + } + } + + private: + /** + * @brief Method for creating the data sources + */ + void createDataSources() { + data_source_ = dataSourceFactory(config_, logger_); + } + + /** + * @brief Method for creating the SBP to ROS2 publishers + */ + void createPublishers() { + auto frame = config_->getFrame(); + const auto publishers = config_->getPublishers(); + + LOG_INFO(logger_, "Creating %u publishers", publishers.size()); + for (const auto& publisher : publishers) { + LOG_INFO(logger_, "Adding publisher %s", publisher.c_str()); + pubs_manager_.add( + publisherFactory(publisher, &state_, this, logger_, frame, config_)); + } + } + + sbp::State state_; /** @brief SBP state object */ + std::thread sbp_thread_; /** @brief SBP messages processing thread */ + bool exit_requested_{false}; /** @brief Thread stopping flag */ + std::shared_ptr config_; /** @brief Node configuration */ + std::shared_ptr data_source_; /** @brief data source object */ + std::shared_ptr logger_; /** @brief ROS 2 logging object */ + PublisherManager + pubs_manager_; /** @brief Manager for all the active publishers */ + std::shared_ptr + sbptoros2_; /** @brief SBP to ROS2 logging object */ +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/utils/config.cpp b/src/utils/config.cpp new file mode 100644 index 00000000..3f8d7773 --- /dev/null +++ b/src/utils/config.cpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +Config::Config(rclcpp::Node* node) { + declareParameters(node); + loadParameters(node); +} + +void Config::declareParameters(rclcpp::Node* node) { + node->declare_parameter("frame_name", "swiftnav-gnss"); + node->declare_parameter("log_sbp_messages", false); + node->declare_parameter("log_sbp_filepath", ""); + node->declare_parameter("interface", 0); + node->declare_parameter("sbp_file", ""); + node->declare_parameter("device_name", ""); + node->declare_parameter("connection_str", ""); + node->declare_parameter("read_timeout", 0); + node->declare_parameter("write_timeout", 0); + node->declare_parameter("host_ip", ""); + node->declare_parameter("host_port", 0); + node->declare_parameter("timestamp_source_gnss", true); +#if defined(FOUND_NEWER) + node->declare_parameter("enabled_publishers", rclcpp::PARAMETER_STRING_ARRAY); +#else + node->declare_parameter("enabled_publishers"); +#endif + node->declare_parameter("baseline_dir_offset_deg", 0.0); + node->declare_parameter("baseline_dip_offset_deg", 0.0); + node->declare_parameter("track_update_min_speed_mps", 0.2); +} + +void Config::loadParameters(rclcpp::Node* node) { + node->get_parameter("frame_name", frame_); + node->get_parameter("log_sbp_messages", log_sbp_messages_); + node->get_parameter("log_sbp_filepath", log_path_); + node->get_parameter("interface", interface_); + node->get_parameter("sbp_file", file_); + node->get_parameter("device_name", device_); + node->get_parameter("connection_str", connection_str_); + node->get_parameter("read_timeout", read_timeout_); + node->get_parameter("write_timeout", write_timeout_); + node->get_parameter("host_ip", ip_); + node->get_parameter("host_port", port_); + node->get_parameter("timestamp_source_gnss", timestamp_source_gnss_); + enabled_publishers_ = + node->get_parameter("enabled_publishers").as_string_array(); + node->get_parameter("baseline_dir_offset_deg", + baseline_dir_offset_deg_); + node->get_parameter("baseline_dip_offset_deg", + baseline_dip_offset_deg_); + node->get_parameter("track_update_min_speed_mps", + track_update_min_speed_mps_); +} diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp new file mode 100644 index 00000000..a9f92b1a --- /dev/null +++ b/src/utils/utils.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include + +namespace TimeUtils { +constexpr uint32_t LINUX_TIME_20200101 = 1577836800U; + +constexpr int32_t FIRST_YEAR = 2020; +constexpr uint32_t DAYS_IN_2020_YEAR = 366U; + +constexpr uint32_t DAYS_IN_YEAR = 365U; +constexpr uint32_t DAYS_IN_LEAP_YEAR(DAYS_IN_YEAR + 1U); +constexpr uint32_t DAYS_IN_WEEK = 7U; +constexpr uint32_t HOURS_IN_DAY = 24U; +constexpr uint32_t MINUTES_IN_HOUR = 60U; +constexpr uint32_t SECONDS_IN_MINUTE = 60; +constexpr uint32_t SECONDS_IN_HOUR = (SECONDS_IN_MINUTE * MINUTES_IN_HOUR); +constexpr uint32_t SECONDS_IN_DAY = (HOURS_IN_DAY * SECONDS_IN_HOUR); +constexpr uint32_t SECONDS_IN_WEEK = (DAYS_IN_WEEK * SECONDS_IN_DAY); + +constexpr int days_in_month[] = {31, 28, 31, 30, 31, 30, + 31, 31, 30, 31, 30, 31}; + +static bool IsLeapYear(const int year) { + return ((0 == (year % 4)) && (0 != (year % 100))) || (0 == (year % 400)); +} + +time_t utcToLinuxTime(const struct tm& utc) { + auto yr = FIRST_YEAR; + uint32_t days = 0; + + while (yr < utc.tm_year) { + days += IsLeapYear(yr) ? DAYS_IN_LEAP_YEAR : DAYS_IN_YEAR; + yr++; + } + + for (int32_t i = 0; i < (utc.tm_mon - 1); i++) { + days += days_in_month[i]; + } + + if (IsLeapYear(utc.tm_year) && (utc.tm_mon > 2)) { + days += utc.tm_mday; + } else { + days += utc.tm_mday - 1; + } + + return static_cast(LINUX_TIME_20200101 + days * SECONDS_IN_DAY + + utc.tm_hour * SECONDS_IN_HOUR + + utc.tm_min * SECONDS_IN_MINUTE + utc.tm_sec); +} + +uint64_t secondsToMilliseconds(const uint64_t seconds) { + return seconds * 1000ULL; +} + +uint64_t secondsToNanoseconds(const uint64_t seconds) { + return seconds * 1000000000ULL; +} +} // namespace TimeUtils + +namespace Covariance { +double covarianceToEstimatedHorizonatalError( + const double cov_n_n, const double cov_n_e, const double cov_e_e) { + const double mx_det = cov_n_n * cov_e_e - cov_n_e * cov_n_e; + const double mx_mean_trace = (cov_n_n + cov_e_e) / 2.0; + + const double a = sqrt(mx_mean_trace * mx_mean_trace - mx_det); + const double e1 = mx_mean_trace + a; + const double e2 = mx_mean_trace - a; + + double ehe_squared = std::max(e1, e2); // 39.35% + ehe_squared *= 2.2952; // 68.27% + + return sqrt(ehe_squared); +} + +double covarianceToEstimatedHorizonatalDirectionError( + const double n, const double e, const double cov_n_n, const double cov_e_e ) { + + const double a = sqrt( n*n + e*e ); + const double c = sqrt( cov_n_n + cov_e_e ); + double ede_deg = atan2( c, a ) * 180.0 / M_PI; + + return ede_deg; +} + +} // namespace Covariance + +namespace FileSystem { +bool createDir(const std::string& dir) { + bool result = true; + + if (!std::filesystem::exists(dir)) + result = std::filesystem::create_directories(dir); + + return result; +} +} // namespace FileSystem diff --git a/test/mocked_logger.cpp b/test/mocked_logger.cpp new file mode 100644 index 00000000..24ab147d --- /dev/null +++ b/test/mocked_logger.cpp @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +#include + +void MockedLogger::logDebug(const std::string_view ss) { + std::cout << "DEBUG->" << ss << std::endl; + last_logged_debug_ = ss; +} + +void MockedLogger::logInfo(const std::string_view ss) { + std::cout << "INFO->" << ss << std::endl; + last_logged_info_ = ss; +} + +void MockedLogger::logWarning(const std::string_view ss) { + std::cout << "WARN->" << ss << std::endl; + last_logged_warning_ = ss; +} + +void MockedLogger::logError(const std::string_view ss) { + std::cout << "ERROR->" << ss << std::endl; + last_logged_error_ = ss; +} + +void MockedLogger::logFatal(const std::string_view ss) { + std::cout << "FATAL->" << ss << std::endl; + last_logged_fatal_ = ss; +} + + std::string MockedLogger::getLastLoggedDebug() { + return last_logged_debug_; + } + std::string MockedLogger::getLastLoggedInfo() { + return last_logged_info_; + } + std::string MockedLogger::getLastLoggedWarning() { + return last_logged_warning_; + } + std::string MockedLogger::getLastLoggedError() { + return last_logged_error_; + } + std::string MockedLogger::getLastLoggedFatal() { + return last_logged_fatal_; + } + diff --git a/test/publishers/test_custom_publishers.cpp b/test/publishers/test_custom_publishers.cpp new file mode 100644 index 00000000..0ee56c1a --- /dev/null +++ b/test/publishers/test_custom_publishers.cpp @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + + +constexpr uint32_t MAX_MSG_SIZE = 255; +constexpr uint64_t SECONDS = 1000000000ULL; + +static bool timedOut(const uint64_t start, const uint64_t timeout) { + const auto now = std::chrono::steady_clock::now().time_since_epoch().count(); + return (now - start) >= timeout; +} + +/** + * @brief Class to Mock a data source + */ +class MockedDataSource : public SbpDataSource { + public: + s32 read(u8* buffer, u32 buffer_length) override { + s32 result = -1; + + std::unique_lock lock(data_lock_); + if (buffer_.size()) { + const uint32_t to_read = + std::min(buffer_.size(), static_cast(buffer_length)); + for (uint32_t i = 0; i < to_read; ++i) { + buffer[i] = buffer_.front(); + buffer_.pop_front(); + } + + result = to_read; + } + + return result; + } + + s32 write(const u8* buffer, u32 buffer_length) override { + std::unique_lock lock(data_lock_); + std::copy(buffer, buffer + buffer_length, std::back_inserter(buffer_)); + return buffer_length; + } + + private: + std::deque buffer_; + std::mutex data_lock_; +}; + +class SBPRunner { + public: + SBPRunner() { + state_.set_reader(&data_source_); + state_.set_writer(&data_source_); + sbp_th_ = std::thread(&SBPRunner::sbp_thread_proc, this); + } + + ~SBPRunner() { + exit_requested_ = true; + sbp_th_.join(); + } + + sbp::State* getState() { return &state_; } + MockedDataSource* getDataSource() { return &data_source_; } + void inject(const sbp_msg_t& msg, const sbp_msg_type_t msg_type) { + state_.send_message(SBP_SENDER_ID, msg_type, msg); + } + + private: + void sbp_thread_proc() { + while (!exit_requested_) { + state_.process(); + } + } + + std::thread sbp_th_; + bool exit_requested_{false}; + sbp::State state_; + MockedDataSource data_source_; +}; + +class TestCustomPublishers : public ::testing::Test { + public: + static void SetUpTestCase() { + rclcpp::init(0, nullptr); + logger_ = std::make_shared(); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + template + void testPublisher(const std::string& pub_type, const sbpT& msg, + const sbp_msg_type_t msg_type, Func comp) { + bool test_finished = false; + bool timed_out = false; + auto node = std::make_shared("TestCustomPublishersNode"); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + auto pub = publisherFactory(pub_type, runner_.getState(), node.get(), + logger_, frame_name_, config); + auto subs_call = [&msg, &test_finished, &comp](const rosT ros_msg) { + comp(msg, ros_msg); + test_finished = true; + }; + auto sub = node->create_subscription(topic_name_, 1, subs_call); + + // publish + runner_.inject(msg, msg_type); + + // wait for result + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + + const auto start = + std::chrono::steady_clock::now().time_since_epoch().count(); + + while (!test_finished && !timed_out) { + executor.spin_once(std::chrono::nanoseconds(10000000LL)); + timed_out = timedOut(start, 2 * SECONDS); + } + + ASSERT_FALSE(timed_out); + } + + const std::string topic_name_ = "test_custom"; + const std::string frame_name_ = "test_frame"; + static LoggerPtr logger_; + static SBPRunner runner_; +}; + +LoggerPtr TestCustomPublishers::logger_; +SBPRunner TestCustomPublishers::runner_; + +TEST_F(TestCustomPublishers, CreateInvalidPublisher) { + auto node = std::make_shared("TestCustomPublishersNode"); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + auto pub = publisherFactory("invalid_one", runner_.getState(), node.get(), + logger_, frame_name_, config); + ASSERT_FALSE(pub); +} + +TEST_F(TestCustomPublishers, CreateBaselinePublisher) { +} diff --git a/test/publishers/test_gps_fix_publisher.cpp b/test/publishers/test_gps_fix_publisher.cpp new file mode 100644 index 00000000..f672779d --- /dev/null +++ b/test/publishers/test_gps_fix_publisher.cpp @@ -0,0 +1,1285 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +#include +#include + +#include +#include + +#include +#include + +class TestGPSFixPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + const std::string topic_name_ = "test_gps_fix"; + const std::string frame_name_ = "test_frame"; + sbp::State state_; + +}; + +TEST_F(TestGPSFixPublisher, sendMessage) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2100; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 2100; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 2100; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 2100; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 2100; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 2100; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); +} + +TEST_F(TestGPSFixPublisher, posllhcovMessageTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2100; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 4500; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher, velCovMsgTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 4500; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher, velNedCovMsgTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 2100; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 4500; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher, orientEulerMsgTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 2100; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 4500; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher,dopsMsgTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 2100; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 4500; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher,gpstimeMsgTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 2100; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 4500; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher, obsMsgTooOld) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 2100; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = + [&is_received]( + const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_FALSE(is_received); +} + +TEST_F(TestGPSFixPublisher, noVelCogMsg) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 4500; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 4500; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 4500; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 4500; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 4500; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 2100; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = + [&is_received]( + const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 0); + ASSERT_EQ(msg->speed, 0); + ASSERT_EQ(msg->climb, 0); + ASSERT_EQ(msg->err_speed, 0); + ASSERT_EQ(msg->err_climb, 0); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); +} + +TEST_F(TestGPSFixPublisher, noVelCovMessage) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2100; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t orient_euler_sbp_msg; + orient_euler_sbp_msg.orient_euler.tow = 2100; + orient_euler_sbp_msg.orient_euler.pitch = 2; + orient_euler_sbp_msg.orient_euler.roll = 2; + orient_euler_sbp_msg.orient_euler.yaw = 2; + orient_euler_sbp_msg.orient_euler.pitch_accuracy = 2; + orient_euler_sbp_msg.orient_euler.roll_accuracy = 2; + orient_euler_sbp_msg.orient_euler.yaw_accuracy = 2; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 2100; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 2100; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 2100; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 2); + ASSERT_EQ(msg->roll, 2); + ASSERT_EQ(msg->dip, 2); + ASSERT_EQ(msg->err_pitch, 2); + ASSERT_EQ(msg->err_roll, 2); + ASSERT_EQ(msg->err_dip, 2); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, orient_euler_sbp_msg.orient_euler); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); +} + +TEST_F(TestGPSFixPublisher, noOrientdMessage) { + auto node = std::make_shared("TestGPSFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + GPSFixPublisher gps_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2100; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 10; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 20; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 5; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + sbp_msg_t vel_ned_cov_sbp_msg; + vel_ned_cov_sbp_msg.vel_ned_cov.tow = 2100; + + sbp_msg_t dops_sbp_msg; + dops_sbp_msg.dops.tow = 2100; + dops_sbp_msg.dops.gdop = 2; + dops_sbp_msg.dops.pdop = 3; + dops_sbp_msg.dops.hdop = 4; + dops_sbp_msg.dops.vdop = 5; + dops_sbp_msg.dops.tdop = 6; + + sbp_msg_t gps_time_sbp_msg; + gps_time_sbp_msg.gps_time.tow = 2100; + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 2100; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 10; + obs_sbp_msg.obs.obs[0] = obs_content; + + bool is_received = false; + auto callback = [&is_received](const gps_msgs::msg::GPSFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.satellites_used, 3); + ASSERT_EQ(msg->err_horz, 1); + ASSERT_EQ(msg->err_vert, 2); + ASSERT_EQ(msg->err_track, 3); + + ASSERT_EQ(msg->latitude, 10); + ASSERT_EQ(msg->longitude, 20); + ASSERT_EQ(msg->altitude, 5); + + ASSERT_EQ(msg->track, 2); + ASSERT_EQ(msg->speed, 2); + ASSERT_EQ(msg->climb, 2); + ASSERT_EQ(msg->err_speed, 2); + ASSERT_EQ(msg->err_climb, 2); + + ASSERT_EQ(msg->pitch, 0); + ASSERT_EQ(msg->roll, 0); + ASSERT_EQ(msg->dip, 0); + ASSERT_EQ(msg->err_pitch, 0); + ASSERT_EQ(msg->err_roll, 0); + ASSERT_EQ(msg->err_dip, 0); + + ASSERT_EQ(msg->gdop, 2); + ASSERT_EQ(msg->pdop, 3); + ASSERT_EQ(msg->hdop, 4); + ASSERT_EQ(msg->vdop, 5); + ASSERT_EQ(msg->tdop, 6); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->status.satellites_visible, 1); + }; + + auto sub = node->create_subscription(topic_name_, 1, + callback); + gps_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + gps_fix_publisher.handle_sbp_msg(0, vel_ned_cov_sbp_msg.vel_ned_cov); + gps_fix_publisher.handle_sbp_msg(0, dops_sbp_msg.dops); + gps_fix_publisher.handle_sbp_msg(0, gps_time_sbp_msg.gps_time); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); +} diff --git a/test/publishers/test_nav_sat_fix_publisher.cpp b/test/publishers/test_nav_sat_fix_publisher.cpp new file mode 100644 index 00000000..aca9f45b --- /dev/null +++ b/test/publishers/test_nav_sat_fix_publisher.cpp @@ -0,0 +1,456 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +#include +#include + +#include +#include + +#include +#include + +class TestNavSatFixPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + const std::string topic_name_ = "test_nav_sat_fix"; + const std::string frame_name_ = "test_frame"; + sbp::State state_; + +}; + +TEST_F(TestNavSatFixPublisher, sendMessage) { + + auto node = std::make_shared("TestNavSatFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + NavSatFixPublisher nav_sat_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 1; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 0; + obs_sbp_msg.obs.obs[0] = obs_content; + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 3; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 4; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 10; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + bool is_received = false; + auto callback = + [&is_received]( + const sensor_msgs::msg::NavSatFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.service, sensor_msgs::msg::NavSatStatus::SERVICE_GPS); + + ASSERT_EQ(msg->latitude, 3); + ASSERT_EQ(msg->longitude, 4); + ASSERT_EQ(msg->altitude, 10); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->position_covariance_type, sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN); + ASSERT_EQ(msg->status.status, sensor_msgs::msg::NavSatStatus::STATUS_FIX); + + }; + auto sub = node->create_subscription(topic_name_, 1, callback); + nav_sat_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + +} + +TEST_F(TestNavSatFixPublisher, SERVICE_GPS_StatusService) { + + auto node = std::make_shared("TestNavSatFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + NavSatFixPublisher nav_sat_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 1; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 0; + obs_sbp_msg.obs.obs[0] = obs_content; + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 3; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 4; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 10; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + bool is_received = false; + auto callback = + [&is_received]( + const sensor_msgs::msg::NavSatFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.service, sensor_msgs::msg::NavSatStatus::SERVICE_GPS); + + ASSERT_EQ(msg->latitude, 3); + ASSERT_EQ(msg->longitude, 4); + ASSERT_EQ(msg->altitude, 10); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->position_covariance_type, sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN); + ASSERT_EQ(msg->status.status, sensor_msgs::msg::NavSatStatus::STATUS_FIX); + + }; + auto sub = node->create_subscription(topic_name_, 1, callback); + + u8 values[] = {0, 1, 5, 7, 8, 9, 10, 11, 56, 57, 58, 2, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43}; + for(u8 &val : values) { + is_received = false; + obs_content.sid.code = val; + obs_sbp_msg.obs.obs[0] = obs_content; + nav_sat_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + +} + +TEST_F(TestNavSatFixPublisher, SERVICE_GLONAS_StatusService) { + + auto node = std::make_shared("TestNavSatFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + NavSatFixPublisher nav_sat_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 1; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 0; + obs_sbp_msg.obs.obs[0] = obs_content; + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 3; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 4; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 10; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + bool is_received = false; + auto callback = + [&is_received]( + const sensor_msgs::msg::NavSatFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.service, sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS); + + ASSERT_EQ(msg->latitude, 3); + ASSERT_EQ(msg->longitude, 4); + ASSERT_EQ(msg->altitude, 10); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->position_covariance_type, sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN); + ASSERT_EQ(msg->status.status, sensor_msgs::msg::NavSatStatus::STATUS_FIX); + + }; + auto sub = node->create_subscription(topic_name_, 1, callback); + + u8 values[] = {3, 4, 29, 30}; + for(u8 &val : values) { + is_received = false; + obs_content.sid.code = val; + obs_sbp_msg.obs.obs[0] = obs_content; + nav_sat_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F(TestNavSatFixPublisher, SERVICE_GALILEO_StatusService) { + auto node = std::make_shared("TestNavSatFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + NavSatFixPublisher nav_sat_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 1; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 0; + obs_sbp_msg.obs.obs[0] = obs_content; + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 3; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 4; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 10; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + bool is_received = false; + auto callback = + [&is_received](const sensor_msgs::msg::NavSatFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.service, + sensor_msgs::msg::NavSatStatus::SERVICE_GALILEO); + + ASSERT_EQ(msg->latitude, 3); + ASSERT_EQ(msg->longitude, 4); + ASSERT_EQ(msg->altitude, 10); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->position_covariance_type, + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN); + ASSERT_EQ(msg->status.status, sensor_msgs::msg::NavSatStatus::STATUS_FIX); + }; + auto sub = node->create_subscription( + topic_name_, 1, callback); + + u8 values[] = {14, 28}; + for (u8 &val : values) { + is_received = false; + obs_content.sid.code = val; + obs_sbp_msg.obs.obs[0] = obs_content; + nav_sat_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F(TestNavSatFixPublisher, SERVICE_COMPASS_StatusService) { + + auto node = std::make_shared("TestNavSatFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + NavSatFixPublisher nav_sat_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 1; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 0; + obs_sbp_msg.obs.obs[0] = obs_content; + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 3; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 4; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 10; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + bool is_received = false; + auto callback = + [&is_received](const sensor_msgs::msg::NavSatFix::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->status.service, + sensor_msgs::msg::NavSatStatus::SERVICE_COMPASS); + + ASSERT_EQ(msg->latitude, 3); + ASSERT_EQ(msg->longitude, 4); + ASSERT_EQ(msg->altitude, 10); + + ASSERT_EQ(msg->position_covariance[0], 1); + ASSERT_EQ(msg->position_covariance[1], 1); + ASSERT_EQ(msg->position_covariance[2], -1); + ASSERT_EQ(msg->position_covariance[3], 1); + ASSERT_EQ(msg->position_covariance[4], 1); + ASSERT_EQ(msg->position_covariance[5], -1); + ASSERT_EQ(msg->position_covariance[6], -1); + ASSERT_EQ(msg->position_covariance[7], -1); + ASSERT_EQ(msg->position_covariance[8], 1); + + ASSERT_EQ(msg->position_covariance_type, + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN); + ASSERT_EQ(msg->status.status, sensor_msgs::msg::NavSatStatus::STATUS_FIX); + }; + auto sub = node->create_subscription(topic_name_, + 1, callback); + + u8 values[] = {12, 13, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55}; + for (u8 &val : values) { + is_received = false; + obs_content.sid.code = val; + obs_sbp_msg.obs.obs[0] = obs_content; + nav_sat_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F(TestNavSatFixPublisher, timeDiff) { + auto node = std::make_shared("TestNavSatFixNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + NavSatFixPublisher nav_sat_fix_publisher(&state_, topic_name_, node.get(), ml, + frame_name_, config); + + sbp_msg_t obs_sbp_msg; + obs_sbp_msg.obs.header.t.tow = 1; + obs_sbp_msg.obs.n_obs = 1; + sbp_packed_obs_content_t obs_content; + obs_content.sid.code = 0; + obs_sbp_msg.obs.obs[0] = obs_content; + + sbp_msg_t pos_llh_cov_sbp_msg; + pos_llh_cov_sbp_msg.pos_llh_cov.tow = 2002; + pos_llh_cov_sbp_msg.pos_llh_cov.lat = 3; + pos_llh_cov_sbp_msg.pos_llh_cov.lon = 4; + pos_llh_cov_sbp_msg.pos_llh_cov.height = 10; + + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_e = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_n = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_e_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_n_d = 1; + pos_llh_cov_sbp_msg.pos_llh_cov.cov_d_d = 1; + + pos_llh_cov_sbp_msg.pos_llh_cov.flags = 1; + + bool is_received = false; + auto callback = + [&is_received](const sensor_msgs::msg::NavSatFix::SharedPtr /*msg*/) -> void { + is_received = true; + }; + auto sub = node->create_subscription( + topic_name_, 1, callback); + nav_sat_fix_publisher.handle_sbp_msg(0, pos_llh_cov_sbp_msg.pos_llh_cov); + ASSERT_EQ( + "Time difference between OBS message and POS_LLH_COV message is larger " + "than Max", + ml->getLastLoggedWarning()); + + ASSERT_FALSE(is_received); +} diff --git a/test/publishers/test_time_reference_publisher.cpp b/test/publishers/test_time_reference_publisher.cpp new file mode 100644 index 00000000..a0f5bbc6 --- /dev/null +++ b/test/publishers/test_time_reference_publisher.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +#include +#include + +#include +#include + +#include + +#include +#include + +class TestTimeReferencePublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + const std::string topic_name_ = "test_time_reference"; + const std::string frame_name_ = "test_frame"; + sbp::State state_; + +}; + +TEST_F(TestTimeReferencePublisher, sendMessage) { + auto node = std::make_shared("TestTimeReferenceNode"); + auto ml = std::make_shared(); + auto node_ptr = node.get(); + auto config = std::make_shared(node_ptr); + TimeReferencePublisher time_reference_publisher( + &state_, topic_name_, node.get(), ml, frame_name_, config); + + sbp_msg_t sbp_msg; + sbp_msg.gps_time.tow = 1000; + sbp_msg.gps_time.ns_residual = 2; + + bool is_received = false; + auto callback = + [&is_received]( + const sensor_msgs::msg::TimeReference::SharedPtr msg) -> void { + is_received = true; + + ASSERT_EQ(msg->time_ref.sec, 1); + ASSERT_FLOAT_EQ(msg->time_ref.nanosec, 2); + }; + auto sub = node->create_subscription(topic_name_, 1, callback); + time_reference_publisher.handle_sbp_msg(0, sbp_msg.gps_time); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + +} diff --git a/test/test_main.cpp b/test/test_main.cpp new file mode 100644 index 00000000..e9cd659f --- /dev/null +++ b/test/test_main.cpp @@ -0,0 +1,18 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_network.cpp b/test/test_network.cpp new file mode 100644 index 00000000..91f629d2 --- /dev/null +++ b/test/test_network.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include +#include + +using ::testing::Return; + +constexpr uint16_t DEFAULT_VALID_PORT = 55555; +constexpr uint16_t DEFAULT_INVALID_PORT = 8082; +const std::string DEFAULT_VALID_IP = "127.0.0.1"; +const std::string DEFAULT_INVALID_IP = "0.0.0.11"; + +class MockedTCP : public TCP { + public: + MockedTCP(const std::string& ip, const uint16_t port, const LoggerPtr& logger, + const uint32_t read_timeout, const uint32_t write_timeout) + : TCP(ip, port, logger, read_timeout, write_timeout) {} + MOCK_METHOD(bool, open, (), (noexcept, override)); + MOCK_METHOD(int32_t, read, (uint8_t * buffer, const uint32_t buffer_length), + (override)); + MOCK_METHOD(int32_t, write, + (const uint8_t* buffer, const uint32_t buffer_length), + (override)); + MOCK_METHOD(bool, isValid, (), (const, noexcept, override)); +}; + +// ************************************************************************* +// TCPDataSource +TEST(TCPDataSource, TestInvalidConnection) { + auto logger = std::make_shared(); + auto mocked_tcp = std::make_shared( + DEFAULT_INVALID_IP, DEFAULT_INVALID_PORT, logger, 2000, 2000); + EXPECT_CALL(*mocked_tcp, open).Times(1).WillOnce(Return(false)); + EXPECT_CALL(*mocked_tcp, isValid).WillOnce(Return(false)); + SbpTCPDataSource reader(logger, mocked_tcp); + ASSERT_FALSE(reader.isValid()); +} + +TEST(TCPDataSource, TestValidConnection) { + auto logger = std::make_shared(); + auto mocked_tcp = std::make_shared( + DEFAULT_VALID_IP, DEFAULT_VALID_PORT, logger, 2000, 2000); + EXPECT_CALL(*mocked_tcp, open).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*mocked_tcp, isValid).WillOnce(Return(true)); + SbpTCPDataSource reader(logger, mocked_tcp); + ASSERT_TRUE(reader.isValid()); +} + +// Reading tests +TEST(TCPDataSource, TestReadingWithInvalidObject) { + auto logger = std::make_shared(); + std::shared_ptr tcp; + std::cout << "1\n"; + SbpTCPDataSource reader(logger, tcp); + std::cout << "2\n"; + ASSERT_FALSE(reader.isValid()); + std::cout << "3\n"; + uint8_t buffer[100]; + ASSERT_EQ(-1, reader.read(buffer, 100)); +} + +TEST(TCPDataSource, TestReadingWithNullBuffer) { + auto logger = std::make_shared(); + auto mocked_tcp = std::make_shared( + DEFAULT_VALID_IP, DEFAULT_VALID_PORT, logger, 2000, 2000); + EXPECT_CALL(*mocked_tcp, open).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*mocked_tcp, read).Times(0); + ON_CALL(*mocked_tcp, isValid).WillByDefault(Return(true)); + SbpTCPDataSource reader(logger, mocked_tcp); + ASSERT_TRUE(reader.isValid()); + ASSERT_EQ(-1, reader.read(nullptr, 100)); +} + +TEST(TCPDataSource, TestReadPackageOK) { + auto logger = std::make_shared(); + auto mocked_tcp = std::make_shared( + DEFAULT_VALID_IP, DEFAULT_VALID_PORT, logger, 2000, 2000); + EXPECT_CALL(*mocked_tcp, open).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*mocked_tcp, read).Times(1).WillOnce(Return(100)); + ON_CALL(*mocked_tcp, isValid).WillByDefault(Return(true)); + SbpTCPDataSource reader(logger, mocked_tcp); + ASSERT_TRUE(reader.isValid()); + uint8_t buffer[100]; + const int32_t result = reader.read(buffer, 100); + ASSERT_TRUE((result > 0) && (result <= 100)); +} + +// Writing tests +TEST(TCPDataSource, TestWritingWithInvalidObject) { + auto logger = std::make_shared(); + std::shared_ptr tcp; + SbpTCPDataSource writer(logger, std::move(tcp)); + ASSERT_FALSE(writer.isValid()); + uint8_t buffer[100]; + ASSERT_EQ(-1, writer.write(buffer, 100)); +} + +TEST(TCPDataSource, TestWritingWithNullBuffer) { + auto logger = std::make_shared(); + auto mocked_tcp = std::make_shared( + DEFAULT_VALID_IP, DEFAULT_VALID_PORT, logger, 2000, 2000); + EXPECT_CALL(*mocked_tcp, open).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*mocked_tcp, write).Times(0); + ON_CALL(*mocked_tcp, isValid).WillByDefault(Return(true)); + SbpTCPDataSource writer(logger, mocked_tcp); + ASSERT_TRUE(writer.isValid()); + ASSERT_EQ(-1, writer.write(nullptr, 100)); +} + +TEST(TCPDataSource, TestWritePackageOK) { + auto logger = std::make_shared(); + auto mocked_tcp = std::make_shared( + DEFAULT_VALID_IP, DEFAULT_VALID_PORT, logger, 2000, 2000); + EXPECT_CALL(*mocked_tcp, open).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*mocked_tcp, write).Times(1).WillOnce(Return(100)); + ON_CALL(*mocked_tcp, isValid).WillByDefault(Return(true)); + SbpTCPDataSource writer(logger, mocked_tcp); + ASSERT_TRUE(writer.isValid()); + uint8_t buffer[100]; + const int32_t result = writer.write(buffer, 100); + ASSERT_TRUE((result > 0) && (result <= 100)); +} diff --git a/test/test_serial.cpp b/test/test_serial.cpp new file mode 100644 index 00000000..81b8817c --- /dev/null +++ b/test/test_serial.cpp @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2015-2023 Swift Navigation Inc. + * Contact: https://support.swiftnav.com + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include +#include + +#include + +using ::testing::Return; + +#if defined(_WIN32) +constexpr char VALID_PORT[] = "COM1"; +constexpr char INVALID_PORT[] = "COM28"; +#else +constexpr char VALID_PORT[] = "/dev/ttyS0"; +constexpr char INVALID_PORT[] = "/dev/ttyTAM32"; +#endif // _WIN32 + +constexpr char VALID_CONNSTR[] = "115200|N|8|1|N"; +constexpr char INVALID_CONNSTR[] = "19200|T|8|1|W"; + +class MockedSerialPort : public SerialPort { + public: + MockedSerialPort(const std::string& device_name, + const std::string& connection_string, + const uint32_t read_timeout, const uint32_t write_timeout, + const LoggerPtr& logger) + : SerialPort(device_name, connection_string, read_timeout, write_timeout, + logger) {} + MOCK_METHOD(bool, open, (), (noexcept, override)); + MOCK_METHOD(int32_t, read, (uint8_t * buffer, const uint32_t buffer_length), + (override)); + MOCK_METHOD(int32_t, write, + (const uint8_t* buffer, const uint32_t buffer_length), + (override)); + MOCK_METHOD(bool, isValid, (), (const, noexcept, override)); +}; + +// ************************************************************************* +// SerialDataSource +TEST(SerialDataSource, ConnectWithUnexistentDevice) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared( + INVALID_PORT, VALID_CONNSTR, 2000, 2000, logger); + EXPECT_CALL(*mocked_sp, open).Times(1).WillOnce(Return(false)); + EXPECT_CALL(*mocked_sp, isValid).WillOnce(Return(false)); + SbpSerialDataSource reader(logger, mocked_sp); + ASSERT_FALSE(reader.isValid()); +} + +TEST(SerialDataSource, ConnectWithExistentDeviceButInvalidConnStr) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared( + VALID_PORT, INVALID_CONNSTR, 2000, 2000, logger); + EXPECT_CALL(*mocked_sp, open).Times(1).WillOnce(Return(false)); + EXPECT_CALL(*mocked_sp, isValid).WillOnce(Return(false)); + SbpSerialDataSource reader(logger, mocked_sp); + ASSERT_FALSE(reader.isValid()); +} + +TEST(SerialDataSource, ConnectWithExistentDevice) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared(VALID_PORT, VALID_CONNSTR, + 2000, 2000, logger); + EXPECT_CALL(*mocked_sp, open).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*mocked_sp, isValid).WillOnce(Return(true)); + SbpSerialDataSource reader(logger, mocked_sp); + ASSERT_TRUE(reader.isValid()); +} + +// Reading tests +TEST(SerialDataSource, ReadPackageWithInvalidSource) { + auto logger = std::make_shared(); + std::shared_ptr serial_port; + SbpSerialDataSource reader(logger, serial_port); + ASSERT_FALSE(reader.isValid()); + u8 buffer[100]; + ASSERT_EQ(-1, reader.read(buffer, 100)); +} + +TEST(SerialDataSource, ReadPackageWithNullBuffer) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared(VALID_PORT, VALID_CONNSTR, + 2000, 2000, logger); + ON_CALL(*mocked_sp, open).WillByDefault(Return(true)); + EXPECT_CALL(*mocked_sp, read).Times(1).WillOnce(Return(-1)); + ON_CALL(*mocked_sp, isValid).WillByDefault(Return(true)); + SbpSerialDataSource reader(logger, mocked_sp); + ASSERT_TRUE(reader.isValid()); + ASSERT_EQ(-1, reader.read(nullptr, 100)); +} + +TEST(SerialDataSource, ReadPackageOK) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared(VALID_PORT, VALID_CONNSTR, + 2000, 2000, logger); + ON_CALL(*mocked_sp, open).WillByDefault(Return(true)); + EXPECT_CALL(*mocked_sp, read).Times(1).WillOnce(Return(100)); + ON_CALL(*mocked_sp, isValid).WillByDefault(Return(true)); + SbpSerialDataSource reader(logger, mocked_sp); + ASSERT_TRUE(reader.isValid()); + u8 buffer[100]; + const int32_t result = reader.read(buffer, 100); + ASSERT_TRUE((result > 0) && (result <= 100)); +} + +// Writing tests +TEST(SerialDataSource, WritePackageWithInvalidSource) { + auto logger = std::make_shared(); + std::shared_ptr serial_port; + SbpSerialDataSource writer(logger, serial_port); + ASSERT_FALSE(writer.isValid()); + u8 buffer[100]; + ASSERT_EQ(-1, writer.write(buffer, 100)); +} + +TEST(SerialDataSource, WritePackageWithNullBuffer) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared(VALID_PORT, VALID_CONNSTR, + 2000, 2000, logger); + ON_CALL(*mocked_sp, open).WillByDefault(Return(true)); + EXPECT_CALL(*mocked_sp, write).Times(1).WillOnce(Return(-1)); + ON_CALL(*mocked_sp, isValid).WillByDefault(Return(true)); + SbpSerialDataSource writer(logger, mocked_sp); + ASSERT_TRUE(writer.isValid()); + ASSERT_EQ(-1, writer.write(nullptr, 100)); +} + +TEST(SerialDataSource, WritePackageOK) { + auto logger = std::make_shared(); + auto mocked_sp = std::make_shared(VALID_PORT, VALID_CONNSTR, + 2000, 2000, logger); + ON_CALL(*mocked_sp, open).WillByDefault(Return(true)); + EXPECT_CALL(*mocked_sp, write).Times(1).WillOnce(Return(100)); + ON_CALL(*mocked_sp, isValid).WillByDefault(Return(true)); + SbpSerialDataSource writer(logger, mocked_sp); + ASSERT_TRUE(writer.isValid()); + u8 buffer[100]; + const int32_t result = writer.write(buffer, 100); + ASSERT_TRUE((result > 0) && (result <= 100)); +}