From d7d28b86dcc4c6a122eebcc399a7d153df2c9a34 Mon Sep 17 00:00:00 2001 From: flipflip Date: Sat, 21 Dec 2024 18:34:35 +0100 Subject: [PATCH] Use Fixposition SDK, add devcontainer config --- .devcontainer/.bash_history.d/.gitkeep | 0 .devcontainer/.bash_logout | 9 + .devcontainer/.bashrc | 173 ++++ .devcontainer/.dummy-dir/.gitkeep | 0 .devcontainer/.dummy-file | 0 .devcontainer/.gitignore | 2 + .devcontainer/.vscode-server/.gitkeep | 0 .devcontainer/helper.sh | 35 + .devcontainer/humble/Dockerfile | 12 + .devcontainer/humble/devcontainer.json | 21 + .devcontainer/jazzy/Dockerfile | 12 + .devcontainer/jazzy/devcontainer.json | 21 + .devcontainer/noetic/Dockerfile | 12 + .devcontainer/noetic/devcontainer.json | 21 + .github/ci.sh | 162 ++++ .github/workflows/build_test_ros.yml | 43 - .github/workflows/build_test_ros2.yml | 43 - .github/workflows/ci.yml | 64 ++ .gitignore | 8 +- .gitmodules | 4 + .pre-commit-config.yaml | 15 + LICENSE | 2 +- README.md | 18 +- create_ros_ws.sh | 192 ++++ fixposition-sdk | 1 + fixposition_driver.code-workspace | 225 +++++ fixposition_driver_lib/CMakeLists.txt | 31 +- .../fixposition_driver.hpp | 247 +++-- .../fixposition_driver_lib/gnss_tf.hpp | 164 ---- .../include/fixposition_driver_lib/helper.hpp | 67 +- .../messages/base_converter.hpp | 142 +-- .../messages/fpa_type.hpp | 61 +- .../messages/fpb_measurements.hpp | 73 -- .../messages/fpb_type.hpp | 109 +-- .../messages/msg_data.hpp | 29 +- .../messages/nmea_type.hpp | 95 +- .../messages/nov_type.hpp | 711 -------------- .../include/fixposition_driver_lib/params.hpp | 69 +- .../include/fixposition_driver_lib/parser.hpp | 43 - .../time_conversions.hpp | 62 +- fixposition_driver_lib/package.xml | 4 + .../src/fixposition_driver.cpp | 889 ++++++++++-------- fixposition_driver_lib/src/gnss_tf.cpp | 197 ---- fixposition_driver_lib/src/helper.cpp | 101 +- .../src/messages/fpa/corrimu.cpp | 32 +- .../src/messages/fpa/eoe.cpp | 30 +- .../src/messages/fpa/gnssant.cpp | 34 +- .../src/messages/fpa/gnsscorr.cpp | 46 +- .../src/messages/fpa/imubias.cpp | 36 +- .../src/messages/fpa/llh.cpp | 32 +- .../src/messages/fpa/odomenu.cpp | 30 +- .../src/messages/fpa/odometry.cpp | 30 +- .../src/messages/fpa/odomsh.cpp | 30 +- .../src/messages/fpa/odomstatus.cpp | 30 +- .../src/messages/fpa/rawimu.cpp | 32 +- .../src/messages/fpa/text.cpp | 32 +- .../src/messages/fpa/tf.cpp | 35 +- .../src/messages/fpa/tp.cpp | 30 +- .../src/messages/nmea/gngsa.cpp | 32 +- .../src/messages/nmea/gpgga.cpp | 40 +- .../src/messages/nmea/gpgll.cpp | 34 +- .../src/messages/nmea/gpgst.cpp | 28 +- .../src/messages/nmea/gphdt.cpp | 28 +- .../src/messages/nmea/gprmc.cpp | 38 +- .../src/messages/nmea/gpvtg.cpp | 28 +- .../src/messages/nmea/gpzda.cpp | 35 +- .../src/messages/nmea/gxgsv.cpp | 41 +- .../src/messages/nmea/nmea.cpp | 47 +- fixposition_driver_lib/src/params.cpp | 30 + fixposition_driver_lib/src/parser.cpp | 139 --- fixposition_driver_ros1/CMakeLists.txt | 27 +- fixposition_driver_ros1/README.md | 2 +- .../fixposition_driver_ros1/data_to_ros1.hpp | 63 +- .../fixposition_driver_node.hpp | 165 ++-- .../fixposition_driver_ros1/params.hpp | 59 +- .../fixposition_driver_ros1/ros_msgs.hpp | 55 +- fixposition_driver_ros1/launch/config.yaml | 65 ++ fixposition_driver_ros1/launch/dev.launch | 6 + .../launch/driver_with_odom_converter.launch | 4 +- .../launch/{tcp.launch => node.launch} | 12 +- .../launch/rosconsole.conf | 5 + .../launch/rosconsole_dev.conf | 3 + fixposition_driver_ros1/launch/serial.launch | 16 - fixposition_driver_ros1/launch/serial.yaml | 17 - fixposition_driver_ros1/launch/tcp.yaml | 17 - fixposition_driver_ros1/msg/fpa/imubias.msg | 2 +- fixposition_driver_ros1/msg/nmea/gphdt.msg | 2 +- fixposition_driver_ros1/msg/nmea/gxgsv.msg | 6 +- fixposition_driver_ros1/package.xml | 2 + fixposition_driver_ros1/src/data_to_ros1.cpp | 94 +- .../src/fixposition_driver_node.cpp | 517 ++++++---- fixposition_driver_ros1/src/params.cpp | 161 ++-- fixposition_driver_ros2/CMakeLists.txt | 6 +- .../fixposition_driver_ros2/data_to_ros2.hpp | 69 +- .../fixposition_driver_node.hpp | 158 ++-- .../fixposition_driver_ros2/params.hpp | 62 +- .../fixposition_driver_ros2/ros2_msgs.hpp | 62 +- fixposition_driver_ros2/launch/serial.yaml | 2 +- fixposition_driver_ros2/launch/tcp.yaml | 2 +- fixposition_driver_ros2/msg/fpa/IMUBIAS.msg | 2 +- fixposition_driver_ros2/msg/nmea/GPHDT.msg | 2 +- fixposition_driver_ros2/msg/nmea/GXGSV.msg | 6 +- fixposition_driver_ros2/package.xml | 2 + fixposition_driver_ros2/src/data_to_ros2.cpp | 103 +- .../src/fixposition_driver_node.cpp | 314 +++++-- fixposition_driver_ros2/src/params.cpp | 37 +- .../odom_converter.hpp | 32 +- .../params.hpp | 30 +- .../ros_msgs.hpp | 21 +- .../src/odom_converter.cpp | 24 +- .../src/odom_converter_node.cpp | 21 +- .../src/params.cpp | 26 +- .../odom_converter_node.hpp | 32 +- .../params.hpp | 32 +- .../ros2_msgs.hpp | 15 +- .../src/odom_converter_node.cpp | 27 +- .../src/params.cpp | 25 +- precommit.sh | 8 + 118 files changed, 3986 insertions(+), 3602 deletions(-) create mode 100644 .devcontainer/.bash_history.d/.gitkeep create mode 100644 .devcontainer/.bash_logout create mode 100644 .devcontainer/.bashrc create mode 100644 .devcontainer/.dummy-dir/.gitkeep create mode 100644 .devcontainer/.dummy-file create mode 100644 .devcontainer/.gitignore create mode 100644 .devcontainer/.vscode-server/.gitkeep create mode 100755 .devcontainer/helper.sh create mode 100644 .devcontainer/humble/Dockerfile create mode 100644 .devcontainer/humble/devcontainer.json create mode 100644 .devcontainer/jazzy/Dockerfile create mode 100644 .devcontainer/jazzy/devcontainer.json create mode 100644 .devcontainer/noetic/Dockerfile create mode 100644 .devcontainer/noetic/devcontainer.json create mode 100755 .github/ci.sh delete mode 100644 .github/workflows/build_test_ros.yml delete mode 100644 .github/workflows/build_test_ros2.yml create mode 100644 .github/workflows/ci.yml create mode 100644 .gitmodules create mode 100644 .pre-commit-config.yaml create mode 100755 create_ros_ws.sh create mode 160000 fixposition-sdk create mode 100644 fixposition_driver.code-workspace delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp delete mode 100644 fixposition_driver_lib/src/gnss_tf.cpp create mode 100644 fixposition_driver_lib/src/params.cpp delete mode 100644 fixposition_driver_lib/src/parser.cpp create mode 100644 fixposition_driver_ros1/launch/config.yaml create mode 100644 fixposition_driver_ros1/launch/dev.launch rename fixposition_driver_ros1/launch/{tcp.launch => node.launch} (51%) create mode 100644 fixposition_driver_ros1/launch/rosconsole_dev.conf delete mode 100644 fixposition_driver_ros1/launch/serial.launch delete mode 100644 fixposition_driver_ros1/launch/serial.yaml delete mode 100644 fixposition_driver_ros1/launch/tcp.yaml create mode 100755 precommit.sh diff --git a/.devcontainer/.bash_history.d/.gitkeep b/.devcontainer/.bash_history.d/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/.devcontainer/.bash_logout b/.devcontainer/.bash_logout new file mode 100644 index 0000000..9265fd4 --- /dev/null +++ b/.devcontainer/.bash_logout @@ -0,0 +1,9 @@ +# ~/.bash_logout: executed by bash(1) when login shell exits. + +# save history +history -a + +# when leaving the console clear the screen to increase privacy +if [ "$SHLVL" = 1 ]; then + [ -x /usr/bin/clear_console ] && /usr/bin/clear_console -q +fi diff --git a/.devcontainer/.bashrc b/.devcontainer/.bashrc new file mode 100644 index 0000000..e602d26 --- /dev/null +++ b/.devcontainer/.bashrc @@ -0,0 +1,173 @@ +# ~/.bashrc: executed by bash(1) for non-login shells. +# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc) +# for examples + +# If not running interactively, don't do anything +case $- in + *i*) ;; + *) return;; +esac + +# don't put duplicate lines or lines starting with space in the history. +# See bash(1) for more options +HISTCONTROL="ignoredups:erasedups:ignorespace" + +# append to the history file, don't overwrite it +shopt -s histappend + +# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) +HISTSIZE=10000 +HISTFILESIZE=10000000 +HISTFILE=/home/fpsdk/.bash_history.d/history + +# Disable history ! expansion +set +H + +export RSYNC_RSH=ssh +export PAGER="less -R -i -x 4 -+C" +export PERLDOC_PAGER="less -R -i -x 4 -+C" +export LESSCHARSET=UTF-8 +export EDITOR='vim' +export GIT_EDITOR=vim + +# nicely coloured man pages +export LESS_TERMCAP_mb=$'\E[01;31m' +export LESS_TERMCAP_md=$'\E[01;31m' +export LESS_TERMCAP_me=$'\E[0m' +export LESS_TERMCAP_se=$'\E[0m' +export LESS_TERMCAP_so=$'\E[01;44;33m' +export LESS_TERMCAP_ue=$'\E[0m' +export LESS_TERMCAP_us=$'\E[01;32m' + +export LESS="FRSX" + +# N.B. -R must be first or lesspipe.sh will be confused +# warning: -R not compatible with +F (follow mode)! +alias less='less -R -i -x 4 -S' + +# Enable core dumps (does it?) +ulimit -c 1048576 + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# If set, the pattern "**" used in a pathname expansion context will +# match all files and zero or more directories and subdirectories. +#shopt -s globstar + +# make less more friendly for non-text input files, see lesspipe(1) +[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, unless we know we "want" color) +case "$TERM" in + xterm-color|*-256color) color_prompt=yes;; +esac + +# uncomment for a colored prompt, if the terminal has the capability; turned +# off by default to not distract the user: the focus in a terminal window +# should be on the output of commands, not on the prompt +#force_color_prompt=yes + +if [ -n "$force_color_prompt" ]; then + if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then + # We have color support; assume it's compliant with Ecma-48 + # (ISO/IEC-6429). (Lack of such support is extremely rare, and such + # a case would tend to support setf rather than setaf.) + color_prompt=yes + else + color_prompt= + fi +fi + +if [ "$color_prompt" = yes ]; then + PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' +else + PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' +fi +unset color_prompt force_color_prompt + +# If this is an xterm set the title to user@host:dir +case "$TERM" in +xterm*|rxvt*) + PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" + ;; +*) + ;; +esac + +# enable color support of ls and also add handy aliases +if [ -x /usr/bin/dircolors ]; then + test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" + alias ls='ls --color=auto' + #alias dir='dir --color=auto' + #alias vdir='vdir --color=auto' + + alias grep='grep --color=auto' + alias fgrep='fgrep --color=auto' + alias egrep='egrep --color=auto' +fi + +# colored GCC warnings and errors +#export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01' + +# some more ls aliases +alias ll='ls -alF' +alias la='ls -A' +alias l='ls -CF' +alias rm='rm -i' +alias mv='mv -i' + +# Add an "alert" alias for long running commands. Use like so: +# sleep 10; alert +alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"' + +# Alias definitions. +# You may want to put all your additions into a separate file like +# ~/.bash_aliases, instead of adding them here directly. +# See /usr/share/doc/bash-doc/examples in the bash-doc package. + +if [ -f ~/.bash_aliases ]; then + . ~/.bash_aliases +fi + +# enable programmable completion features (you don't need to enable +# this, if it's already enabled in /etc/bash.bashrc and /etc/profile +# sources /etc/bash.bashrc). +if ! shopt -oq posix; then + if [ -f /usr/share/bash-completion/bash_completion ]; then + . /usr/share/bash-completion/bash_completion + elif [ -f /etc/bash_completion ]; then + . /etc/bash_completion + fi +fi + +if [ -f ~/.bash-git-prompt/gitprompt.sh ]; then + # https://github.com/magicmonty/bash-git-prompt + GIT_PROMPT_THEME=fpsdk + source ~/.bash-git-prompt/gitprompt.sh +fi + +# Source ROS environment +if [ -n "${ROS_DISTRO}" ]; then + source /opt/ros/${ROS_DISTRO}/setup.bash + export ROSCONSOLE_STDOUT_LINE_BUFFERED=1 + export ROSCONSOLE_FORMAT='${severity} ${time:%Y-%m-%d %H:%M:%S.%f} ${logger} - ${message}' + # export RCUTILS_LOGGING_BUFFERED_STREAM=1 + # export RCUTILS_LOGGING_USE_STDOUT=1 + export RCUTILS_COLORIZED_OUTPUT=1 + case "${ROS_DISTRO}" in + jazzy|rolling) + export RCUTILS_CONSOLE_OUTPUT_FORMAT="{severity} {date_time_with_ms} {name} - {message}" + ;; + *) + export RCUTILS_CONSOLE_OUTPUT_FORMAT="{severity} {time} {name} - {message}" + ;; + esac + +fi diff --git a/.devcontainer/.dummy-dir/.gitkeep b/.devcontainer/.dummy-dir/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/.devcontainer/.dummy-file b/.devcontainer/.dummy-file new file mode 100644 index 0000000..e69de29 diff --git a/.devcontainer/.gitignore b/.devcontainer/.gitignore new file mode 100644 index 0000000..a13a478 --- /dev/null +++ b/.devcontainer/.gitignore @@ -0,0 +1,2 @@ +/.bash_history.d +/.vscode-server \ No newline at end of file diff --git a/.devcontainer/.vscode-server/.gitkeep b/.devcontainer/.vscode-server/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/.devcontainer/helper.sh b/.devcontainer/helper.sh new file mode 100755 index 0000000..3719d3b --- /dev/null +++ b/.devcontainer/helper.sh @@ -0,0 +1,35 @@ +#!/bin/bash +set -eEu -o pipefail + +command=$1 +shift + +case ${command} in + onCreateCommand) + containerWorkspaceFolder=$1 + # ls -la ${containerWorkspaceFolder} + # id + case ${ROS_DISTRO} in + noetic) + if [ ! -d ${containerWorkspaceFolder}/ros1_ws ]; then + ${containerWorkspaceFolder}/create_ros_ws.sh -d ros1_ws + fi + ;; + humble|jazzy) + if [ ! -d ${containerWorkspaceFolder}/ros2_ws ]; then + ${containerWorkspaceFolder}/create_ros_ws.sh -d ros2_ws + fi + ;; + *) + echo "Don't know how to create a ROS workspace for ROS_DISTRO=${ROS_DISTRO}" + exit 1 + ;; + esac + ;; + *) + echo "bad command: ${command}" + exit 1 + ;; +esac + +exit 0 diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile new file mode 100644 index 0000000..feb4f37 --- /dev/null +++ b/.devcontainer/humble/Dockerfile @@ -0,0 +1,12 @@ +FROM ghcr.io/fixposition/fixposition-sdk:humble-dev + +# RUN <" + echo + echo " -r 1|2 -- ROS version, default from ROS_DISTRO environment variable" + echo " -d -- Setup for debug build (default: release)" + echo " -- Path to workspace root (absolute, or relative to ${SCRIPTDIR})" + echo + exit 0 + ;; + *) + error "Illegal option '$opt'!" 1>&2 + exit 1 + ;; + esac + done + if [ ${OPTIND} -gt 1 ]; then + shift $(expr $OPTIND - 1) + fi + + # Path can be relative to script directory (= fixposition_dirver repo root) or absolute + if [ -z "${1:-}" ]; then + exit_fail "Need a path" + fi + local abspath=$1 + local relpath= + if [ "${abspath:0:1}" != "/" ]; then + relpath=${abspath} + abspath=${SCRIPTDIR}/${relpath} + fi + debug "abspath=${abspath} relpath=${relpath}" + if [ -f ${abspath} -o -d ${abspath} ]; then + exit_fail "Path ${abspath} already exists" + fi + + # Get and check for ROS version + if [ ${rosver} -eq 0 ]; then + case "${ROS_DISTRO:-}" in + noetic) + rosver=1 + ;; + humble|jazzy) + rosver=2 + ;; + esac + fi + debug "rosver=${rosver}" + if ! [ ${rosver} -eq 1 -o ${rosver} -eq 2 ]; then + exit_fail "Need a (valid) ROS version" + fi + + local res=0 + + # Create workspace + if [ ${dev} -gt 0 ]; then + notice "Creating debug ROS${rosver} workspace in ${abspath}" + else + notice "Creating release ROS${rosver} workspace in ${abspath}" + fi + mkdir -p ${abspath}/src + + # Path for symlink + local srcpath= + if [ -n "${relpath}" ]; then + srcpath=$(realpath -s --relative-to=${abspath}/src ${SCRIPTDIR}) + else + srcpath=${SCRIPTDIR} + fi + debug "srcpath=${srcpath}" + + # Symlink source packages + ln -s ${srcpath}/fixposition-sdk/fpsdk_common ${abspath}/src + ln -s ${srcpath}/fixposition-sdk/fpsdk_apps ${abspath}/src + ln -s ${srcpath}/fixposition_driver_lib ${abspath}/src + ln -s ${srcpath}/rtcm_msgs ${abspath}/src + if [ ${rosver} -eq 1 ]; then + ln -s ${srcpath}/fixposition-sdk/fpsdk_ros1 ${abspath}/src + ln -s ${srcpath}/fixposition-sdk/ros1_fpsdk_demo ${abspath}/src + ln -s ${srcpath}/fixposition_driver_ros1 ${abspath}/src + ln -s ${srcpath}/fixposition_odometry_converter_ros1 ${abspath}/src + else + ln -s ${srcpath}/fixposition-sdk/fpsdk_ros2 ${abspath}/src + ln -s ${srcpath}/fixposition-sdk/ros2_fpsdk_demo ${abspath}/src + ln -s ${srcpath}/fixposition_driver_ros2 ${abspath}/src + ln -s ${srcpath}/fixposition_odometry_converter_ros2 ${abspath}/src + fi + + # Initialise workspace + cd ${abspath} + if [ ${rosver} -eq 1 ]; then + catkin init + if [ ${dev} -gt 0 ]; then + catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug + else + catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Release + fi + else + if [ ${dev} -gt 0 ]; then + echo 'build:' > ${abspath}/colcon_defaults.yaml + echo ' event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml + echo ' cmake-args: [ "-DCMAKE_BUILD_TYPE=Debug", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" ]' >> ${abspath}/colcon_defaults.yaml + else + echo 'build:' > ${abspath}/colcon_defaults.yaml + echo '# event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml + echo ' cmake-args: [ "-DCMAKE_BUILD_TYPE=Release", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" ]' >> ${abspath}/colcon_defaults.yaml + fi + fi + + # Happy? + debug "res=${res}" + if [ ${res} -eq 0 ]; then + info "Splendid!" + exit 0 + else + error "Ouch" + exit 1 + fi + +} + +function exit_fail +{ + error "$@" + echo "Try '$0 -h' for help." 1>&2 + exit 1 +} + +function notice +{ + echo -e "\033[1;37m$@\033[m" 1>&2 +} + +function info +{ + echo -e "\033[0m$@\033[m" 1>&2 +} + +function warning +{ + echo -e "\033[1;33mWarning: $@\033[m" 1>&2 +} + +function error +{ + echo -e "\033[1;31mError: $@\033[m" 1>&2 +} + +function debug +{ + if [ ${DEBUG} -gt 0 ]; then + echo -e "\033[0;36mDebug: $@\033[m" 1>&2 + fi +} + +function panic +{ + local res=$? + echo -e "\033[1;35mPanic at ${BASH_SOURCE[1]}:${BASH_LINENO[0]}! ${BASH_COMMAND} (res=$res)\033[m" 1>&2 + exit $res +} + +main "$@" +exit 99 diff --git a/fixposition-sdk b/fixposition-sdk new file mode 160000 index 0000000..593d6b3 --- /dev/null +++ b/fixposition-sdk @@ -0,0 +1 @@ +Subproject commit 593d6b3120bf6efe5bf26600b6c496d8d6f62f1a diff --git a/fixposition_driver.code-workspace b/fixposition_driver.code-workspace new file mode 100644 index 0000000..d7e02c0 --- /dev/null +++ b/fixposition_driver.code-workspace @@ -0,0 +1,225 @@ +{ + "folders": [ + { "path": ".", "name": "fixposition_driver" } + ], + + "settings": { + "editor.detectIndentation": false, + "editor.tabSize": 4, // Note: only active if editor.- + "editor.insertSpaces": true, // detectIndentation is false! + //"editor.trimAutoWhitespace": true, + "files.trimTrailingWhitespace": true, + + // Leave big brother in the dark, mainly settings from @tag:usesOnlineServices + "telemetry.enableTelemetry": false, + "gitlens.telemetry.enabled": false, + "redhat.telemetry.enabled": false, + "telemetry.telemetrylevel": "off", + "workbench.enableExperiments": false, + "workbench.settings.enableNaturalLanguageSearch": false, + "npm.fetchOnlinePackageInfo": false, + + // Exclude files and folders from explorer (it automatically excludes .git) + "files.exclude": { + ".ackrc": true, + ".gitattributes": true, + ".vscode-cache": true, + "**/*~": true, + ".vstags": true, + "core": true, + ".devcontainer/.vscode-server/**": true + }, + // Exclude things from search (this automatically uses files.exclude + what you add here) + "search.exclude": { + "output": true, + "tmp": true, + "**/ros1_ws/**": true, + "**/ros2_ws/**": true + }, + // Exclude some things from the files watcher + "files.watcherExclude": { + "**/.git/objects/**": true, + "**/.git/refs/**": true, + "**/.git/logs/**": true, + "**/.git/subtree-cache/**": true, + "**/.git/worktrees/**": true, + "**/.vscode-*/**": true, + "**/ros1_ws/**": true, + "**/ros2_ws/**": true + }, + + // Microsoft C++ extension (ms-vscode.cpptools) + "C_Cpp.intelliSenseCachePath": "${workspaceFolder}/.vscode-cache", + // defaults for .vscode/c_cpp_properties.json (which makes that file optional) + // see .vscode/c_cpp_properties.json-example for details + // https://code.visualstudio.com/docs/cpp/customize-default-settings-cpp, + // https://code.visualstudio.com/docs/cpp/c-cpp-properties-schema-reference) + "C_Cpp.default.includePath": [ + "${workspaceFolder}/fpsdk_common/**", "${workspaceFolder}/fpsdk_ros1/**", "${workspaceFolder}/fpsdk_ros2/**", "${workspaceFolder}/fpsdk_apps/**", + "/opt/ros/noetic/include", "/opt/ros/humble/include", "/opt/ros/jazzy/include" ], + "C_Cpp.default.defines": [ "FP_USE_ROS1" ], + //"C_Cpp.default.compileCommands": "", + //"C_Cpp.default.forcedIncludes": [ ], + "C_Cpp.default.intelliSenseMode": "gcc-x64", + "C_Cpp.default.compilerPath": "/usr/bin/gcc", + "C_Cpp.default.cStandard": "gnu99", + "C_Cpp.default.cppStandard": "gnu++17", + "C_Cpp.default.browse.path": [ "${workspaceFolder}/**" ], + //"C_Cpp.default.configurationProvider": "b2.catkin_tools", + "C_Cpp.default.browse.databaseFilename": "${workspaceFolder}/.vscode-cache/browse.db", + //"C_Cpp.default.compileCommands": "${workspaceFolder}/build/Debug/compile_commands.json", + "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": false, + //"C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 120 }", + "C_Cpp.files.exclude": { + ".devcontainer/.vscode-server/**": true + }, + "editor.formatOnSave": false, + + "cmake.configureOnOpen": false, + "[shellscript]": { + "files.eol": "\n", + "files.trimTrailingWhitespace": false + }, + "[yaml]": { + "editor.indentSize": 4 + }, + "files.associations": { + "memory": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp", + "*.ipp": "cpp", + "ranges": "cpp" + }, + "workbench.remoteIndicator.showExtensionRecommendations": true, + "yaml.extension.recommendations": false, + "catkin_tools.cppStandard": "c++17", + "catkin_tools.defaultRosWorkspaces": [ + "/home/fpsdk/fixposition_driver/ros1_ws", + "/home/fpsdk/fixposition_driver/ros2_ws", + "ros1_ws", + "ros2_ws" + ] + }, + "extensions": { + "recommendations": [ + "ms-vscode.cpptools", + "twxs.cmake" + //,"betwo.b2-catkin-tools" + ], + "unwantedRecommendations": [ + ] + }, + + // Build and test tasks. Users can add more tasks in .vscode/tasks.json. + "tasks": { + // https://code.visualstudio.com/docs/editor/tasks + // https://code.visualstudio.com/docs/editor/tasks-appendix + "version": "2.0.0", + + // Defaults for all tasks + "options": { + "cwd": "${workspaceFolder}", + "shell": { "executable": "/bin/bash", "args": [ "-c" ] }, + "setupCommands": [ + { "text": "-enable-pretty-printing", "description": "enable pretty printing", "ignoreFailures": true }, + { "text": "handle SIGPIPE nostop noprint pass", "description": "ignore SIGPIPE", "ignoreFailures": true } + ] + }, + "problemMatcher": "$gcc", + "type": "shell", + + // Tasks definitions + "tasks": [ + // ----------------------------------------------------------------------------------------- + { + "label": "debug workspaceFolder path", + "group": "build", + "command": "echo ${workspaceFolder} && readlink -f ${workspaceFolder}" + } + ], + + // Input variables + "inputs": [ + // https://code.visualstudio.com/docs/editor/variables-reference#_input-variables + ] + }, + // Launch (debugging) + "launch": { + // https://code.visualstudio.com/docs/cpp/launch-json-reference + "configurations": [ + ], + "compounds": [] + } +} diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 88fb4c6..ac400dc 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -1,23 +1,37 @@ # GENERAL ============================================================================================================== -cmake_minimum_required(VERSION 3.5) + +cmake_minimum_required(VERSION 3.16) + project(fixposition_driver_lib VERSION 5.0.0 LANGUAGES CXX) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic\ - -Wshadow -Wunused-parameter -Wformat -Wpointer-arith") + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\ + -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual") set(CMAKE_CXX_FLAGS_RELEASE "-O3") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() +if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + add_compile_definitions(NDEBUG) +endif() + # DEPENDENCIES ========================================================================================================= + find_package(Boost 1.65.0 REQUIRED) find_package(Eigen3 REQUIRED) +find_package(fpsdk_common REQUIRED) include_directories(include ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} + ${fpsdk_common_INCLUDE_DIR} ) + # BUILD EXECUTABLE ===================================================================================================== + add_library( ${PROJECT_NAME} SHARED src/messages/fpa/corrimu.cpp @@ -46,13 +60,14 @@ add_library( src/messages/nmea/nmea.cpp src/fixposition_driver.cpp src/helper.cpp - src/parser.cpp - src/gnss_tf.cpp + src/params.cpp ) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} pthread) +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${fpsdk_common_LIBRARIES} pthread) + # INSTALL ============================================================================================================== + list(APPEND PACKAGE_LIBRARIES ${PROJECT_NAME}) install( diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp index 9d1bc80..e60f63e 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp @@ -1,156 +1,243 @@ /** - * @file - * @brief Declaration of FixpositionDriver class - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of FixpositionDriver class */ -#ifndef __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ -#define __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ +#ifndef __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER_HPP__ +#define __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER_HPP__ -/* SYSTEM / STL */ -#include +/* LIBC/STL */ +#include +#include +#include +#include /* EXTERNAL */ -#include -#include -#include -#include -#include -#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +/* PACKAGE */ +#include "fixposition_driver_lib/helper.hpp" +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" +#include "fixposition_driver_lib/params.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ -class FixpositionDriver { +/** + * @brief Fixposition driver + * + * This handles the connection to a Fixposition Vision-RTK 2 sensor. It receives and decodes the received messages, + * which can be used by consumers by observing ("subscribing to") the driver. On various events, such as receiving + * a new message or completing and "epoch", the observers are notified by the driver. The driver also handles sending + * data to the sensor, namely GNSS correction data (typically, RTCM3 messages) or measurements, such as wheelspeed. + */ +class FixpositionDriver : private boost::noncopyable { public: /** - * @brief Construct a new FixpositionDriver object + * @brief Constructor * + * @params[in] params Parameters */ - FixpositionDriver(const FixpositionDriverParams& params); + FixpositionDriver(const SensorParams& params); /** - * @brief Destroy the Fixposition Driver object, close all open connections - * + * @brief Destructor */ - ~FixpositionDriver(); + virtual ~FixpositionDriver(); + + // + // ----- Methods for starting and stopping the driver ----- /** - * @brief Run in Loop the Read Convert and Publish cycle + * @brief Connect to sensor and start the worker thread * + * @returns true on success, false otherwise */ - virtual bool RunOnce(); + bool StartDriver(); - protected: /** - * @brief - * - * @param[in] sensors_meas map wheelspeed of sensors, each sensor containing speed values and their validity flag + * @brief Stop the worker thread and disconnect from sensor */ - virtual void WsCallback(const std::unordered_map>>& sensors_meas); + void StopDriver(); + + // + // ----- Methods for observing messages received from the sensor ----- + + //! Observer function for FP_A messages + using FpaObserver = std::function; + + //! Observer function for NMEA messages + using NmeaObserver = std::function; + + //! Observer function for NOV_B messages + using NovbObserver = std::function; /** - * @brief + * @brief Add observer for FP_A message * - * @param[in] rtcm_msg string with ASCII data for RTK correction + * @param[in] message_name The message name to observe, e.g. "FP_A-ODOMETRY" + * @param[in] observer The function to call to process the message */ - virtual void RtcmCallback(const void *rtcm_msg, const size_t msg_size); + void AddFpaObserver(const std::string& message_name, FpaObserver observer); /** - * @brief + * @brief Add observer for NMEA message * - * @param[in] meas_vec measurements from one specific wheelspeed sensor, with their validity flag - * @param[in] meas_loc location from the specific wheelspeed sensor - * @param[out] meas_fpb fpb measurement to be filled from the vector - * @return true if the measurement was successfully filled, false otherwise + * @param[in] formatter The formatter for the messages to observe, e.g. "GGA" + * @param[in] observer The function to call to process the message */ - virtual bool FillWsSensorMeas(const std::vector>& meas_vec, - const FpbMeasurementsMeasLoc meas_loc, FpbMeasurementsMeas& meas_fpb); + void AddNmeaObserver(const std::string& formatter, NmeaObserver observer); /** - * @brief Converts the measurement location from string to the enum values + * @brief Add observer for NOV_B message * - * @param[in] meas_loc user input location in string format - * @return FpbMeasurementsMeasLoc converted measurement location + * @param[in] message_name The message name to observe, e.g. "NOV_B-BESTGNSSPOS" + * @param[in] observer The function to call to process the message */ - virtual FpbMeasurementsMeasLoc WsMeasStringToLoc(const std::string& meas_loc); + void AddNovbObserver(const std::string& message_name, NovbObserver observer); /** - * @brief Convert the Nmea like string using correct converter - * - * @param[in] msg NMEA like string to be converted. $HEADER,,,,,,,*CHECKSUM + * @brief Remove all observers for FP_A messages */ - virtual void NmeaConvertAndPublish(const std::string& msg); + void RemoveFpaObservers(); /** - * @brief Convert the buffer after identified as Nov msg + * @brief Remove all observers for NMEA messages + */ + void RemoveNmeaObservers(); + + /** + * @brief Remove all observers for NOV_B messages + */ + void RemoveNovbObservers(); + + protected: + SensorParams params_; + + // ---- Methods to handle the sensor connection ---- + + /** + * @brief Connect to sensor * - * @param[in] msg ptr to the start of the msg + * @returns true on success, false otherwise */ - virtual void NovConvertAndPublish(const uint8_t* msg); + virtual bool Connect(); /** - * @brief Initialize convertes based on config + * @brief Disconnect from sensor * - * @return true - * @return false + * @returns true on success, false otherwise */ - virtual bool InitializeConverters(); + virtual void Disconnect(); /** - * @brief Read data and publish to ros if possible + * @brief Check connection * - * @return true data read success or no data - * @return false connection problems, restart the connection + * @returns true if connection is up, false otherwise */ - virtual bool ReadAndPublish(); + virtual bool IsConnected() const; /** - * @brief Connect the defined TCP or Serial socket + * @brief Read data * - * @return true success - * @return false cannot connect + * @param[out] buf Buffer to store read data + * @param[in] size Size of buffer + * @param[in] timeout Timeout [ms] to wait for data + * + * @returns the size read */ - virtual bool Connect(); + virtual std::size_t Read(uint8_t* buf, const std::size_t size, const int timeout); /** - * @brief Initialize TCP connection + * @brief Write data * - * @return true success - * @return false fail + * @param[in] bu f Buffer to the data + * @param[in] size Size of the data + * + * @returns true if the data was sent, false otherwise */ - virtual bool CreateTCPSocket(); + virtual bool Write(const uint8_t* buf, const std::size_t size); + + // + // + // + // + // + // + // TODO... + // /** - * @brief Initialize Serial connection + * @brief * - * @return true success - * @return false fail + * @param[in] sensors_meas map wheelspeed of sensors, each sensor containing speed values and their validity flag */ - virtual bool CreateSerialConnection(); + void WsCallback(const std::unordered_map>>& sensors_meas); - FixpositionDriverParams params_; + /** + * @brief + * + * @param[in] rtcm_msg string with ASCII data for RTK correction + */ + void RtcmCallback(const uint8_t* rtcm_msg, const size_t msg_size); std::unordered_map> a_converters_; //!< ascii converters corresponding to the input formats - using BestgnssposObserver = std::function; + using BestgnssposObserver = std::function; std::vector bestgnsspos_obs_; //!< observers for bestgnsspos // TODO: Add more NOV types - int client_fd_ = -1; //!< TCP or Serial file descriptor - int connection_status_ = -1; - struct termios options_save_; - uint8_t readBuf[8192]; - int buf_size = 0; + private: + // Sensor connection + bool ConnectTcp(const std::string& ip, const int port); //!< Connect() for TCP + void DisconnectTcp(); //!< Disconnect() for TCP + bool ConnectSerial(const std::string& dev, const int baud); //!< Connect() for serial + void DisconnectSerial(); //!< Disconnect() for serial + bool serial_ = false; //!< Serial (true) or TCP client (false) + int sensor_fd_ = -1; //!< TCP or serial file descriptor + std::string sensor_name_; //!< Name of the sensor, for debugging + std::array poll_fds_; //!< poll() config + struct termios options_save_; //!< Saved serial port parameters + + // Worker thread + fpsdk::common::parser::Parser parser_; //!< Protocol parser for incoming messages + fpsdk::common::thread::Thread worker_; //!< Worker thread handle + void Worker(void* arg); //!< Worker thread + void ProcessFpa(const fpsdk::common::parser::ParserMsg& msg); //!< Process FP_A message + void ProcessNmea(const fpsdk::common::parser::ParserMsg& msg); //!< Process NMEA message + void ProcessNovb(const fpsdk::common::parser::ParserMsg& msg); //!< Process NOV_B message + + std::unordered_map> fpa_observers_; + std::unordered_map> nmea_observers_; + std::unordered_map> novb_observers_; + + // TODO: replace these + void NmeaConvertAndPublish(const fpsdk::common::parser::ParserMsg& msg); + void NovConvertAndPublish(const fpsdk::common::parser::ParserMsg& msg); }; + +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ +#endif //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp deleted file mode 100644 index 4a7fe13..0000000 --- a/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp +++ /dev/null @@ -1,164 +0,0 @@ -/** - * @file - * @brief Helper functions - * - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * \endverbatim - * - */ - -#ifndef __FIXPOSITION_DRIVER_LIB_GNSS_TF__ -#define __FIXPOSITION_DRIVER_LIB_GNSS_TF__ - -/* PACKAGE */ -#include - -namespace fixposition { - -/** - * @brief Calculate the rotation matrix from ECEF to - * ENU for a given reference latitude/longitude. - * - * @param[in] lat Reference latitude [rad] - * @param[in] lon Reference longitude [rad] - * @return Eigen::Matrix3d Rotation matrix from ECEF -> ENU - */ -Eigen::Matrix3d RotEnuEcef(double lat, double lon); - -/** - * @brief Calculate the rotation matrix from ECEF to - * ENU for a given reference origin. - * - * @param[in] in_pos Reference position in ECEF [m] - * @return Eigen::Matrix3d Rotation matrix ECEF -> ENU - */ -Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d &ecef); - -/** - * @brief Returns rotation matrix between NED and ENU - * @details | 0, 1, 0 | - * | 1, 0, 0 | - * | 0, 0,-1 | - * - * @return Eigen::Matrix3d - */ -Eigen::Matrix3d RotNedEnu(); - -/** - * @brief Calculate the rotation matrix from ECEF to - * NED for a given reference latitude/longitude. - * - * @param[in] lat Reference latitude [rad] - * @param[in] lon Reference longitude [rad] - * @return Eigen::Matrix3d Rotation matrix from ECEF -> NED - */ -Eigen::Matrix3d RotNedEcef(double lat, double lon); - -/** - * @brief Calculate the rotation matrix from ECEF to - * NED for a given reference origin. - * - * @param[in] in_pos Reference position in ECEF [m] - * @return Eigen::Matrix3d Rotation matrix ECEF -> NED - */ -Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d &ecef); - -/** - * @brief Transform ECEF coordinate to ENU with specified ENU-origin - * - * @param[in] xyz ECEF position to be transsformed [m] - * @param[in] wgs84llh_ref ENU-origin in Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) - * @return Eigen::Vector3d Position in ENU coordinates - */ -Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref); - -/** - * @brief Transform ENU coordinate to ECEF with specified ENU-origin - * - * @param[in] enu ENU position to be transsformed [m] - * @param[in] wgs84llh_ref ENU-origin in Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) - * @return Eigen::Vector3d - */ -Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref); - -Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref); -Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref); - -/** - * @brief Convert Geodetic coordinates (latitude, longitude, height) to - * ECEF (x, y, z). - * - * @param[in] wgs84llh Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) - * @return Eigen::Vector3d ECEF coordinates [m] - */ -Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh); - -/** - * @brief Convert ECEF (x, y, z) coordinates to Lat Lon Height coordinates - * (latitude, longitude, altitude). - * - * @param[in] ecef ECEF coordinates [m] - * @return Eigen::Vector3d Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) - */ -Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef); - -/** - * @brief Given Pose in ECEF, calculate Yaw-Pitch-Roll in ENU - * @details Yaw will be -Pi/2 when X is pointing North, because ENU starts with East - * - * @param ecef_p 3D Position Vector in ECEF - * @param ecef_r 3x3 Rotation matrix representing rotation Body --> ECEF - * @return Eigen::Vector3d Yaw-Pitch-Roll in ENU - * (Yaw will be -Pi/2 when X is pointing North, because ENU starts with - - */ -Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r); - -/** - * @brief - * - * @param ecef_p 3D Position Vector in ECEF - * @param ecef_r 3x3 Rotation matrix representing rotation Body --> ECEF - * @return Eigen::Vector3d Yaw-Pitch-Roll in NED - */ -Eigen::Vector3d EcefPoseToNedEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r); - -/** - * @brief Vector4 quaternion to intrinsic Euler Angles in ZYX (yaw,pitch,roll) - * - * @param[in] quat Eigen::Quaterniond in w,i,j,k - * @return Eigen::Vector3d intrinsic euler angle in ZYX (Yaw/Pitch/Roll) order - */ -inline Eigen::Vector3d QuatToEul(const Eigen::Quaterniond &q) { - auto qw = q.w(); - auto qx = q.x(); - auto qy = q.y(); - auto qz = q.z(); - auto eul0 = atan2(2 * (qx * qy + qw * qz), qw * qw + qx * qx - qy * qy - qz * qz); - auto eul1 = asin(-2.0 * (qx * qz - qw * qy)); - auto eul2 = atan2(2 * (qy * qz + qw * qx), qw * qw - qx * qx - qy * qy + qz * qz); - - Eigen::Vector3d eul; - eul << eul0, eul1, eul2; - - return eul; -} - -/** - * @brief Rotation Matrix to intrinsic Euler Angles in ZYX (Yaw-Pitch-Roll) order in radian - * - * @param rot Eigen::Matrix3d - * @return Eigen::Vector3d intrinsic euler angle in ZYX (Yaw-Pitch-Roll) order in radian - */ -inline Eigen::Vector3d RotToEul(const Eigen::Matrix3d &rot) { - const Eigen::Quaterniond quat(rot); - return QuatToEul(quat); -} - -} // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_GNSS_TF__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp index 839673e..b309fbc 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp @@ -1,62 +1,63 @@ /** - * @file - * @brief Helper functions - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Helper functions */ -#ifndef __FIXPOSITION_DRIVER_LIB_HELPER__ -#define __FIXPOSITION_DRIVER_LIB_HELPER__ +#ifndef __FIXPOSITION_DRIVER_LIB_HELPER_HPP__ +#define __FIXPOSITION_DRIVER_LIB_HELPER_HPP__ -/* SYSTEM / STL */ +/* LIBC/STL */ #include #include /* EXTERNAL */ -#include -#include -#include -#include +#include +#include + +/* PACKAGE */ +#include "messages/msg_data.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /** * @brief * - * @param[in] msg - * @param[in] delim - * @param[out] tokens + * @param[in] header + * @param[in] bestgnsspos + * @param[out] navsatfix */ -void SplitMessage(std::vector& tokens, const std::string& msg, const std::string& delim); +void BestGnssPosToNavSatFix(const fpsdk::common::parser::novb::NovbLongHeader& header, + const fpsdk::common::parser::novb::NovbBestgnsspos& bestgnsspos, NavSatFixData& navsatfix); /** * @brief * - * @param[in] header - * @param[in] bestgnsspos - * @param[out] navsatfix + * @param[in] meas_vec measurements from one specific wheelspeed sensor, with their validity flag + * @param[in] meas_loc location from the specific wheelspeed sensor + * @param[out] meas_fpb fpb measurement to be filled from the vector + * @return true if the measurement was successfully filled, false otherwise */ -void BestGnssPosToNavSatFix(const Oem7MessageHeaderMem* const header, const BESTGNSSPOSMem* const bestgnsspos, - NavSatFixData& navsatfix); +bool FillWsSensorMeas(const std::vector>& meas_vec, + const fpsdk::common::parser::fpb::FpbMeasurementsMeasLoc meas_loc, + fpsdk::common::parser::fpb::FpbMeasurementsMeas& meas_fpb); /** - * @brief Convert NOV to other data structs + * @brief Converts the measurement location from string to the enum values * - * @tparam T_nov NOV struct type - * @tparam T_data Data struct type - * @param[in] header - * @param[in] nov - * @param[out] data + * @param[in] meas_loc user input location in string format + * @return FpbMeasurementsMeasLoc converted measurement location */ -template -void NovToData(const Oem7MessageHeaderMem* const header, const T_nov* const nov, T_data& data); +fpsdk::common::parser::fpb::FpbMeasurementsMeasLoc WsMeasStringToLoc(const std::string& meas_loc); +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_HELPER__ +#endif // __FIXPOSITION_DRIVER_LIB_HELPER_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp index 8c88650..9bb96a9 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp @@ -1,80 +1,83 @@ /** - * @file - * @brief Base Converter to define the interfaces - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Base Converter to define the interfaces */ -#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__ -#define __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__ +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER_HPP__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER_HPP__ -/* SYSTEM / STL */ +/* LIBC/STL */ #include #include +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_driver_lib/time_conversions.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ class BaseAsciiConverter { - public: - BaseAsciiConverter() = default; - ~BaseAsciiConverter() = default; - - /** - * @brief Virtual interface to convert the split tokens into ros messages - * - * @param[in] tokens vector of strings split by comma - */ - virtual void ConvertTokens(const std::vector& tokens) = 0; + public: + BaseAsciiConverter() = default; + ~BaseAsciiConverter() = default; + + /** + * @brief Virtual interface to convert the split tokens into ros messages + * + * @param[in] tokens vector of strings split by comma + */ + virtual void ConvertTokens(const std::vector& tokens) = 0; }; -template class NmeaConverter : public BaseAsciiConverter { - public: - using Observer = std::function; - - /** - * @brief Construct a new Fixposition Msg Converter object - * - */ - - NmeaConverter() : BaseAsciiConverter() {} - - ~NmeaConverter() = default; - - /** - * @brief Comma Delimited FP_A message, convert to Data structs and, if available, call observers - * - * @param[in] state state message as string - * @return FP_A message struct - */ - void ConvertTokens(const std::vector& tokens) { - msg_.ConvertFromTokens(tokens); - - // process all observers - for (auto& ob : obs_) { - ob(msg_); - } - }; - - /** - * @brief Add Observer to call at the end of ConvertTokens() - * - * @param[in] ob - */ - void AddObserver(Observer ob) { obs_.push_back(ob); } - - private: - MessageType msg_; - std::vector obs_; +template +class NmeaConverter : public BaseAsciiConverter { + public: + using Observer = std::function; + + /** + * @brief Construct a new Fixposition Msg Converter object + * + */ + + NmeaConverter() : BaseAsciiConverter() {} + + ~NmeaConverter() = default; + + /** + * @brief Comma Delimited FP_A message, convert to Data structs and, if available, call observers + * + * @param[in] state state message as string + * @return FP_A message struct + */ + void ConvertTokens(const std::vector& tokens) { + msg_.ConvertFromTokens(tokens); + + // process all observers + for (auto& ob : obs_) { + ob(msg_); + } + }; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(Observer ob) { obs_.push_back(ob); } + + private: + MessageType msg_; + std::vector obs_; }; //=================================================== @@ -183,20 +186,20 @@ inline times::GpsTime ConvertGpsTime(const std::string& gps_wno, const std::stri * @param[in] xz * @return Eigen::Matrix the 3x3 matrix */ -inline Eigen::Matrix BuildCovMat3D(const double xx, const double yy, const double zz, - const double xy, const double yz, const double xz) { +inline Eigen::Matrix BuildCovMat3D(const double xx, const double yy, const double zz, const double xy, + const double yz, const double xz) { Eigen::Matrix cov; cov.setZero(); // Diagonals - cov(0, 0) = xx; // 0 - cov(1, 1) = yy; // 4 - cov(2, 2) = zz; // 8 + cov(0, 0) = xx; // 0 + cov(1, 1) = yy; // 4 + cov(2, 2) = zz; // 8 // Rest of values - cov(1, 0) = cov(0, 1) = xy; // 1 = 3 - cov(2, 1) = cov(1, 2) = yz; // 2 = 6 - cov(2, 0) = cov(0, 2) = xz; // 5 = 7 + cov(1, 0) = cov(0, 1) = xy; // 1 = 3 + cov(2, 1) = cov(1, 2) = yz; // 2 = 6 + cov(2, 0) = cov(0, 2) = xz; // 5 = 7 return cov; } @@ -264,5 +267,6 @@ constexpr inline T RadToDeg(T radians) { return radians * 57.295779513082320876798154814105; } +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__ +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp index 59609f3..0d3a8d3 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp @@ -1,25 +1,29 @@ /** - * @file - * @brief Declaration of FP_A type messages - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of FP_A type messages */ -#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__ -#define __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__ +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE_HPP__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/msg_data.hpp" +#include "fixposition_driver_lib/time_conversions.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ // ------------ FP_A-ODOMETRY ------------ @@ -35,7 +39,7 @@ struct FP_ODOMETRY { int gnss2_status; int wheelspeed_status; std::string version; - + // Message structure const std::string frame_id = "FP_ECEF"; const std::string child_frame_id = "FP_POI"; @@ -80,7 +84,7 @@ struct FP_ODOMETRY { struct FP_ODOMENU { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields OdometryData odom; Eigen::Vector3d acceleration; @@ -132,7 +136,7 @@ struct FP_ODOMENU { struct FP_ODOMSH { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields OdometryData odom; Eigen::Vector3d acceleration; @@ -184,7 +188,7 @@ struct FP_ODOMSH { struct FP_ODOMSTATUS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields times::GpsTime stamp; int init_status; @@ -268,9 +272,9 @@ struct FP_ODOMSTATUS { struct FP_LLH { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields - times::GpsTime stamp; + times::GpsTime stamp; Eigen::Vector3d llh; Eigen::Matrix cov; @@ -300,7 +304,7 @@ struct FP_LLH { struct FP_TF { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields TfData tf; bool valid_tf; @@ -335,7 +339,7 @@ struct FP_TF { struct FP_RAWIMU { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields ImuData imu; @@ -365,7 +369,7 @@ struct FP_RAWIMU { struct FP_IMUBIAS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields times::GpsTime stamp; std::string frame_id; @@ -416,7 +420,7 @@ struct FP_IMUBIAS { struct FP_CORRIMU { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields ImuData imu; @@ -446,7 +450,7 @@ struct FP_CORRIMU { struct FP_GNSSANT { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields times::GpsTime stamp; std::string gnss1_state; @@ -488,7 +492,7 @@ struct FP_GNSSANT { struct FP_GNSSCORR { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields times::GpsTime stamp; int gnss1_fix; @@ -551,7 +555,7 @@ struct FP_GNSSCORR { struct FP_TEXT { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields std::string level; std::string text; @@ -578,7 +582,7 @@ struct FP_TEXT { struct FP_EOE { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields times::GpsTime stamp; std::string epoch; @@ -605,7 +609,7 @@ struct FP_EOE { struct FP_TP { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + // Message fields std::string tp_name; std::string timebase; @@ -640,5 +644,6 @@ struct FP_TP { } }; +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__ +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp deleted file mode 100644 index f7edd36..0000000 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp +++ /dev/null @@ -1,73 +0,0 @@ -/** - * @file - * @brief Declaration of FpbMeasurementsMeas message struct - * - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * \endverbatim - * - */ - -#ifndef __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__ -#define __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__ - -/* EXTERNAL */ -#include - -/* PACKAGE */ -#include - -namespace fixposition { - -struct FpbMeasurementsHeader { - uint8_t version; //!< Message version (= FpbMeasurementsVersion for the current version of this message) - uint8_t num_meas; //!< Number of measurements in the body (1..FP_B_MEASUREMENTS_MAX_NUM_MEAS) - uint8_t reserved0[6]; //!< Reserved for future use. Set to 0. -}; -static_assert(sizeof(FpbMeasurementsHeader) == 8, ""); -const int FP_B_MEASUREMENTS_HEAD_SIZE = 8; - -enum FpbMeasurementsMeasType { - MEASTYPE_UNSPECIFIED = 0, //!< Message type unspecified, measurement will not be processed - MEASTYPE_VELOCITY = 1, //!< Velocity message, measurement will be interpreted as a wheel speed -}; - -enum FpbMeasurementsMeasLoc { - MEASLOC_UNSPECIFIED = 0, //!< Location of the sensor unspecified, measurement will not be processed - MEASLOC_RC = 1, //!< Measurement coming from the RC sensor - MEASLOC_FR = 2, //!< Measurement coming from the FR sensor - MEASLOC_FL = 3, //!< Measurement coming from the FL sensor - MEASLOC_RR = 4, //!< Measurement coming from the RR sensor - MEASLOC_RL = 5, //!< Measurement coming from the RL sensor -}; - -enum FpbMeasurementsTimestampType { - TIME_UNSPECIFIED = 0, //!< Timestamp type unspecified, measurement will not be processed - TIME_TOA = 1, //!< Use time of arrival of the measurement -}; - -struct FpbMeasurementsMeas { - int32_t meas_x; //!< Measurement x - int32_t meas_y; //!< Measurement y - int32_t meas_z; //!< Measurement z - uint8_t meas_x_valid; //!< Validity of measurement x (1 = meas_x contains valid data, 0 = data invalid or n/a) - uint8_t meas_y_valid; //!< Validity of measurement y (1 = meas_x contains valid data, 0 = data invalid or n/a) - uint8_t meas_z_valid; //!< Validity of measurement z (1 = meas_x contains valid data, 0 = data invalid or n/a) - uint8_t meas_type; //!< See FpbMeasurementsMeasType - uint8_t meas_loc; //!< See FpbMeasurementsMeasLoc - uint8_t reserved1[4]; //!< Reserved for future use. Set to 0. - uint8_t timestamp_type; //!< See FpbMeasurementsTimestampType - uint16_t gps_wno; //!< GPS week number [-] - uint32_t gps_tow; //!< GPS time of week [ms] or monotonic time [-] -}; - -static_assert(sizeof(FpbMeasurementsMeas) == 28, ""); -const int FP_B_MEASUREMENTS_BODY_SIZE = 28; - -} // namespace fixposition - -#endif // __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp index 02417cc..2333cb0 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp @@ -1,70 +1,59 @@ /** - * @file - * @brief Declaration of Fpb message framework - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of Fpb message framework */ -#ifndef __FIXPOSITION_DRIVER_LIB_FPB__ -#define __FIXPOSITION_DRIVER_LIB_FPB__ +#ifndef __FIXPOSITION_DRIVER_LIB_FPB_HPP__ +#define __FIXPOSITION_DRIVER_LIB_FPB_HPP__ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ -/* SYSTEM / STL */ -#include +/* PACKAGE */ namespace fixposition { +/* ****************************************************************************************************************** */ static uint32_t const k_crc32_fpb[] = { - 0x00000000, 0x32c00699, 0x65800d32, 0x57400bab, 0xcb001a64, 0xf9c01cfd, - 0xae801756, 0x9c4011cf, 0xa4c03251, 0x960034c8, 0xc1403f63, 0xf38039fa, - 0x6fc02835, 0x5d002eac, 0x0a402507, 0x3880239e, 0x7b40623b, 0x498064a2, - 0x1ec06f09, 0x2c006990, 0xb040785f, 0x82807ec6, 0xd5c0756d, 0xe70073f4, - 0xdf80506a, 0xed4056f3, 0xba005d58, 0x88c05bc1, 0x14804a0e, 0x26404c97, - 0x7100473c, 0x43c041a5, 0xf680c476, 0xc440c2ef, 0x9300c944, 0xa1c0cfdd, - 0x3d80de12, 0x0f40d88b, 0x5800d320, 0x6ac0d5b9, 0x5240f627, 0x6080f0be, - 0x37c0fb15, 0x0500fd8c, 0x9940ec43, 0xab80eada, 0xfcc0e171, 0xce00e7e8, - 0x8dc0a64d, 0xbf00a0d4, 0xe840ab7f, 0xda80ade6, 0x46c0bc29, 0x7400bab0, - 0x2340b11b, 0x1180b782, 0x2900941c, 0x1bc09285, 0x4c80992e, 0x7e409fb7, - 0xe2008e78, 0xd0c088e1, 0x8780834a, 0xb54085d3, 0xdfc18e75, 0xed0188ec, - 0xba418347, 0x888185de, 0x14c19411, 0x26019288, 0x71419923, 0x43819fba, - 0x7b01bc24, 0x49c1babd, 0x1e81b116, 0x2c41b78f, 0xb001a640, 0x82c1a0d9, - 0xd581ab72, 0xe741adeb, 0xa481ec4e, 0x9641ead7, 0xc101e17c, 0xf3c1e7e5, - 0x6f81f62a, 0x5d41f0b3, 0x0a01fb18, 0x38c1fd81, 0x0041de1f, 0x3281d886, - 0x65c1d32d, 0x5701d5b4, 0xcb41c47b, 0xf981c2e2, 0xaec1c949, 0x9c01cfd0, - 0x29414a03, 0x1b814c9a, 0x4cc14731, 0x7e0141a8, 0xe2415067, 0xd08156fe, - 0x87c15d55, 0xb5015bcc, 0x8d817852, 0xbf417ecb, 0xe8017560, 0xdac173f9, - 0x46816236, 0x744164af, 0x23016f04, 0x11c1699d, 0x52012838, 0x60c12ea1, - 0x3781250a, 0x05412393, 0x9901325c, 0xabc134c5, 0xfc813f6e, 0xce4139f7, - 0xf6c11a69, 0xc4011cf0, 0x9341175b, 0xa18111c2, 0x3dc1000d, 0x0f010694, - 0x58410d3f, 0x6a810ba6, 0x8d431a73, 0xbf831cea, 0xe8c31741, 0xda0311d8, - 0x46430017, 0x7483068e, 0x23c30d25, 0x11030bbc, 0x29832822, 0x1b432ebb, - 0x4c032510, 0x7ec32389, 0xe2833246, 0xd04334df, 0x87033f74, 0xb5c339ed, - 0xf6037848, 0xc4c37ed1, 0x9383757a, 0xa14373e3, 0x3d03622c, 0x0fc364b5, - 0x58836f1e, 0x6a436987, 0x52c34a19, 0x60034c80, 0x3743472b, 0x058341b2, - 0x99c3507d, 0xab0356e4, 0xfc435d4f, 0xce835bd6, 0x7bc3de05, 0x4903d89c, - 0x1e43d337, 0x2c83d5ae, 0xb0c3c461, 0x8203c2f8, 0xd543c953, 0xe783cfca, - 0xdf03ec54, 0xedc3eacd, 0xba83e166, 0x8843e7ff, 0x1403f630, 0x26c3f0a9, - 0x7183fb02, 0x4343fd9b, 0x0083bc3e, 0x3243baa7, 0x6503b10c, 0x57c3b795, - 0xcb83a65a, 0xf943a0c3, 0xae03ab68, 0x9cc3adf1, 0xa4438e6f, 0x968388f6, - 0xc1c3835d, 0xf30385c4, 0x6f43940b, 0x5d839292, 0x0ac39939, 0x38039fa0, - 0x52829406, 0x6042929f, 0x37029934, 0x05c29fad, 0x99828e62, 0xab4288fb, - 0xfc028350, 0xcec285c9, 0xf642a657, 0xc482a0ce, 0x93c2ab65, 0xa102adfc, - 0x3d42bc33, 0x0f82baaa, 0x58c2b101, 0x6a02b798, 0x29c2f63d, 0x1b02f0a4, - 0x4c42fb0f, 0x7e82fd96, 0xe2c2ec59, 0xd002eac0, 0x8742e16b, 0xb582e7f2, - 0x8d02c46c, 0xbfc2c2f5, 0xe882c95e, 0xda42cfc7, 0x4602de08, 0x74c2d891, - 0x2382d33a, 0x1142d5a3, 0xa4025070, 0x96c256e9, 0xc1825d42, 0xf3425bdb, - 0x6f024a14, 0x5dc24c8d, 0x0a824726, 0x384241bf, 0x00c26221, 0x320264b8, - 0x65426f13, 0x5782698a, 0xcbc27845, 0xf9027edc, 0xae427577, 0x9c8273ee, - 0xdf42324b, 0xed8234d2, 0xbac23f79, 0x880239e0, 0x1442282f, 0x26822eb6, - 0x71c2251d, 0x43022384, 0x7b82001a, 0x49420683, 0x1e020d28, 0x2cc20bb1, - 0xb0821a7e, 0x82421ce7, 0xd502174c, 0xe7c211d5 -}; + 0x00000000, 0x32c00699, 0x65800d32, 0x57400bab, 0xcb001a64, 0xf9c01cfd, 0xae801756, 0x9c4011cf, 0xa4c03251, + 0x960034c8, 0xc1403f63, 0xf38039fa, 0x6fc02835, 0x5d002eac, 0x0a402507, 0x3880239e, 0x7b40623b, 0x498064a2, + 0x1ec06f09, 0x2c006990, 0xb040785f, 0x82807ec6, 0xd5c0756d, 0xe70073f4, 0xdf80506a, 0xed4056f3, 0xba005d58, + 0x88c05bc1, 0x14804a0e, 0x26404c97, 0x7100473c, 0x43c041a5, 0xf680c476, 0xc440c2ef, 0x9300c944, 0xa1c0cfdd, + 0x3d80de12, 0x0f40d88b, 0x5800d320, 0x6ac0d5b9, 0x5240f627, 0x6080f0be, 0x37c0fb15, 0x0500fd8c, 0x9940ec43, + 0xab80eada, 0xfcc0e171, 0xce00e7e8, 0x8dc0a64d, 0xbf00a0d4, 0xe840ab7f, 0xda80ade6, 0x46c0bc29, 0x7400bab0, + 0x2340b11b, 0x1180b782, 0x2900941c, 0x1bc09285, 0x4c80992e, 0x7e409fb7, 0xe2008e78, 0xd0c088e1, 0x8780834a, + 0xb54085d3, 0xdfc18e75, 0xed0188ec, 0xba418347, 0x888185de, 0x14c19411, 0x26019288, 0x71419923, 0x43819fba, + 0x7b01bc24, 0x49c1babd, 0x1e81b116, 0x2c41b78f, 0xb001a640, 0x82c1a0d9, 0xd581ab72, 0xe741adeb, 0xa481ec4e, + 0x9641ead7, 0xc101e17c, 0xf3c1e7e5, 0x6f81f62a, 0x5d41f0b3, 0x0a01fb18, 0x38c1fd81, 0x0041de1f, 0x3281d886, + 0x65c1d32d, 0x5701d5b4, 0xcb41c47b, 0xf981c2e2, 0xaec1c949, 0x9c01cfd0, 0x29414a03, 0x1b814c9a, 0x4cc14731, + 0x7e0141a8, 0xe2415067, 0xd08156fe, 0x87c15d55, 0xb5015bcc, 0x8d817852, 0xbf417ecb, 0xe8017560, 0xdac173f9, + 0x46816236, 0x744164af, 0x23016f04, 0x11c1699d, 0x52012838, 0x60c12ea1, 0x3781250a, 0x05412393, 0x9901325c, + 0xabc134c5, 0xfc813f6e, 0xce4139f7, 0xf6c11a69, 0xc4011cf0, 0x9341175b, 0xa18111c2, 0x3dc1000d, 0x0f010694, + 0x58410d3f, 0x6a810ba6, 0x8d431a73, 0xbf831cea, 0xe8c31741, 0xda0311d8, 0x46430017, 0x7483068e, 0x23c30d25, + 0x11030bbc, 0x29832822, 0x1b432ebb, 0x4c032510, 0x7ec32389, 0xe2833246, 0xd04334df, 0x87033f74, 0xb5c339ed, + 0xf6037848, 0xc4c37ed1, 0x9383757a, 0xa14373e3, 0x3d03622c, 0x0fc364b5, 0x58836f1e, 0x6a436987, 0x52c34a19, + 0x60034c80, 0x3743472b, 0x058341b2, 0x99c3507d, 0xab0356e4, 0xfc435d4f, 0xce835bd6, 0x7bc3de05, 0x4903d89c, + 0x1e43d337, 0x2c83d5ae, 0xb0c3c461, 0x8203c2f8, 0xd543c953, 0xe783cfca, 0xdf03ec54, 0xedc3eacd, 0xba83e166, + 0x8843e7ff, 0x1403f630, 0x26c3f0a9, 0x7183fb02, 0x4343fd9b, 0x0083bc3e, 0x3243baa7, 0x6503b10c, 0x57c3b795, + 0xcb83a65a, 0xf943a0c3, 0xae03ab68, 0x9cc3adf1, 0xa4438e6f, 0x968388f6, 0xc1c3835d, 0xf30385c4, 0x6f43940b, + 0x5d839292, 0x0ac39939, 0x38039fa0, 0x52829406, 0x6042929f, 0x37029934, 0x05c29fad, 0x99828e62, 0xab4288fb, + 0xfc028350, 0xcec285c9, 0xf642a657, 0xc482a0ce, 0x93c2ab65, 0xa102adfc, 0x3d42bc33, 0x0f82baaa, 0x58c2b101, + 0x6a02b798, 0x29c2f63d, 0x1b02f0a4, 0x4c42fb0f, 0x7e82fd96, 0xe2c2ec59, 0xd002eac0, 0x8742e16b, 0xb582e7f2, + 0x8d02c46c, 0xbfc2c2f5, 0xe882c95e, 0xda42cfc7, 0x4602de08, 0x74c2d891, 0x2382d33a, 0x1142d5a3, 0xa4025070, + 0x96c256e9, 0xc1825d42, 0xf3425bdb, 0x6f024a14, 0x5dc24c8d, 0x0a824726, 0x384241bf, 0x00c26221, 0x320264b8, + 0x65426f13, 0x5782698a, 0xcbc27845, 0xf9027edc, 0xae427577, 0x9c8273ee, 0xdf42324b, 0xed8234d2, 0xbac23f79, + 0x880239e0, 0x1442282f, 0x26822eb6, 0x71c2251d, 0x43022384, 0x7b82001a, 0x49420683, 0x1e020d28, 0x2cc20bb1, + 0xb0821a7e, 0x82421ce7, 0xd502174c, 0xe7c211d5}; inline uint32_t Crc32fpb(const uint8_t* data, const int size) { uint32_t crc = 0; @@ -77,8 +66,8 @@ inline uint32_t Crc32fpb(const uint8_t* data, const int size) { } struct FpbHeader { - uint8_t sync1; //!< FP_B frame sync char 1 (0x66) - uint8_t sync2; //!< FP_B frame sync char 2 (0x21) + uint8_t sync1; //!< FP_B frame sync char 1 (0x66) + uint8_t sync2; //!< FP_B frame sync char 2 (0x21) uint16_t msg_id; //!< ID of the FP_B message (2001 for measurements message) uint16_t payload_size; //!< Size of the payload uint16_t time; //!< Time of the message. Unused, set to 0. @@ -87,6 +76,6 @@ static_assert(sizeof(FpbHeader) == 8, ""); const int FP_B_HEAD_SIZE = 8; //!< Size of FP_B frame header const int FP_B_CRC_SIZE = 4; //!< Size of FP_B crc +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif // __FIXPOSITION_DRIVER_LIB_FPB__ +#endif // __FIXPOSITION_DRIVER_LIB_FPB_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp index 3eb58ab..1902700 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp @@ -1,27 +1,29 @@ /** - * @file - * @brief Declaration of Data types - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of Data types */ -#ifndef __FIXPOSITION_DRIVER_LIB_MSG_DATA__ -#define __FIXPOSITION_DRIVER_LIB_MSG_DATA__ +#ifndef __FIXPOSITION_DRIVER_LIB_MSG_DATA_HPP__ +#define __FIXPOSITION_DRIVER_LIB_MSG_DATA_HPP__ -/* SYSTEM / STL */ +/* LIBC/STL */ #include +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_driver_lib/time_conversions.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ enum class FusionStatus : int { NOT_STARTED = 0, @@ -186,5 +188,6 @@ struct GprmcData { GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0), valid(false) {} }; +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif //__FIXPOSITION_DRIVER_LIB_MSG_DATA__ +#endif //__FIXPOSITION_DRIVER_LIB_MSG_DATA_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp index cb57b5a..e99a508 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp @@ -1,25 +1,29 @@ /** - * @file - * @brief Declaration of NMEA type messages - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of NMEA type messages */ -#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE__ -#define __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE__ +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE_HPP__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/msg_data.hpp" +#include "fixposition_driver_lib/time_conversions.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ // ------------ NMEA-GP-GGA ------------ @@ -228,10 +232,10 @@ struct GX_GSV { unsigned int kSize_ = 4; // Maximum size: 4 + num_sats * 4 SignalType string2enum(const std::string& name) { - if (name == "GP") return SignalType::GPS; // GPS - if (name == "GA") return SignalType::Galileo; // Galileo - if (name == "GB") return SignalType::BeiDou; // BeiDou - if (name == "GL") return SignalType::GLONASS; // GLONASS + if (name == "GP") return SignalType::GPS; // GPS + if (name == "GA") return SignalType::Galileo; // Galileo + if (name == "GB") return SignalType::BeiDou; // BeiDou + if (name == "GL") return SignalType::GLONASS; // GLONASS return SignalType::Invalid; } @@ -452,34 +456,34 @@ struct NmeaMessage { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Message fields - std::string gpgga_time_str; // GP_GGA - std::string gpzda_time_str; // GP_ZDA - std::string time_str; // GP_ZDA (alt. GP_GGA, GP_GST, GP_RMC) - std::string date_str; // GP_ZDA (alt. GP_RMC) - times::GpsTime stamp; // GP_ZDA - Eigen::Vector3d llh; // GP_GGA (alt. LL only for GP_GLL, GP_RMC) - uint8_t quality; // GP_GGA (alt. GP_RMC, GP_VTG, or limited GP_GLL, GP_GSA) - uint8_t num_sv; // GP_GGA - std::vector ids; // GN_GSA - float hdop_receiver; // GP_GGA - float pdop; // GN_GSA - float hdop; // GN_GSA (alt. GP_GGA) - float vdop; // GN_GSA - float rms_range; // GP_GST - float std_major; // GP_GST - float std_minor; // GP_GST - float angle_major; // GP_GST - float std_lat; // GP_GST (alt. GP_GGA) - float std_lon; // GP_GST (alt. GP_GGA) - float std_alt; // GP_GST (alt. GP_GGA) - Eigen::Matrix cov; // GP_GST (alt. GP_GGA) - uint8_t cov_type; // GP_GST (alt. GP_GGA) - float heading; // GP_HDT - float speed; // GP_RMC (alt. GP_VTG) - float course; // GP_RMC (alt. GP_VTG) - float diff_age; // GP_GGA - std::string diff_sta; // GP_GGA - std::unordered_map> gnss_signals; // GX_GSV + std::string gpgga_time_str; // GP_GGA + std::string gpzda_time_str; // GP_ZDA + std::string time_str; // GP_ZDA (alt. GP_GGA, GP_GST, GP_RMC) + std::string date_str; // GP_ZDA (alt. GP_RMC) + times::GpsTime stamp; // GP_ZDA + Eigen::Vector3d llh; // GP_GGA (alt. LL only for GP_GLL, GP_RMC) + uint8_t quality; // GP_GGA (alt. GP_RMC, GP_VTG, or limited GP_GLL, GP_GSA) + uint8_t num_sv; // GP_GGA + std::vector ids; // GN_GSA + float hdop_receiver; // GP_GGA + float pdop; // GN_GSA + float hdop; // GN_GSA (alt. GP_GGA) + float vdop; // GN_GSA + float rms_range; // GP_GST + float std_major; // GP_GST + float std_minor; // GP_GST + float angle_major; // GP_GST + float std_lat; // GP_GST (alt. GP_GGA) + float std_lon; // GP_GST (alt. GP_GGA) + float std_alt; // GP_GST (alt. GP_GGA) + Eigen::Matrix cov; // GP_GST (alt. GP_GGA) + uint8_t cov_type; // GP_GST (alt. GP_GGA) + float heading; // GP_HDT + float speed; // GP_RMC (alt. GP_VTG) + float course; // GP_RMC (alt. GP_VTG) + float diff_age; // GP_GGA + std::string diff_sta; // GP_GGA + std::unordered_map> gnss_signals; // GX_GSV /** * @brief Check if GNSS epoch is complete @@ -563,5 +567,6 @@ struct NmeaMessage { void AddNmeaEpoch(const GP_ZDA& msg); }; +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE__ +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp deleted file mode 100644 index 7433c48..0000000 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp +++ /dev/null @@ -1,711 +0,0 @@ -/*! - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG - * / /\ \ All right reserved - * /__/ \__\ - * - * Portions copyright NovAtel: - * - * Copyright (c) 2020 NovAtel Inc. - * - * 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. - * \endverbatim - * - * @file - * @brief NovAtel types and utilities - */ - -/* ****************************************************************************************************************** */ -#ifndef __FIXPOSITION_DRIVER_LIB_NOV_TYPE__ -#define __FIXPOSITION_DRIVER_LIB_NOV_TYPE__ - -// Fundamental types -#include -#include -#include - -/* ****************************************************************************************************************** */ - -namespace fixposition { - -/** - * @brief CRC32 calculation - * - * @param[in] data - * @param[in] size - * @return uint32_t - */ -inline uint32_t nov_crc32(const uint8_t* data, const int size) { - uint32_t crc = 0; - for (int i = 0; i < size; i++) { - crc ^= data[i]; - for (int j = 0; j < 8; j++) { - if (crc & 1) { - crc = (crc >> 1) ^ 0xedb88320u; - } else { - crc >>= 1; - } - } - } - return crc; -} - -static constexpr uint8_t SYNC_CHAR_1 = 0xaa; -static constexpr uint8_t SYNC_CHAR_2 = 0x44; -static constexpr uint8_t SYNC_CHAR_3_LONG = 0x12; -static constexpr uint8_t SYNC_CHAR_3_SHORT = 0x13; - -/* ****************************************************************************************************************** */ -/** - * @name Novatel oem7 message flag definitions - * @{ - */ -enum class MessageId : uint16_t { - GPGGA = 218, - RAWIMU = 268, - INSPVA = 507, - HEADING2 = 1335, - BESTPOS = 42, - BERSTXYZ = 241, - BESTGNSSPOS = 1429, - INSPVAX = 1465, - BESTXYZ = 241, - BESTUTM = 726, - BESTVEL = 99, - CORRIMUS = 2264, - IMURATECORRIMUS = 1362, - INSCONFIG = 1945, - INSPVAS = 508, - INSSTDEV = 2051, - PSRDOP2 = 1163, - RXSTATUS = 93, - TIME = 101 -}; - -/** - * @brief See https://docs.novatel.com/OEM7/Content/SPAN_Logs/INSATTX.htm#ExtendedSolutionStatus - * Bitwise Masks for all 32 bits. For usage please refer to INSPVAX converter. - */ -enum class ExtendedSolutionStatusIns : uint32_t { - POSITION_UPDATE = 0x00000001, - PHASE_UPDATE = 0x00000002, - ZERO_VEOLICYT_UPDATE = 0x00000004, - WHEEL_SENSOR_UPDATE = 0x00000008, - HEADING_UPDATE = 0x00000010, - EXTERNAL_POSITION_UPDATE = 0x00000020, - INS_SOLUTION_CONVERGENCE = 0x00000040, - DOPPLER_UPDATE = 0x00000080, - PSEUDORANGE_UPDATE = 0x00000100, - VELOCITY_UPDATE = 0x00000200, - // RESERVED = 0x00000400, - DR_UPDATE = 0x000000800, - PHASE_WINDUP_UPDATE = 0x00001000, - COURSE_OVER_GROUND_UPDATE = 0x00002000, - EXTERNAL_VELOCITY_UPDATE = 0x00004000, - EXTERNAL_ATTITUDE_UPDATE = 0x00008000, - - EXTERNAL_HEADING_UPDATE = 0x00010000, - EXTERNAL_HEIGHT_UPDATE = 0x00020000, - // RESERVED = 0x00040000, - // RESERVED = 0x00080000, - - // RESERVED = 0x00100000, - // RESERVED = 0x00200000, - // RESERVED = 0x00400000, - // RESERVED = 0x00800000, - - TURN_ON_BIAS_ESTIMATED = 0x01000000, - ALIGNMENT_DIRECTION_VERIFIED = 0x02000000, - ALIGNMENT_INDICATION_1 = 0x04000000, - ALIGNMENT_INDICATION_2 = 0x08000000, - - ALIGNMENT_INDICATION_3 = 0x10000000, - NVM_SEED_INDICATION_1 = 0x20000000, - NVM_SEED_INDICATION_2 = 0x40000000, - NVM_SEED_INDICATION_3 = 0x80000000, -}; - -/** - * @brief See https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#GPS_GLONASSSignalUsedMask - */ -enum class GpsGlonassSignalUsed : uint8_t { - GPS_L1 = 0x01, - GPS_L2 = 0x02, - GPS_L5 = 0x04, - GLONASS_L1 = 0x10, - GLONASS_L2 = 0x20, - GLONASS_L5 = 0x40, -}; - -/** - * @brief See https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#Galileo_BeiDouSignalUsedMask - */ -enum class GalileoBeidouSignalUsed : uint8_t { - GALILEO_L1 = 0x01, - GALILEO_L2 = 0x02, - GALILEO_L5 = 0x04, - BEIDOU_L1 = 0x10, - BEIDOU_L2 = 0x20, - BEIDOU_L5 = 0x40, -}; - -/** - * @brief See https://docs.novatel.com/OEM7/Content/SPAN_Logs/INSATT.htm#InertialSolutionStatus - */ -enum class InertialSolutionStatus : uint32_t { - INS_INACTIVE = 0, - INS_ALIGNING = 1, - INS_HIGH_VARIANCE = 2, - INS_SOLUTION_GOOD = 3, - INS_SOLUTION_FREE = 6, - INS_ALIGNMENT_COMPLETE = 7, - DETERMINING_ORIENTATION = 8, - WAITING_INITIAL_POS = 9, - WAITING_AZIMUTH = 10, - INITIALIZING_BIASES = 11, - MOTION_DETECT = 12 -}; - -/** - * @brief See https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#Position_VelocityType - */ -enum class PositionOrVelocityType : uint32_t { - NONE = 0, - FIXEDPOS = 1, - FIXEDHEIGHT = 2, - DOPPLER_VELOCITY = 8, - SINGLE = 16, - PSRDIFF = 17, - WAAS = 18, - PROPAGATED = 19, - L1_FLOAT = 32, - NARROW_FLOAT = 34, - L1_INT = 48, - WIDE_INT = 49, - NARROW_INT = 50, - RTK_DIRECT_INS = 51, - INS_SBAS = 52, - INS_PSRSP = 53, - INS_PSRDIFF = 54, - INS_RTKFLOAT = 55, - INS_RTKFIXED = 56, - PPP_CONVERGING = 68, - PPP = 69, - OPERATIONAL = 70, - WARNING = 71, - OUT_OF_BOUNDS = 72, - INS_PPP_CONVERGING = 73, - INS_PPP = 74, - PPP_BASIC_CONVERGING = 77, - PPP_BASIC = 78, - INS_PPP_BASIC_CONVERGING = 79, - INS_PPP_BASIC = 80 -}; - -/** - * @brief See https://docs.novatel.com/OEM7/Content/Messages/GPS_Reference_Time_Statu.htm#Table_GPSReferenceTimeStatus - */ -enum class GpsReferenceTimeStatus : uint8_t { - UNKNOWN = 20, - APPROXIMATE = 60, - COARSEADJUSTING = 80, - COARSE = 100, - COARSESTEERING = 120, - FREEWHEELING = 130, - FINEADJUSTING = 140, - FINE = 160, - FINEBACKUPSTEERING = 170, - FINESTEERING = 180, // I think we mainly use this one - SATTIME = 200 -}; - -/** - * @brief Stringify time status - * @param[in] time_status The time status - * @returns a unique string for the time status - */ -const char* GpsReferenceTimeStatusStr(const GpsReferenceTimeStatus time_status); - -enum class MessageTypeSource : uint8_t { // clang-format off - PRIMARY = 0b00000000, //!< Primary antenna - SECONDARY = 0b00000001, //!< Secondary antenna - _MASK = 0b00011111, //!< Mask for the source part of the message_type field -}; // clang-format on - -/** - * @brief See - * https://docs.novatel.com/OEM7/Content/Messages/Binary.htm?tocpath=Commands%20%2526%20Logs%7CMessages%7C_____3#Table_BinaryMessageHeaderStructure - */ -enum class MessageTypeFormat : uint8_t { // clang-format off - BINARY = 0b00000000, //!< Binary - ASCII = 0b00100000, //!< ASCII - AASCII_NMEA = 0b01000000, //!< Abbreviated ASCII, NMEA - RESERVED = 0b01100000, //!< Reserved - _MASK = 0b01100000, //!< Mask for the format part of the message_type field -}; // clang-format on - -enum class MessageTypeResponse : uint8_t { // clang-format off - ORIGINAL = 0b00000000, - RESPONSE = 0b10000000, - _MASK = 0b10000000, //!< Mask for the response part of the message_type field -}; // clang-format on - -enum class PortAddress : uint8_t { // clang-format off - NO_PORTS = 0x00, //!< No ports specified - ALL_PORTS = 0x80, //!< All virtual ports for all ports - THISPORT = 0xc0, //!< Current COM port - // there are many more... -}; // clang-format on - -// https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#SolutionStatus -enum class SolStat : uint32_t { // clang-format off - - SOL_COMPUTED = 0, //!< Solution computed - INSUFFICIENT_OBS = 1, //!< Insufficient observations - NO_CONVERGENCE = 2, //!< No convergence - SINGULARITY = 3, //!< Singularity at parameters matrix - COV_TRACE = 4, //!< Covariance trace exceeds maximum (trace > 1000 m) - TEST_DIST = 5, //!< Test distance exceeded (maximum of 3 rejections if distance >10 km) - COLD_START = 6, //!< Not yet converged from cold start - V_H_LIMIT = 7, //!< Height or velocity limits exceeded (in accordance with export licensing restrictions) - VARIANCE = 8, //!< Variance exceeds limits - RESIDUALS = 9, //!< Residuals are too large - // there are some more... -}; // clang-format on - -enum class DatumId : uint32_t { - WGS84 = 61, - USER = 63, -}; - -// https://docs.novatel.com/OEM7/Content/Logs/HEADING2.htm#SolutionSource -enum class SolutionSource : uint8_t { // clang-format off - PRIMARY = 0b00000100, //!< Primary antenna - SECONDARY = 0b00001000, //!< Secondary antenna - _MASK = 0b00001100, //!< Mask -}; // clang-format on - -// https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#ExtendedSolutionStatus -enum class ExtendedSolutionStatusGnss : uint8_t { // clang-format off - SOL_VERIFIED = 0b00000001, //!< Solution verified - // There are more... -}; // clang-format on - -/** - * @} - */ -/* ****************************************************************************************************************** */ -/** - * @name Binary format definitions for Oem7 messages - * - * All Oem7 messages are 4-byte aligned, allowing simple casting into structs - * @{ - */ - -typedef uint32_t oem7_enum_t; -typedef uint32_t oem7_bool_t; -typedef uint8_t oem7_hex_t; -typedef char oem7_char_t; - -static_assert(sizeof(oem7_char_t) == 1, ""); -static_assert(sizeof(double) == 8, ""); -static_assert(sizeof(float) == 4, ""); - -struct __attribute__((packed)) Oem7MessageCommonHeaderMem { - char sync1; - char sync2; - char sync3; - - uint8_t message_length; - uint16_t message_id; -}; - -struct __attribute__((packed)) Oem7MessageHeaderMem { - uint8_t sync1; - uint8_t sync2; - uint8_t sync3; - - uint8_t header_length; - uint16_t message_id; - uint8_t message_type; - uint8_t port_address; - uint16_t message_length; - uint16_t sequence; - uint8_t idle_time; - uint8_t time_status; - uint16_t gps_week; - int32_t gps_milliseconds; - uint32_t receiver_status; - uint16_t reserved; - uint16_t receiver_version; -}; - -struct __attribute__((packed)) Oem7MessgeShortHeaderMem { - uint8_t sync1; - uint8_t sync2; - uint8_t sync3; - - uint8_t message_length; - uint16_t message_id; - uint16_t gps_week; - int32_t gps_milliseconds; -}; - -struct __attribute__((packed)) BESTPOSMem { - oem7_enum_t sol_stat; - oem7_enum_t pos_type; - double lat; - double lon; - double hgt; - float undulation; - oem7_enum_t datum_id; - float lat_stdev; - float lon_stdev; - float hgt_stdev; - oem7_char_t stn_id[4]; - float diff_age; - float sol_age; - uint8_t num_svs; - uint8_t num_sol_svs; - uint8_t num_sol_l1_svs; - uint8_t num_sol_multi_svs; - oem7_hex_t reserved; - oem7_hex_t ext_sol_stat; - uint8_t galileo_beidou_sig_mask; - uint8_t gps_glonass_sig_mask; -}; -static_assert(sizeof(BESTPOSMem) == 72, ""); -// Could be changed: include more fields to become exactly same as pos, include short header or long while reading -struct __attribute__((packed)) BESTXYZMem { - oem7_enum_t p_sol_stat; - oem7_enum_t pos_type; - double p_x; - double p_y; - double p_z; - float p_x_stdev; - float p_y_stdev; - float p_z_stdev; - oem7_enum_t v_sol_stat; - oem7_enum_t vel_type; - double v_x; - double v_y; - double v_z; - float v_x_stdev; - float v_y_stdev; - float v_z_stdev; -}; -static_assert(sizeof(BESTXYZMem) == 88, ""); - -struct __attribute__((packed)) BESTVELMem { - uint32_t sol_stat; - uint32_t vel_type; - float latency; - float diff_age; - double hor_speed; - double track_gnd; - double ver_speed; - float reserved; -}; -static_assert(sizeof(BESTVELMem) == 44, ""); - -struct __attribute__((packed)) INSPVASmem { - uint32_t gnss_week; - double seconds; - double latitude; - double longitude; - double height; - double north_velocity; - double east_velocity; - double up_velocity; - double roll; - double pitch; - double azimuth; - oem7_enum_t status; -}; -static_assert(sizeof(INSPVASmem) == 88, ""); - -struct __attribute__((packed)) CORRIMUSMem { - uint32_t imu_data_count; - double pitch_rate; - double roll_rate; - double yaw_rate; - double lateral_acc; - double longitudinal_acc; - double vertical_acc; - uint32_t reserved1; - uint32_t reserved2; -}; -static_assert(sizeof(CORRIMUSMem) == 60, ""); - -struct __attribute__((packed)) IMURATECORRIMUSMem { - uint32_t week; - double seconds; - double pitch_rate; - double roll_rate; - double yaw_rate; - double lateral_acc; - double longitudinal_acc; - double vertical_acc; -}; -static_assert(sizeof(IMURATECORRIMUSMem) == 60, ""); - -struct __attribute__((packed)) INSSTDEVMem { - float latitude_stdev; - float longitude_stdev; - float height_stdev; - float north_velocity_stdev; - float east_velocity_stdev; - float up_velocity_stdev; - float roll_stdev; - float pitch_stdev; - float azimuth_stdev; - uint32_t ext_sol_status; - uint16_t time_since_last_update; - uint16_t reserved1; - uint32_t reserved2; - uint32_t reserved3; -}; -static_assert(sizeof(INSSTDEVMem) == 52, ""); - -struct __attribute__((packed)) INSCONFIG_FixedMem { - oem7_enum_t imu_type; - uint8_t mapping; - uint8_t initial_alignment_velocity; - uint16_t heave_window; - oem7_enum_t profile; - oem7_hex_t enabled_updates[4]; - oem7_enum_t alignment_mode; - oem7_enum_t relative_ins_output_frame; - oem7_bool_t relative_ins_output_direction; - oem7_hex_t ins_receiver_status[4]; - uint8_t ins_seed_enabled; - uint8_t ins_seed_validation; - uint16_t reserved_1; - uint32_t reserved_2; - uint32_t reserved_3; - uint32_t reserved_4; - uint32_t reserved_5; - uint32_t reserved_6; - uint32_t reserved_7; -}; -static_assert(sizeof(INSCONFIG_FixedMem) == 60, ""); - -struct __attribute__((packed)) INSCONFIG_TranslationMem { - uint32_t translation; - uint32_t frame; - float x_offset; - float y_offset; - float z_offset; - float x_uncertainty; - float y_uncertainty; - float z_uncertainty; - uint32_t translation_source; -}; - -struct __attribute__((packed)) INSCONFIG_RotationMem { - uint32_t rotation; - uint32_t frame; - float x_rotation; - float y_rotation; - float z_rotation; - float x_rotation_stdev; - float y_rotation_stdev; - float z_rotation_stdev; - uint32_t rotation_source; -}; - -struct __attribute__((packed)) INSPVAXMem { - oem7_enum_t ins_status; - oem7_enum_t pos_type; - double latitude; - double longitude; - double height; - float undulation; - double north_velocity; - double east_velocity; - double up_velocity; - double roll; - double pitch; - double azimuth; - float latitude_stdev; - float longitude_stdev; - float height_stdev; - float north_velocity_stdev; - float east_velocity_stdev; - float up_velocity_stdev; - float roll_stdev; - float pitch_stdev; - float azimuth_stdev; - uint32_t extended_status; - uint16_t time_since_update; -}; -static_assert(sizeof(INSPVAXMem) == 126, ""); - -struct __attribute__((packed)) HEADING2Mem { - oem7_enum_t sol_status; - oem7_enum_t pos_type; - float length; - float heading; - float pitch; - float reserved; - float heading_stdev; - float pitch_stdev; - oem7_char_t rover_stn_id[4]; - oem7_char_t master_stn_id[4]; - uint8_t num_sv_tracked; - uint8_t num_sv_in_sol; - uint8_t num_sv_obs; - uint8_t num_sv_multi; - uint8_t sol_source; - uint8_t ext_sol_status; - uint8_t galileo_beidou_sig_mask; - uint8_t gps_glonass_sig_mask; -}; -static_assert(sizeof(HEADING2Mem) == 48, ""); - -struct __attribute__((packed)) BESTUTMMem { - oem7_enum_t sol_stat; - oem7_enum_t pos_type; - uint32_t lon_zone_number; - uint32_t lat_zone_letter; - double northing; - double easting; - double height; - float undulation; - uint32_t datum_id; - float northing_stddev; - float easting_stddev; - float height_stddev; - char stn_id[4]; - float diff_age; - float sol_age; - uint8_t num_svs; - uint8_t num_sol_svs; - uint8_t num_sol_ggl1_svs; - uint8_t num_sol_multi_svs; - uint8_t reserved; - uint8_t ext_sol_stat; - uint8_t galileo_beidou_sig_mask; - uint8_t gps_glonass_sig_mask; -}; -static_assert(sizeof(BESTUTMMem) == 80, ""); - -struct __attribute__((packed)) RXSTATUSMem { - uint32_t error; - uint32_t num_status_codes; - uint32_t rxstat; - uint32_t rxstat_pri_mask; - uint32_t rxstat_set_mask; - uint32_t rxstat_clr_mask; - uint32_t aux1_stat; - uint32_t aux1_stat_pri; - uint32_t aux1_stat_set; - uint32_t aux1_stat_clr; - uint32_t aux2_stat; - uint32_t aux2_stat_pri; - uint32_t aux2_stat_set; - uint32_t aux2_stat_clr; - uint32_t aux3_stat; - uint32_t aux3_stat_pri; - uint32_t aux3_stat_set; - uint32_t aux3_stat_clr; - uint32_t aux4_stat; - uint32_t aux4_stat_pri; - uint32_t aux4_stat_set; - uint32_t aux4_stat_clr; -}; -static_assert(sizeof(RXSTATUSMem) == 88, ""); - -struct __attribute__((packed)) TIMEMem { - uint32_t clock_status; - double offset; - double offset_std; - double utc_offset; - uint32_t utc_year; - uint8_t utc_month; - uint8_t utc_day; - uint8_t utc_hour; - uint8_t utc_min; - uint32_t utc_msec; - uint32_t utc_status; -}; -static_assert(sizeof(TIMEMem) == 44, ""); - -struct __attribute__((packed)) PSRDOP2_FixedMem { - float gdop; - float pdop; - float hdop; - float vdop; -}; -static_assert(sizeof(PSRDOP2_FixedMem) == 16, ""); - -struct __attribute__((packed)) PSRDOP2_SystemMem { - uint32_t system; - float tdop; -}; - -struct __attribute__((packed)) RAWIMUMem { - uint32_t week; - double seconds; - uint32_t imu_stat; - int32_t z_accel; - int32_t y_accel; - int32_t x_accel; - int32_t z_gyro; - int32_t y_gyro; - int32_t x_gyro; -}; -static_assert(sizeof(RAWIMUMem) == 40, ""); - -/** - * @brief See https://docs.novatel.com/OEM7/Content/SPAN_Logs/BESTGNSSPOS.htm - */ -struct __attribute__((packed)) BESTGNSSPOSMem { - oem7_enum_t sol_stat; - oem7_enum_t pos_type; - double lat; - double lon; - double hgt; - float undulation; - oem7_enum_t datum_id; - float lat_stdev; - float lon_stdev; - float hgt_stdev; - oem7_char_t stn_id[4]; - float diff_age; - float sol_age; - uint8_t num_svs; - uint8_t num_sol_svs; - uint8_t num_sol_l1_svs; - uint8_t num_sol_multi_svs; - oem7_hex_t reserved; - oem7_hex_t ext_sol_stat; - uint8_t galileo_beidou_sig_mask; - uint8_t gps_glonass_sig_mask; -}; -static_assert(sizeof(BESTGNSSPOSMem) == 72, ""); - -const size_t OEM7_BINARY_MSG_HDR_LEN = sizeof(Oem7MessageHeaderMem); -const size_t OEM7_BINARY_MSG_SHORT_HDR_LEN = sizeof(Oem7MessgeShortHeaderMem); - -/* ****************************************************************************************************************** */ -} // namespace fixposition -#endif // __LIB_NOVATEL_TYPE_H__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp index 03f5b7d..a07fb4f 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp @@ -1,51 +1,58 @@ /** - * @file - * @brief Parameters - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Parameters */ #ifndef __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__ #define __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__ -/* SYSTEM / STL */ +/* LIBC/STL */ #include #include -namespace fixposition { +/* EXTERNAL */ -enum class INPUT_TYPE { TCP = 1, SERIAL = 2 }; +/* PACKAGE */ -struct FpOutputParams { - int rate; //!< loop rate of the main read loop - double reconnect_delay; //!< wait time in [s] until retry connection - INPUT_TYPE type; //!< TCP or SERIAL - std::vector formats; //!< data formats to convert, supports "FP" for now - std::string qos_type; //!< ROS QoS type, supports "sensor_" and "default_" - bool cov_warning; //!< enable/disable covariance warning - bool nav2_mode; //!< enable/disable nav2 mode +namespace fixposition { +/* ****************************************************************************************************************** */ - std::string ip; //!< IP address for TCP connection - std::string port; //!< Port for TCP connection - int baudrate; //!< baudrate of serial connection -}; -struct CustomerInputParams { - std::string speed_topic; //!< Input ROS topic for Speed measurements - std::string rtcm_topic; //!< Input ROS topic for RTCM3 messages +/** + * @brief Parameters for the sensor driver + * + * See launch/driver.yaml for documentation + */ +struct SensorParams { + std::string stream_; + double reconnect_delay_ = 5.0; + std::vector messages_; + bool cov_warning_ = false; + bool nav2_mode_ = false; + + // Check if entry is in messges_ + bool MessageEnabled(const std::string& message_name) const; + + // TODO remove + std::vector formats; //!< Data formats (= messages) to process }; -struct FixpositionDriverParams { - FpOutputParams fp_output; - CustomerInputParams customer_input; +/** + * @brief Parameters for the ROS nodes + */ +struct NodeParams { + std::string speed_topic_; //!< Topic name for wheelspeed input + std::string corr_topic_; //!< Topic name for correction data input + std::string qos_type_; //!< ROS2 QoS type, supports "sensor_" and "default_" }; +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp deleted file mode 100644 index 39aa118..0000000 --- a/fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp +++ /dev/null @@ -1,43 +0,0 @@ -/** - * @file - * @brief Parser functions - * - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * \endverbatim - * - */ - -#ifndef __FIXPOSITION_DRIVER_LIB_PARSER__ -#define __FIXPOSITION_DRIVER_LIB_PARSER__ - -/* SYSTEM / STL */ -#include -#include - -namespace fixposition { - -/** - * @brief Check If msg is NMEA - * - * @param[in] buf start pointer of a char* buffer - * @param[in] size size of the buffer to check - * @return int the length of the NMEA message found. If no NMEA found then 0. If size argument is too small then -1 - */ -int IsNmeaMessage(const char* buf, const int size); - -/** - * @brief Check If msg is NOV_B - * - * @param[in] buf buffer ptr - * @param[in] size size - * @return int the length of the NOV_B message found. If no NOV_B found then 0. If size argument is too small then -1 - */ -int IsNovMessage(const uint8_t* buf, const int size); - -} // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_HELPER__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp index 57b20d2..9533a42 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp @@ -1,28 +1,33 @@ /** - * @file - * @brief Declaration of time conversion functions - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of time conversion functions */ -#ifndef __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__ -#define __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__ +#ifndef __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS_HPP__ +#define __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS_HPP__ + +/* LIBC/STL */ /* EXTERNAL */ +#include #include #include -#include +#include + +/* PACKAGE */ namespace BOOST_POSIX = boost::posix_time; namespace fixposition { +/* ****************************************************************************************************************** */ namespace times { /** @@ -88,9 +93,7 @@ class GpsTime { GpsTime& operator+=(const double sec) { double gps_sec_tmp = tow + sec; - // std::cout << gps_sec_tmp << std::endl; int delta_week = std::floor(gps_sec_tmp / Constants::sec_per_week); - // std::cout << delta_week << std::endl; wno += delta_week; tow = gps_sec_tmp - delta_week * Constants::sec_per_week; return *this; @@ -196,21 +199,6 @@ inline BOOST_POSIX::ptime GpsTimeToPtime(const GpsTime& gps_time) { return Constants::gps_epoch_begin + BOOST_POSIX::microseconds(micro_s); } -/** - * @brief - * Only work after 2017.1.1 - * - * @param[in] boost_ptime - * @return GpsTime - */ -inline GpsTime PtimeToGpsTime(const BOOST_POSIX::ptime& boost_ptime) { - BOOST_POSIX::time_duration gps_duration = boost_ptime - Constants::gps_epoch_begin; - int weekcount = gps_duration.total_seconds() / Constants::sec_per_week; - double sec_in_week = - gps_duration.total_microseconds() / 1e6 - weekcount * Constants::sec_per_week + Constants::gps_leap_time_s; - return GpsTime(weekcount, sec_in_week); -} - /** * @brief Convert UTC time with milliseconds to GPS time * @@ -221,7 +209,7 @@ inline GpsTime PtimeToGpsTime(const BOOST_POSIX::ptime& boost_ptime) { */ inline void convertToGPSTime(const std::string& utcTimeString, std::string& gpsWeek, std::string& gpsTimeOfWeek) { // Define constants - const double secondsInWeek = 604800.0; // 7 days in seconds + const double secondsInWeek = 604800.0; // 7 days in seconds // Parse the input string std::tm tmTime = {}; @@ -233,20 +221,20 @@ inline void convertToGPSTime(const std::string& utcTimeString, std::string& gpsW std::string milliseconds; iss >> dot >> milliseconds; double ms = std::stod("0." + milliseconds); - + if (iss.fail()) { - std::cerr << "Error parsing input string.\n"; + WARNING("Error parsing input string."); return; } - + // Convert UTC time to time since epoch std::time_t utcTime = std::mktime(&tmTime); // GPS epoch time (January 6, 1980) std::tm gpsEpoch = {}; - gpsEpoch.tm_year = 80; // years since 1900 - gpsEpoch.tm_mon = 0; // months since January - gpsEpoch.tm_mday = 6; // day of the month + gpsEpoch.tm_year = 80; // years since 1900 + gpsEpoch.tm_mon = 0; // months since January + gpsEpoch.tm_mday = 6; // day of the month std::time_t gpsEpochTime = std::mktime(&gpsEpoch); // Calculate GPS time of week and GPS week number @@ -267,5 +255,7 @@ inline void convertToGPSTime(const std::string& utcTimeString, std::string& gpsW } } // namespace times + +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__ +#endif // __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS_HPP__ diff --git a/fixposition_driver_lib/package.xml b/fixposition_driver_lib/package.xml index 09ca91a..f88a700 100644 --- a/fixposition_driver_lib/package.xml +++ b/fixposition_driver_lib/package.xml @@ -20,6 +20,10 @@ geometry_msgs message_generation message_runtime + fpsdk_common + fpsdk_ros1 + fpsdk_ros2 + cmake diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index e41bf00..49aa2ca 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -1,365 +1,514 @@ /** - * @file - * @brief Implementation of FixpositionDriver class - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FixpositionDriver class */ -/* SYSTEM / STL */ +/* LIBC/STL */ +#include +#include +#include +#include + +/* EXTERNAL */ #include -#include #include -#include -/* PACKAGE */ -#include -#include +#include +#include +#include +#include +#include -#ifndef B460800 -#define B460800 460800 -#endif -#ifndef B500000 -#define B500000 500000 -#endif -#ifndef B921600 -#define B921600 921600 -#endif -#ifndef B1000000 -#define B1000000 1000000 -#endif +/* PACKAGE */ +#include "fixposition_driver_lib/fixposition_driver.hpp" namespace fixposition { -FixpositionDriver::FixpositionDriver(const FixpositionDriverParams& params) : params_(params) { - // connect to the sensor - if (!Connect()) { - if (params_.fp_output.type == INPUT_TYPE::TCP) { - throw std::runtime_error("Unable to connect to the sensor via TCP"); - } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) { - throw std::runtime_error("Unable to connect to the sensor via Serial"); - } else { - throw std::runtime_error("Unable to connect to the sensor, verify configuration"); +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common; +using namespace fpsdk::common::parser; + +FixpositionDriver::FixpositionDriver(const SensorParams& params) + : /* clang-format off */ + params_ { params }, + worker_ { "driver", std::bind(&FixpositionDriver::Worker, this, std::placeholders::_1) } // clang-format on +{ + // Initialize converters based on config + bool ok = true; + for (const auto& format : params_.formats) { + // clang-format off + // FP_A messages + if (format == "ODOMETRY") { a_converters_["ODOMETRY"] = std::make_unique>(); } + else if (format == "ODOMENU") { a_converters_["ODOMENU"] = std::make_unique>(); } + else if (format == "ODOMSH") { a_converters_["ODOMSH"] = std::make_unique>(); } + else if (format == "ODOMSTATUS") { a_converters_["ODOMSTATUS"] = std::make_unique>(); } + else if (format == "LLH") { a_converters_["LLH"] = std::make_unique>(); } + else if (format == "TF") { a_converters_["TF"] = std::make_unique>(); } + else if (format == "TP") { a_converters_["TP"] = std::make_unique>(); } + else if (format == "RAWIMU") { a_converters_["RAWIMU"] = std::make_unique>(); } + else if (format == "CORRIMU") { a_converters_["CORRIMU"] = std::make_unique>(); } + else if (format == "IMUBIAS") { a_converters_["IMUBIAS"] = std::make_unique>(); } + else if (format == "GNSSANT") { a_converters_["GNSSANT"] = std::make_unique>(); } + else if (format == "GNSSCORR") { a_converters_["GNSSCORR"] = std::make_unique>(); } + else if (format == "TEXT") { a_converters_["TEXT"] = std::make_unique>(); } + else if (format == "EOE") { a_converters_["EOE"] = std::make_unique>(); } + // NMEA messages + else if (format == "GPGGA") { a_converters_["GPGGA"] = std::make_unique>(); } + else if (format == "GPGLL") { a_converters_["GPGLL"] = std::make_unique>(); } + else if (format == "GNGSA") { a_converters_["GNGSA"] = std::make_unique>(); } + else if (format == "GPGST") { a_converters_["GPGST"] = std::make_unique>(); } + else if (format == "GXGSV") { a_converters_["GXGSV"] = std::make_unique>(); } + else if (format == "GPHDT") { a_converters_["GPHDT"] = std::make_unique>(); } + else if (format == "GPRMC") { a_converters_["GPRMC"] = std::make_unique>(); } + else if (format == "GPVTG") { a_converters_["GPVTG"] = std::make_unique>(); } + else if (format == "GPZDA") { a_converters_["GPZDA"] = std::make_unique>(); } + // clang-format on + else { + WARNING("Unknown input format '%s'", format.c_str()); } } - - // initialize converters - if (!InitializeConverters()) { - throw std::runtime_error("Could not initialize output converter!"); + if (a_converters_.empty()) { + WARNING("No converter formats defined"); } -} - -FixpositionDriver::~FixpositionDriver() { - if (client_fd_ != -1) { - if (params_.fp_output.type == INPUT_TYPE::SERIAL) { - tcsetattr(client_fd_, TCSANOW, &options_save_); - } - close(client_fd_); + if (!ok) { + throw std::runtime_error("Could not initialize output converter(s)"); } } +FixpositionDriver::~FixpositionDriver() { StopDriver(); } + +// --------------------------------------------------------------------------------------------------------------------- + bool FixpositionDriver::Connect() { - switch (params_.fp_output.type) { - case INPUT_TYPE::TCP: - return CreateTCPSocket(); - break; - case INPUT_TYPE::SERIAL: - return CreateSerialConnection(); - break; - default: - std::cerr << "Unknown connection type!\n"; - return false; + if (sensor_fd_ >= 0) { + WARNING("Already connected to sensor"); + return true; + } + + // Work out connection type. A bit of a hack, but this will soon be replaced by fpsdk::common::stream anyways... + const auto parts = string::StrSplit(params_.stream_, ":"); + int port_or_baudrate = 0; + if ((parts.size() != 2) || parts[0].empty() || parts[1].empty() || + !string::StrToValue(parts[1], port_or_baudrate)) { + WARNING("Bad sensor stream spec: %s", params_.stream_.c_str()); + return false; + } + const std::string& ip_or_dev = parts[0]; + serial_ = (parts[0].substr(0, 1) == "/"); + + INFO("Connecting to %s", params_.stream_.c_str()); + + if (serial_) { + return ConnectSerial(ip_or_dev, port_or_baudrate); + } else { + return ConnectTcp(ip_or_dev, port_or_baudrate); } } -void FixpositionDriver::WsCallback( - const std::unordered_map>>& sensors_meas) { - std::vector sensor_measurements; - for (const auto& sensor : sensors_meas) { - const FpbMeasurementsMeasLoc location = WsMeasStringToLoc(sensor.first); - if (location == MEASLOC_UNSPECIFIED) { - std::cerr << "Unknown measurement type will not be processed!\n"; - continue; - } - FpbMeasurementsMeas fpb_meas; - if (FillWsSensorMeas(sensor.second, location, fpb_meas)) { - sensor_measurements.push_back(fpb_meas); +void FixpositionDriver::Disconnect() { + if (IsConnected()) { + INFO("Disconnecting from %s", params_.stream_.c_str()); + if (serial_) { + DisconnectSerial(); + } else { + DisconnectTcp(); } + sensor_fd_ = -1; + params_.stream_.clear(); } +} - const size_t num_meas = sensor_measurements.size(); - if (num_meas == 0 || num_meas > 10) { - std::cerr << "Number of wheel speed sensors is invalid.\n"; - return; +bool FixpositionDriver::IsConnected() const { return sensor_fd_ >= 0; } + +bool FixpositionDriver::ConnectTcp(const std::string& ip, const int port) { + DisconnectTcp(); + + int fd = socket(AF_INET, SOCK_STREAM, 0); + if (fd < 0) { + WARNING("Failed connecting to %s: %s", params_.stream_.c_str(), string::StrError(errno).c_str()); + return false; } - FpbHeader header; - header.sync1 = 0x66; - header.sync2 = 0x21; - header.msg_id = 2001; - header.payload_size = FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas); - header.time = 0; + struct sockaddr_in server_address; + server_address.sin_family = AF_INET; + server_address.sin_addr.s_addr = INADDR_ANY; + server_address.sin_addr.s_addr = inet_addr(ip.c_str()); + server_address.sin_port = htons(port); - FpbMeasurementsHeader meas_header; - meas_header.version = 1; - meas_header.num_meas = num_meas; - std::fill(&meas_header.reserved0[0], &meas_header.reserved0[6], 0); - - const int msg_sz = - FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas) + FP_B_CRC_SIZE; - std::vector message(msg_sz); - - memcpy(&message[0], (uint8_t*)&header, sizeof(header)); - memcpy(&message[FP_B_HEAD_SIZE], (uint8_t*)&meas_header, sizeof(meas_header)); - for (size_t i = 0; i < num_meas; i++) { - memcpy(&message[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * i)], - (uint8_t*)&sensor_measurements[i], sizeof(sensor_measurements[i])); - } - const uint32_t crc = Crc32fpb( - message.data(), FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas)); - memcpy(&message[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas)], &crc, - sizeof(crc)); - - switch (params_.fp_output.type) { - case INPUT_TYPE::TCP: - send(this->client_fd_, &message[0], message.size(), MSG_DONTWAIT); - break; - case INPUT_TYPE::SERIAL: - (void)!write(this->client_fd_, &message[0], message.size()); - // Suppress warning - https://stackoverflow.com/a/64407070/7944565 - break; - default: - std::cerr << "Unknown connection type!\n"; - break; + const int res = connect(fd, (struct sockaddr*)&server_address, sizeof server_address); + + if (res != 0) { + WARNING("Failed connecting to %s: %s", params_.stream_.c_str(), string::StrError(errno).c_str()); + return false; } + + INFO("Connected to %s", params_.stream_.c_str()); + sensor_fd_ = fd; + poll_fds_[0].fd = fd; + poll_fds_[0].events = POLLIN | POLLERR | POLLHUP; + return true; } -void FixpositionDriver::RtcmCallback(const void *rtcm_msg, const size_t msg_size) { - // TODO: Check that RTCM message is valid +void FixpositionDriver::DisconnectTcp() { + if (sensor_fd_ >= 0) { + close(sensor_fd_); + } +} + +#ifndef B460800 +#define B460800 460800 +#endif +#ifndef B500000 +#define B500000 500000 +#endif +#ifndef B921600 +#define B921600 921600 +#endif - switch (params_.fp_output.type) { - case INPUT_TYPE::TCP: - send(this->client_fd_, rtcm_msg, msg_size, MSG_DONTWAIT); - break; - case INPUT_TYPE::SERIAL: - (void)!write(this->client_fd_, rtcm_msg, msg_size); - // Suppress warning - https://stackoverflow.com/a/64407070/7944565 - break; +bool FixpositionDriver::ConnectSerial(const std::string& dev, const int baud) { + speed_t speed; + switch (baud) { // clang-format off + case 9600: speed = B9600; break; + case 38400: speed = B38400; break; + case 57600: speed = B57600; break; + case 115200: speed = B115200; break; + case 230400: speed = B230400; break; + case 460800: speed = B460800; break; + case 500000: speed = B500000; break; + case 921600: speed = B921600; break; // clang-format on default: - std::cerr << "Unknown connection type!\n"; - break; + WARNING("Unsupported baudrate %d", baud); + return false; } -} -bool FixpositionDriver::FillWsSensorMeas(const std::vector>& meas_vec, - const FpbMeasurementsMeasLoc meas_loc, FpbMeasurementsMeas& meas_fpb) { - const size_t num_axis = meas_vec.size(); - if (num_axis != 3) { - std::cerr << "Wheelspeed sensor has an invalid number of measurements.\n"; + DisconnectSerial(); + + int fd = open(dev.c_str(), O_RDWR | O_NOCTTY); + + if (fd == -1) { + WARNING("Failed connecting to %s: %s", params_.stream_.c_str(), string::StrError(errno).c_str()); return false; } - meas_fpb.meas_type = MEASTYPE_VELOCITY; - meas_fpb.meas_loc = meas_loc; - std::fill(&meas_fpb.reserved1[0], &meas_fpb.reserved1[4], 0); - // In the current setup, the sensor will handle the timestamping as time of arrival. - meas_fpb.timestamp_type = TIME_TOA; - meas_fpb.gps_wno = 0; - meas_fpb.gps_tow = 0; - meas_fpb.meas_x_valid = meas_vec[0].first; - meas_fpb.meas_x = meas_vec[0].second; - meas_fpb.meas_y_valid = meas_vec[1].first; - meas_fpb.meas_y = meas_vec[1].second; - meas_fpb.meas_z_valid = meas_vec[2].first; - meas_fpb.meas_z = meas_vec[2].second; + + // Get current serial port options: + struct termios options; + tcgetattr(fd, &options); + options_save_ = options; + + char speed_buf[10]; + snprintf(speed_buf, sizeof(speed_buf), "0%06o", (int)cfgetispeed(&options)); + + options.c_iflag &= ~(IXOFF | IXON | ICRNL); + options.c_oflag &= ~(OPOST | ONLCR); + options.c_lflag &= ~(ISIG | ICANON | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | IEXTEN); + options.c_cc[VEOL] = 0; + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 50; + + cfsetospeed(&options, speed); /* baud rate */ + tcsetattr(fd, TCSANOW, &options); + + INFO("Connected to %s", params_.stream_.c_str()); + sensor_fd_ = fd; + poll_fds_[0].fd = fd; + poll_fds_[0].events = POLLIN | POLLERR | POLLHUP; return true; } -FpbMeasurementsMeasLoc FixpositionDriver::WsMeasStringToLoc(const std::string& meas_loc) { - if (meas_loc == "RC") return MEASLOC_RC; - if (meas_loc == "FR") return MEASLOC_FR; - if (meas_loc == "FL") return MEASLOC_FL; - if (meas_loc == "RR") return MEASLOC_RR; - if (meas_loc == "RL") return MEASLOC_RL; - return MEASLOC_UNSPECIFIED; +void FixpositionDriver::DisconnectSerial() { + if (sensor_fd_ >= 0) { + tcsetattr(sensor_fd_, TCSANOW, &options_save_); + close(sensor_fd_); + } } -bool FixpositionDriver::InitializeConverters() { - for (const auto& format : params_.fp_output.formats) { - - // FP_A messages - if (format == "ODOMETRY") { - a_converters_["ODOMETRY"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "ODOMENU") { - a_converters_["ODOMENU"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "ODOMSH") { - a_converters_["ODOMSH"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "ODOMSTATUS") { - a_converters_["ODOMSTATUS"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "LLH") { - a_converters_["LLH"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "TF") { - a_converters_["TF"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "TP") { - a_converters_["TP"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "RAWIMU") { - a_converters_["RAWIMU"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "CORRIMU") { - a_converters_["CORRIMU"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "IMUBIAS") { - a_converters_["IMUBIAS"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GNSSANT") { - a_converters_["GNSSANT"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GNSSCORR") { - a_converters_["GNSSCORR"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "TEXT") { - a_converters_["TEXT"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "EOE") { - a_converters_["EOE"] = std::unique_ptr>(new NmeaConverter()); - - // NMEA messages - } else if (format == "GPGGA") { - a_converters_["GPGGA"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GPGLL") { - a_converters_["GPGLL"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GNGSA") { - a_converters_["GNGSA"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GPGST") { - a_converters_["GPGST"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GXGSV") { - a_converters_["GXGSV"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GPHDT") { - a_converters_["GPHDT"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GPRMC") { - a_converters_["GPRMC"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GPVTG") { - a_converters_["GPVTG"] = std::unique_ptr>(new NmeaConverter()); - } else if (format == "GPZDA") { - a_converters_["GPZDA"] = std::unique_ptr>(new NmeaConverter()); +std::size_t FixpositionDriver::Read(uint8_t* buf, const std::size_t size, const int timeout) { + if (!IsConnected()) { + WARNING_THR(1000, "no connection, cannot read"); + return 0; + } + + const int res = poll(poll_fds_.data(), poll_fds_.size(), timeout); + // Something's wrong + if ((res < 0) && (errno != EINTR)) { + WARNING("poll() fail: %s", string::StrError(errno).c_str()); + } + // We should be able to read some data + else if ((poll_fds_[0].revents & POLLIN) != 0) { + ssize_t rv = 0; + if (serial_) { + rv = recv(sensor_fd_, buf, size, MSG_DONTWAIT); } else { - std::cerr << "Unknown input format: " << format << "\n"; + rv = read(sensor_fd_, buf, sizeof(buf)); + } + // We have some data + if (rv >= 0) { + return rv; + } + // Perhaps a problem + else { + if (errno == EAGAIN) { + return 0; + } else { + WARNING("read/recv fail: %s", string::StrError(errno).c_str()); + } } } - return !a_converters_.empty(); + // Connection is broken + else if ((poll_fds_[0].revents & (POLLHUP | POLLERR)) != 0) { + WARNING("poll() fail"); + } + // Timeout, no data at the moment + else { + return 0; + } + + Disconnect(); + return 0; } -bool FixpositionDriver::RunOnce() { - if ((client_fd_ > 0) && (connection_status_ == 0) && ReadAndPublish()) { - return true; + +bool FixpositionDriver::Write(const uint8_t* buf, const std::size_t size) { + if (!IsConnected()) { + WARNING_THR(1000, "no connection, cannot write"); + return 0; + } + + int res = 0; + if (serial_) { + res = write(this->sensor_fd_, buf, size); } else { - close(client_fd_); - client_fd_ = -1; - return false; + res = send(this->sensor_fd_, buf, size, MSG_DONTWAIT); } + if (res < 0) { + WARNING("send/write fail: %s", string::StrError(errno).c_str()); + return 0; + } + return (res > 0) && (res == (int)size); +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool FixpositionDriver::StartDriver() { return Connect() && worker_.Start(); } + +void FixpositionDriver::StopDriver() { + worker_.Stop(); + Disconnect(); } -bool FixpositionDriver::ReadAndPublish() { - int msg_size = 0; - - // Extract messages until the buffer is empty - while (buf_size > 0) { - // Check whether the message is NOV_B - msg_size = IsNovMessage(readBuf, buf_size); - - // a) Message is NOV_B --> Process message and remove it from the buffer - if (msg_size > 0) { - NovConvertAndPublish(readBuf); - buf_size -= msg_size; - if (buf_size > 0) { - memmove(readBuf, &readBuf[msg_size], buf_size); +void FixpositionDriver::Worker(void* /*arg*/) { + INFO("Driver running..."); + + while (!worker_.ShouldAbort()) { + // While we're connected to the sensor... + if (IsConnected()) { + // Read more data from sensor and feed the parser + uint8_t buf[parser::MAX_ADD_SIZE]; + const std::size_t size = Read(buf, sizeof(buf), 337); + if (size == 0) { + continue; // try again } - // Look for new messages in the buffer - } - - // b) Message is not NOV_B. Check whether it is NMEA/FP_A - else if (msg_size == 0) { - msg_size = IsNmeaMessage((char*)readBuf, buf_size); - - // Message is NMEA/FP_A --> Process message and remove it from the buffer - if (msg_size > 0) { - NmeaConvertAndPublish({(const char*)readBuf, (const char*)readBuf + msg_size}); - buf_size -= msg_size; - if (buf_size > 0) { - memmove(readBuf, &readBuf[msg_size], buf_size); - } - // Look for new messages in the buffer + if (!parser_.Add(buf, size)) { + WARNING("Parser overflow"); // should not happen, as we respect the parser's limits (MAX_ADD_SIZE) + parser_.Reset(); + parser_.Add(buf, size); } - - // Message is neither NMEA/FP_A or NOV_B --> Remove one byte from the buffer and try again until it is empty - else if (msg_size == 0) { - if (buf_size > 0) { - buf_size -= 1; - memmove(readBuf, &readBuf[1], buf_size); - } - - // Buffer is empty --> Wait for more data - else /* buf_size == 0 */ { - break; + + // Process received message(s) + parser::ParserMsg msg; + while (parser_.Process(msg)) { + IF_TRACE(msg.MakeInfo()); + TRACE("received %s %" PRIuMAX " %s", msg.name_.c_str(), msg.data_.size(), msg.info_.c_str()); + switch (msg.proto_) { + case parser::Protocol::FP_A: + ProcessFpa(msg); + break; + case parser::Protocol::NMEA: + ProcessNmea(msg); + break; + case parser::Protocol::NOV_B: + ProcessNovb(msg); + break; + case parser::Protocol::FP_B: + case parser::Protocol::UBX: + case parser::Protocol::RTCM3: + case parser::Protocol::UNI_B: + case parser::Protocol::SPARTN: + case parser::Protocol::OTHER: + break; } } - - // NMEA message might be incomplete --> Wait for more data - else /* msg_size < 0 */ { + } + // Reconnect after some time... + else { + INFO("Reconnecting in %.1f seconds...", params_.reconnect_delay_); + if (worker_.Sleep(params_.reconnect_delay_ * 1000)) { break; } + Connect(); } + } +} - // c) NOV_B message might be incomplete --> Wait for more data - else /* msg_size < 0 */ { - break; - } +// --------------------------------------------------------------------------------------------------------------------- + +void FixpositionDriver::AddFpaObserver(const std::string& message_name, FpaObserver observer) { + DEBUG("Adding FP_A observer for %s", message_name.c_str()); + auto entry = fpa_observers_.find(message_name); + if (entry == fpa_observers_.end()) { + entry = fpa_observers_.insert({message_name, {}}).first; } + entry->second.push_back(observer); +} - // Read more data from the TCP/Serial port - int rem_size = sizeof(readBuf) - buf_size; - if (rem_size > 0) { - ssize_t rv; - if (params_.fp_output.type == INPUT_TYPE::TCP) { - rv = recv(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size, MSG_DONTWAIT); - } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) { - rv = read(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size); - } else { - rv = 0; - } +void FixpositionDriver::RemoveFpaObservers() { fpa_observers_.clear(); } + +void FixpositionDriver::ProcessFpa(const ParserMsg& msg) { + auto entry = fpa_observers_.find(msg.name_); + if ((entry != fpa_observers_.end()) && !entry->second.empty()) { + TRACE("process %s", msg.name_.c_str()); + +#define _DISPATCH(_type_) \ + else if (msg.name_ == _type_::MSG_NAME) { \ + _type_ payload; \ + if (payload.SetFromMsg(msg.data_.data(), msg.data_.size())) { \ + for (auto& obs : entry->second) { \ + obs(payload); \ + } \ + } else { \ + WARNING_THR(1000, "Bad %s", msg.name_.c_str()); \ + } \ + } - if (rv == 0) { - std::cerr << "Connection closed.\n"; - return false; + if (false) { } - - if (rv < 0 && errno == EAGAIN) { - /* no data for now, call back when the socket is readable */ - return true; - } - - if (rv < 0) { - std::cerr << "Connection error.\n"; - return false; + _DISPATCH(fpa::FpaEoePayload) + _DISPATCH(fpa::FpaGnssantPayload) + _DISPATCH(fpa::FpaGnsscorrPayload) + _DISPATCH(fpa::FpaRawimuPayload) + _DISPATCH(fpa::FpaCorrimuPayload) + _DISPATCH(fpa::FpaImubiasPayload) + _DISPATCH(fpa::FpaLlhPayload) + _DISPATCH(fpa::FpaOdometryPayload) + _DISPATCH(fpa::FpaOdomshPayload) + _DISPATCH(fpa::FpaOdomenuPayload) + _DISPATCH(fpa::FpaOdomstatusPayload) + _DISPATCH(fpa::FpaTextPayload) + _DISPATCH(fpa::FpaTfPayload) + _DISPATCH(fpa::FpaTpPayload) + +#undef _DISPATCH + } else { + TRACE("ignore %s", msg.name_.c_str()); + } + + NmeaConvertAndPublish(msg); // TODO: replace this +} + +// --------------------------------------------------------------------------------------------------------------------- + +void FixpositionDriver::AddNmeaObserver(const std::string& formatter, NmeaObserver observer) { + DEBUG("Adding NMEA observer for %s", formatter.c_str()); + auto entry = nmea_observers_.find(formatter); + if (entry == nmea_observers_.end()) { + entry = nmea_observers_.insert({formatter, {}}).first; + } + entry->second.push_back(observer); +} + +void FixpositionDriver::RemoveNmeaObservers() { nmea_observers_.clear(); } + +void FixpositionDriver::ProcessNmea(const fpsdk::common::parser::ParserMsg& msg) { + // NMEA observers are registered using the formatter (e.g. "RMC"), ignoring the talker ("GP", "GN", etc.) + nmea::NmeaMessageMeta meta; + if (!nmea::NmeaGetMessageMeta(meta, msg.data_.data(), msg.data_.size())) { + WARNING_THR(1000, "Bad %s", msg.name_.c_str()); + return; + } + + auto entry = nmea_observers_.find(meta.formatter_); + if ((entry != nmea_observers_.end()) && !entry->second.empty()) { + TRACE("process %s (%s)", meta.formatter_, msg.name_.c_str()); + +#define _DISPATCH(_type_) \ + else if (std::strcmp(meta.formatter_, _type_::FORMATTER) == 0) { \ + _type_ payload; \ + if (payload.SetFromMsg(msg.data_.data(), msg.data_.size())) { \ + for (auto& obs : entry->second) { \ + obs(payload); \ + } \ + } else { \ + WARNING_THR(1000, "Bad %s", msg.name_.c_str()); \ + } \ + } + + if (false) { } + _DISPATCH(nmea::NmeaGgaPayload) + _DISPATCH(nmea::NmeaGllPayload) + _DISPATCH(nmea::NmeaRmcPayload) + _DISPATCH(nmea::NmeaVtgPayload) + _DISPATCH(nmea::NmeaGstPayload) + _DISPATCH(nmea::NmeaHdtPayload) + _DISPATCH(nmea::NmeaZdaPayload) + _DISPATCH(nmea::NmeaGsaPayload) + _DISPATCH(nmea::NmeaGsvPayload) + +#undef _DISPATCH + } else { + TRACE("ignore %s (%s)", meta.formatter_, msg.name_.c_str()); + } + + NmeaConvertAndPublish(msg); // TODO: replace this +} - buf_size += rv; +// --------------------------------------------------------------------------------------------------------------------- + +void FixpositionDriver::AddNovbObserver(const std::string& message_name, NovbObserver observer) { + DEBUG("Adding NOV_B observer for %s", message_name.c_str()); + auto entry = novb_observers_.find(message_name); + if (entry == novb_observers_.end()) { + entry = novb_observers_.insert({message_name, {}}).first; } + entry->second.push_back(observer); +} - return true; +void FixpositionDriver::RemoveNovbObservers() { novb_observers_.clear(); } + +void FixpositionDriver::ProcessNovb(const fpsdk::common::parser::ParserMsg& msg) { + auto entry = novb_observers_.find(msg.name_); + if ((entry != novb_observers_.end()) && !entry->second.empty()) { + TRACE("process %s", msg.name_.c_str()); + for (auto& obs : entry->second) { + obs(msg); + } + } else { + TRACE("ignore %s", msg.name_.c_str()); + } + + NovConvertAndPublish(msg); // TODO: replace this } -void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { +// --------------------------------------------------------------------------------------------------------------------- + +void FixpositionDriver::NmeaConvertAndPublish(const fpsdk::common::parser::ParserMsg& msg) { + const std::string sentence = {(const char*)msg.data_.data(), (const char*)msg.data_.data() + msg.data_.size()}; // split the msg into tokens, removing the *XX checksum - std::vector tokens; - std::size_t star_pos = msg.find_last_of("*"); - SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); + const std::size_t star_pos = sentence.find_last_of("*"); + const std::vector tokens = string::StrSplit(sentence.substr(1, star_pos - 1), ","); // if it doesn't start with FP then do nothing - if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && - (tokens.at(0) != "GPGLL") && (tokens.at(0) != "GNGSA") && - (tokens.at(0) != "GPGST") && (tokens.at(0) != "GPHDT") && - (tokens.at(0) != "GPRMC") && (tokens.at(0) != "GPVTG") && - (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPGSV") && - (tokens.at(0) != "GAGSV") && (tokens.at(0) != "GBGSV") && - (tokens.at(0) != "GLGSV")) { + if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPGLL") && (tokens.at(0) != "GNGSA") && + (tokens.at(0) != "GPGST") && (tokens.at(0) != "GPHDT") && (tokens.at(0) != "GPRMC") && + (tokens.at(0) != "GPVTG") && (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPGSV") && + (tokens.at(0) != "GAGSV") && (tokens.at(0) != "GBGSV") && (tokens.at(0) != "GLGSV")) { return; } @@ -381,141 +530,89 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { _header = "GPVTG"; } else if (tokens.at(0) == "GPZDA") { _header = "GPZDA"; - } else if (tokens.at(0) == "GPGSV" || tokens.at(0) == "GAGSV" || tokens.at(0) == "GBGSV" || tokens.at(0) == "GLGSV") { + } else if (tokens.at(0) == "GPGSV" || tokens.at(0) == "GAGSV" || tokens.at(0) == "GBGSV" || + tokens.at(0) == "GLGSV") { _header = "GXGSV"; } else { _header = tokens.at(1); } const std::string header = _header; - // If we have a converter available, convert to ros. + // If we have a converter available, convert to ROS msg if (a_converters_[header] != nullptr) { a_converters_[header]->ConvertTokens(tokens); } } -void FixpositionDriver::NovConvertAndPublish(const uint8_t* msg) { - auto* header = reinterpret_cast(msg); - const auto msg_id = header->message_id; +void FixpositionDriver::NovConvertAndPublish(const fpsdk::common::parser::ParserMsg& msg) { + auto* header = reinterpret_cast(msg.data_.data()); + const uint16_t msg_id = header->message_id; - if (msg_id == static_cast(MessageId::BESTGNSSPOS)) { + if (msg_id == parser::novb::NOV_B_BESTGNSSPOS_MSGID) { for (auto& ob : bestgnsspos_obs_) { - auto* payload = reinterpret_cast(msg + sizeof(Oem7MessageHeaderMem)); + auto* payload = reinterpret_cast( + msg.data_.data() + sizeof(parser::novb::NovbLongHeader)); ob(header, payload); } } // TODO add more msg types } -bool FixpositionDriver::CreateTCPSocket() { - if (client_fd_ != -1) { - std::cerr << "TCP connection already exists.\n"; - return true; - } +// --------------------------------------------------------------------------------------------------------------------- - client_fd_ = socket(AF_INET, SOCK_STREAM, 0); +void FixpositionDriver::WsCallback( + const std::unordered_map>>& sensors_meas) { + std::vector sensor_measurements; + for (const auto& sensor : sensors_meas) { + const parser::fpb::FpbMeasurementsMeasLoc location = WsMeasStringToLoc(sensor.first); + if (location == parser::fpb::FpbMeasurementsMeasLoc::UNSPECIFIED) { + WARNING("Unknown measurement type will not be processed"); + continue; + } + parser::fpb::FpbMeasurementsMeas fpb_meas; + if (FillWsSensorMeas(sensor.second, location, fpb_meas)) { + sensor_measurements.push_back(fpb_meas); + } + } - if (client_fd_ < 0) { - std::cerr << "Error in client creation.\n"; - return false; - } else { - std::cout << "Client created.\n"; + const size_t num_meas = sensor_measurements.size(); + if ((num_meas == 0) || (num_meas > parser::fpb::FP_B_MEASUREMENTS_MAX_NUM_MEAS)) { + WARNING("Number of wheel speed sensors is invalid"); + return; } - struct sockaddr_in server_address; - server_address.sin_family = AF_INET; - server_address.sin_addr.s_addr = INADDR_ANY; - server_address.sin_port = htons(std::stoi(params_.fp_output.port)); - server_address.sin_addr.s_addr = inet_addr(params_.fp_output.ip.c_str()); + uint8_t payload[parser::fpb::FP_B_MEASUREMENTS_HEAD_SIZE + + (parser::fpb::FP_B_MEASUREMENTS_MAX_NUM_MEAS * parser::fpb::FP_B_MEASUREMENTS_MEAS_SIZE)]; + const std::size_t payload_size = parser::fpb::FP_B_MEASUREMENTS_HEAD_SIZE + + (sensor_measurements.size() * parser::fpb::FP_B_MEASUREMENTS_MEAS_SIZE); - connection_status_ = connect(client_fd_, (struct sockaddr*)&server_address, sizeof server_address); + parser::fpb::FpbMeasurementsHead meas_header; + meas_header.version = parser::fpb::FP_B_MEASUREMENTS_V1; + meas_header.num_meas = num_meas; + std::memcpy(&payload[0], &meas_header, sizeof(meas_header)); - if (connection_status_ != 0) { - std::cerr << "Error on connection of TCP socket: " << strerror(errno) << "\n"; - return false; + for (std::size_t i = 0; i < num_meas; i++) { + memcpy(&payload[parser::fpb::FP_B_MEASUREMENTS_HEAD_SIZE + (parser::fpb::FP_B_MEASUREMENTS_MEAS_SIZE * i)], + &sensor_measurements[i], sizeof(sensor_measurements[i])); } - return true; -} -bool FixpositionDriver::CreateSerialConnection() { - if (client_fd_ != -1) { - std::cerr << "Serial connection already exists.\n"; - return true; + std::vector message; + if (parser::fpb::FpbMakeMessage(message, parser::fpb::FP_B_MEASUREMENTS_MSGID, 0, payload, payload_size)) { + if (!Write(message.data(), message.size())) { + WARNING_THR(1000, "Failed sending FP_B-MEASUREMENTS message"); + } + } else { + WARNING_THR(1000, "Failed making FP_B-MEASUREMENTS message"); } +} - client_fd_ = open(params_.fp_output.port.c_str(), O_RDWR | O_NOCTTY); - - struct termios options; - speed_t speed; - - switch (params_.fp_output.baudrate) { - case 9600: - speed = B9600; - break; - - case 38400: - speed = B38400; - break; - - case 57600: - speed = B57600; - break; - - case 115200: - speed = B115200; - break; - - case 230400: - speed = B230400; - break; - - case 460800: - speed = B460800; - break; - - case 500000: - speed = B500000; - break; - - case 921600: - speed = B921600; - break; - - case 1000000: - speed = B1000000; - break; - - default: - speed = B115200; - std::cerr << "Unsupported baudrate: " << params_.fp_output.baudrate - << "\n\tsupported examples:\n\t9600, " - "19200, " - "38400, " - "57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n"; - } +void FixpositionDriver::RtcmCallback(const uint8_t* rtcm_msg, const size_t msg_size) { + // TODO: Check that RTCM message is valid - if (client_fd_ == -1) { - // Could not open the port. - std::cerr << "Failed to open serial port " << strerror(errno) << "\n"; - return false; - } else { - // Get current serial port options: - tcgetattr(client_fd_, &options); - options_save_ = options; - char speed_buf[10]; - snprintf(speed_buf, sizeof(speed_buf), "0%06o", (int)cfgetispeed(&options)); - - options.c_iflag &= ~(IXOFF | IXON | ICRNL); - options.c_oflag &= ~(OPOST | ONLCR); - options.c_lflag &= ~(ISIG | ICANON | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | IEXTEN); - options.c_cc[VEOL] = 0; - options.c_cc[VMIN] = 0; - options.c_cc[VTIME] = 50; - - cfsetospeed(&options, speed); /* baud rate */ - tcsetattr(client_fd_, TCSANOW, &options); - connection_status_ = 0; // not used for serial, set to 0 (success) - return true; + if (!Write(rtcm_msg, msg_size)) { + WARNING_THR(1000, "Failed sending XXX message"); } } + +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/gnss_tf.cpp b/fixposition_driver_lib/src/gnss_tf.cpp deleted file mode 100644 index 309d32a..0000000 --- a/fixposition_driver_lib/src/gnss_tf.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/** - * @file - * @brief Helper functions - * - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * \endverbatim - * - */ - -/* PACKAGE */ -#include - -namespace fixposition { - -namespace Constants { -// static constexpr values representing the earth WGS84 -// https://en.wikipedia.org/wiki/World_Geodetic_System#Defining_Parameters -static constexpr double wgs84_a = 6378137.0; //!< earth radius major axis [m] -static constexpr double wgs84_b = 6356752.314245; //!< earth radius minor axis [m] -static constexpr double wgs84_inv_f = 298.257223563; //!< 1/f inverse of flattening parameter -static constexpr double wgs84_e_2 = 6.69437999014e-3; //!< first eccentricity Squared - -static constexpr double wgs84_a_2 = wgs84_a * wgs84_a; //!< a_^2 -static constexpr double wgs84_b_2 = wgs84_b * wgs84_b; //!< a_^2 -static constexpr double wgs84_e_prime_2 = wgs84_a_2 / wgs84_b_2 - 1; //!< e'^2 second eccentricity squared -} // namespace Constants - -/** - * @details - * - * \f[ - * - * \text{Rotate a vector from ECEF to ENU:} \\ - * V_{ENU} = M \cdot V_{ECEF} \\ - * \text{Rotate a covariance matrix from ECEF to ENU:} \\ - * Cov_{ENU} = M \cdot Cov_{ECEF} \cdot M^{T} \\ - * - * \f] - * - * Reference https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates - * - */ -Eigen::Matrix3d RotEnuEcef(double lat, double lon) { - const double s_lon = sin(lon); - const double c_lon = cos(lon); - const double s_lat = sin(lat); - const double c_lat = cos(lat); - const double m00 = -s_lon; - const double m01 = c_lon; - const double m02 = 0; - const double m10 = -c_lon * s_lat; - const double m11 = -s_lon * s_lat; - const double m12 = c_lat; - const double m20 = c_lon * c_lat; - const double m21 = s_lon * c_lat; - const double m22 = s_lat; - - Eigen::Matrix3d res; - // This initializes the matrix row by row - res << m00, m01, m02, m10, m11, m12, m20, m21, m22; - - return res; -} - -/** - * @details - * - * \f[ - * - * \text{Rotate a vector from ECEF to ENU:} \\ - * V_{ENU} = M \cdot V_{ECEF} \\ - * \text{Rotate a covariance matrix from ECEF to ENU:} \\ - * Cov_{ENU} = M \cdot Cov_{ECEF} \cdot M^{T} \\ - * - * \f] - * - */ -Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d &ecef) { - const Eigen::Vector3d wgs84llh = TfWgs84LlhEcef(ecef); - return RotEnuEcef(wgs84llh.x(), wgs84llh.y()); -} - -Eigen::Matrix3d RotNedEnu() { - /** - * | 0, 1, 0 | - * | 1, 0, 0 | - * | 0, 0,-1 | - * - */ - - Eigen::Matrix3d rot_ned_enu; - rot_ned_enu << 0, 1, 0, 1, 0, 0, 0, 0, -1; - return rot_ned_enu; -} - -Eigen::Matrix3d RotNedEcef(double lat, double lon) { return RotNedEnu() * RotEnuEcef(lat, lon); } - -Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d &ecef) { - const Eigen::Vector3d wgs84llh = TfWgs84LlhEcef(ecef); - return RotNedEcef(wgs84llh.x(), wgs84llh.y()); -} - -Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref) { - const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); - return RotEnuEcef(wgs84llh_ref.x(), wgs84llh_ref.y()) * (ecef - ecef_ref); -} - -Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref) { - const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); - return ecef_ref + RotEnuEcef(wgs84llh_ref.x(), wgs84llh_ref.y()).transpose() * enu; -} - -Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref) { - const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); - return RotNedEcef(wgs84llh_ref.x(), wgs84llh_ref.y()) * (ecef - ecef_ref); -} - -Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref) { - const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); - return ecef_ref + RotNedEcef(wgs84llh_ref.x(), wgs84llh_ref.y()).transpose() * ned; -} -/** - * @details Implementation based on paper - * https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution - * - */ -Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef) { - const double x = ecef.x(); - const double y = ecef.y(); - const double z = ecef.z(); - - const double x_2 = x * x; //!< x^2 - const double y_2 = y * y; //!< y^2 - const double z_2 = z * z; //!< z^2 - - const double r_2 = (x_2 + y_2); //!< r^2 - const double r = sqrt(r_2); //!< r - - const double F = 54.0 * Constants::wgs84_b_2 * z_2; - const double G = - r_2 + (1 - Constants::wgs84_e_2) * z_2 - Constants::wgs84_e_2 * (Constants::wgs84_a_2 - Constants::wgs84_b_2); - const double c = Constants::wgs84_e_2 * Constants::wgs84_e_2 * F * r_2 / (G * G * G); - const double s = cbrt(1 + c + sqrt(c * c + 2 * c)); - const double P = F / (3.0 * (s + 1.0 + 1.0 / s) * (s + 1.0 + 1.0 / s) * G * G); - const double Q = sqrt(1 + 2 * Constants::wgs84_e_2 * Constants::wgs84_e_2 * P); - const double r0 = -P * Constants::wgs84_e_2 * r / (1 + Q) + - sqrt(0.5 * Constants::wgs84_a_2 * (1.0 + 1.0 / Q) - - (P * (1 - Constants::wgs84_e_2) * z_2 / (Q + Q * Q)) - 0.5 * P * r_2); - const double t1 = (r - Constants::wgs84_e_2 * r0); - const double t1_2 = t1 * t1; - const double U = sqrt(t1_2 + z_2); - const double V = sqrt(t1_2 + (1 - Constants::wgs84_e_2) * z_2); - const double a_V = Constants::wgs84_a * V; - const double z0 = Constants::wgs84_b_2 * z / a_V; - - const double h = U * (1 - Constants::wgs84_b_2 / a_V); - const double lat = atan((z + Constants::wgs84_e_prime_2 * z0) / r); - const double lon = atan2(y, x); - - return Eigen::Vector3d(lat, lon, h); -} - -Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh) { - const double lat = wgs84llh.x(); - const double lon = wgs84llh.y(); - const double height = wgs84llh.z(); - const double s_lat = sin(lat); - const double c_lat = cos(lat); - const double s_lon = sin(lon); - const double c_lon = cos(lon); - // N is in meters - const double n = Constants::wgs84_a / sqrt(1.0 - Constants::wgs84_e_2 * s_lat * s_lat); - const double n_plus_height = n + height; - - Eigen::Vector3d ecef; - ecef.x() = n_plus_height * c_lat * c_lon; - ecef.y() = n_plus_height * c_lat * s_lon; - ecef.z() = (n * (1 - Constants::wgs84_e_2) + height) * s_lat; - - return ecef; -} - -Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r) { - //! Rotation Matrix to convert to ENU frame - const Eigen::Matrix3d rot_enu_ecef = RotEnuEcef(ecef_p); - //! Convert the Pose into ENU frame - const Eigen::Matrix3d rot_enu_body = rot_enu_ecef * ecef_r; - //! Convert Rotation Matrix into Yaw-Pitch-Roll - return RotToEul(rot_enu_body); -} - -} // namespace fixposition diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 222c802..9986c3d 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -1,52 +1,55 @@ /** - * @file - * @brief Helper functions - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Helper functions */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/helper.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common; static constexpr char kNmeaPreamble = '$'; static constexpr int kLibParserMaxNmeaSize = 400; -void SplitMessage(std::vector& tokens, const std::string& msg, const std::string& delim) { - boost::split(tokens, msg, boost::is_any_of(delim)); -} - -void BestGnssPosToNavSatFix(const Oem7MessageHeaderMem* const header, const BESTGNSSPOSMem* const bestgnsspos, - NavSatFixData& navsatfix) { +void BestGnssPosToNavSatFix(const parser::novb::NovbLongHeader& header, + const parser::novb::NovbBestgnsspos& bestgnsspos, NavSatFixData& navsatfix) { // Header timestamp - navsatfix.stamp.wno = header->gps_week; - navsatfix.stamp.tow = header->gps_milliseconds * 1e-3; + navsatfix.stamp.wno = header.gps_week; + navsatfix.stamp.tow = header.gps_milliseconds * 1e-3; // Data - navsatfix.latitude = bestgnsspos->lat; - navsatfix.longitude = bestgnsspos->lon; - navsatfix.altitude = bestgnsspos->hgt; + navsatfix.latitude = bestgnsspos.lat; + navsatfix.longitude = bestgnsspos.lon; + navsatfix.altitude = bestgnsspos.hgt; - Eigen::Array3d cov_diag(bestgnsspos->lat_stdev, bestgnsspos->lon_stdev, bestgnsspos->hgt_stdev); + Eigen::Array3d cov_diag(bestgnsspos.lat_stdev, bestgnsspos.lon_stdev, bestgnsspos.hgt_stdev); navsatfix.cov = (cov_diag * cov_diag).matrix().asDiagonal(); navsatfix.position_covariance_type = 2; - switch (static_cast(bestgnsspos->pos_type)) { - case PositionOrVelocityType::NARROW_INT: + switch (static_cast(bestgnsspos.pos_type)) { + case parser::novb::NovbPosOrVelType::NARROW_INT: navsatfix.status.status = static_cast(NavSatStatusData::Status::STATUS_GBAS_FIX); break; - case PositionOrVelocityType::NARROW_FLOAT: - case PositionOrVelocityType::SINGLE: + case parser::novb::NovbPosOrVelType::NARROW_FLOAT: + case parser::novb::NovbPosOrVelType::SINGLE: navsatfix.status.status = static_cast(NavSatStatusData::Status::STATUS_FIX); break; default: @@ -60,25 +63,51 @@ void BestGnssPosToNavSatFix(const Oem7MessageHeaderMem* const header, const BEST navsatfix.status.service |= 4; navsatfix.status.service |= 8; - switch (static_cast(header->message_type & static_cast(MessageTypeSource::_MASK))) { - case MessageTypeSource::PRIMARY: + switch (static_cast(header.message_type & + types::EnumToVal(parser::novb::NovbMsgTypeSource::_MASK))) { + case parser::novb::NovbMsgTypeSource::PRIMARY: navsatfix.frame_id = "GNSS1"; break; - case MessageTypeSource::SECONDARY: + case parser::novb::NovbMsgTypeSource::SECONDARY: navsatfix.frame_id = "GNSS2"; break; - case MessageTypeSource::_MASK: - navsatfix.frame_id = "GNSS"; - break; default: navsatfix.frame_id = "GNSS"; } } -template <> -void NovToData(const Oem7MessageHeaderMem* const header, const BESTGNSSPOSMem* const nov, - NavSatFixData& data) { - BestGnssPosToNavSatFix(header, nov, data); +bool FillWsSensorMeas(const std::vector>& meas_vec, + const parser::fpb::FpbMeasurementsMeasLoc meas_loc, parser::fpb::FpbMeasurementsMeas& meas_fpb) { + const size_t num_axis = meas_vec.size(); + if (num_axis != 3) { + WARNING("Wheelspeed sensor has an invalid number of measurements"); + return false; + } + meas_fpb.meas_type = types::EnumToVal(parser::fpb::FpbMeasurementsMeasType::VELOCITY); + meas_fpb.meas_loc = types::EnumToVal(meas_loc); + // In the current setup, the sensor will handle the timestamping as time of arrival. + meas_fpb.timestamp_type = types::EnumToVal(parser::fpb::FpbMeasurementsTimestampType::TIMEOFARRIVAL); + meas_fpb.gps_wno = 0; + meas_fpb.gps_tow = 0; + meas_fpb.meas_x_valid = meas_vec[0].first; + meas_fpb.meas_x = meas_vec[0].second; + meas_fpb.meas_y_valid = meas_vec[1].first; + meas_fpb.meas_y = meas_vec[1].second; + meas_fpb.meas_z_valid = meas_vec[2].first; + meas_fpb.meas_z = meas_vec[2].second; + return true; +} + +parser::fpb::FpbMeasurementsMeasLoc WsMeasStringToLoc(const std::string& meas_loc) { + // clang-format off + if (meas_loc == "RC") { return parser::fpb::FpbMeasurementsMeasLoc::RC; } + else if (meas_loc == "FR") { return parser::fpb::FpbMeasurementsMeasLoc::FR; } + else if (meas_loc == "FL") { return parser::fpb::FpbMeasurementsMeasLoc::FL; } + else if (meas_loc == "RR") { return parser::fpb::FpbMeasurementsMeasLoc::RR; } + else if (meas_loc == "RL") { return parser::fpb::FpbMeasurementsMeasLoc::RL; } + else { return parser::fpb::FpbMeasurementsMeasLoc::UNSPECIFIED; } + // clang-format on } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/corrimu.cpp b/fixposition_driver_lib/src/messages/fpa/corrimu.cpp index 1ce1398..73c962b 100644 --- a/fixposition_driver_lib/src/messages/fpa/corrimu.cpp +++ b/fixposition_driver_lib/src/messages/fpa/corrimu.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-CORRIMU parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-CORRIMU parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -34,7 +39,7 @@ void FP_CORRIMU::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-CORRIMU string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-CORRIMU string with " << tokens.size() << " fields"); } else { // If size is ok, check version @@ -43,7 +48,7 @@ void FP_CORRIMU::ConvertFromTokens(const std::vector& tokens) { ok = version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-CORRIMU string with version " << version << "!\n"; + WARNING_S("Error in parsing FP_A-CORRIMU string with version " << version); } } @@ -52,7 +57,7 @@ void FP_CORRIMU::ConvertFromTokens(const std::vector& tokens) { ResetData(); return; } - + // Populate fields imu.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); imu.linear_acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx)); @@ -60,4 +65,5 @@ void FP_CORRIMU::ConvertFromTokens(const std::vector& tokens) { imu.frame_id = "FP_VRTK"; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/eoe.cpp b/fixposition_driver_lib/src/messages/fpa/eoe.cpp index e1bda7f..24ae273 100644 --- a/fixposition_driver_lib/src/messages/fpa/eoe.cpp +++ b/fixposition_driver_lib/src/messages/fpa/eoe.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-EOE parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-EOE parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -29,7 +34,7 @@ void FP_EOE::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-EOE string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-EOE string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -37,7 +42,7 @@ void FP_EOE::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-EOE string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-EOE string with version " << _version << ""); } } @@ -54,4 +59,5 @@ void FP_EOE::ConvertFromTokens(const std::vector& tokens) { epoch = tokens.at(epoch_idx); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/gnssant.cpp b/fixposition_driver_lib/src/messages/fpa/gnssant.cpp index cb07c17..10b5f6b 100644 --- a/fixposition_driver_lib/src/messages/fpa/gnssant.cpp +++ b/fixposition_driver_lib/src/messages/fpa/gnssant.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-GNSSANT parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-GNSSANT parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -34,7 +39,7 @@ void FP_GNSSANT::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-GNSSANT string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-GNSSANT string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -42,7 +47,7 @@ void FP_GNSSANT::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-GNSSANT string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-GNSSANT string with version " << _version << ""); } } @@ -58,10 +63,11 @@ void FP_GNSSANT::ConvertFromTokens(const std::vector& tokens) { // GNSS status data gnss1_state = tokens.at(gnss1_state_idx); gnss1_power = tokens.at(gnss1_power_idx); - gnss1_age = StringToInt(tokens.at(gnss1_age_idx)); + gnss1_age = StringToInt(tokens.at(gnss1_age_idx)); gnss2_state = tokens.at(gnss2_state_idx); gnss2_power = tokens.at(gnss2_power_idx); - gnss2_age = StringToInt(tokens.at(gnss2_age_idx)); + gnss2_age = StringToInt(tokens.at(gnss2_age_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp b/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp index 1d8a06d..d3b2434 100644 --- a/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp +++ b/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-GNSSCORR parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-GNSSCORR parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -43,7 +48,7 @@ void FP_GNSSCORR::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-GNSSCORR string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-GNSSCORR string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -51,7 +56,7 @@ void FP_GNSSCORR::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-GNSSCORR string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-GNSSCORR string with version " << _version << ""); } } @@ -65,21 +70,22 @@ void FP_GNSSCORR::ConvertFromTokens(const std::vector& tokens) { stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); // GNSS status data - gnss1_fix = ParseStatusFlag(tokens, gnss1_fix_idx); + gnss1_fix = ParseStatusFlag(tokens, gnss1_fix_idx); gnss1_nsig_l1 = StringToInt(tokens.at(gnss1_nsig_l1_idx)); gnss1_nsig_l2 = StringToInt(tokens.at(gnss1_nsig_l2_idx)); - gnss2_fix = ParseStatusFlag(tokens, gnss2_fix_idx); + gnss2_fix = ParseStatusFlag(tokens, gnss2_fix_idx); gnss2_nsig_l1 = StringToInt(tokens.at(gnss2_nsig_l1_idx)); gnss2_nsig_l2 = StringToInt(tokens.at(gnss2_nsig_l2_idx)); // Correction data status - corr_latency = StringToDouble(tokens.at(corr_latency_idx)); + corr_latency = StringToDouble(tokens.at(corr_latency_idx)); corr_update_rate = StringToDouble(tokens.at(corr_update_rate_idx)); - corr_data_rate = StringToDouble(tokens.at(corr_data_rate_idx)); - corr_msg_rate = StringToDouble(tokens.at(corr_msg_rate_idx)); - sta_id = StringToInt(tokens.at(sta_id_idx)); - sta_llh = Vector3ToEigen(tokens.at(sta_lat_idx), tokens.at(sta_lon_idx), tokens.at(sta_height_idx)); - sta_dist = StringToInt(tokens.at(sta_dist_idx)); + corr_data_rate = StringToDouble(tokens.at(corr_data_rate_idx)); + corr_msg_rate = StringToDouble(tokens.at(corr_msg_rate_idx)); + sta_id = StringToInt(tokens.at(sta_id_idx)); + sta_llh = Vector3ToEigen(tokens.at(sta_lat_idx), tokens.at(sta_lon_idx), tokens.at(sta_height_idx)); + sta_dist = StringToInt(tokens.at(sta_dist_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/imubias.cpp b/fixposition_driver_lib/src/messages/fpa/imubias.cpp index 66c4ad0..e7f4c63 100644 --- a/fixposition_driver_lib/src/messages/fpa/imubias.cpp +++ b/fixposition_driver_lib/src/messages/fpa/imubias.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-IMUBIAS parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-IMUBIAS parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -44,7 +49,7 @@ void FP_IMUBIAS::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-IMUBIAS string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-IMUBIAS string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -52,7 +57,7 @@ void FP_IMUBIAS::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-IMUBIAS string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-IMUBIAS string with version " << _version << ""); } } @@ -73,8 +78,11 @@ void FP_IMUBIAS::ConvertFromTokens(const std::vector& tokens) { imu_conv = ParseStatusFlag(tokens, imu_conv_idx); bias_acc = Vector3ToEigen(tokens.at(bias_acc_x_idx), tokens.at(bias_acc_y_idx), tokens.at(bias_acc_z_idx)); bias_gyr = Vector3ToEigen(tokens.at(bias_gyr_x_idx), tokens.at(bias_gyr_y_idx), tokens.at(bias_gyr_z_idx)); - bias_cov_acc = Vector3ToEigen(tokens.at(bias_cov_acc_x_idx), tokens.at(bias_cov_acc_y_idx), tokens.at(bias_cov_acc_z_idx)); - bias_cov_gyr = Vector3ToEigen(tokens.at(bias_cov_gyr_x_idx), tokens.at(bias_cov_gyr_y_idx), tokens.at(bias_cov_gyr_z_idx)); + bias_cov_acc = + Vector3ToEigen(tokens.at(bias_cov_acc_x_idx), tokens.at(bias_cov_acc_y_idx), tokens.at(bias_cov_acc_z_idx)); + bias_cov_gyr = + Vector3ToEigen(tokens.at(bias_cov_gyr_x_idx), tokens.at(bias_cov_gyr_y_idx), tokens.at(bias_cov_gyr_z_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/llh.cpp b/fixposition_driver_lib/src/messages/fpa/llh.cpp index 6766799..aea507c 100644 --- a/fixposition_driver_lib/src/messages/fpa/llh.cpp +++ b/fixposition_driver_lib/src/messages/fpa/llh.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-LLH parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-LLH parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -37,7 +42,7 @@ void FP_LLH::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-LLH string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-LLH string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -45,7 +50,7 @@ void FP_LLH::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-LLH string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-LLH string with version " << _version << ""); } } @@ -60,11 +65,12 @@ void FP_LLH::ConvertFromTokens(const std::vector& tokens) { // LLH position llh = Vector3ToEigen(tokens.at(latitude_idx), tokens.at(longitude_idx), tokens.at(height_idx)); - + // Covariance cov = BuildCovMat3D(StringToDouble(tokens.at(pos_cov_ee_idx)), StringToDouble(tokens.at(pos_cov_nn_idx)), StringToDouble(tokens.at(pos_cov_uu_idx)), StringToDouble(tokens.at(pos_cov_en_idx)), StringToDouble(tokens.at(pos_cov_nu_idx)), StringToDouble(tokens.at(pos_cov_eu_idx))); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/odomenu.cpp b/fixposition_driver_lib/src/messages/fpa/odomenu.cpp index 8aa45c8..f1a2069 100644 --- a/fixposition_driver_lib/src/messages/fpa/odomenu.cpp +++ b/fixposition_driver_lib/src/messages/fpa/odomenu.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-ODOMENU parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-ODOMENU parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -67,7 +72,7 @@ void FP_ODOMENU::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-ODOMENU string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-ODOMENU string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -75,7 +80,7 @@ void FP_ODOMENU::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-ODOMENU string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-ODOMENU string with version " << _version << ""); } } @@ -136,4 +141,5 @@ void FP_ODOMENU::ConvertFromTokens(const std::vector& tokens) { } } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/odometry.cpp b/fixposition_driver_lib/src/messages/fpa/odometry.cpp index a86ce89..e8e3123 100644 --- a/fixposition_driver_lib/src/messages/fpa/odometry.cpp +++ b/fixposition_driver_lib/src/messages/fpa/odometry.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-ODOMETRY parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-ODOMETRY parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -68,7 +73,7 @@ void FP_ODOMETRY::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-ODOMETRY string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-ODOMETRY string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -76,7 +81,7 @@ void FP_ODOMETRY::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-ODOMETRY string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-ODOMETRY string with version " << _version << ""); } } @@ -138,4 +143,5 @@ void FP_ODOMETRY::ConvertFromTokens(const std::vector& tokens) { } } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/odomsh.cpp b/fixposition_driver_lib/src/messages/fpa/odomsh.cpp index d3a151e..e2939ff 100644 --- a/fixposition_driver_lib/src/messages/fpa/odomsh.cpp +++ b/fixposition_driver_lib/src/messages/fpa/odomsh.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-ODOMSH parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-ODOMSH parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -67,7 +72,7 @@ void FP_ODOMSH::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-ODOMSH string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-ODOMSH string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -75,7 +80,7 @@ void FP_ODOMSH::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-ODOMSH string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-ODOMSH string with version " << _version << ""); } } @@ -136,4 +141,5 @@ void FP_ODOMSH::ConvertFromTokens(const std::vector& tokens) { } } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp b/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp index 0baa931..6b0d821 100644 --- a/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp +++ b/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-ODOMSTATUS parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-ODOMSTATUS parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -64,7 +69,7 @@ void FP_ODOMSTATUS::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-ODOMSTATUS string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-ODOMSTATUS string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -72,7 +77,7 @@ void FP_ODOMSTATUS::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-ODOMSTATUS string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-ODOMSTATUS string with version " << _version << ""); } } @@ -108,4 +113,5 @@ void FP_ODOMSTATUS::ConvertFromTokens(const std::vector& tokens) { markers_conv = ParseStatusFlag(tokens, markers_conv_idx); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/rawimu.cpp b/fixposition_driver_lib/src/messages/fpa/rawimu.cpp index 8d324c1..61c540d 100644 --- a/fixposition_driver_lib/src/messages/fpa/rawimu.cpp +++ b/fixposition_driver_lib/src/messages/fpa/rawimu.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-RAWIMU parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-RAWIMU parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -34,7 +39,7 @@ void FP_RAWIMU::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-RAWIMU string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-RAWIMU string with " << tokens.size() << " fields"); } else { // If size is ok, check version @@ -43,7 +48,7 @@ void FP_RAWIMU::ConvertFromTokens(const std::vector& tokens) { ok = version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-RAWIMU string with version " << version << "!\n"; + WARNING_S("Error in parsing FP_A-RAWIMU string with version " << version << ""); } } @@ -52,7 +57,7 @@ void FP_RAWIMU::ConvertFromTokens(const std::vector& tokens) { ResetData(); return; } - + // Populate fields imu.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); imu.linear_acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx)); @@ -60,4 +65,5 @@ void FP_RAWIMU::ConvertFromTokens(const std::vector& tokens) { imu.frame_id = "FP_VRTK"; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/text.cpp b/fixposition_driver_lib/src/messages/fpa/text.cpp index e050e98..bc92ce6 100644 --- a/fixposition_driver_lib/src/messages/fpa/text.cpp +++ b/fixposition_driver_lib/src/messages/fpa/text.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-TEXT parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-TEXT parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -28,7 +33,7 @@ void FP_TEXT::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-TEXT string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-TEXT string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int version = std::stoi(tokens.at(msg_version_idx)); @@ -36,7 +41,7 @@ void FP_TEXT::ConvertFromTokens(const std::vector& tokens) { ok = version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-TEXT string with version " << version << "!\n"; + WARNING_S("Error in parsing FP_A-TEXT string with version " << version << ""); } } @@ -48,7 +53,8 @@ void FP_TEXT::ConvertFromTokens(const std::vector& tokens) { // Populate fields level = tokens.at(level_idx); - text = tokens.at(text_idx); + text = tokens.at(text_idx); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/tf.cpp b/fixposition_driver_lib/src/messages/fpa/tf.cpp index d0bb1e2..650d536 100644 --- a/fixposition_driver_lib/src/messages/fpa/tf.cpp +++ b/fixposition_driver_lib/src/messages/fpa/tf.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-TF parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-TF parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -37,7 +42,7 @@ void FP_TF::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-TF string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-TF string with " << tokens.size() << " fields"); } else { // If size is ok, check version @@ -46,7 +51,7 @@ void FP_TF::ConvertFromTokens(const std::vector& tokens) { ok = version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-TF string with version " << version << "!\n"; + WARNING_S("Error in parsing FP_A-TF string with version " << version << ""); } } @@ -61,9 +66,8 @@ void FP_TF::ConvertFromTokens(const std::vector& tokens) { tf.frame_id = "FP_" + tokens.at(from_frame_idx); tf.child_frame_id = "FP_" + tokens.at(to_frame_idx); - tf.translation = Vector3ToEigen(tokens.at(translation_x_idx), - tokens.at(translation_y_idx), - tokens.at(translation_z_idx)); + tf.translation = + Vector3ToEigen(tokens.at(translation_x_idx), tokens.at(translation_y_idx), tokens.at(translation_z_idx)); tf.rotation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx), tokens.at(orientation_y_idx), tokens.at(orientation_z_idx)); @@ -71,4 +75,5 @@ void FP_TF::ConvertFromTokens(const std::vector& tokens) { valid_tf = !(tf.rotation.w() == 0 && tf.rotation.vec().isZero()); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/fpa/tp.cpp b/fixposition_driver_lib/src/messages/fpa/tp.cpp index af29cfd..96286eb 100644 --- a/fixposition_driver_lib/src/messages/fpa/tp.cpp +++ b/fixposition_driver_lib/src/messages/fpa/tp.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of FP_A-TP parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of FP_A-TP parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/fpa_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int msg_type_idx = 1; @@ -32,7 +37,7 @@ void FP_TP::ConvertFromTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { // Size is wrong - std::cout << "Error in parsing FP_A-TP string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing FP_A-TP string with " << tokens.size() << " fields"); } else { // If size is ok, check version const int _version = std::stoi(tokens.at(msg_version_idx)); @@ -40,7 +45,7 @@ void FP_TP::ConvertFromTokens(const std::vector& tokens) { ok = _version == kVersion_; if (!ok) { // Version is wrong - std::cout << "Error in parsing FP_A-TP string with version " << _version << "!\n"; + WARNING_S("Error in parsing FP_A-TP string with version " << _version << ""); } } @@ -59,4 +64,5 @@ void FP_TP::ConvertFromTokens(const std::vector& tokens) { gps_leaps = StringToInt(tokens.at(gps_leaps_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gngsa.cpp b/fixposition_driver_lib/src/messages/nmea/gngsa.cpp index ad69ce4..ce640a5 100644 --- a/fixposition_driver_lib/src/messages/nmea/gngsa.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gngsa.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of NMEA-GN-GSA parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GN-GSA parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int mode_op_idx = 1; @@ -31,11 +36,11 @@ void GN_GSA::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GN-GSA string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GN-GSA string with " << tokens.size() << " fields"); ResetData(); return; } - + // Populate mode mode_op = StringToChar(tokens.at(mode_op_idx)); mode_nav = StringToInt(tokens.at(mode_nav_idx)); @@ -44,7 +49,7 @@ void GN_GSA::ConvertFromTokens(const std::vector& tokens) { ids.clear(); for (unsigned int offset = 0; offset < 11; offset++) { int value = StringToInt(tokens.at(ids_idx + offset)); - + if (value != 0) { ids.push_back(value); } else { @@ -61,4 +66,5 @@ void GN_GSA::ConvertFromTokens(const std::vector& tokens) { gnss_id = StringToInt(tokens.at(gnss_id_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gpgga.cpp b/fixposition_driver_lib/src/messages/nmea/gpgga.cpp index 242ad10..4ddd4d8 100644 --- a/fixposition_driver_lib/src/messages/nmea/gpgga.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gpgga.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of NMEA-GP-GGA parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-GGA parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int time_idx = 1; @@ -49,7 +54,7 @@ void GP_GGA::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-GGA string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-GGA string with " << tokens.size() << " fields"); ResetData(); return; } @@ -64,23 +69,23 @@ void GP_GGA::ConvertFromTokens(const std::vector& tokens) { const std::string _lonstr = tokens.at(lon_idx); if (!_latstr.empty()) { - _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + _lat = StringToDouble(_latstr.substr(0, 2)) + StringToDouble((_latstr.substr(2))) / 60; if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; } if (!_lonstr.empty()) { - _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + _lon = StringToDouble(_lonstr.substr(0, 3)) + StringToDouble((_lonstr.substr(3))) / 60; if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; } llh = Eigen::Vector3d(_lat, _lon, StringToDouble(tokens.at(alt_idx))); // LLH indicators - lat_ns = StringToChar(tokens.at(lat_ns_idx)); - lon_ew = StringToChar(tokens.at(lon_ew_idx)); + lat_ns = StringToChar(tokens.at(lat_ns_idx)); + lon_ew = StringToChar(tokens.at(lon_ew_idx)); alt_unit = StringToChar(tokens.at(alt_unit_idx)); - quality = ParseStatusFlag(tokens, quality_idx); - num_sv = StringToInt(tokens.at(num_sv_idx)); + quality = ParseStatusFlag(tokens, quality_idx); + num_sv = StringToInt(tokens.at(num_sv_idx)); // Dilution of precision hdop = StringToDouble(tokens.at(hdop_idx)); @@ -93,4 +98,5 @@ void GP_GGA::ConvertFromTokens(const std::vector& tokens) { sentence = join(tokens, ','); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gpgll.cpp b/fixposition_driver_lib/src/messages/nmea/gpgll.cpp index 9adcb3b..7f8ca27 100644 --- a/fixposition_driver_lib/src/messages/nmea/gpgll.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gpgll.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of NMEA-GP-GLL parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-GLL parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int lat_idx = 1; @@ -31,7 +36,7 @@ void GP_GLL::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-GLL string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-GLL string with " << tokens.size() << " fields"); ResetData(); return; } @@ -46,12 +51,12 @@ void GP_GLL::ConvertFromTokens(const std::vector& tokens) { const std::string _lonstr = tokens.at(lon_idx); if (!_latstr.empty()) { - _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + _lat = StringToDouble(_latstr.substr(0, 2)) + StringToDouble((_latstr.substr(2))) / 60; if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; } if (!_lonstr.empty()) { - _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + _lon = StringToDouble(_lonstr.substr(0, 3)) + StringToDouble((_lonstr.substr(3))) / 60; if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; } @@ -61,7 +66,8 @@ void GP_GLL::ConvertFromTokens(const std::vector& tokens) { status = StringToChar(tokens.at(status_idx)); lat_ns = StringToChar(tokens.at(lat_ns_idx)); lon_ew = StringToChar(tokens.at(lon_ew_idx)); - mode = StringToChar(tokens.at(mode_idx)); + mode = StringToChar(tokens.at(mode_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gpgst.cpp b/fixposition_driver_lib/src/messages/nmea/gpgst.cpp index cdc6dd5..ba53311 100644 --- a/fixposition_driver_lib/src/messages/nmea/gpgst.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gpgst.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of NMEA-GP-GST parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-GST parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int time_idx = 1; @@ -32,7 +37,7 @@ void GP_GST::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-GST string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-GST string with " << tokens.size() << " fields"); ResetData(); return; } @@ -52,4 +57,5 @@ void GP_GST::ConvertFromTokens(const std::vector& tokens) { std_alt = StringToDouble(tokens.at(std_alt_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gphdt.cpp b/fixposition_driver_lib/src/messages/nmea/gphdt.cpp index 1f8bc91..ebbd42e 100644 --- a/fixposition_driver_lib/src/messages/nmea/gphdt.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gphdt.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of NMEA-GP-HDT parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-HDT parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int heading_idx = 1; @@ -26,7 +31,7 @@ void GP_HDT::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-HDT string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-HDT string with " << tokens.size() << " fields"); ResetData(); return; } @@ -36,4 +41,5 @@ void GP_HDT::ConvertFromTokens(const std::vector& tokens) { true_ind = StringToChar(tokens.at(true_ind_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gprmc.cpp b/fixposition_driver_lib/src/messages/nmea/gprmc.cpp index b119b03..b7894ed 100644 --- a/fixposition_driver_lib/src/messages/nmea/gprmc.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gprmc.cpp @@ -1,25 +1,28 @@ /** - * @file - * @brief Implementation of NMEA-GP-RMC parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-RMC parser */ +/* LIBC/STL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ // unit conversion constant -static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s +static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s /// msg field indices static constexpr int time_idx = 1; @@ -39,7 +42,7 @@ void GP_RMC::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-RMC string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-RMC string with " << tokens.size() << " fields"); ResetData(); return; } @@ -55,12 +58,12 @@ void GP_RMC::ConvertFromTokens(const std::vector& tokens) { const std::string _lonstr = tokens.at(lon_idx); if (!_latstr.empty()) { - _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + _lat = StringToDouble(_latstr.substr(0, 2)) + StringToDouble((_latstr.substr(2))) / 60; if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; } if (!_lonstr.empty()) { - _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + _lon = StringToDouble(_lonstr.substr(0, 3)) + StringToDouble((_lonstr.substr(3))) / 60; if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; } @@ -70,12 +73,13 @@ void GP_RMC::ConvertFromTokens(const std::vector& tokens) { status = StringToChar(tokens.at(status_idx)); lat_ns = StringToChar(tokens.at(lat_ns_idx)); lon_ew = StringToChar(tokens.at(lon_ew_idx)); - mode = StringToChar(tokens.at(mode_idx)); + mode = StringToChar(tokens.at(mode_idx)); // Speed and course over ground - speed = StringToDouble(tokens.at(speed_idx)); + speed = StringToDouble(tokens.at(speed_idx)); speed_ms = speed * knots_to_ms; - course = StringToDouble(tokens.at(course_idx)); + course = StringToDouble(tokens.at(course_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp b/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp index c7b296e..7db87f2 100644 --- a/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of NMEA-GP-VTG parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-VTG parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int cog_true_idx = 1; @@ -33,7 +38,7 @@ void GP_VTG::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-VTG string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-VTG string with " << tokens.size() << " fields"); ResetData(); return; } @@ -54,4 +59,5 @@ void GP_VTG::ConvertFromTokens(const std::vector& tokens) { mode = StringToChar(tokens.at(mode_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gpzda.cpp b/fixposition_driver_lib/src/messages/nmea/gpzda.cpp index ed65ee0..3b86a3a 100644 --- a/fixposition_driver_lib/src/messages/nmea/gpzda.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gpzda.cpp @@ -1,24 +1,29 @@ /** - * @file - * @brief Implementation of NMEA-GP-ZDA parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GP-ZDA parser */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ -/// msg field indices +// msg field indices static constexpr int time_idx = 1; static constexpr int day_idx = 2; static constexpr int month_idx = 3; @@ -30,7 +35,7 @@ void GP_ZDA::ConvertFromTokens(const std::vector& tokens) { // Check if message size is wrong bool ok = tokens.size() == kSize_; if (!ok) { - std::cout << "Error in parsing NMEA-GP-ZDA string with " << tokens.size() << " fields!\n"; + WARNING_S("Error in parsing NMEA-GP-ZDA string with " << tokens.size() << " fields"); ResetData(); return; } @@ -48,15 +53,17 @@ void GP_ZDA::ConvertFromTokens(const std::vector& tokens) { // Generate GPS timestamp if (!time_str.empty() && !date_str.empty()) { - std::string utcTimeString = date_str + " " + time_str.substr(0,2) + ":" + time_str.substr(2,2) + ":" + time_str.substr(4); + std::string utcTimeString = + date_str + " " + time_str.substr(0, 2) + ":" + time_str.substr(2, 2) + ":" + time_str.substr(4); std::string gps_tow, gps_week; times::convertToGPSTime(utcTimeString, gps_week, gps_tow); stamp = ConvertGpsTime(gps_week, gps_tow); } // Get local time - local_hr = StringToInt(tokens.at(local_hr_idx)); + local_hr = StringToInt(tokens.at(local_hr_idx)); local_min = StringToInt(tokens.at(local_min_idx)); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp b/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp index b80c2c4..8bdafe7 100644 --- a/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp @@ -1,22 +1,26 @@ /** - * @file - * @brief Implementation of NMEA-GX-GSV parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GX-GSV parser */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /// msg field indices static constexpr int sentences_idx = 1; @@ -26,12 +30,12 @@ static constexpr int sat_id_idx = 4; static constexpr int elev_idx = 5; static constexpr int azim_idx = 6; static constexpr int cno_idx = 7; -//static constexpr int signal_id_idx = 4 + (4*num_sats); +// static constexpr int signal_id_idx = 4 + (4*num_sats); void GX_GSV::ConvertFromTokens(const std::vector& tokens) { // Reset vectors ResetData(); - + // Populate fields sentences = StringToInt(tokens.at(sentences_idx)); sent_num = StringToInt(tokens.at(sent_num_idx)); @@ -39,11 +43,11 @@ void GX_GSV::ConvertFromTokens(const std::vector& tokens) { // Populate satellite information unsigned int offset = 0; - for (unsigned int i = 3; i < (tokens.size() - 2); i+=4) { - sat_id.push_back(StringToInt(tokens.at(sat_id_idx + 4*offset))); - elev.push_back(StringToInt(tokens.at(elev_idx + 4*offset))); - azim.push_back(StringToInt(tokens.at(azim_idx + 4*offset))); - cno.push_back(StringToInt(tokens.at(cno_idx + 4*offset))); + for (unsigned int i = 3; i < (tokens.size() - 2); i += 4) { + sat_id.push_back(StringToInt(tokens.at(sat_id_idx + 4 * offset))); + elev.push_back(StringToInt(tokens.at(elev_idx + 4 * offset))); + azim.push_back(StringToInt(tokens.at(azim_idx + 4 * offset))); + cno.push_back(StringToInt(tokens.at(cno_idx + 4 * offset))); offset++; } @@ -52,10 +56,11 @@ void GX_GSV::ConvertFromTokens(const std::vector& tokens) { signal_id = tokens.back(); if (constellation.size() >= 2) { - type = string2enum(constellation.substr(0,2)); + type = string2enum(constellation.substr(0, 2)); } else { type = SignalType::Invalid; } } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/nmea.cpp b/fixposition_driver_lib/src/messages/nmea/nmea.cpp index f56d2f9..93f1a55 100644 --- a/fixposition_driver_lib/src/messages/nmea/nmea.cpp +++ b/fixposition_driver_lib/src/messages/nmea/nmea.cpp @@ -1,22 +1,26 @@ /** - * @file - * @brief Implementation of NMEA-GX-GSV parser - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of NMEA-GX-GSV parser */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include -#include +#include "fixposition_driver_lib/messages/base_converter.hpp" +#include "fixposition_driver_lib/messages/nmea_type.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ void NmeaMessage::AddNmeaEpoch(const GP_GGA& msg) { // Populate time if empty @@ -24,7 +28,7 @@ void NmeaMessage::AddNmeaEpoch(const GP_GGA& msg) { time_str = msg.time_str; } gpgga_time_str = msg.time_str; - + // Populate LLH position llh = msg.llh; @@ -40,7 +44,7 @@ void NmeaMessage::AddNmeaEpoch(const GP_GGA& msg) { // Populate differential data information diff_age = msg.diff_age; diff_sta = msg.diff_sta; - + // Populate position covariance [m^2] cov(0, 0) = msg.hdop * msg.hdop; cov(1, 1) = msg.hdop * msg.hdop; @@ -55,7 +59,7 @@ void NmeaMessage::AddNmeaEpoch(const GP_GLL& msg) { if (time_str == "") { time_str = msg.time_str; } - + // Populate latitude and longitude if the vector is empty if (llh.isZero()) { llh(0) = msg.latlon(0); @@ -89,14 +93,18 @@ void NmeaMessage::AddNmeaEpoch(const GP_GST& msg) { std_alt = msg.std_alt; } -void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) { +void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) { // Populate GNSS signal stats for (u_int8_t i = 0; i < msg.sat_id.size(); i++) { - GnssSignalStats *stats = &gnss_signals[msg.type][msg.sat_id.at(i)]; + GnssSignalStats* stats = &gnss_signals[msg.type][msg.sat_id.at(i)]; // Populate necessary fields - if (stats->elev == 0) { stats->elev = msg.elev.at(i); } - if (stats->azim == 0) { stats->azim = msg.azim.at(i); } + if (stats->elev == 0) { + stats->elev = msg.elev.at(i); + } + if (stats->azim == 0) { + stats->azim = msg.azim.at(i); + } // Populate CNO if (msg.signal_id == "1" || msg.signal_id == "7") { @@ -115,7 +123,7 @@ void NmeaMessage::AddNmeaEpoch(const GP_HDT& msg) { void NmeaMessage::AddNmeaEpoch(const GP_VTG& msg) { // Populate SOG and COG if empty if (speed == 0.0) { - speed = msg.sog_kph / 3.6; // Convert km/h to m/s + speed = msg.sog_kph / 3.6; // Convert km/h to m/s } if (course == 0.0) { @@ -137,7 +145,7 @@ void NmeaMessage::AddNmeaEpoch(const GP_RMC& msg) { // Populate SOG and COG (priority) speed = msg.speed_ms; course = msg.course; - + // Populate latitude and longitude if the vector is empty if (llh.isZero()) { llh(0) = msg.latlon(0); @@ -152,4 +160,5 @@ void NmeaMessage::AddNmeaEpoch(const GP_ZDA& msg) { gpzda_time_str = msg.time_str; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_lib/src/params.cpp b/fixposition_driver_lib/src/params.cpp new file mode 100644 index 0000000..f78a265 --- /dev/null +++ b/fixposition_driver_lib/src/params.cpp @@ -0,0 +1,30 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Parameters + */ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ + +/* PACKAGE */ +#include "fixposition_driver_lib/params.hpp" + +namespace fixposition { +/* ****************************************************************************************************************** */ + +bool SensorParams::MessageEnabled(const std::string& message_name) const { + return std::find(messages_.cbegin(), messages_.cend(), message_name) != messages_.end(); +} + +/* ****************************************************************************************************************** */ +} // namespace fixposition diff --git a/fixposition_driver_lib/src/parser.cpp b/fixposition_driver_lib/src/parser.cpp deleted file mode 100644 index 73dfd38..0000000 --- a/fixposition_driver_lib/src/parser.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/** - * @file - * @brief Parser functions - * - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * \endverbatim - * - */ - -/* PACKAGE */ -#include -#include - -namespace fixposition { - -static constexpr char kNmeaPreamble = '$'; -static constexpr int kLibParserMaxNmeaSize = 400; -static constexpr int kLibParserMaxNovSize = 4096; - -int IsNmeaMessage(const char* buf, const int size) { - // Start of sentence - if (buf[0] != kNmeaPreamble) { - return 0; - } - - // Find end of sentence, calculate checksum along the way - int len = 1; // Length of sentence excl. "$" - char ck = 0; // checksum - while (true) { - if (len > kLibParserMaxNmeaSize) { - return 0; - } - if (len >= size) // len doesn't include '$' - { - return -1; - } - if ((buf[len] == '\r') || (buf[len] == '\n') || (buf[len] == '*')) { - break; - } - if ( // ((buf[len] & 0x80) != 0) || // 7-bit only - (buf[len] < 0x20) || (buf[len] > 0x7e) || // valid range - (buf[len] == '$') || (buf[len] == '\\') || (buf[len] == '!') || (buf[len] == '~')) // reserved - { - return 0; - } - ck ^= buf[len]; - len++; - } - - // Not nough data for sentence end (star + checksum + \r\n)? - if (size < (len + 1 + 2 + 2)) { - return -1; - } - - // Properly terminated sentence? - if ((buf[len] == '*') && (buf[len + 3] == '\r') && (buf[len + 4] == '\n')) { - char n1 = buf[len + 1]; - char n2 = buf[len + 2]; - char c1 = '0' + ((ck >> 4) & 0x0f); - char c2 = '0' + (ck & 0x0f); - if (c2 > '9') { - c2 += 'A' - '9' - 1; - } - // Checksum valid? - if ((n1 == c1) && (n2 == c2)) { - return len + 5; - } - } - return 0; -} - -int IsNovMessage(const uint8_t* buf, const int size) { - if (buf[0] != SYNC_CHAR_1) { - return 0; - } - - if (size < 3) { - return -1; - } - - if ((buf[1] != SYNC_CHAR_2) || ((buf[2] != SYNC_CHAR_3_LONG) && (buf[2] != SYNC_CHAR_3_SHORT))) { - return 0; - } - - // https://docs.novatel.com/OEM7/Content/Messages/Binary.htm - // https://docs.novatel.com/OEM7/Content/Messages/Description_of_Short_Headers.htm - // Offset Type Long header Short header - // 0 uint8_t 0xaa uint8_t 0xaa - // 1 uint8_t 0x44 uint8_t 0x44 - // 2 uint8_t 0x12 uint8_t 0x13 - // 3 uint8_t header len uint8_t msg len - // 4 uint16_t msg ID uint16_t msg ID - // 6 uint8_t msg type uint16_t wno - // 7 uint8_t port addr - // 8 uint16_t msg len int32_t tow - // ... - // 3+hlen payload offs 13 payload - - if (size < 12) { - return -1; - } - - int len = 0; - - // Long header - if (buf[2] == SYNC_CHAR_3_LONG) { - const uint8_t headerLen = buf[3]; - const uint16_t msgLen = ((uint16_t)buf[9] << 8) | (uint16_t)buf[8]; - len = headerLen + msgLen + sizeof(uint32_t); - } - // Short header - else { - const uint8_t headerLen = 12; - const uint8_t msgLen = buf[3]; - len = headerLen + msgLen + sizeof(uint32_t); - } - - if (len > kLibParserMaxNovSize) { - return 0; - } - - if (size < len) { - return -1; - } - - const uint32_t crc = (buf[len - 1] << 24) | (buf[len - 2] << 16) | (buf[len - 3] << 8) | (buf[len - 4]); - if (crc == nov_crc32(buf, len - sizeof(uint32_t))) { - return len; - } else { - return 0; - } -} - -} // namespace fixposition diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index a002a20..8ed9e83 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -1,17 +1,28 @@ # GENERAL ============================================================================================================== -cmake_minimum_required(VERSION 3.5) + +cmake_minimum_required(VERSION 3.16) project(fixposition_driver_ros1 LANGUAGES CXX) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic\ - -Wshadow -Wunused-parameter -Wformat -Wpointer-arith") + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\ + -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual") set(CMAKE_CXX_FLAGS_RELEASE "-O3") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() +if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + add_compile_definitions(NDEBUG) +endif() + # DEPENDENCIES ========================================================================================================= + find_package(Boost 1.65.0 REQUIRED) find_package(Eigen3 REQUIRED) find_package(fixposition_driver_lib REQUIRED) +find_package(fpsdk_common REQUIRED) +find_package(fpsdk_ros1 REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp tf @@ -74,9 +85,12 @@ include_directories( ${fixposition_driver_lib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} + ${fpsdk_common_INLCUDE_DIR} ${fpsdk_ros1_INLCUDE_DIR} ) + # BUILD EXECUTABLE ===================================================================================================== + add_executable( ${PROJECT_NAME} src/fixposition_driver_node.cpp @@ -89,12 +103,15 @@ target_link_libraries( ${catkin_LIBRARIES} ${fixposition_driver_lib_LIBRARIES} ${Boost_LIBRARIES} + ${fpsdk_common_LIBRARIES} ${fpsdk_ros1_LIBRARIES} pthread ) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) + # INSTALL ============================================================================================================== + install(DIRECTORY include/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) diff --git a/fixposition_driver_ros1/README.md b/fixposition_driver_ros1/README.md index 726d789..f6c43ca 100644 --- a/fixposition_driver_ros1/README.md +++ b/fixposition_driver_ros1/README.md @@ -18,4 +18,4 @@ Run `doxygen Doxyfile` to generate Doxygen code documentation. ## License -This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details \ No newline at end of file +This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 9872d38..25c20c9 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -1,28 +1,31 @@ /** - * @file - * @brief Convert Data classes to ROS1 msgs - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Convert Data classes to ROS1 msgs */ -#ifndef __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__ -#define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__ +#ifndef __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__ +#define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__ -/* FIXPOSITION DRIVER LIB */ -#include +/* LIBC/STL */ + +/* EXTERNAL */ #include +#include +#include /* PACKAGE */ -#include +#include "ros_msgs.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /** * @brief @@ -30,19 +33,19 @@ namespace fixposition { * @param[in] data * @param[out] msg */ -void FpToRosMsg( const OdometryData& data, ros::Publisher& pub); -void FpToRosMsg( const ImuData& data, ros::Publisher& pub); -void FpToRosMsg( const FP_EOE& data, ros::Publisher& pub); -void FpToRosMsg( const FP_GNSSANT& data, ros::Publisher& pub); -void FpToRosMsg( const FP_GNSSCORR& data, ros::Publisher& pub); -void FpToRosMsg( const FP_IMUBIAS& data, ros::Publisher& pub); -void FpToRosMsg( const FP_LLH& data, ros::Publisher& pub); -void FpToRosMsg( const FP_ODOMENU& data, ros::Publisher& pub); -void FpToRosMsg( const FP_ODOMETRY& data, ros::Publisher& pub); -void FpToRosMsg( const FP_ODOMSH& data, ros::Publisher& pub); +void FpToRosMsg(const OdometryData& data, ros::Publisher& pub); +void FpToRosMsg(const ImuData& data, ros::Publisher& pub); +void FpToRosMsg(const FP_EOE& data, ros::Publisher& pub); +void FpToRosMsg(const FP_GNSSANT& data, ros::Publisher& pub); +void FpToRosMsg(const FP_GNSSCORR& data, ros::Publisher& pub); +void FpToRosMsg(const FP_IMUBIAS& data, ros::Publisher& pub); +void FpToRosMsg(const FP_LLH& data, ros::Publisher& pub); +void FpToRosMsg(const FP_ODOMENU& data, ros::Publisher& pub); +void FpToRosMsg(const FP_ODOMETRY& data, ros::Publisher& pub); +void FpToRosMsg(const FP_ODOMSH& data, ros::Publisher& pub); void FpToRosMsg(const FP_ODOMSTATUS& data, ros::Publisher& pub); -void FpToRosMsg( const FP_TP& data, ros::Publisher& pub); -void FpToRosMsg( const FP_TEXT& data, ros::Publisher& pub); +void FpToRosMsg(const FP_TP& data, ros::Publisher& pub); +void FpToRosMsg(const FP_TEXT& data, ros::Publisher& pub); void FpToRosMsg(const GP_GGA& data, ros::Publisher& pub); void FpToRosMsg(const GP_GLL& data, ros::Publisher& pub); @@ -109,7 +112,8 @@ void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf); * @param[out] static_br_ * @param[out] br_ */ -void PublishNav2Tf(const std::map>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_); +void PublishNav2Tf(const std::map>& tf_map, + tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_); /** * @brief @@ -143,8 +147,9 @@ void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub); * @param[in] prev_cov * @param[out] msg */ -void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, const Eigen::MatrixXd& prev_cov, ros::Publisher& pub); +void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, const Eigen::MatrixXd& prev_cov, + ros::Publisher& pub); +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__ diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 58b5fad..07f0ebe 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -1,73 +1,72 @@ /** - * @file - * @brief Declaration of FixpositionDriver ROS1 Node - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of FixpositionDriver ROS1 Node */ -#ifndef __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_ -#define __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_ +#ifndef __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_HPP__ +#define __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_HPP__ -/* SYSTEM / STL */ -#include +/* LIBC/STL */ -/* ROS */ -#include -#include +/* EXTERNAL */ +#include -/* FIXPOSITION */ #include -#include +#include +#include /* PACKAGE */ -#include +#include "data_to_ros1.hpp" +#include "params.hpp" +#include "ros_msgs.hpp" namespace fixposition { -class FixpositionDriverNode : public FixpositionDriver { +/* ****************************************************************************************************************** */ + +class FixpositionDriverNode { public: /** - * @brief Construct a new Fixposition Driver Node object + * @brief Constructor * - * @param[in] params + * @param[in] sensor_params Parameters + * @param[in] node_params Parameters + * @param[in] nh Node handle */ - FixpositionDriverNode(const FixpositionDriverParams& params); - - void Run(); + FixpositionDriverNode(const SensorParams& sensor_params, const NodeParams& node_params, ros::NodeHandle& nh); - void RegisterObservers(); - - void WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg); - - void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg); + /** + * @brief Destructor + */ + ~FixpositionDriverNode(); - private: /** - * @brief Observer Functions to publish NavSatFix from BestGnssPos + * @brief Start the node * - * @param[in] header - * @param[in] payload + * @returns true on success, false otherwise */ - void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + bool StartNode(); /** - * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete - * - * @param[in] data + * @brief Stop the node */ - void PublishNmea(); + void StopNode(); + + private: + ros::NodeHandle nh_; //!< ROS node handle + SensorParams sensor_params_; //!< Sensor/driver parameters + NodeParams node_params_; //!< Node parameters + FixpositionDriver driver_; //!< Sensor driver - // ROS node handler - ros::NodeHandle nh_; - // ROS subscribers - ros::Subscriber ws_sub_; //!< wheelspeed message subscriber + ros::Subscriber ws_sub_; //!< wheelspeed message subscriber ros::Subscriber rtcm_sub_; //!< RTCM3 message subscriber // ROS publishers @@ -85,57 +84,69 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher fpa_tp_pub_; //!< FP_A-TP message // NMEA messages - ros::Publisher nmea_gpgga_pub_; //!< NMEA-GP-GGA message - ros::Publisher nmea_gpgll_pub_; //!< NMEA-GP-GLL message - ros::Publisher nmea_gngsa_pub_; //!< NMEA-GP-GSA message - ros::Publisher nmea_gpgst_pub_; //!< NMEA-GP-GST message - ros::Publisher nmea_gxgsv_pub_; //!< NMEA-GP-GSV message - ros::Publisher nmea_gphdt_pub_; //!< NMEA-GP-HDT message - ros::Publisher nmea_gprmc_pub_; //!< NMEA-GP-RMC message - ros::Publisher nmea_gpvtg_pub_; //!< NMEA-GP-VTG message - ros::Publisher nmea_gpzda_pub_; //!< NMEA-GP-ZDA message + ros::Publisher nmea_gpgga_pub_; //!< NMEA-GP-GGA message + ros::Publisher nmea_gpgll_pub_; //!< NMEA-GP-GLL message + ros::Publisher nmea_gngsa_pub_; //!< NMEA-GP-GSA message + ros::Publisher nmea_gpgst_pub_; //!< NMEA-GP-GST message + ros::Publisher nmea_gxgsv_pub_; //!< NMEA-GP-GSV message + ros::Publisher nmea_gphdt_pub_; //!< NMEA-GP-HDT message + ros::Publisher nmea_gprmc_pub_; //!< NMEA-GP-RMC message + ros::Publisher nmea_gpvtg_pub_; //!< NMEA-GP-VTG message + ros::Publisher nmea_gpzda_pub_; //!< NMEA-GP-ZDA message // ODOMETRY - ros::Publisher odometry_ecef_pub_; //!< ECEF Odometry - ros::Publisher odometry_llh_pub_; //!< LLH Odometry - ros::Publisher odometry_enu_pub_; //!< ENU Odometry - ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF) - + ros::Publisher odometry_ecef_pub_; //!< ECEF Odometry + ros::Publisher odometry_llh_pub_; //!< LLH Odometry + ros::Publisher odometry_enu_pub_; //!< ENU Odometry + ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF) + // Orientation - ros::Publisher eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU - ros::Publisher eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal + ros::Publisher eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU + ros::Publisher eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal // IMU - ros::Publisher rawimu_pub_; //!< Raw IMU data in IMU frame - ros::Publisher corrimu_pub_; //!< Bias corrected IMU data in IMU frame - ros::Publisher poiimu_pub_; //!< Bias corrected IMU data in POI frame + ros::Publisher rawimu_pub_; //!< Raw IMU data in IMU frame + ros::Publisher corrimu_pub_; //!< Bias corrected IMU data in IMU frame + ros::Publisher poiimu_pub_; //!< Bias corrected IMU data in POI frame // GNSS - ros::Publisher nmea_pub_; //!< Pose estimation based only on GNSS - ros::Publisher navsatfix_gnss1_pub_; //!< GNSS1 position and status - ros::Publisher navsatfix_gnss2_pub_; //!< GNSS2 position and status - NmeaMessage nmea_message_; //!< Collector class for NMEA messages - + ros::Publisher nmea_pub_; //!< Pose estimation based only on GNSS + ros::Publisher navsatfix_gnss1_pub_; //!< GNSS1 position and status + ros::Publisher navsatfix_gnss2_pub_; //!< GNSS2 position and status + NmeaMessage nmea_message_; //!< Collector class for NMEA messages + // TF tf2_ros::TransformBroadcaster br_; tf2_ros::StaticTransformBroadcaster static_br_; // Jump warning topic - ros::Publisher extras_jump_pub_; //!< Jump warning topic + ros::Publisher extras_jump_pub_; //!< Jump warning topic - // Previous state - Eigen::Vector3d prev_pos; - Eigen::MatrixXd prev_cov; + // State + Eigen::Vector3d prev_pos_; + Eigen::MatrixXd prev_cov_; // Nav2 TF map std::map> tf_map = { - {"ECEFENU0", nullptr}, - {"POIPOISH", nullptr}, - {"ECEFPOISH", nullptr}, - {"ENU0POI", nullptr} - }; + {"ECEFENU0", nullptr}, {"POIPOISH", nullptr}, {"ECEFPOISH", nullptr}, {"ENU0POI", nullptr}}; + + void HandleNovbMessage(const fpsdk::common::parser::ParserMsg& msg); //!< Handle NOV_B messages + + // // Observe received and decoded messages from the sensor + // void RegisterObservers(); + + // // Handle ROS input messages + // void WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg); + // void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg); + + // /** + // * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete + // * + // * @param[in] data + // */ + // void PublishNmea(); }; +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_HPP__ diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp index ffeebbc..dbdf6e2 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp @@ -1,58 +1,49 @@ /** - * @file - * @brief Parameters - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Parameters */ #ifndef __FIXPOSITION_DRIVER_ROS1_PARAMS_HPP__ #define __FIXPOSITION_DRIVER_ROS1_PARAMS_HPP__ -/* ROS */ -#include +/* LIBC/STL */ -/* FIXPOSITION */ +/* EXTERNAL */ #include +/* PACKAGE */ + namespace fixposition { +/* ****************************************************************************************************************** */ /** - * @brief Load all parameters from ROS parameter server + * @brief Load sensor parameters from rosparam server * - * @param[in] ns namespace to load the parameters from - * @param[out] params - * @return true - * @return false - */ -bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params); - -/** - * @brief Load all parameters from ROS parameter server + * @param[in] ns Namespace to load the parameters from + * @param[out] params The sensor parameters * - * @param[in] ns namespace to load the parameters from - * @param[out] params - * @return true - * @return false + * @returns true on success, false otherwise */ -bool LoadParamsFromRos1(const std::string& ns, CustomerInputParams& params); +bool LoadParamsFromRos1(const std::string& ns, SensorParams& params); /** - * @brief Load all parameters from ROS parameter server + * @brief Load node parameters from rosparam server + * + * @param[in] ns Namespace to load the parameters from + * @param[out] params The sensor parameters * - * @param[in] ns namespace to load the parameters from - * @param[out] params - * @return true - * @return false + * @returns true on success, false otherwise */ -bool LoadParamsFromRos1(const std::string& ns, FixpositionDriverParams& params); +bool LoadParamsFromRos1(const std::string& ns, NodeParams& params); +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_ROS1_PARAMS_HPP__ diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp index 03c5e3d..31a83b7 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp @@ -1,46 +1,39 @@ -// Wrapper to include ROS stuff without quoting the same warning suppressions all over -#ifndef __ROS1_DRIVER_ROS_MSGS_HPP__ -#define __ROS1_DRIVER_ROS_MSGS_HPP__ +// Wrapper to suppress warnings from ROS headers +#ifndef __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__ +#define __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wshadow" -#include -#include - -#include - +// Standard ROS messages +#include +#include #include - #include #include - -#include -#include - +#include +#include +#include #include #include #include -#include -#include -// Include generic -#include -#include -#include - -// Include RTCM +// RTCM #include -// Include extras +// Fixposition ROS messages +// - Generic +#include +#include +#include +// -Extras #include - -// Include FP-A +// - FP-A +#include #include #include #include -#include #include #include #include @@ -48,19 +41,17 @@ #include #include #include - -// Include NMEA +// - NMEA +#include +#include #include #include -#include #include -#include #include #include #include #include - -#include +#include #pragma GCC diagnostic pop -#endif // __ROS1_DRIVER_ROS_MSGS_HPP__ +#endif // __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__ diff --git a/fixposition_driver_ros1/launch/config.yaml b/fixposition_driver_ros1/launch/config.yaml new file mode 100644 index 0000000..e730c95 --- /dev/null +++ b/fixposition_driver_ros1/launch/config.yaml @@ -0,0 +1,65 @@ +# Configuration for the Vision-RTK 2 sensor +sensor: + # Connection to the sensor one of: + # - Serial (:) + # stream: /dev/ttyUSB0:115200 + # - TCP client (:) + stream: 172.22.1.44:21000 + # Wait time in [s] to attempt reconnecting after connection lost + reconnect_delay: 5.0 + + # Messages that should be used by the driver. Note that the sensor must be configured accordingly for the correct + # port used in the "connection" above. Where multiple messages of the same type are configurable, such as the NMEA + # messages, the list below always and only refers to the "_FUSION" variant, unless stated otherwise. + messages: + # Fusion odometry and status + - "FP_A-ODOMETRY" + - "FP_A-ODOMENU" + - "FP_A-ODOMSH" + - "FP_A-ODOMSTATUS" + - "FP_A-IMUBIAS" + - "FP_A-EOE" + # Transforms + - "FP_A-TF" + # IMU data + - "FP_A-RAWIMU" + - "FP_A-CORRIMU" + - "NOV_B-RAWIMU" + # GNSS related data + - "FP_A-GNSSANT" + - "FP_A-GNSSCORR" + - "FP_A-TP" + # Fusion LLH output. Not recommended to use! + - "FP_A-LLH" + # Exotic formats. Not recommended to use! + - "NOV_B-INSPVAX" + - "NOV_B-BESTVEL" + # NMEA. Not recommended to use! + - "NMEA-GP-GGA" + - "NMEA-GN-GSA" + - "NMEA-GP-GLL" + - "NMEA-GP-GST" + - "NMEA-GX-GSV" + - "NMEA-GP-HDT" + - "NMEA-GP-RMC" + - "NMEA-GP-VTG" + - "NMEA-GP-ZDA" + # GNSS only position + - "NOV_B-BESTGNSSPOS" # This can one or both of the _GNSS1 and _GNSS2 variants + + # TODO remove + formats: [ + "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", + "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A + "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA + ] + + # Driver behaviour + cov_warning: false # Enable covariance warnings + nav2_mode: false # Enable nav2 mode + +# Node parameters +node: + # Input topics + speed_topic: "/fixposition/speed" # Wheelspeed input, empty to disable + corr_topic: "/rtcm" # Correction data input, empty to disable diff --git a/fixposition_driver_ros1/launch/dev.launch b/fixposition_driver_ros1/launch/dev.launch new file mode 100644 index 0000000..eb2ae3e --- /dev/null +++ b/fixposition_driver_ros1/launch/dev.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/fixposition_driver_ros1/launch/driver_with_odom_converter.launch b/fixposition_driver_ros1/launch/driver_with_odom_converter.launch index bf789cf..3db8310 100644 --- a/fixposition_driver_ros1/launch/driver_with_odom_converter.launch +++ b/fixposition_driver_ros1/launch/driver_with_odom_converter.launch @@ -1,6 +1,4 @@ - - - + diff --git a/fixposition_driver_ros1/launch/tcp.launch b/fixposition_driver_ros1/launch/node.launch similarity index 51% rename from fixposition_driver_ros1/launch/tcp.launch rename to fixposition_driver_ros1/launch/node.launch index fb1c88b..9660d4d 100644 --- a/fixposition_driver_ros1/launch/tcp.launch +++ b/fixposition_driver_ros1/launch/node.launch @@ -1,16 +1,6 @@ - - - - - - + diff --git a/fixposition_driver_ros1/launch/rosconsole.conf b/fixposition_driver_ros1/launch/rosconsole.conf index 36d81cb..85ec64f 100644 --- a/fixposition_driver_ros1/launch/rosconsole.conf +++ b/fixposition_driver_ros1/launch/rosconsole.conf @@ -1,2 +1,7 @@ # Set the default ros output to info and higher +# General/main ROS logger log4j.logger.ros=INFO +# - Fixposition ROS driver (fixposition_driver) +log4j.logger.ros.fixposition_driver_ros1=INFO +# - Fixposition SDK (fpsdk::common::logging functions) +log4j.logger.ros.fpsdk_ros1=INFO diff --git a/fixposition_driver_ros1/launch/rosconsole_dev.conf b/fixposition_driver_ros1/launch/rosconsole_dev.conf new file mode 100644 index 0000000..629e6f7 --- /dev/null +++ b/fixposition_driver_ros1/launch/rosconsole_dev.conf @@ -0,0 +1,3 @@ +log4j.logger.ros=INFO +log4j.logger.ros.fixposition_driver_ros1=DEBUG +log4j.logger.ros.fpsdk_ros1=DEBUG diff --git a/fixposition_driver_ros1/launch/serial.launch b/fixposition_driver_ros1/launch/serial.launch deleted file mode 100644 index d6fe44f..0000000 --- a/fixposition_driver_ros1/launch/serial.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - diff --git a/fixposition_driver_ros1/launch/serial.yaml b/fixposition_driver_ros1/launch/serial.yaml deleted file mode 100644 index 1db13c7..0000000 --- a/fixposition_driver_ros1/launch/serial.yaml +++ /dev/null @@ -1,17 +0,0 @@ -fp_output: - formats: [ - "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", - "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A - "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA - ] - type: serial - port: "/dev/ttyUSB0" - baudrate: 115200 - rate: 200 - reconnect_delay: 5.0 # wait time in [s] until retry connection - cov_warning: false - nav2_mode: false - -customer_input: - speed_topic: "/fixposition/speed" - rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml deleted file mode 100644 index 1f7745f..0000000 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ /dev/null @@ -1,17 +0,0 @@ -fp_output: - formats: [ - "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", - "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A - "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA - ] - type: tcp - port: "21000" - ip: "10.0.2.1" # change to VRTK2's IP address in the network - rate: 200 - reconnect_delay: 5.0 # wait time in [s] until retry connection - cov_warning: false - nav2_mode: false - -customer_input: - speed_topic: "/fixposition/speed" - rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros1/msg/fpa/imubias.msg b/fixposition_driver_ros1/msg/fpa/imubias.msg index 6eb123f..978f645 100644 --- a/fixposition_driver_ros1/msg/fpa/imubias.msg +++ b/fixposition_driver_ros1/msg/fpa/imubias.msg @@ -68,4 +68,4 @@ geometry_msgs/Vector3 bias_cov_gyr # Gyroscope bias covariance # | 3 | Insufficient motion | # | 4 | Converging | # | 5...6 | Reserved | -# | 7 | Idle | \ No newline at end of file +# | 7 | Idle | diff --git a/fixposition_driver_ros1/msg/nmea/gphdt.msg b/fixposition_driver_ros1/msg/nmea/gphdt.msg index acad6da..040f0ec 100644 --- a/fixposition_driver_ros1/msg/nmea/gphdt.msg +++ b/fixposition_driver_ros1/msg/nmea/gphdt.msg @@ -16,4 +16,4 @@ # Format | Field | Unit | Description # -------|----------|-------|---------------| float64 heading # [deg] | True heading. -char true_ind # [-] | Always T. \ No newline at end of file +char true_ind # [-] | Always T. diff --git a/fixposition_driver_ros1/msg/nmea/gxgsv.msg b/fixposition_driver_ros1/msg/nmea/gxgsv.msg index a7ba73a..dd5f850 100644 --- a/fixposition_driver_ros1/msg/nmea/gxgsv.msg +++ b/fixposition_driver_ros1/msg/nmea/gxgsv.msg @@ -18,8 +18,8 @@ int16 sentences # [-] | Total number of sentences (1 to 9). int16 sent_num # [-] | Sentence number (1 to 9). int32 num_sats # [-] | Total number of satellites in view. -int16[] sat_id # [-] | Satellite ID number. -int16[] elev # [deg] | Satellite elevation. +int16[] sat_id # [-] | Satellite ID number. +int16[] elev # [deg] | Satellite elevation. int16[] azim # [deg] | Satellite azimuth from true North. int16[] cno # [db-Hz] | Satellite SNR (C/No). string signal_id # [Hex] | Signal ID (see below). @@ -36,4 +36,4 @@ string signal_id # [Hex] | Signal ID (see below). # | 1 | BeiDou B1I (talker ID GB) | # | B | BeiDou B2I (talker ID GB) | # | 1 | GLONASS G1 C/A (talker ID GL) | -# | 3 | GLONASS G2 C/A (talker ID GL) | \ No newline at end of file +# | 3 | GLONASS G2 C/A (talker ID GL) | diff --git a/fixposition_driver_ros1/package.xml b/fixposition_driver_ros1/package.xml index 7956e19..5cb0cf3 100644 --- a/fixposition_driver_ros1/package.xml +++ b/fixposition_driver_ros1/package.xml @@ -21,4 +21,6 @@ rtcm_msgs tf2_ros tf2_geometry_msgs + fpsdk_common + fpsdk_ros1 diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 7910e8f..aa61406 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -1,27 +1,34 @@ /** - * @file - * @brief Convert Data classes to ROS1 msgs - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Convert Data classes to ROS1 msgs */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ #include namespace fixposition { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common; void FpToRosMsg(const OdometryData& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message nav_msgs::Odometry msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = ros::Time::now(); @@ -34,7 +41,7 @@ void FpToRosMsg(const OdometryData& data, ros::Publisher& pub) { PoseWithCovDataToMsg(data.pose, msg.pose); TwistWithCovDataToMsg(data.twist, msg.twist); - + // Publish message pub.publish(msg); } @@ -51,7 +58,7 @@ void FpToRosMsg(const ImuData& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); } - + msg.header.frame_id = data.frame_id; tf::vectorEigenToMsg(data.linear_acceleration, msg.linear_acceleration); tf::vectorEigenToMsg(data.angular_velocity, msg.angular_velocity); @@ -72,7 +79,7 @@ void FpToRosMsg(const FP_IMUBIAS& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); } - + msg.header.frame_id = data.frame_id; msg.fusion_imu = data.fusion_imu; msg.imu_status = data.imu_status; @@ -92,7 +99,7 @@ void FpToRosMsg(const FP_GNSSANT& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message fixposition_driver_ros1::gnssant msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = ros::Time::now(); @@ -106,7 +113,7 @@ void FpToRosMsg(const FP_GNSSANT& data, ros::Publisher& pub) { msg.gnss2_state = data.gnss2_state; msg.gnss2_power = data.gnss2_power; msg.gnss2_age = data.gnss2_age; - + // Publish message pub.publish(msg); } @@ -116,7 +123,7 @@ void FpToRosMsg(const FP_GNSSCORR& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message fixposition_driver_ros1::gnsscorr msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = ros::Time::now(); @@ -177,7 +184,7 @@ void FpToRosMsg(const FP_ODOMENU& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); } - + msg.header.frame_id = data.odom.frame_id; msg.pose_frame = data.odom.child_frame_id; msg.kin_frame = data.odom.child_frame_id; @@ -207,7 +214,7 @@ void FpToRosMsg(const FP_ODOMETRY& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); } - + msg.header.frame_id = data.odom.frame_id; msg.pose_frame = data.odom.child_frame_id; msg.kin_frame = data.odom.child_frame_id; @@ -221,7 +228,7 @@ void FpToRosMsg(const FP_ODOMETRY& data, ros::Publisher& pub) { msg.gnss2_status = data.gnss2_status; msg.wheelspeed_status = data.wheelspeed_status; msg.version = data.version; - + // Publish message pub.publish(msg); } @@ -238,7 +245,7 @@ void FpToRosMsg(const FP_ODOMSH& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); } - + msg.header.frame_id = data.odom.frame_id; msg.pose_frame = data.odom.child_frame_id; msg.kin_frame = data.odom.child_frame_id; @@ -397,11 +404,11 @@ void FpToRosMsg(const GN_GSA& data, ros::Publisher& pub) { // Populate message msg.mode_op = data.mode_op; msg.mode_nav = data.mode_nav; - + for (unsigned int i = 0; i < data.ids.size(); i++) { - msg.ids.push_back(data.ids.at(i)); + msg.ids.push_back(data.ids.at(i)); } - + msg.pdop = data.pdop; msg.hdop = data.hdop; msg.vdop = data.vdop; @@ -426,7 +433,7 @@ void FpToRosMsg(const GP_GST& data, ros::Publisher& pub) { msg.std_lat = data.std_lat; msg.std_lon = data.std_lon; msg.std_alt = data.std_alt; - + // Publish message pub.publish(msg); } @@ -441,7 +448,7 @@ void FpToRosMsg(const GX_GSV& data, ros::Publisher& pub) { msg.sentences = data.sentences; msg.sent_num = data.sent_num; msg.num_sats = data.num_sats; - + for (unsigned int i = 0; i < data.sat_id.size(); i++) { msg.sat_id.push_back(data.sat_id.at(i)); msg.elev.push_back(data.elev.at(i)); @@ -456,7 +463,6 @@ void FpToRosMsg(const GX_GSV& data, ros::Publisher& pub) { } } - void FpToRosMsg(const GP_HDT& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message @@ -595,7 +601,7 @@ void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pu if (data.odom.pose.orientation.vec().isZero() && (data.odom.pose.orientation.w() == 0)) { return; } - + // Populate message geometry_msgs::TransformStamped msg; OdomToTf(data.odom, msg); @@ -620,7 +626,8 @@ void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf) { tf::vectorEigenToMsg(data.pose.position, tf.transform.translation); } -void PublishNav2Tf(const std::map>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_) { +void PublishNav2Tf(const std::map>& tf_map, + tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_) { if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) { // Publish FP_ECEF -> map tf_map.at("ECEFENU0")->child_frame_id = "map"; @@ -642,7 +649,7 @@ void PublishNav2Tf(const std::map 0) { // Create message sensor_msgs::NavSatFix msg; - + // Populate message header if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { msg.header.stamp = ros::Time::now(); @@ -689,26 +696,26 @@ void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); } msg.header.frame_id = data.odom.child_frame_id; - + // Populate LLH position Eigen::Map> cov_map = Eigen::Map>(msg.position_covariance.data()); if (data.odom.pose.position.isZero()) { - msg.latitude = 0; + msg.latitude = 0; msg.longitude = 0; - msg.altitude = 0; + msg.altitude = 0; msg.position_covariance_type = 0; cov_map = Eigen::Matrix3d::Zero(); } else { - const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position); - msg.latitude = RadToDeg(llh_pos(0)); + const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(data.odom.pose.position); + msg.latitude = RadToDeg(llh_pos(0)); msg.longitude = RadToDeg(llh_pos(1)); - msg.altitude = llh_pos(2); + msg.altitude = llh_pos(2); // Populate LLH covariance const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position); + const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(data.odom.pose.position); const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); cov_map = p_cov_l; msg.position_covariance_type = 3; @@ -721,7 +728,8 @@ void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) { msg.status.status = static_cast(NavSatStatusData::Status::STATUS_NO_FIX); msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_NONE); - } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_S2D) || status_flag < static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { + } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_S2D) || + status_flag < static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { msg.status.status = static_cast(NavSatStatusData::Status::STATUS_FIX); msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_ALL); @@ -750,7 +758,7 @@ void OdomToImuMsg(const FP_ODOMETRY& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); } - + msg.header.frame_id = data.odom.frame_id; tf::vectorEigenToMsg(data.acceleration, msg.linear_acceleration); tf::vectorEigenToMsg(data.odom.twist.angular, msg.angular_velocity); @@ -774,7 +782,7 @@ void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub) { msg.header.frame_id = "FP_ENU"; // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll - Eigen::Vector3d enu_euler = RotToEul(data.pose.orientation.toRotationMatrix()); + Eigen::Vector3d enu_euler = trafo::RotToEul(data.pose.orientation.toRotationMatrix()); tf::vectorEigenToMsg(enu_euler, msg.vector); // Publish message @@ -782,7 +790,8 @@ void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub) { } } -void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, const Eigen::MatrixXd& prev_cov, ros::Publisher& pub) { +void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, const Eigen::MatrixXd& prev_cov, + ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message fixposition_driver_ros1::CovWarn msg; @@ -797,15 +806,16 @@ void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff std::stringstream warn_msg; warn_msg << "Position jump detected! The change in position is greater than the estimated covariances. " << "Position difference: [" << pos_diff[0] << ", " << pos_diff[1] << ", " << pos_diff[2] << "], " - << "Covariances: [" << prev_cov(0,0) << ", " << prev_cov(1,1) << ", " << prev_cov(2,2) << "]"; + << "Covariances: [" << prev_cov(0, 0) << ", " << prev_cov(1, 1) << ", " << prev_cov(2, 2) << "]"; ROS_WARN("%s", warn_msg.str().c_str()); tf::vectorEigenToMsg(pos_diff, msg.jump); - tf::vectorEigenToMsg(Eigen::Vector3d(prev_cov(0,0),prev_cov(1,1),prev_cov(2,2)), msg.covariance); + tf::vectorEigenToMsg(Eigen::Vector3d(prev_cov(0, 0), prev_cov(1, 1), prev_cov(2, 2)), msg.covariance); // Publish message pub.publish(msg); } } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index a88ef7f..36e724a 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -1,112 +1,260 @@ /** - * @file - * @brief Main function for the fixposition driver ros node - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Main function for the fixposition driver ros node */ +/* LIBC/STL */ +#include +#include +#include + +/* EXTERNAL */ +#include +#include +#include +#include +#include +#include + /* PACKAGE */ -#include +#include "fixposition_driver_ros1/fixposition_driver_node.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common; +using namespace fpsdk::common::parser; + +FixpositionDriverNode::FixpositionDriverNode(const SensorParams& sensor_params, const NodeParams& node_params, + ros::NodeHandle& nh) /* clang-format off */ : + nh_ { nh }, + sensor_params_ { sensor_params }, + node_params_ { node_params }, + driver_ { sensor_params } // clang-format on +{} + +FixpositionDriverNode::~FixpositionDriverNode() { StopNode(); } + +// --------------------------------------------------------------------------------------------------------------------- + +// Helper for subscribing to input topics +#define _SUB(_handle_, _msg_type_, _topic_, ...) \ + ROS_INFO("Subscribe %s (" #_msg_type_ ")", _topic_); \ + _handle_ = nh_.subscribe<_msg_type_>(_topic_, __VA_ARGS__) + +// Helper for advertising output topics +#define _PUB(_pub_, _msg_type_, _topic_, ...) \ + if (_pub_.getTopic().empty()) { \ + ROS_INFO("Advertise %s (" #_msg_type_ ")", _topic_); \ + _pub_ = nh_.advertise<_msg_type_>(_topic_, __VA_ARGS__); \ + } + +bool FixpositionDriverNode::StartNode() { + ROS_INFO("Starting..."); + + // const auto hints = ros::TransportHints().tcpNoDelay(true); + // _SUB(sub_fusion_odom_status_, fixposition_msgs::FusionOdomStatus, t.odom_status_topic_, 25, + // &UserIoCore::ProcessFusionOdomStatus, iocore_.get(), hints); -FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& params) - : FixpositionDriver(params), - nh_("~"), - // FP_A messages - fpa_odometry_pub_(nh_.advertise("/fixposition/fpa/odometry", 5)), - fpa_imubias_pub_(nh_.advertise("/fixposition/fpa/imubias", 5)), - fpa_eoe_pub_(nh_.advertise("/fixposition/fpa/eoe", 5)), - fpa_llh_pub_(nh_.advertise("/fixposition/fpa/llh", 5)), - fpa_odomenu_pub_(nh_.advertise("/fixposition/fpa/odomenu", 5)), - fpa_odomsh_pub_(nh_.advertise("/fixposition/fpa/odomsh", 5)), - fpa_odomstatus_pub_(nh_.advertise("/fixposition/fpa/odomstatus", 5)), - fpa_gnssant_pub_(nh_.advertise("/fixposition/fpa/gnssant", 5)), - fpa_gnsscorr_pub_(nh_.advertise("/fixposition/fpa/gnsscorr", 5)), - fpa_text_pub_(nh_.advertise("/fixposition/fpa/text", 5)), - fpa_tp_pub_(nh_.advertise("/fixposition/fpa/tp", 5)), - - // NMEA messages - nmea_gpgga_pub_(nh_.advertise("/fixposition/nmea/gpgga", 5)), - nmea_gpgll_pub_(nh_.advertise("/fixposition/nmea/gpgll", 5)), - nmea_gngsa_pub_(nh_.advertise("/fixposition/nmea/gngsa", 5)), - nmea_gpgst_pub_(nh_.advertise("/fixposition/nmea/gpgst", 5)), - nmea_gxgsv_pub_(nh_.advertise("/fixposition/nmea/gxgsv", 5)), - nmea_gphdt_pub_(nh_.advertise("/fixposition/nmea/gphdt", 5)), - nmea_gprmc_pub_(nh_.advertise("/fixposition/nmea/gprmc", 5)), - nmea_gpvtg_pub_(nh_.advertise("/fixposition/nmea/gpvtg", 5)), - nmea_gpzda_pub_(nh_.advertise("/fixposition/nmea/gpzda", 5)), - - // ODOMETRY - odometry_ecef_pub_(nh_.advertise("/fixposition/odometry_ecef", 5)), - odometry_llh_pub_(nh_.advertise("/fixposition/odometry_llh", 5)), - odometry_enu_pub_(nh_.advertise("/fixposition/odometry_enu", 5)), - odometry_smooth_pub_(nh_.advertise("/fixposition/odometry_smooth", 5)), - - // Orientation - eul_pub_(nh_.advertise("/fixposition/ypr", 5)), - eul_imu_pub_(nh_.advertise("/fixposition/imu_ypr", 5)), - - // IMU - rawimu_pub_(nh_.advertise("/fixposition/rawimu", 5)), - corrimu_pub_(nh_.advertise("/fixposition/corrimu", 5)), - poiimu_pub_(nh_.advertise("/fixposition/poiimu", 5)), - - // GNSS - nmea_pub_(nh_.advertise("/fixposition/nmea", 5)), - navsatfix_gnss1_pub_(nh_.advertise("/fixposition/gnss1", 5)), - navsatfix_gnss2_pub_(nh_.advertise("/fixposition/gnss2", 5)) - -{ - ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 10, - &FixpositionDriverNode::WsCallbackRos, this, - ros::TransportHints().tcpNoDelay()); - rtcm_sub_ = nh_.subscribe(params_.customer_input.rtcm_topic, 10, - &FixpositionDriverNode::RtcmCallbackRos, this, - ros::TransportHints().tcpNoDelay()); - - // Configure jump warning message - if (params_.fp_output.cov_warning) { - extras_jump_pub_ = nh_.advertise("/fixposition/extras/jump", 5); - prev_pos.setZero(); - prev_cov.setZero(); + // Add observers and advertise output topics, depending on configuration + + // NOV_B-BESTGNSSPOS + if (sensor_params_.MessageEnabled(novb::NOV_B_BESTGNSSPOS_STRID)) { + _PUB(navsatfix_gnss1_pub_, sensor_msgs::NavSatFix, "/fixposition/gnss1", 5); + _PUB(navsatfix_gnss2_pub_, sensor_msgs::NavSatFix, "/fixposition/gnss2", 5); + driver_.AddNovbObserver(novb::NOV_B_BESTGNSSPOS_STRID, + std::bind(&FixpositionDriverNode::HandleNovbMessage, this, std::placeholders::_1)); } - RegisterObservers(); + + // // Advertise output topics + // // - FP_A messages + // fpa_odometry_pub_ = nh_.advertise("/fixposition/fpa/odometry", 5); + // fpa_imubias_pub_ = nh_.advertise("/fixposition/fpa/imubias", 5); + // fpa_eoe_pub_ = nh_.advertise("/fixposition/fpa/eoe", 5); + // fpa_llh_pub_ = nh_.advertise("/fixposition/fpa/llh", 5); + // fpa_odomenu_pub_ = nh_.advertise("/fixposition/fpa/odomenu", 5); + // fpa_odomsh_pub_ = nh_.advertise("/fixposition/fpa/odomsh", 5); + // fpa_odomstatus_pub_ = nh_.advertise("/fixposition/fpa/odomstatus", 5); + // fpa_gnssant_pub_ = nh_.advertise("/fixposition/fpa/gnssant", 5); + // fpa_gnsscorr_pub_ = nh_.advertise("/fixposition/fpa/gnsscorr", 5); + // fpa_text_pub_ = nh_.advertise("/fixposition/fpa/text", 5); + // fpa_tp_pub_ = nh_.advertise("/fixposition/fpa/tp", 5); + // // - NMEA messages + // nmea_gpgga_pub_ = nh_.advertise("/fixposition/nmea/gpgga", 5); + // nmea_gpgll_pub_ = nh_.advertise("/fixposition/nmea/gpgll", 5); + // nmea_gngsa_pub_ = nh_.advertise("/fixposition/nmea/gngsa", 5); + // nmea_gpgst_pub_ = nh_.advertise("/fixposition/nmea/gpgst", 5); + // nmea_gxgsv_pub_ = nh_.advertise("/fixposition/nmea/gxgsv", 5); + // nmea_gphdt_pub_ = nh_.advertise("/fixposition/nmea/gphdt", 5); + // nmea_gprmc_pub_ = nh_.advertise("/fixposition/nmea/gprmc", 5); + // nmea_gpvtg_pub_ = nh_.advertise("/fixposition/nmea/gpvtg", 5); + // nmea_gpzda_pub_ = nh_.advertise("/fixposition/nmea/gpzda", 5); + // // - Odometry + // odometry_ecef_pub_ = nh_.advertise("/fixposition/odometry_ecef", 5); + // odometry_llh_pub_ = nh_.advertise("/fixposition/odometry_llh", 5); + // odometry_enu_pub_ = nh_.advertise("/fixposition/odometry_enu", 5); + // odometry_smooth_pub_ = nh_.advertise("/fixposition/odometry_smooth", 5); + // // - Orientation + // eul_pub_ = nh_.advertise("/fixposition/ypr", 5); + // eul_imu_pub_ = nh_.advertise("/fixposition/imu_ypr", 5); + // // - IMU + // rawimu_pub_ = nh_.advertise("/fixposition/rawimu", 5); + // corrimu_pub_ = nh_.advertise("/fixposition/corrimu", 5); + // poiimu_pub_ = nh_.advertise("/fixposition/poiimu", 5); + // // - GNSS + // nmea_pub_ = nh_.advertise("/fixposition/nmea", 5); + + // // Subscribe to input topics + // ws_sub_ = nh_.subscribe(node_params_.speed_topic_, 10, + // &FixpositionDriverNode::WsCallbackRos, this); + // rtcm_sub_ = + // nh_.subscribe(node_params_.corr_topic_, 10, &FixpositionDriverNode::RtcmCallbackRos, + // this); + + // // Configure jump warning message + // if (sensor_params_.cov_warning_) { + // extras_jump_pub_ = nh_.advertise("/fixposition/extras/jump", 5); + // } + // prev_pos_.setZero(); + // prev_cov_.setZero(); + + // AddFpaObserver(fpa::FpaEoePayload::MSG_NAME, [](const fpa::FpaPayload& payload) { + // auto eoe = dynamic_cast(payload); + // ROS_WARN("EOE --> %.1f %d", eoe.gps_time.tow.value, types::EnumToVal(eoe.epoch)); + // }); + + // AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [](const fpa::FpaPayload& payload) { + // auto odo = dynamic_cast(payload); + // ROS_WARN("ODOMETRY --> %.1f %d", odo.gps_time.tow.value, types::EnumToVal(odo.which)); + // }); + + // AddNmeaObserver(nmea::NmeaHdtPayload::FORMATTER, [](const nmea::NmeaPayload& payload) { + // auto hdt = dynamic_cast(payload); + // ROS_WARN("HDT --> %.1f", hdt.heading.value); + + // }); + + // AddNovbObserver(novb::NOV_B_BESTGNSSPOS_STRID, [](const ParserMsg& msg) { + // ROS_WARN("NOVB --> %s", msg.name_.c_str()); + // }); + + return driver_.StartDriver(); } -void FixpositionDriverNode::Run() { - ros::Rate rate(params_.fp_output.rate); - - while (ros::ok()) { - // Read data and publish to ros - const bool connection_ok = RunOnce(); - // process Incoming ROS msgs - ros::spinOnce(); - // Handle connection loss - if (!connection_ok) { - printf("Reconnecting in %.1f seconds ...\n", params_.fp_output.reconnect_delay); - ros::Duration(params_.fp_output.reconnect_delay).sleep(); - Connect(); +#undef _PUB +#undef _SUB + +void FixpositionDriverNode::StopNode() { + ROS_INFO("Stopping..."); + + driver_.RemoveFpaObservers(); + driver_.RemoveNmeaObservers(); + driver_.RemoveNovbObservers(); + + driver_.StopDriver(); + + // Stop advertising output topics + // // - FP_A messages + // fpa_odometry_pub_.shutdown(); + // fpa_imubias_pub_.shutdown(); + // fpa_eoe_pub_.shutdown(); + // fpa_llh_pub_.shutdown(); + // fpa_odomenu_pub_.shutdown(); + // fpa_odomsh_pub_.shutdown(); + // fpa_odomstatus_pub_.shutdown(); + // fpa_gnssant_pub_.shutdown(); + // fpa_gnsscorr_pub_.shutdown(); + // fpa_text_pub_.shutdown(); + // fpa_tp_pub_.shutdown(); + // // - NMEA messages + // nmea_gpgga_pub_.shutdown(); + // nmea_gpgll_pub_.shutdown(); + // nmea_gngsa_pub_.shutdown(); + // nmea_gpgst_pub_.shutdown(); + // nmea_gxgsv_pub_.shutdown(); + // nmea_gphdt_pub_.shutdown(); + // nmea_gprmc_pub_.shutdown(); + // nmea_gpvtg_pub_.shutdown(); + // nmea_gpzda_pub_.shutdown(); + // // - Odometry + // odometry_ecef_pub_.shutdown(); + // odometry_llh_pub_.shutdown(); + // odometry_enu_pub_.shutdown(); + // odometry_smooth_pub_.shutdown(); + // // - Orientation + // eul_pub_.shutdown(); + // eul_imu_pub_.shutdown(); + // // - IMU + // rawimu_pub_.shutdown(); + // corrimu_pub_.shutdown(); + // poiimu_pub_.shutdown(); + // // - GNSS + // nmea_pub_.shutdown(); + navsatfix_gnss1_pub_.shutdown(); + navsatfix_gnss2_pub_.shutdown(); + + // Stop input message subscribers + // ws_sub_.shutdown(); + // rtcm_sub_.shutdown(); + // extras_jump_pub_.shutdown(); +} + +// --------------------------------------------------------------------------------------------------------------------- + +void FixpositionDriverNode::HandleNovbMessage(const fpsdk::common::parser::ParserMsg& msg) { + TRACE("HandleNovbMessage() %s", msg.name_.c_str()); + + const uint8_t* frame = msg.data_.data(); + const uint16_t msg_id = novb::NovbMsgId(frame); + const bool is_long_header = novb::NovbIsLongHeader(frame); + + // NOV_B-BESTGNSSPOS + if ((msg_id == novb::NOV_B_BESTGNSSPOS_MSGID) && is_long_header) { + // Get message header and payload + novb::NovbLongHeader header; + novb::NovbBestgnsspos payload; + std::memcpy(&header, &frame[0], sizeof(header)); + std::memcpy(&payload, &frame[sizeof(header)], sizeof(payload)); + + // Convert to data + NavSatFixData nav_sat_fix; + BestGnssPosToNavSatFix(header, payload, nav_sat_fix); + + // Publish + if (nav_sat_fix.frame_id == "GNSS1") { + if (navsatfix_gnss1_pub_.getNumSubscribers() > 0) { + sensor_msgs::NavSatFix ros_msg; + NavSatFixDataToMsg(nav_sat_fix, ros_msg); + navsatfix_gnss1_pub_.publish(ros_msg); + } + } else if (nav_sat_fix.frame_id == "GNSS2") { + if (navsatfix_gnss2_pub_.getNumSubscribers() > 0) { + sensor_msgs::NavSatFix ros_msg; + NavSatFixDataToMsg(nav_sat_fix, ros_msg); + navsatfix_gnss2_pub_.publish(ros_msg); + } } else { - rate.sleep(); // ensure the loop runs at the desired rate + ROS_WARN_THROTTLE(1.0, "Bad %s (frame_id %s)", msg.name_.c_str(), nav_sat_fix.frame_id.c_str()); } } } +// --------------------------------------------------------------------------------------------------------------------- + +#if 0 void FixpositionDriverNode::RegisterObservers() { // NOV_B - bestgnsspos_obs_.push_back(std::bind(&FixpositionDriverNode::BestGnssPosToPublishNavSatFix, this, - std::placeholders::_1, std::placeholders::_2)); // FP_A - for (const auto& format : params_.fp_output.formats) { + for (const auto& format : sensor_params_.formats) { if (format == "ODOMETRY") { dynamic_cast*>(a_converters_["ODOMETRY"].get()) ->AddObserver([this](const FP_ODOMETRY& data) { @@ -117,16 +265,17 @@ void FixpositionDriverNode::RegisterObservers() { OdometryDataToTf(data, br_); // Output jump warning - if (params_.fp_output.cov_warning) { - if (!prev_pos.isZero() && !prev_cov.isZero()) { - Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs(); + if (sensor_params_.cov_warning_) { + if (!prev_pos_.isZero() && !prev_cov_.isZero()) { + Eigen::Vector3d pos_diff = (prev_pos_ - data.odom.pose.position).cwiseAbs(); - if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) { - JumpWarningMsg(data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_); + if ((pos_diff[0] > prev_cov_(0, 0)) || (pos_diff[1] > prev_cov_(1, 1)) || + (pos_diff[2] > prev_cov_(2, 2))) { + JumpWarningMsg(data.odom.stamp, pos_diff, prev_cov_, extras_jump_pub_); } } - prev_pos = data.odom.pose.position; - prev_cov = data.odom.pose.cov; + prev_pos_ = data.odom.pose.position; + prev_cov_ = data.odom.pose.cov; } }); } else if (format == "ODOMENU") { @@ -137,7 +286,7 @@ void FixpositionDriverNode::RegisterObservers() { OdomToYprMsg(data.odom, eul_pub_); // Append TF if Nav2 mode is selected - if (params_.fp_output.nav2_mode) { + if (sensor_params_.nav2_mode_) { // Get FP_ENU0 -> FP_POI geometry_msgs::TransformStamped tf; OdomToTf(data.odom, tf); @@ -151,7 +300,7 @@ void FixpositionDriverNode::RegisterObservers() { FpToRosMsg(data.odom, odometry_smooth_pub_); // Append TF if Nav2 mode is selected - if (params_.fp_output.nav2_mode) { + if (sensor_params_.nav2_mode_) { // Get FP_ECEF -> FP_POISH geometry_msgs::TransformStamped tf; OdomToTf(data.odom, tf); @@ -165,18 +314,18 @@ void FixpositionDriverNode::RegisterObservers() { dynamic_cast*>(a_converters_["IMUBIAS"].get()) ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); }); } else if (format == "EOE") { - dynamic_cast*>(a_converters_["EOE"].get()) - ->AddObserver([this](const FP_EOE& data) { - FpToRosMsg(data, fpa_eoe_pub_); + dynamic_cast*>(a_converters_["EOE"].get())->AddObserver([this](const FP_EOE& data) { + FpToRosMsg(data, fpa_eoe_pub_); - // Generate Nav2 TF tree - if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) { - PublishNav2Tf(tf_map, static_br_, br_); - } - }); + // Generate Nav2 TF tree + if (data.epoch == "FUSION" && sensor_params_.nav2_mode_) { + PublishNav2Tf(tf_map, static_br_, br_); + } + }); } else if (format == "LLH") { - dynamic_cast*>(a_converters_["LLH"].get()) - ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); }); + dynamic_cast*>(a_converters_["LLH"].get())->AddObserver([this](const FP_LLH& data) { + FpToRosMsg(data, fpa_llh_pub_); + }); } else if (format == "GNSSANT") { dynamic_cast*>(a_converters_["GNSSANT"].get()) ->AddObserver([this](const FP_GNSSANT& data) { FpToRosMsg(data, fpa_gnssant_pub_); }); @@ -202,7 +351,7 @@ void FixpositionDriverNode::RegisterObservers() { br_.sendTransform(tf); // Publish Pitch Roll based on IMU only - Eigen::Vector3d imu_ypr_eigen = QuatToEul(data.tf.rotation); + Eigen::Vector3d imu_ypr_eigen = trafo::QuatToEul(data.tf.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone geometry_msgs::Vector3Stamped imu_ypr; imu_ypr.header.stamp = tf.header.stamp; @@ -214,7 +363,7 @@ void FixpositionDriverNode::RegisterObservers() { br_.sendTransform(tf); // Append TF if Nav2 mode is selected - if (params_.fp_output.nav2_mode) { + if (sensor_params_.nav2_mode_) { // Get FP_POI -> FP_POISH tf_map["POIPOISH"] = std::make_shared(tf); } @@ -222,7 +371,7 @@ void FixpositionDriverNode::RegisterObservers() { static_br_.sendTransform(tf); // Append TF if Nav2 mode is selected - if (params_.fp_output.nav2_mode) { + if (sensor_params_.nav2_mode_) { // Get FP_ECEF -> FP_ENU0 tf_map["ECEFENU0"] = std::make_shared(tf); } @@ -232,60 +381,79 @@ void FixpositionDriverNode::RegisterObservers() { } }); } else if (format == "TP") { - dynamic_cast*>(a_converters_["TP"].get()) - ->AddObserver([this](const FP_TP& data) { FpToRosMsg(data, fpa_tp_pub_); }); + dynamic_cast*>(a_converters_["TP"].get())->AddObserver([this](const FP_TP& data) { + FpToRosMsg(data, fpa_tp_pub_); + }); } else if (format == "GPGGA") { dynamic_cast*>(a_converters_["GPGGA"].get())->AddObserver([this](const GP_GGA& data) { FpToRosMsg(data, nmea_gpgga_pub_); if (nmea_pub_.getNumSubscribers() > 0) { nmea_message_.AddNmeaEpoch(data); - PublishNmea(); // GPGGA controls the NMEA output + PublishNmea(); // Receiving a GPGGA triggers the NMEA output... :-/ } }); } else if (format == "GPGLL") { dynamic_cast*>(a_converters_["GPGLL"].get())->AddObserver([this](const GP_GLL& data) { FpToRosMsg(data, nmea_gpgll_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GNGSA") { dynamic_cast*>(a_converters_["GNGSA"].get())->AddObserver([this](const GN_GSA& data) { FpToRosMsg(data, nmea_gngsa_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GPGST") { dynamic_cast*>(a_converters_["GPGST"].get())->AddObserver([this](const GP_GST& data) { FpToRosMsg(data, nmea_gpgst_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GXGSV") { dynamic_cast*>(a_converters_["GXGSV"].get())->AddObserver([this](const GX_GSV& data) { FpToRosMsg(data, nmea_gxgsv_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GPHDT") { dynamic_cast*>(a_converters_["GPHDT"].get())->AddObserver([this](const GP_HDT& data) { FpToRosMsg(data, nmea_gphdt_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GPRMC") { dynamic_cast*>(a_converters_["GPRMC"].get())->AddObserver([this](const GP_RMC& data) { FpToRosMsg(data, nmea_gprmc_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GPVTG") { dynamic_cast*>(a_converters_["GPVTG"].get())->AddObserver([this](const GP_VTG& data) { FpToRosMsg(data, nmea_gpvtg_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } else if (format == "GPZDA") { dynamic_cast*>(a_converters_["GPZDA"].get())->AddObserver([this](const GP_ZDA& data) { FpToRosMsg(data, nmea_gpzda_pub_); - if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + } }); } } } +// --------------------------------------------------------------------------------------------------------------------- + void FixpositionDriverNode::PublishNmea() { // If epoch message is complete, generate NMEA output if (nmea_message_.checkEpoch()) { @@ -405,6 +573,8 @@ void FixpositionDriverNode::PublishNmea() { } } +// --------------------------------------------------------------------------------------------------------------------- + void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg) { std::unordered_map>> measurements; for (const auto& sensor : msg->sensors) { @@ -415,56 +585,87 @@ void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros1::SpeedCo WsCallback(measurements); } -void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg) { - const void* rtcm_msg = &(msg->message[0]); - size_t msg_size = msg->message.size(); - RtcmCallback(rtcm_msg, msg_size); -} +// --------------------------------------------------------------------------------------------------------------------- -void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, - const BESTGNSSPOSMem* payload) { - // Buffer to data struct - NavSatFixData nav_sat_fix; - NovToData(header, payload, nav_sat_fix); - - // Publish - if (nav_sat_fix.frame_id == "GNSS1" || nav_sat_fix.frame_id == "GNSS") { - if (navsatfix_gnss1_pub_.getNumSubscribers() > 0) { - sensor_msgs::NavSatFix msg; - NavSatFixDataToMsg(nav_sat_fix, msg); - navsatfix_gnss1_pub_.publish(msg); - } - } else if (nav_sat_fix.frame_id == "GNSS2") { - if (navsatfix_gnss2_pub_.getNumSubscribers() > 0) { - sensor_msgs::NavSatFix msg; - NavSatFixDataToMsg(nav_sat_fix, msg); - navsatfix_gnss2_pub_.publish(msg); - } - } +void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg) { + RtcmCallback(msg->message.data(), msg->message.size()); } +#endif } // namespace fixposition +/* ****************************************************************************************************************** */ + +using namespace fixposition; int main(int argc, char** argv) { +#ifndef NDEBUG + fpsdk::common::app::StacktraceHelper stacktrace; + WARNING("***** Running debug build *****"); +#endif + + bool ok = true; + ros::init(argc, argv, "fixposition_driver"); - ros::NodeHandle node_handle; + ros::NodeHandle node_handle("~"); ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - fixposition::FixpositionDriverParams params; + // Redirect Fixposition SDK logging to ROS console (the logger for these is "ros.fpsdk_ros1") + fpsdk::ros1::utils::RedirectLoggingToRosConsole(); - if (fixposition::LoadParamsFromRos1("~", params)) { - ROS_INFO("Params Loaded!"); - fixposition::FixpositionDriverNode node(params); - ROS_DEBUG("Starting node..."); + // Load parameters + ROS_INFO("Loading parameters..."); + SensorParams sensor_params; + if (!LoadParamsFromRos1("~", sensor_params)) { + ROS_ERROR("Failed loading sensor params"); + ok = false; + } + NodeParams node_params; + if (!LoadParamsFromRos1("~", node_params)) { + ROS_ERROR("Failed loading node params"); + ok = false; + } - node.Run(); - ros::waitForShutdown(); + // Handle CTRL-C / SIGINT ourselves + fpsdk::common::app::SigIntHelper sigint; + + // Start node + std::unique_ptr node; + if (ok) { + try { + node = std::make_unique(sensor_params, node_params, node_handle); + } catch (const std::exception& ex) { + ROS_ERROR("Failed creating node: %s", ex.what()); + ok = false; + } + } + if (ok) { + ROS_INFO("Starting node..."); + if (node->StartNode()) { + ROS_INFO("main() spinning..."); + // Use multiple spinner threads. Callback execute in one of them. + ros::AsyncSpinner spinner(2); + spinner.start(); + sigint.WaitAbort(); + spinner.stop(); + ROS_INFO("main() stopping"); + } else { + ROS_ERROR("Failed starting node"); + ok = false; + } + node.reset(); + } - ROS_DEBUG("Exiting."); - return 0; + // Are we happy? + if (ok) { + ROS_INFO("Done"); } else { - ROS_ERROR("Params Loading Failed!"); - ros::shutdown(); - return 1; + ROS_FATAL("Ouch!"); } + + // Shutdown ROS + ros::shutdown(); + + exit(ok ? EXIT_SUCCESS : EXIT_FAILURE); } + +/* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp index 0337b30..98214c0 100644 --- a/fixposition_driver_ros1/src/params.cpp +++ b/fixposition_driver_ros1/src/params.cpp @@ -1,128 +1,95 @@ /** - * @file - * @brief Implementation of Parameter Loading - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of Parameter Loading */ -/* PACKAGE */ +/* LIBC/STL */ +#include + +/* EXTERNAL */ #include +#include +#include +#include + +/* PACKAGE */ namespace fixposition { +/* ****************************************************************************************************************** */ -bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params) { - const std::string RATE = ns + "/rate"; - const std::string RECONNECT_DELAY = ns + "/reconnect_delay"; - const std::string TYPE = ns + "/type"; - const std::string FORMATS = ns + "/formats"; - const std::string IP = ns + "/ip"; - const std::string PORT = ns + "/port"; - const std::string BAUDRATE = ns + "/baudrate"; - const std::string COV_WARNING = ns + "/cov_warning"; - const std::string NAV2_MODE = ns + "/nav2_mode"; - - // read parameters - if (!ros::param::get(RATE, params.rate)) { - params.rate = 100; - ROS_WARN("Using Default Rate : %d", params.rate); - } - if (!ros::param::get(RECONNECT_DELAY, params.reconnect_delay)) { - params.reconnect_delay = 5.0; - ROS_WARN("Using Default Reconnect Delay : %f", params.reconnect_delay); +using namespace fpsdk::ros1; + +bool LoadParamsFromRos1(const std::string& ns, SensorParams& params) { + bool ok = true; + + if (!utils::LoadRosParam(ns + "/sensor/stream", params.stream_)) { + ROS_WARN("Failed loading /sensor/stream param"); + ok = false; } - if (!ros::param::get(COV_WARNING, params.cov_warning)) { - params.cov_warning = false; - ROS_WARN("Using Default Covariance Warning option : %i", params.cov_warning); + if (!utils::LoadRosParam(ns + "/sensor/reconnect_delay", params.reconnect_delay_)) { + ROS_WARN("Failed loading /sensor/reconnect_delay param"); + ok = false; } - if (!ros::param::get(NAV2_MODE, params.nav2_mode)) { - params.nav2_mode = false; - ROS_WARN("Using Default Nav2 mode option : %i", params.nav2_mode); + if (!utils::LoadRosParam(ns + "/sensor/messages", params.messages_)) { + ROS_WARN("Failed loading /sensor/messages param"); + ok = false; } - - std::string type_str; - if (!ros::param::get(TYPE, type_str)) { - return false; + if (!utils::LoadRosParam(ns + "/sensor/cov_warning", params.cov_warning_)) { + ROS_WARN("Failed loading /sensor/cov_warning param"); + ok = false; } - if (type_str == "tcp") { - params.type = INPUT_TYPE::TCP; - } else if (type_str == "serial") { - params.type = INPUT_TYPE::SERIAL; - } else { - ROS_ERROR("Input type has to be tcp or serial!"); - return false; + if (!utils::LoadRosParam(ns + "/sensor/nav2_mode", params.nav2_mode_)) { + ROS_WARN("Failed loading /sensor/nav2_mode param"); + ok = false; } - if (!ros::param::get(FORMATS, params.formats)) { - params.formats = {"ODOMETRY", "RAWIMU", "CORRIMU", "TF"}; + ROS_INFO("SensorParams: stream=%s", params.stream_.c_str()); + ROS_INFO("SensorParams: reconnect_delay=%.1f", params.reconnect_delay_); + for (std::size_t ix = 0; ix < params.messages_.size(); ix++) { + ROS_INFO("SensorParams: messages[%" PRIuMAX "]=%s", ix, params.messages_[ix].c_str()); } + ROS_INFO("SensorParams: cov_warning=%s", params.cov_warning_ ? "true" : "false"); + ROS_INFO("SensorParams: nav2_mode=%s", params.nav2_mode_ ? "true" : "false"); - for (size_t i = 0; i < params.formats.size(); i++) { - ROS_INFO("%s[%ld] : %s", FORMATS.c_str(), i, params.formats.at(i).c_str()); + // TODO remove + if (!utils::LoadRosParam(ns + "/sensor/formats", params.formats)) { + ROS_WARN("Failed loading /sensor/formats param"); + ok = false; } - - if (params.type == INPUT_TYPE::TCP) { - if (!ros::param::get(IP, params.ip)) { - // default value for IP - params.ip = "10.0.2.1"; - ROS_WARN("Using Default IP : %s", params.ip.c_str()); - } - if (!ros::param::get(PORT, params.port)) { - // default value for TCP port - params.port = "21000"; - ROS_WARN("Using Default Port : %s", params.port.c_str()); - } - ROS_INFO("%s : %s", IP.c_str(), params.ip.c_str()); - ROS_INFO("%s : %s", PORT.c_str(), params.port.c_str()); - } else if (params.type == INPUT_TYPE::SERIAL) { - if (!ros::param::get(BAUDRATE, params.baudrate)) { - // default value for baudrate - params.baudrate = 115200; - } - if (!ros::param::get(PORT, params.port)) { - // default value for serial port - params.port = "/dev/ttyUSB0"; - } - ROS_INFO("%s : %d", BAUDRATE.c_str(), params.baudrate); - ROS_INFO("%s : %s", PORT.c_str(), params.port.c_str()); + for (size_t i = 0; i < params.formats.size(); i++) { + ROS_INFO("formats[%ld] : %s", i, params.formats[i].c_str()); } - return true; + return ok; } -bool LoadParamsFromRos1(const std::string& ns, CustomerInputParams& params) { - const std::string SPEED_TOPIC = ns + "/speed_topic"; - if (!ros::param::get(SPEED_TOPIC, params.speed_topic)) { - // default value for the topic name - params.speed_topic = "/fixposition/speed"; - ROS_WARN("Using Default Speed Topic : %s", params.speed_topic.c_str()); - } - ROS_INFO("%s : %s", SPEED_TOPIC.c_str(), params.speed_topic.c_str()); - - const std::string RTCM_TOPIC = ns + "/rtcm_topic"; - if (!ros::param::get(RTCM_TOPIC, params.rtcm_topic)) { - // default value for the topic name - params.rtcm_topic = "/fixposition/rtcm"; - ROS_WARN("Using Default Rtcm Topic : %s", params.rtcm_topic.c_str()); - } - ROS_INFO("%s : %s", RTCM_TOPIC.c_str(), params.rtcm_topic.c_str()); +// --------------------------------------------------------------------------------------------------------------------- - return true; -} - -bool LoadParamsFromRos1(const std::string& ns, FixpositionDriverParams& params) { +bool LoadParamsFromRos1(const std::string& ns, NodeParams& params) { bool ok = true; - ok &= LoadParamsFromRos1(ns + "fp_output", params.fp_output); - ok &= LoadParamsFromRos1(ns + "customer_input", params.customer_input); + if (!utils::LoadRosParam(ns + "/node/speed_topic", params.speed_topic_)) { + ROS_WARN("Failed loading /node/speed_topic param"); + ok = false; + } + if (!utils::LoadRosParam(ns + "/node/corr_topic", params.corr_topic_)) { + ROS_WARN("Failed loading /node/corr_topic param"); + ok = false; + } + + ROS_INFO("NodeParams: speed_topic=%s", params.speed_topic_.c_str()); + ROS_INFO("NodeParams: corr_topic=%s", params.corr_topic_.c_str()); return ok; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index 96205b0..714396c 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5) project(fixposition_driver_ros2) set(CMAKE_CXX_STANDARD 14) set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic\ +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\ -Wshadow -Wunused-parameter -Wformat -Wpointer-arith") set(CMAKE_CXX_FLAGS_RELEASE "-O3") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -24,6 +24,8 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(fixposition_driver_lib REQUIRED) find_package(rtcm_msgs REQUIRED) +find_package(fpsdk_common REQUIRED) +find_package(fpsdk_ros2 REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/Speed.msg @@ -66,6 +68,7 @@ include_directories( ${rtcm_msgs_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} + ${fpsdk_common_INLCUDE_DIR} ${fpsdk_ros2_INLCUDE_DIR} ) add_executable( @@ -91,6 +94,7 @@ target_link_libraries( ${Boost_LIBRARIES} ${EIGEN3_LIBRARIES} ${cpp_typesupport_target} + ${fpsdk_common_LIBRARIES} ${fpsdk_ros2_LIBRARIES} pthread ) diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index 31959e8..114429a 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -1,28 +1,31 @@ /** - * @file - * @brief Convert Data classes to ROS2 msgs - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Convert Data classes to ROS2 msgs */ -#ifndef __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2__ -#define __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2__ +#ifndef __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__ +#define __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__ -/* FIXPOSITION DRIVER LIB */ -#include +/* LIBC/STL */ + +/* EXTERNAL */ #include +#include /* PACKAGE */ -#include +#include "fixposition_driver_node.hpp" +#include "ros2_msgs.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ /** * @brief Convert to ROS2 message time @@ -51,19 +54,19 @@ inline builtin_interfaces::msg::Time GpsTimeToMsgTime(times::GpsTime input) { * @param[in] data * @param[out] msg */ -void FpToRosMsg( const OdometryData& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const ImuData& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_EOE& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_GNSSANT& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_GNSSCORR& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_IMUBIAS& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_LLH& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_ODOMENU& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_ODOMETRY& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_ODOMSH& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const OdometryData& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const ImuData& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_EOE& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_GNSSANT& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_GNSSCORR& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_IMUBIAS& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_LLH& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_ODOMENU& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_ODOMETRY& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_ODOMSH& data, rclcpp::Publisher::SharedPtr pub); void FpToRosMsg(const FP_ODOMSTATUS& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_TP& data, rclcpp::Publisher::SharedPtr pub); -void FpToRosMsg( const FP_TEXT& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_TP& data, rclcpp::Publisher::SharedPtr pub); +void FpToRosMsg(const FP_TEXT& data, rclcpp::Publisher::SharedPtr pub); void FpToRosMsg(const GP_GGA& data, rclcpp::Publisher::SharedPtr pub); void FpToRosMsg(const GP_GLL& data, rclcpp::Publisher::SharedPtr pub); @@ -130,7 +133,9 @@ void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf * @param[out] static_br_ * @param[out] br_ */ -void PublishNav2Tf(const std::map>& tf_map, std::shared_ptr static_br_, std::shared_ptr br_); +void PublishNav2Tf(const std::map>& tf_map, + std::shared_ptr static_br_, + std::shared_ptr br_); /** * @brief @@ -138,7 +143,8 @@ void PublishNav2Tf(const std::map::SharedPtr pub); +void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, + rclcpp::Publisher::SharedPtr pub); /** * @brief @@ -164,9 +170,10 @@ void OdomToYprMsg(const OdometryData& data, rclcpp::Publisher node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, - const Eigen::MatrixXd& prev_cov, rclcpp::Publisher::SharedPtr pub); +void JumpWarningMsg(std::shared_ptr node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, + const Eigen::MatrixXd& prev_cov, + rclcpp::Publisher::SharedPtr pub); +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__ diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index adf480a..ef1a271 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -1,45 +1,63 @@ /** - * @file - * @brief Declaration of FixpositionDriver ROS2 Node - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of FixpositionDriver ROS2 Node */ -#ifndef __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_ -#define __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_ +#ifndef __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_HPP__ +#define __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_HPP__ -/* SYSTEM / STL */ -#include +/* LIBC/STL */ -/* ROS2 */ -#include -#include +/* EXTERNAL */ +#include -/* FIXPOSITION */ #include -#include +#include /* PACKAGE */ -#include +#include "fixposition_driver_ros2/data_to_ros2.hpp" +#include "fixposition_driver_ros2/params.hpp" +#include "fixposition_driver_ros2/ros2_msgs.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common; + class FixpositionDriverNode : public FixpositionDriver { public: /** - * @brief Construct a new Fixposition Driver Node object + * @brief Constructor * - * @param[in] params + * @param[in] params Parameters */ - FixpositionDriverNode(std::shared_ptr node, const FixpositionDriverParams& params, rclcpp::QoS qos_settings); + FixpositionDriverNode(std::shared_ptr node, const FixpositionDriverParams& params, + rclcpp::QoS qos_settings); - void Run(); + /** + * @brief Destructor + */ + ~FixpositionDriverNode(); + + /** + * @brief Start the node + * + * @returns true on success, false otherwise + */ + bool StartNode(); + + /** + * @brief Stop the node + */ + void StopNode(); void RegisterObservers(); @@ -54,7 +72,8 @@ class FixpositionDriverNode : public FixpositionDriver { * @param[in] header * @param[in] payload */ - void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + void BestGnssPosToPublishNavSatFix(const parser::novb::NovbLongHeader* header, + const parser::novb::NovbBestgnsspos* payload); /** * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete @@ -62,9 +81,9 @@ class FixpositionDriverNode : public FixpositionDriver { * @param[in] data */ void PublishNmea(); - - // ROS node handler - std::shared_ptr node_; + + std::shared_ptr node_; //!< Node handle + rclcpp::Logger logger_; //!< Logger // ROS subscribers rclcpp::Subscription::SharedPtr ws_sub_; //!< wheelspeed message subscriber @@ -72,70 +91,69 @@ class FixpositionDriverNode : public FixpositionDriver { // ROS publishers // FP_A messages - rclcpp::Publisher::SharedPtr fpa_odometry_pub_; //!< FP_A-ODOMETRY message - rclcpp::Publisher::SharedPtr fpa_imubias_pub_; //!< FP_A-IMUBIAS message - rclcpp::Publisher::SharedPtr fpa_eoe_pub_; //!< FP_A-EOE message - rclcpp::Publisher::SharedPtr fpa_llh_pub_; //!< FP_A-LLH message - rclcpp::Publisher::SharedPtr fpa_odomenu_pub_; //!< FP_A-ODOMENU message - rclcpp::Publisher::SharedPtr fpa_odomsh_pub_; //!< FP_A-ODOMSH message - rclcpp::Publisher::SharedPtr fpa_odomstatus_pub_; //!< FP_A-ODOMSTATUS message - rclcpp::Publisher::SharedPtr fpa_gnssant_pub_; //!< FP_A-GNSSANT message - rclcpp::Publisher::SharedPtr fpa_gnsscorr_pub_; //!< FP_A-GNSSCORR message - rclcpp::Publisher::SharedPtr fpa_text_pub_; //!< FP_A-TEXT message - rclcpp::Publisher::SharedPtr fpa_tp_pub_; //!< FP_A-TP message + rclcpp::Publisher::SharedPtr fpa_odometry_pub_; //!< FP_A-ODOMETRY message + rclcpp::Publisher::SharedPtr fpa_imubias_pub_; //!< FP_A-IMUBIAS message + rclcpp::Publisher::SharedPtr fpa_eoe_pub_; //!< FP_A-EOE message + rclcpp::Publisher::SharedPtr fpa_llh_pub_; //!< FP_A-LLH message + rclcpp::Publisher::SharedPtr fpa_odomenu_pub_; //!< FP_A-ODOMENU message + rclcpp::Publisher::SharedPtr fpa_odomsh_pub_; //!< FP_A-ODOMSH message + rclcpp::Publisher::SharedPtr + fpa_odomstatus_pub_; //!< FP_A-ODOMSTATUS message + rclcpp::Publisher::SharedPtr fpa_gnssant_pub_; //!< FP_A-GNSSANT message + rclcpp::Publisher::SharedPtr fpa_gnsscorr_pub_; //!< FP_A-GNSSCORR message + rclcpp::Publisher::SharedPtr fpa_text_pub_; //!< FP_A-TEXT message + rclcpp::Publisher::SharedPtr fpa_tp_pub_; //!< FP_A-TP message // NMEA messages - rclcpp::Publisher::SharedPtr nmea_gpgga_pub_; //!< NMEA-GP-GGA message - rclcpp::Publisher::SharedPtr nmea_gpgll_pub_; //!< NMEA-GP-GLL message - rclcpp::Publisher::SharedPtr nmea_gngsa_pub_; //!< NMEA-GP-GSA message - rclcpp::Publisher::SharedPtr nmea_gpgst_pub_; //!< NMEA-GP-GST message - rclcpp::Publisher::SharedPtr nmea_gxgsv_pub_; //!< NMEA-GP-GSV message - rclcpp::Publisher::SharedPtr nmea_gphdt_pub_; //!< NMEA-GP-HDT message - rclcpp::Publisher::SharedPtr nmea_gprmc_pub_; //!< NMEA-GP-RMC message - rclcpp::Publisher::SharedPtr nmea_gpvtg_pub_; //!< NMEA-GP-VTG message - rclcpp::Publisher::SharedPtr nmea_gpzda_pub_; //!< NMEA-GP-ZDA message + rclcpp::Publisher::SharedPtr nmea_gpgga_pub_; //!< NMEA-GP-GGA message + rclcpp::Publisher::SharedPtr nmea_gpgll_pub_; //!< NMEA-GP-GLL message + rclcpp::Publisher::SharedPtr nmea_gngsa_pub_; //!< NMEA-GP-GSA message + rclcpp::Publisher::SharedPtr nmea_gpgst_pub_; //!< NMEA-GP-GST message + rclcpp::Publisher::SharedPtr nmea_gxgsv_pub_; //!< NMEA-GP-GSV message + rclcpp::Publisher::SharedPtr nmea_gphdt_pub_; //!< NMEA-GP-HDT message + rclcpp::Publisher::SharedPtr nmea_gprmc_pub_; //!< NMEA-GP-RMC message + rclcpp::Publisher::SharedPtr nmea_gpvtg_pub_; //!< NMEA-GP-VTG message + rclcpp::Publisher::SharedPtr nmea_gpzda_pub_; //!< NMEA-GP-ZDA message // ODOMETRY - rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF Odometry - rclcpp::Publisher::SharedPtr odometry_llh_pub_; //!< LLH Odometry - rclcpp::Publisher::SharedPtr odometry_enu_pub_; //!< ENU Odometry - rclcpp::Publisher::SharedPtr odometry_smooth_pub_; //!< Smooth Odometry (ECEF) + rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF Odometry + rclcpp::Publisher::SharedPtr odometry_llh_pub_; //!< LLH Odometry + rclcpp::Publisher::SharedPtr odometry_enu_pub_; //!< ENU Odometry + rclcpp::Publisher::SharedPtr odometry_smooth_pub_; //!< Smooth Odometry (ECEF) // Orientation - rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU - rclcpp::Publisher::SharedPtr eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal + rclcpp::Publisher::SharedPtr + eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU + rclcpp::Publisher::SharedPtr + eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal // IMU - rclcpp::Publisher::SharedPtr rawimu_pub_; //!< Raw IMU data in IMU frame - rclcpp::Publisher::SharedPtr corrimu_pub_; //!< Bias corrected IMU data in IMU frame - rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU data in POI frame + rclcpp::Publisher::SharedPtr rawimu_pub_; //!< Raw IMU data in IMU frame + rclcpp::Publisher::SharedPtr corrimu_pub_; //!< Bias corrected IMU data in IMU frame + rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU data in POI frame // GNSS - rclcpp::Publisher::SharedPtr nmea_pub_; //!< Pose estimation based only on GNSS - rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; //!< GNSS1 position and status - rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; //!< GNSS2 position and status - NmeaMessage nmea_message_; //!< Collector class for NMEA messages - + rclcpp::Publisher::SharedPtr nmea_pub_; //!< Pose estimation based only on GNSS + rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; //!< GNSS1 position and status + rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; //!< GNSS2 position and status + NmeaMessage nmea_message_; //!< Collector class for NMEA messages + // TF std::shared_ptr br_; std::shared_ptr static_br_; // Jump warning topic - rclcpp::Publisher::SharedPtr extras_jump_pub_; //!< Jump warning topic + rclcpp::Publisher::SharedPtr extras_jump_pub_; //!< Jump warning topic // Previous state - Eigen::Vector3d prev_pos; - Eigen::MatrixXd prev_cov; + Eigen::Vector3d prev_pos_; + Eigen::MatrixXd prev_cov_; // Nav2 TF map std::map> tf_map = { - {"ECEFENU0", nullptr}, - {"POIPOISH", nullptr}, - {"ECEFPOISH", nullptr}, - {"ENU0POI", nullptr} - }; + {"ECEFENU0", nullptr}, {"POIPOISH", nullptr}, {"ECEFPOISH", nullptr}, {"ENU0POI", nullptr}}; }; +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_HPP__ diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp index 09349c1..732e6cf 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp @@ -1,61 +1,63 @@ /** - * @file - * @brief Parameters - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Parameters */ #ifndef __FIXPOSITION_DRIVER_ROS2_PARAMS_HPP__ #define __FIXPOSITION_DRIVER_ROS2_PARAMS_HPP__ -/* ROS */ -#include +/* LIBC/STL */ -/* FIXPOSITION */ +/* EXTERNAL */ #include +/* PACKAGE */ + namespace fixposition { +/* ****************************************************************************************************************** */ /** * @brief Load all parameters from ROS parameter server * - * @param[in] node - * @param[in] ns namespace to load the parameters from - * @param[out] params - * @return true - * @return false + * @param[in] node Node handle + * @param[in] ns Namespace to load the parameters from + * @param[out] params The parameters + * + * @returns true on success, false otherwise */ -bool LoadParamsFromRos2(std::shared_ptr node, const std::string& ns, FpOutputParams& params); +bool LoadParamsFromRos2(std::shared_ptr& node, const std::string& ns, FpOutputParams& params); /** * @brief Load all parameters from ROS parameter server * - * @param[in] node - * @param[in] ns namespace to load the parameters from - * @param[out] params - * @return true - * @return false + * @param[in] node Node handle + * @param[in] ns Namespace to load the parameters from + * @param[out] params The parameters + * + * @returns true on success, false otherwise */ -bool LoadParamsFromRos2(std::shared_ptr node, const std::string& ns, CustomerInputParams& params); +bool LoadParamsFromRos2(std::shared_ptr& node, const std::string& ns, CustomerInputParams& params); /** * @brief Load all parameters from ROS parameter server * - * @param[in] node - * @param[out] params - * @return true - * @return false + * @param[in] node Node handle + * @param[in] ns Namespace to load the parameters from + * @param[out] params The parameters + * + * @returns true on success, false otherwise */ -bool LoadParamsFromRos2(std::shared_ptr node, FixpositionDriverParams& params); +bool LoadParamsFromRos2(std::shared_ptr& node, FixpositionDriverParams& params); +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_DRIVER_ROS2_PARAMS_HPP__ diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp index 32347f5..f620501 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -1,47 +1,41 @@ // Wrapper to include ROS2 stuff without quoting the same warning suppressions all over -#ifndef __ROS2_DRIVER_ROS2_MSGS_HPP__ -#define __ROS2_DRIVER_ROS2_MSGS_HPP__ +#ifndef __FIXPOSITION_DRIVER_ROS2_ROS2_MSGS_HPP__ +#define __FIXPOSITION_DRIVER_ROS2_ROS2_MSGS_HPP__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wshadow" -#include -#include -#include - -#include +// Standard ROS messages +#include +#include +#include +#include +#include +#include #include - #include #include - -#include -#include - -#include -#include -#include -#include +#include +#include #include -// Include generic -#include -#include -#include - -// Include RTCM +// RTCM #include -// Include extras +// Fixposition ROS messages +// - Generic +#include +#include +#include +// - Extras #include - -// Include FP-A +// - FP-A +#include #include #include #include -#include #include #include #include @@ -49,24 +43,16 @@ #include #include #include - -// Include NMEA +// - NMEA +#include #include #include -#include #include -#include #include #include #include #include - -#if __has_include() -#include -#else -// This header was deprecated as of ROS2 Humble, but is still required in order to support Foxy. -#include -#endif +#include #pragma GCC diagnostic pop -#endif // __ROS2_DRIVER_ROS2_MSGS_HPP__ +#endif // __FIXPOSITION_DRIVER_ROS2_ROS2_MSGS_HPP__ diff --git a/fixposition_driver_ros2/launch/serial.yaml b/fixposition_driver_ros2/launch/serial.yaml index 3692448..b77062e 100644 --- a/fixposition_driver_ros2/launch/serial.yaml +++ b/fixposition_driver_ros2/launch/serial.yaml @@ -2,7 +2,7 @@ fixposition_driver_ros2: ros__parameters: fp_output: formats: [ - "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", + "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA ] diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml index 5955c72..518b244 100644 --- a/fixposition_driver_ros2/launch/tcp.yaml +++ b/fixposition_driver_ros2/launch/tcp.yaml @@ -2,7 +2,7 @@ fixposition_driver_ros2: ros__parameters: fp_output: formats: [ - "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", + "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA ] diff --git a/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg b/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg index 10ae6fd..5912446 100644 --- a/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg +++ b/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg @@ -65,4 +65,4 @@ geometry_msgs/Vector3 bias_cov_gyr # Gyroscope bias covariance # | 3 | Insufficient motion | # | 4 | Converging | # | 5...6 | Reserved | -# | 7 | Idle | \ No newline at end of file +# | 7 | Idle | diff --git a/fixposition_driver_ros2/msg/nmea/GPHDT.msg b/fixposition_driver_ros2/msg/nmea/GPHDT.msg index 5e4b001..bf76c27 100644 --- a/fixposition_driver_ros2/msg/nmea/GPHDT.msg +++ b/fixposition_driver_ros2/msg/nmea/GPHDT.msg @@ -13,4 +13,4 @@ # Format | Field | Unit | Description # -------|----------|-------|---------------| float64 heading # [deg] | True heading. -char true_ind # [-] | Always T. \ No newline at end of file +char true_ind # [-] | Always T. diff --git a/fixposition_driver_ros2/msg/nmea/GXGSV.msg b/fixposition_driver_ros2/msg/nmea/GXGSV.msg index be4fc4c..ee32b93 100644 --- a/fixposition_driver_ros2/msg/nmea/GXGSV.msg +++ b/fixposition_driver_ros2/msg/nmea/GXGSV.msg @@ -15,8 +15,8 @@ int16 sentences # [-] | Total number of sentences (1 to 9). int16 sent_num # [-] | Sentence number (1 to 9). int32 num_sats # [-] | Total number of satellites in view. -int16[] sat_id # [-] | Satellite ID number. -int16[] elev # [deg] | Satellite elevation. +int16[] sat_id # [-] | Satellite ID number. +int16[] elev # [deg] | Satellite elevation. int16[] azim # [deg] | Satellite azimuth from true North. int16[] cno # [db-Hz] | Satellite SNR (C/No). string signal_id # [Hex] | Signal ID (see below). @@ -33,4 +33,4 @@ string signal_id # [Hex] | Signal ID (see below). # | 1 | BeiDou B1I (talker ID GB) | # | B | BeiDou B2I (talker ID GB) | # | 1 | GLONASS G1 C/A (talker ID GL) | -# | 3 | GLONASS G2 C/A (talker ID GL) | \ No newline at end of file +# | 3 | GLONASS G2 C/A (talker ID GL) | diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml index bf40bcf..0bb9350 100644 --- a/fixposition_driver_ros2/package.xml +++ b/fixposition_driver_ros2/package.xml @@ -25,6 +25,8 @@ tf2_ros tf2_geometry_msgs rtcm_msgs + fpsdk_common + fpsdk_ros2 rclcpp rosidl_default_runtime diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 28048fb..32cb2d5 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -1,27 +1,34 @@ /** - * @file - * @brief Convert Data classes to ROS2 msgs - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Convert Data classes to ROS2 msgs */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include +#include "fixposition_driver_ros2/data_to_ros2.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common; void FpToRosMsg(const fixposition::OdometryData& data, rclcpp::Publisher::SharedPtr pub) { if (pub->get_subscription_count() > 0) { // Create message nav_msgs::msg::Odometry msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = rclcpp::Clock().now(); @@ -43,7 +50,7 @@ void FpToRosMsg(const ImuData& data, rclcpp::Publisher::S if (pub->get_subscription_count() > 0) { // Create message sensor_msgs::msg::Imu msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = rclcpp::Clock().now(); @@ -60,7 +67,6 @@ void FpToRosMsg(const ImuData& data, rclcpp::Publisher::S } } - void FpToRosMsg(const FP_IMUBIAS& data, rclcpp::Publisher::SharedPtr pub) { if (pub->get_subscription_count() > 0) { // Create message @@ -72,7 +78,7 @@ void FpToRosMsg(const FP_IMUBIAS& data, rclcpp::Publisherget_subscription_count() > 0) { // Create message fixposition_driver_ros2::msg::GNSSANT msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = rclcpp::Clock().now(); @@ -106,7 +112,7 @@ void FpToRosMsg(const FP_GNSSANT& data, rclcpp::Publisherpublish(msg); } @@ -116,7 +122,7 @@ void FpToRosMsg(const FP_GNSSCORR& data, rclcpp::Publisherget_subscription_count() > 0) { // Create message fixposition_driver_ros2::msg::GNSSCORR msg; - + // Populate message if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = rclcpp::Clock().now(); @@ -177,7 +183,7 @@ void FpToRosMsg(const FP_ODOMENU& data, rclcpp::Publisherpublish(msg); } @@ -238,7 +244,7 @@ void FpToRosMsg(const FP_ODOMSH& data, rclcpp::Publisherpublish(msg); } @@ -441,7 +447,7 @@ void FpToRosMsg(const GX_GSV& data, rclcpp::Publisher::SharedPtr pub) { if (pub->get_subscription_count() > 0) { // Create message @@ -619,7 +624,9 @@ void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf tf2::toMsg(data.pose.position, tf.transform.translation); } -void PublishNav2Tf(const std::map>& tf_map, std::shared_ptr static_br_, std::shared_ptr br_) { +void PublishNav2Tf(const std::map>& tf_map, + std::shared_ptr static_br_, + std::shared_ptr br_) { if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) { // Publish FP_ECEF -> map tf_map.at("ECEFENU0")->child_frame_id = "map"; @@ -641,7 +648,7 @@ void PublishNav2Tf(const std::map::SharedPtr pub) { +void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, + rclcpp::Publisher::SharedPtr pub) { if (pub->get_subscription_count() > 0) { // Create message sensor_msgs::msg::NavSatFix msg; - + // Populate message header if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { msg.header.stamp = rclcpp::Clock().now(); @@ -688,26 +696,26 @@ void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher> cov_map = Eigen::Map>(msg.position_covariance.data()); - + if (data.odom.pose.position.isZero()) { - msg.latitude = 0; + msg.latitude = 0; msg.longitude = 0; - msg.altitude = 0; + msg.altitude = 0; msg.position_covariance_type = 0; cov_map = Eigen::Matrix3d::Zero(); } else { - const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position); - msg.latitude = RadToDeg(llh_pos(0)); + const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(data.odom.pose.position); + msg.latitude = RadToDeg(llh_pos(0)); msg.longitude = RadToDeg(llh_pos(1)); - msg.altitude = llh_pos(2); + msg.altitude = llh_pos(2); // Populate LLH covariance const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position); + const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(data.odom.pose.position); const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); cov_map = p_cov_l; msg.position_covariance_type = 3; @@ -720,7 +728,8 @@ void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher(NavSatStatusData::Status::STATUS_NO_FIX); msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_NONE); - } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_S2D) || status_flag < static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { + } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_S2D) || + status_flag < static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { msg.status.status = static_cast(NavSatStatusData::Status::STATUS_FIX); msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_ALL); @@ -742,14 +751,14 @@ void OdomToImuMsg(const fixposition::FP_ODOMETRY& data, rclcpp::Publisherget_subscription_count() > 0) { // Create message sensor_msgs::msg::Imu msg; - + // Populate message if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { msg.header.stamp = rclcpp::Clock().now(); } else { msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp); } - + msg.header.frame_id = data.odom.frame_id; tf2::toMsg(data.acceleration, msg.linear_acceleration); tf2::toMsg(data.odom.twist.angular, msg.angular_velocity); @@ -773,7 +782,7 @@ void OdomToYprMsg(const OdometryData& data, rclcpp::Publisher node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, - const Eigen::MatrixXd& prev_cov, rclcpp::Publisher::SharedPtr pub) { +void JumpWarningMsg(std::shared_ptr node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, + const Eigen::MatrixXd& prev_cov, + rclcpp::Publisher::SharedPtr pub) { if (pub->get_subscription_count() > 0) { // Create message fixposition_driver_ros2::msg::COVWARN msg; @@ -799,15 +809,16 @@ void JumpWarningMsg(std::shared_ptr node, const times::GpsTime& st std::stringstream warn_msg; warn_msg << "Position jump detected! The change in position is greater than the estimated covariances. " << "Position difference: [" << pos_diff[0] << ", " << pos_diff[1] << ", " << pos_diff[2] << "], " - << "Covariances: [" << prev_cov(0,0) << ", " << prev_cov(1,1) << ", " << prev_cov(2,2) << "]"; + << "Covariances: [" << prev_cov(0, 0) << ", " << prev_cov(1, 1) << ", " << prev_cov(2, 2) << "]"; RCLCPP_WARN(node->get_logger(), "%s", warn_msg.str().c_str()); tf2::toMsg(pos_diff, msg.jump); - tf2::toMsg(Eigen::Vector3d(prev_cov(0,0),prev_cov(1,1),prev_cov(2,2)), msg.covariance); + tf2::toMsg(Eigen::Vector3d(prev_cov(0, 0), prev_cov(1, 1), prev_cov(2, 2)), msg.covariance); // Publish message pub->publish(msg); } } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index b35c2d2..4b6e2aa 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -1,55 +1,87 @@ /** - * @file - * @brief Main function for the fixposition driver ros node - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Main function for the fixposition driver ros node */ +/* LIBC/STL */ +#include +#include +#include +#include + +/* EXTERNAL */ +#include +#include +#include +#include + /* PACKAGE */ -#include +#include "fixposition_driver_ros2/fixposition_driver_node.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ -FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, const FixpositionDriverParams& params, rclcpp::QoS qos_settings) - : FixpositionDriver(params), - node_(node), +FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, const FixpositionDriverParams& params, + rclcpp::QoS qos_settings) /* clang-format off */ : + FixpositionDriver(params), + node_ { node }, + logger_ { rclcpp::get_logger("fixposition_driver") }, // FP_A messages - fpa_odometry_pub_(node_->create_publisher("/fixposition/fpa/odometry", qos_settings)), - fpa_imubias_pub_(node_->create_publisher("/fixposition/fpa/imubias", qos_settings)), + fpa_odometry_pub_( + node_->create_publisher("/fixposition/fpa/odometry", qos_settings)), + fpa_imubias_pub_( + node_->create_publisher("/fixposition/fpa/imubias", qos_settings)), fpa_eoe_pub_(node_->create_publisher("/fixposition/fpa/eoe", qos_settings)), fpa_llh_pub_(node_->create_publisher("/fixposition/fpa/llh", qos_settings)), - fpa_odomenu_pub_(node_->create_publisher("/fixposition/fpa/odomenu", qos_settings)), - fpa_odomsh_pub_(node_->create_publisher("/fixposition/fpa/odomsh", qos_settings)), - fpa_odomstatus_pub_(node_->create_publisher("/fixposition/fpa/odomstatus", qos_settings)), - fpa_gnssant_pub_(node_->create_publisher("/fixposition/fpa/gnssant", qos_settings)), - fpa_gnsscorr_pub_(node_->create_publisher("/fixposition/fpa/gnsscorr", qos_settings)), + fpa_odomenu_pub_( + node_->create_publisher("/fixposition/fpa/odomenu", qos_settings)), + fpa_odomsh_pub_( + node_->create_publisher("/fixposition/fpa/odomsh", qos_settings)), + fpa_odomstatus_pub_(node_->create_publisher( + "/fixposition/fpa/odomstatus", qos_settings)), + fpa_gnssant_pub_( + node_->create_publisher("/fixposition/fpa/gnssant", qos_settings)), + fpa_gnsscorr_pub_( + node_->create_publisher("/fixposition/fpa/gnsscorr", qos_settings)), fpa_text_pub_(node_->create_publisher("/fixposition/fpa/text", qos_settings)), fpa_tp_pub_(node_->create_publisher("/fixposition/fpa/tp", qos_settings)), // NMEA messages - nmea_gpgga_pub_(node_->create_publisher("/fixposition/nmea/gpgga", qos_settings)), - nmea_gpgll_pub_(node_->create_publisher("/fixposition/nmea/gpgll", qos_settings)), - nmea_gngsa_pub_(node_->create_publisher("/fixposition/nmea/gngsa", qos_settings)), - nmea_gpgst_pub_(node_->create_publisher("/fixposition/nmea/gpgst", qos_settings)), - nmea_gxgsv_pub_(node_->create_publisher("/fixposition/nmea/gxgsv", qos_settings)), - nmea_gphdt_pub_(node_->create_publisher("/fixposition/nmea/gphdt", qos_settings)), - nmea_gprmc_pub_(node_->create_publisher("/fixposition/nmea/gprmc", qos_settings)), - nmea_gpvtg_pub_(node_->create_publisher("/fixposition/nmea/gpvtg", qos_settings)), - nmea_gpzda_pub_(node_->create_publisher("/fixposition/nmea/gpzda", qos_settings)), + nmea_gpgga_pub_( + node_->create_publisher("/fixposition/nmea/gpgga", qos_settings)), + nmea_gpgll_pub_( + node_->create_publisher("/fixposition/nmea/gpgll", qos_settings)), + nmea_gngsa_pub_( + node_->create_publisher("/fixposition/nmea/gngsa", qos_settings)), + nmea_gpgst_pub_( + node_->create_publisher("/fixposition/nmea/gpgst", qos_settings)), + nmea_gxgsv_pub_( + node_->create_publisher("/fixposition/nmea/gxgsv", qos_settings)), + nmea_gphdt_pub_( + node_->create_publisher("/fixposition/nmea/gphdt", qos_settings)), + nmea_gprmc_pub_( + node_->create_publisher("/fixposition/nmea/gprmc", qos_settings)), + nmea_gpvtg_pub_( + node_->create_publisher("/fixposition/nmea/gpvtg", qos_settings)), + nmea_gpzda_pub_( + node_->create_publisher("/fixposition/nmea/gpzda", qos_settings)), // ODOMETRY odometry_ecef_pub_(node_->create_publisher("/fixposition/odometry_ecef", qos_settings)), - odometry_llh_pub_(node_->create_publisher("/fixposition/odometry_llh", qos_settings)), + odometry_llh_pub_( + node_->create_publisher("/fixposition/odometry_llh", qos_settings)), odometry_enu_pub_(node_->create_publisher("/fixposition/odometry_enu", qos_settings)), - odometry_smooth_pub_(node_->create_publisher("/fixposition/odometry_smooth", qos_settings)), + odometry_smooth_pub_( + node_->create_publisher("/fixposition/odometry_smooth", qos_settings)), // Orientation eul_pub_(node_->create_publisher("/fixposition/ypr", qos_settings)), @@ -79,34 +111,53 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, // Configure jump warning message if (params_.fp_output.cov_warning) { - extras_jump_pub_ = node_->create_publisher("/fixposition/extras/jump", qos_settings); - prev_pos.setZero(); - prev_cov.setZero(); + extras_jump_pub_ = + node_->create_publisher("/fixposition/extras/jump", qos_settings); + prev_pos_.setZero(); + prev_cov_.setZero(); } RegisterObservers(); } -void FixpositionDriverNode::Run() { - rclcpp::Rate rate(params_.fp_output.rate); - const auto reconnect_delay = - std::chrono::nanoseconds((uint64_t)params_.fp_output.reconnect_delay * 1000 * 1000 * 1000); - - while (rclcpp::ok()) { - // Read data and publish to ros - const bool connection_ok = RunOnce(); - // process Incoming ROS msgs - rclcpp::spin_some(node_); - // Handle connection loss - if (!connection_ok) { - printf("Reconnecting in %.1f seconds ...\n", params_.fp_output.reconnect_delay); - rclcpp::sleep_for(reconnect_delay); - Connect(); - } else { - rate.sleep(); // ensure the loop runs at the desired rate - } - } +FixpositionDriverNode::~FixpositionDriverNode() {} + +// --------------------------------------------------------------------------------------------------------------------- + +bool FixpositionDriverNode::StartNode() { + RCLCPP_INFO(logger_, "Starting node..."); + + return true; +} + + +void FixpositionDriverNode::StopNode() { + RCLCPP_INFO(logger_, "Stopping..."); } + +// --------------------------------------------------------------------------------------------------------------------- + +// void FixpositionDriverNode::Run() { +// rclcpp::Rate rate(params_.fp_output.rate); +// const auto reconnect_delay = +// std::chrono::nanoseconds((uint64_t)params_.fp_output.reconnect_delay * 1000 * 1000 * 1000); + +// while (rclcpp::ok()) { +// // Read data and publish to ros +// const bool connection_ok = RunOnce(); +// // process Incoming ROS msgs +// rclcpp::spin_some(node_); +// // Handle connection loss +// if (!connection_ok) { +// printf("Reconnecting in %.1f seconds ...\n", params_.fp_output.reconnect_delay); +// rclcpp::sleep_for(reconnect_delay); +// Connect(); +// } else { +// rate.sleep(); // ensure the loop runs at the desired rate +// } +// } +// } + void FixpositionDriverNode::RegisterObservers() { // NOV_B bestgnsspos_obs_.push_back(std::bind(&FixpositionDriverNode::BestGnssPosToPublishNavSatFix, this, @@ -124,15 +175,16 @@ void FixpositionDriverNode::RegisterObservers() { // Output jump warning if (params_.fp_output.cov_warning) { - if (!prev_pos.isZero() && !prev_cov.isZero()) { - Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs(); + if (!prev_pos_.isZero() && !prev_cov_.isZero()) { + Eigen::Vector3d pos_diff = (prev_pos_ - data.odom.pose.position).cwiseAbs(); - if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) { - JumpWarningMsg(node_, data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_); + if ((pos_diff[0] > prev_cov_(0, 0)) || (pos_diff[1] > prev_cov_(1, 1)) || + (pos_diff[2] > prev_cov_(2, 2))) { + JumpWarningMsg(node_, data.odom.stamp, pos_diff, prev_cov_, extras_jump_pub_); } } - prev_pos = data.odom.pose.position; - prev_cov = data.odom.pose.cov; + prev_pos_ = data.odom.pose.position; + prev_cov_ = data.odom.pose.cov; } }); } else if (format == "ODOMENU") { @@ -171,18 +223,18 @@ void FixpositionDriverNode::RegisterObservers() { dynamic_cast*>(a_converters_["IMUBIAS"].get()) ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); }); } else if (format == "EOE") { - dynamic_cast*>(a_converters_["EOE"].get()) - ->AddObserver([this](const FP_EOE& data) { - FpToRosMsg(data, fpa_eoe_pub_); + dynamic_cast*>(a_converters_["EOE"].get())->AddObserver([this](const FP_EOE& data) { + FpToRosMsg(data, fpa_eoe_pub_); - // Generate Nav2 TF tree - if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) { - PublishNav2Tf(tf_map, static_br_, br_); - } - }); + // Generate Nav2 TF tree + if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) { + PublishNav2Tf(tf_map, static_br_, br_); + } + }); } else if (format == "LLH") { - dynamic_cast*>(a_converters_["LLH"].get()) - ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); }); + dynamic_cast*>(a_converters_["LLH"].get())->AddObserver([this](const FP_LLH& data) { + FpToRosMsg(data, fpa_llh_pub_); + }); } else if (format == "GNSSANT") { dynamic_cast*>(a_converters_["GNSSANT"].get()) ->AddObserver([this](const FP_GNSSANT& data) { FpToRosMsg(data, fpa_gnssant_pub_); }); @@ -204,11 +256,11 @@ void FixpositionDriverNode::RegisterObservers() { // TF Observer Lambda geometry_msgs::msg::TransformStamped tf; TfDataToMsg(data.tf, tf); - if (tf.child_frame_id == "FP_IMUH" && tf.header.frame_id == "FP_POI") { + if ((tf.child_frame_id == "FP_IMUH") && (tf.header.frame_id == "FP_POI")) { br_->sendTransform(tf); // Publish Pitch Roll based on IMU only - Eigen::Vector3d imu_ypr_eigen = QuatToEul(data.tf.rotation); + Eigen::Vector3d imu_ypr_eigen = trafo::QuatToEul(data.tf.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone geometry_msgs::msg::Vector3Stamped imu_ypr; imu_ypr.header.stamp = tf.header.stamp; @@ -218,7 +270,7 @@ void FixpositionDriverNode::RegisterObservers() { imu_ypr.vector.set__z(imu_ypr_eigen.z()); eul_imu_pub_->publish(imu_ypr); - } else if (tf.child_frame_id == "FP_POISH" && tf.header.frame_id == "FP_POI") { + } else if ((tf.child_frame_id == "FP_POISH") && (tf.header.frame_id == "FP_POI")) { br_->sendTransform(tf); // Append TF if Nav2 mode is selected @@ -226,7 +278,7 @@ void FixpositionDriverNode::RegisterObservers() { // Get FP_POI -> FP_POISH tf_map["POIPOISH"] = std::make_shared(tf); } - } else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") { + } else if ((tf.child_frame_id == "FP_ENU0") && (tf.header.frame_id == "FP_ECEF")) { static_br_->sendTransform(tf); // Append TF if Nav2 mode is selected @@ -240,8 +292,9 @@ void FixpositionDriverNode::RegisterObservers() { } }); } else if (format == "TP") { - dynamic_cast*>(a_converters_["TP"].get()) - ->AddObserver([this](const FP_TP& data) { FpToRosMsg(data, fpa_tp_pub_); }); + dynamic_cast*>(a_converters_["TP"].get())->AddObserver([this](const FP_TP& data) { + FpToRosMsg(data, fpa_tp_pub_); + }); } else if (format == "GPGGA") { dynamic_cast*>(a_converters_["GPGGA"].get())->AddObserver([this](const GP_GGA& data) { FpToRosMsg(data, nmea_gpgga_pub_); @@ -429,8 +482,8 @@ void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::msg::Message::Const RtcmCallback(rtcm_msg, msg_size); } -void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, - const BESTGNSSPOSMem* payload) { +void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const parser::novb::NovbLongHeader* header, + const parser::novb::NovbBestgnsspos* payload) { // Buffer to data struct NavSatFixData nav_sat_fix; NovToData(header, payload, nav_sat_fix); @@ -451,39 +504,112 @@ void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeade } } +/* ****************************************************************************************************************** */ } // namespace fixposition int main(int argc, char** argv) { +#ifndef NDEBUG + fpsdk::common::app::StacktraceHelper stacktrace; + WARNING("***** Running debug build *****"); +#endif + + auto logger = rclcpp::get_logger("node_main"); + + bool ok = true; + + // Initialise, create node rclcpp::init(argc, argv); std::shared_ptr node = rclcpp::Node::make_shared("fixposition_driver"); - fixposition::FixpositionDriverParams params; - RCLCPP_INFO(node->get_logger(), "Starting node..."); + // Redirect Fixposition SDK logging to ROS console + fpsdk::ros2::utils::RedirectLoggingToRosConsole(); - if (fixposition::LoadParamsFromRos2(node, params)) { - RCLCPP_INFO(node->get_logger(), "Params Loaded!"); - - // Define ROS QoS - rclcpp::QoS qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default); + // Load parameters + RCLCPP_INFO(logger, "Loading parameters..."); + fixposition::FixpositionDriverParams params; + if (!fixposition::LoadParamsFromRos2(node, params)) { + RCLCPP_ERROR(logger, "Failed loading params"); + ok = false; + } - if (params.fp_output.qos_type == "sensor_short") { // Short-queue sensor-type QoS + // Define ROS QoS + rclcpp::QoS qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default); + if (ok) { + // Short-queue sensor-type QoS + if (params.fp_output.qos_type == "sensor_short") { qos_settings = rclcpp::QoS(rclcpp::KeepLast(5), rmw_qos_profile_sensor_data); - } else if (params.fp_output.qos_type == "sensor_long") { // Long-queue sensor-type QoS + } + // Long-queue sensor-type QoS + else if (params.fp_output.qos_type == "sensor_long") { qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_sensor_data); - } else if (params.fp_output.qos_type == "default_short") { // Short-queue default-type QoS + } + // Short-queue default-type QoS + else if (params.fp_output.qos_type == "default_short") { qos_settings = rclcpp::QoS(rclcpp::KeepLast(5), rmw_qos_profile_default); - } else if (params.fp_output.qos_type == "default_long") { // Long-queue default-type QoS + } + // Long-queue default-type QoS + else if (params.fp_output.qos_type == "default_long") { qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default); - } else { // Default QoS profile + } + // Default QoS profile + else { qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default); } + } + + // Handle CTRL-C / SIGINT ourselves + fpsdk::common::app::SigIntHelper sigint; + + // Start node + std::unique_ptr driver_node; + if (ok) { + try { + driver_node = std::make_unique(node, params, qos_settings); + } catch (const std::exception& ex) { + RCLCPP_ERROR(logger, "Failed creating node: %s", ex.what()); + ok = false; + } + } + + if (ok) { + RCLCPP_INFO(logger, "Starting node..."); + if (driver_node->StartNode()) { + RCLCPP_INFO(logger, "main() spinning..."); + + // Do the same as rclpp::spin(), but also handle CTRL-C / SIGINT nicely + // Callbacks execute in main thread + while (rclcpp::ok() && !sigint.ShouldAbort()) { + rclcpp::spin_until_future_complete( + node, std::promise().get_future(), std::chrono::milliseconds(337)); + } + + // TODO: we'd rather do this, but it doesn't seem to work + // Use multiple spinner threads. Callback execute in one of them. + // rclcpp::executors::MultiThreadedExecutor executor{ rclcpp::ExecutorOptions(), 2 }; + // executor.add_node(node); + // while (rclcpp::ok() && !sigint.ShouldAbort()) { + // executor.spin_once(std::chrono::milliseconds(345)); + // } + + RCLCPP_INFO(logger, "main() stopping"); + } else { + RCLCPP_ERROR(logger, "Failed starting node"); + ok = false; + } + driver_node.reset(); + } - fixposition::FixpositionDriverNode driver_node(node, params, qos_settings); - driver_node.Run(); - RCLCPP_INFO(node->get_logger(), "Exiting."); + // Are we happy? + if (ok) { + RCLCPP_INFO(logger, "Done"); } else { - RCLCPP_ERROR(node->get_logger(), "Params Loading Failed!"); - rclcpp::shutdown(); - return 1; + RCLCPP_FATAL(logger, "Ouch!"); } + + // Shutdown ROS + rclcpp::shutdown(); + + exit(ok ? EXIT_SUCCESS : EXIT_FAILURE); } + +/* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros2/src/params.cpp b/fixposition_driver_ros2/src/params.cpp index 90376aa..79ad510 100644 --- a/fixposition_driver_ros2/src/params.cpp +++ b/fixposition_driver_ros2/src/params.cpp @@ -1,24 +1,28 @@ /** - * @file - * @brief Implementation of Parameter Loading - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of Parameter Loading */ +/* LIBC/STL */ + +/* EXTERNAL */ +#include + /* PACKAGE */ -#include +#include "fixposition_driver_ros2/params.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ -bool LoadParamsFromRos2(std::shared_ptr node, const std::string& ns, FpOutputParams& params) { - const std::string RATE = ns + ".rate"; +bool LoadParamsFromRos2(std::shared_ptr& node, const std::string& ns, FpOutputParams& params) { const std::string RECONNECT_DELAY = ns + ".reconnect_delay"; const std::string TYPE = ns + ".type"; const std::string FORMATS = ns + ".formats"; @@ -29,7 +33,6 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n const std::string COV_WARNING = ns + ".cov_warning"; const std::string NAV2_MODE = ns + ".nav2_mode"; - node->declare_parameter(RATE, 100); node->declare_parameter(RECONNECT_DELAY, 5.0); node->declare_parameter(TYPE, "tcp"); node->declare_parameter(FORMATS, std::vector()); @@ -40,11 +43,6 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n node->declare_parameter(COV_WARNING, false); node->declare_parameter(NAV2_MODE, false); // read parameters - if (node->get_parameter(RATE, params.rate)) { - RCLCPP_INFO(node->get_logger(), "%s : %d", RATE.c_str(), params.rate); - } else { - RCLCPP_WARN(node->get_logger(), "Using Default %s : %d", RATE.c_str(), params.rate); - } if (node->get_parameter(RECONNECT_DELAY, params.reconnect_delay)) { RCLCPP_INFO(node->get_logger(), "%s : %f", RECONNECT_DELAY.c_str(), params.reconnect_delay); } else { @@ -106,7 +104,7 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n return true; } -bool LoadParamsFromRos2(std::shared_ptr node, const std::string& ns, CustomerInputParams& params) { +bool LoadParamsFromRos2(std::shared_ptr& node, const std::string& ns, CustomerInputParams& params) { const std::string SPEED_TOPIC = ns + ".speed_topic"; node->declare_parameter(SPEED_TOPIC, "/fixposition/speed"); node->get_parameter(SPEED_TOPIC, params.speed_topic); @@ -120,7 +118,7 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n return true; } -bool LoadParamsFromRos2(std::shared_ptr node, FixpositionDriverParams& params) { +bool LoadParamsFromRos2(std::shared_ptr& node, FixpositionDriverParams& params) { bool ok = true; ok &= LoadParamsFromRos2(node, "fp_output", params.fp_output); @@ -129,4 +127,5 @@ bool LoadParamsFromRos2(std::shared_ptr node, FixpositionDriverPar return ok; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp index 27fee2d..33cd03b 100644 --- a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp +++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp @@ -1,25 +1,29 @@ /** - * @file - * @brief Declaration of OdomConverter class - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of OdomConverter class */ -#ifndef __ODOM_CONVERTER_HPP__ -#define __ODOM_CONVERTER_HPP__ +#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ODOM_CONVERTER_HPP__ +#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ODOM_CONVERTER_HPP__ + +/* LIBC/STL */ /* EXTERNAL */ -#include -#include + +/* PACKAGE */ +#include "fixposition_odometry_converter_ros1/params.hpp" +#include "fixposition_odometry_converter_ros1/ros_msgs.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ class OdomConverter { public: @@ -77,5 +81,7 @@ class OdomConverter { OdomInputParams params_; }; + +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif //__FIXPOSITION_DRIVER_FIXPOSITION_DRIVER__ +#endif //__FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ODOM_CONVERTER_HPP__ diff --git a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp index dda2432..9d74030 100644 --- a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp +++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp @@ -1,24 +1,28 @@ /** - * @file - * @brief Parameters for the odometry converter - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Parameters for the odometry converter */ -#ifndef __ODOM_CONVERTER_PARAMS_HPP__ -#define __ODOM_CONVERTER_PARAMS_HPP__ +#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_PARAMS_HPP__ +#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_PARAMS_HPP__ -/* SYSTEM / STL */ +/* LIBC/STL */ #include +/* EXTERNAL */ + +/* PACKAGE */ + namespace fixposition { +/* ****************************************************************************************************************** */ enum class VelTopicType : int8_t { Twist = 0, TwistWithCov = 1, Odometry = 2 }; @@ -40,6 +44,6 @@ struct OdomInputParams { bool LoadFromRos(const std::string& ns); }; +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_PARAMS_HPP__ diff --git a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp index b5760d5..f2355d5 100644 --- a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp +++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp @@ -1,24 +1,21 @@ // Wrapper to include ROS stuff without quoting the same warning suppressions all over -#ifndef __ROS1_DRIVER_CONVERTER_MSGS_HPP__ -#define __ROS1_DRIVER_CONVERTER_MSGS_HPP__ +#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ROS_MSGS_HPP__ +#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ROS_MSGS_HPP__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wshadow" -#include -#include - -#include - -#include -#include +#include #include #include #include #include - -#include +#include +#include +#include +#include +#include #pragma GCC diagnostic pop -#endif // __ROS1_DRIVER_CONVERTER_MSGS_HPP__ +#endif // __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ROS_MSGS_HPP__ diff --git a/fixposition_odometry_converter_ros1/src/odom_converter.cpp b/fixposition_odometry_converter_ros1/src/odom_converter.cpp index d50b7f1..009a6cc 100644 --- a/fixposition_odometry_converter_ros1/src/odom_converter.cpp +++ b/fixposition_odometry_converter_ros1/src/odom_converter.cpp @@ -1,21 +1,26 @@ /** - * @file - * @brief Implementation of OdomConverter class - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of OdomConverter class */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_odometry_converter_ros1/odom_converter.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ + OdomConverter::OdomConverter(ros::NodeHandle* nh) : nh_(*nh) { // read parameters if (!params_.LoadFromRos("/fixposition_odometry_converter_ros1")) { @@ -89,4 +94,5 @@ void OdomConverter::TwistCallback(const geometry_msgs::TwistConstPtr& msg) { ConvertAndPublish(velocities); } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp b/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp index b8385fc..9e93440 100644 --- a/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp +++ b/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp @@ -1,19 +1,22 @@ /** - * @file - * @brief Main function for the odometry converter ros node - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Main function for the odometry converter ros node */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_odometry_converter_ros1/odom_converter.hpp" int main(int argc, char** argv) { ros::init(argc, argv, "fixposition_odometry_converter_ros1"); diff --git a/fixposition_odometry_converter_ros1/src/params.cpp b/fixposition_odometry_converter_ros1/src/params.cpp index 2b6665d..f5fff92 100644 --- a/fixposition_odometry_converter_ros1/src/params.cpp +++ b/fixposition_odometry_converter_ros1/src/params.cpp @@ -1,22 +1,27 @@ /** - * @file - * @brief Implementation of Parameter Loading for the odometry parameters - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of Parameter Loading for the odometry parameters */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include -#include +#include "fixposition_odometry_converter_ros1/params.hpp" + +#include "fixposition_odometry_converter_ros1/ros_msgs.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ bool OdomInputParams::LoadFromRos(const std::string& ns) { if (!ros::param::get(ns + "/fixposition_speed_topic", fixposition_speed_topic)) @@ -48,4 +53,5 @@ bool OdomInputParams::LoadFromRos(const std::string& ns) { return true; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp index a879724..e41ea21 100644 --- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp +++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp @@ -1,26 +1,28 @@ /** - * @file - * @brief Declaration of OdomConverterNode class - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - * Port to ROS 2 by Husarion + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Declaration of OdomConverterNode class */ -#ifndef __ODOM_CONVERTER_NODE_HPP__ -#define __ODOM_CONVERTER_NODE_HPP__ +#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ODOM_CONVERTER_HPP__ +#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ODOM_CONVERTER_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ /* PACKAGE */ -#include +#include "fixposition_odometry_converter_ros2/params.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ class OdomConverterNode : public rclcpp::Node { public: @@ -76,5 +78,7 @@ class OdomConverterNode : public rclcpp::Node { rclcpp::SubscriptionBase::SharedPtr ws_sub_; rclcpp::Publisher::SharedPtr ws_pub_; }; + +/* ****************************************************************************************************************** */ } // namespace fixposition -#endif //__ODOM_CONVERTER_NODE_HPP__ +#endif //__FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ODOM_CONVERTER_HPP__ diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp index 181571d..fbafe24 100644 --- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp +++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp @@ -1,29 +1,29 @@ /** - * @file - * @brief Parameters for the odometry converter - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - * Port to ROS 2 by Husarion + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Parameters for the odometry converter */ -#ifndef __ODOM_CONVERTER_PARAMS_HPP__ -#define __ODOM_CONVERTER_PARAMS_HPP__ +#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_PARAMS_HPP__ +#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_PARAMS_HPP__ -/* SYSTEM / STL */ +/* LIBC/STL */ #include +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_odometry_converter_ros2/ros2_msgs.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ enum class VelTopicType : int8_t { Twist = 0, TwistWithCov = 1, Odometry = 2 }; @@ -47,6 +47,6 @@ struct OdomInputParams { const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_itf); }; +/* ****************************************************************************************************************** */ } // namespace fixposition - -#endif +#endif // __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_PARAMS_HPP__ diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp index 0d0cc97..c2016c3 100644 --- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp +++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp @@ -1,19 +1,16 @@ // Wrapper to include ROS2 stuff without quoting the same warning suppressions all over -#ifndef __ROS2_DRIVER_CONVERTER_MSGS_HPP__ -#define __ROS2_DRIVER_CONVERTER_MSGS_HPP__ +#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ROS2_MSGS_HPP__ +#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ROS2_MSGS_HPP__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wshadow" -#include - -#include - +#include #include #include - -#include +#include +#include #pragma GCC diagnostic pop -#endif // __ROS2_DRIVER_CONVERTER_MSGS_HPP__ +#endif // __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ROS2_MSGS_HPP__ diff --git a/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp b/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp index 21214a3..5a0ecc5 100644 --- a/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp +++ b/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp @@ -1,23 +1,25 @@ /** - * @file - * @brief Implementation of OdomConverterNode class - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - * Port to ROS 2 by Husarion + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of OdomConverterNode class */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_odometry_converter_ros2/odom_converter_node.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ OdomConverterNode::OdomConverterNode(const rclcpp::NodeOptions& options) : Node("odom_converter", options) { // read parameters @@ -95,8 +97,9 @@ void OdomConverterNode::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr ConvertAndPublish(velocities); } +/* ****************************************************************************************************************** */ } // namespace fixposition // Register the component with class_loader #include -RCLCPP_COMPONENTS_REGISTER_NODE(fixposition::OdomConverterNode) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(fixposition::OdomConverterNode) diff --git a/fixposition_odometry_converter_ros2/src/params.cpp b/fixposition_odometry_converter_ros2/src/params.cpp index 709e98b..0d5350d 100644 --- a/fixposition_odometry_converter_ros2/src/params.cpp +++ b/fixposition_odometry_converter_ros2/src/params.cpp @@ -1,23 +1,25 @@ /** - * @file - * @brief Implementation of Parameter Loading for the odometry parameters - * * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - * Port to ROS 2 by Husarion + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ * \endverbatim * + * @file + * @brief Implementation of Parameter Loading for the odometry parameters */ +/* LIBC/STL */ + +/* EXTERNAL */ + /* PACKAGE */ -#include +#include "fixposition_odometry_converter_ros2/params.hpp" namespace fixposition { +/* ****************************************************************************************************************** */ bool OdomInputParams::LoadFromRos(const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& param_itf, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_itf) { @@ -67,4 +69,5 @@ bool OdomInputParams::LoadFromRos(const rclcpp::node_interfaces::NodeParametersI return true; } +/* ****************************************************************************************************************** */ } // namespace fixposition diff --git a/precommit.sh b/precommit.sh new file mode 100755 index 0000000..9c0db5e --- /dev/null +++ b/precommit.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -eEu +set -o pipefail +set -o errtrace + +SCRIPTDIR=$(dirname $(readlink -f $0)) +cd ${SCRIPTDIR} +pre-commit run --all-files --hook-stage manual