diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0a9e6bec..851d9906 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,308 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.2 (2023-11-19) +------------------ +* Commits + * Merge pull request `#97 `_ from thomasemter/dev/next + Integrate README changes from master + * Fix topics namespace + * Fix units of imu angular rates + * Merge upstream README pt2 + * Merge upstream README + * Update README.md + * Update README.md + * Update README.md + * v1.3.1 + * Updated package.xml + * v1.3.1 + * Updated package.xml + * v1.3.1 + * Updated package.xml + * v1.3.1 + * Updated changelog + * Merge pull request `#95 `_ from thomasemter/dev/next + Fix navsatfix and gpsfix frame ids + * Update README.md + * Fix navsatfix and gpsfix frame ids + * Merge pull request `#92 `_ from thomasemter/dev/next + Fix single antenna receiver setup + * Update changelog + * Merge + * Fix single antenna receiver setup + * Merge pull request `#90 `_ from thomasemter/dev/next + Fix empty headers + * Merge branch 'dev' into dev/next + * Bump version + * Fix empty headers + * Merge pull request `#88 `_ from thomasemter/dev/next + Fix navsatfix containing only zeros for INS + * Align indent + * Fix navsatfix containig only zeros for INS + * Merge pull request `#87 `_ from thomasemter/dev/next + Update firmware info + * Reduce INS firmware version info to released version + * Update firmware info + * v1.3.0 + * Updated package.xml + * v1.3.0 + * Update firmware info + * Updated package.xml + * v1.3.0 + * Merge pull request `#84 `_ from thomasemter/dev/next + Update readme + * Add expected release dates + * Add known issues to readme + * Update version + * Update readme + * Merge pull request `#81 `_ from thomasemter/dev/next + Fix spelling + * Improve explanations in readme + * Categorize stream params + * Add keep alive check for TCP + * Fix spelling + * Change angle wrapping + * Add TCP communication via static IP server + * Add units to msgs + * Fix spelling + * Merge pull request `#75 `_ from thomasemter/dev/next + upcoming release + * Add heading to GPSFix msg + * Move constant + * Change log level of firmware check + * Add improved VSM handling + * Change INS in GNSS node detection to auto + * Fix invald v_x var case + * Refine readme on UDP + * Improve server duplicate check + * Add more info un UDP configuration + * Fix publish check + * Add more publishing checks for configured Rx + * Add const for max udp packet size + * Update readme and changelog + * Add device check to node + * Add checks for IP server duplicates + * Add latency compensation to att msgs + * Add device check logic + * Add UDP params and setup logic + * Fix multi msg per packet + * Fix localization stamp and tf publishing + * Change VSM to be averaged and published with 2 Hz + * Always publish raw IMU data as indicated + * Change to empty fields + * Refine diagnostics naming scheme and add trigger to ensure emission of ReceiverSetup + * Change diagnostics naming scheme + * Expand readme on AIM+ + * Reformulate readme about ROS and ROS2 + * Rename msg var + * Add custom message to report AIM+ status + * Catch invalid UTM conversion + * Robustify command reset + * Add RFStatus diagnostics + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Add VelCovCartesian output + * Add VelCovCartesian output + * Refine Rx type check + * Ensure latency reporting block is activated + * Add option for latency compensation + * Fix param type misinterpretation + * Add missing param to example in readme + * Add OSNMA diagnostics output + * Add keep_open option to OSNMA + * Add OSNMA msg + * Update changelog + * Refine README and fix compiled message logic + * Update changelog + * Add warning for configuring INS as GNSS + * Add warn log for misconfiguration + * Fix pose publishing rate + * Fix navsatfix publishing + * Make vars const + * Replace log functions + * Add small fixes and cleanup + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Add option to bypass configuration of Rx + * Add option to bypass confugration of Rx + * Add diagnostics and time ref msgs again + * Update README on how to add new messages + * Add automiatic detection of serial port ID + * Refine changelog + * Change invalid timestamp handling for reading from file + * Add USB serial by ID info to README + * Fix leap seconds for timestamp if reading from file + * Fix reconnection logic + * Replace flow control setup + * Refactor ioservice + * notify semaphores in destructor + * Send port reset only once + * Fix serial connection + * Fix talker ID setting for INS + * Add NMEA talker ID setting to ensure reception + * Prepare communication for UDP option (still inactive) + * Fix UDP message identification logic + * Add test code for UDP, WIP + * Add UDP client, WIP + * Set do-not-use for temp to NaN + * Add processing latency correction for ROS timestamps + * Fix extsensor parser + * Add units to remaining msgs + * Add nodiscard attribute to functions + * Add nodiscard attribute to functions + * Add nodiscard attribute to functions + * Add const specifiers to functions + * Make settings access const + * Move SBF ID handling + * Refactor header assembly + * Rename message handler again + * Change parsing utilities and crc to get message + * Add namespace to enum + * Change timestamp code + * Update changelog + * Change class privacy + * Add assembled messages, to be tested + * Add units to come msg definitions + * Add custom BlockHeader constructor + * Move wait + * Remove copy paste vars + * Add file readers + * Fix reset main connection on exit hang + * Fix handling of INS NMEA talker ID + * Fix error response detection + * Add packing of generic string messages + * Exchange concurrent queue + * Remove obsolete includes + * Add NMEA handling + * Change syncronization to semaphore + * Add message parser, WIP + * Rearrange io handling, WIP + * Refactor and cleanup + * Improve io handling, WIP + * Refactor message parser, WIP + * Add message handler + * Add io modules + * Add new low level interface, WIP + * Change connection thread + * Fix attitude cov flipped twice + * Add cov alignment from true north to grid north + * Rename meridian convergence and fix sense + * Remove obsolete define + * Fix spelling errors + * Merge branch 'master' into dev/next + * v1.2.3 + * Update package.xml + * v1.2.3 + * Update package.xml + * v1.2.3 + * Update package.xml + * v1.2.3 + * Update package.xml + * Merge pull request `#68 `_ from thomasemter/master + dev + * Fix lat/long in rad + * Reorder localization msg filling + * Update readme + * Fix NED to ECEF rotation matrix + * Add localization ECEF publishing + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Add ecef localization msg + * Add local to ecef transforms + * Change default datum to Default + * Clean up block data size defines + * Change default datum to WGS84 + * Set correct value for max number of base vector info + * Add check on shutdown to only close ports on hardware + * Refine readme + * Merge branch 'master' of https://github.com/thomasemter/septentrio_gnss_driver + * Add missing param + * Add possibility to use multiple RTK corrections of the same type + * Only set baud rates on real serial ports + * Fix decimal places trimming + * Update changelog + * Merge branch 'dev/next' + * Add base vecotr info to README + * Change param to empty vector + * Change param to empty vector + * Fix template argument + * Add quotation marks to pw if it contains spaces + * Add quotation marks to pw if it contains spaces + * Add option to keep aiding connections open on shutdown + * Merge branch 'master' into dev/next + * Add option to keep aiding connections open on shutdown + * Disable ntrip on shutdown + * Disable ntrip on shutdown + * Add base vector callbacks and publishing, WIP + * Add base vector msgs and parsers, WIP + * Fix comment swap + * Add send_gga option to serial RTK and fix IP server id + * Add possibility to specify IP server connection + * Increase version number in package.xml and harmonize it with ROS2 + * Reset main port to auto input + * Add reset all used ports on shutdown + * Improve readme + * Change vsm options to allow simultaneous input + * Change corrections settings to receiver simultaneously + * Change correction options to be used simultenously + * Change param name for future extensibility + * Change param name for future extensibility + * Rework corretion parameters and add more flexible options + * Fix some spelling in readme + * Add receiver type INS as GNSS + * Add option to use external VSM input + * Add more log output for vsm + * Add VSM from odometry or twist ROS messages + * Fix GPGGA and GPRMC timestamp from GNSS + * Use only one stream for NMEA messages + * Fix merge error + * Fix merge error + * Add all possible periods and rework validity check + * Update changelog + * Add 5 ms period option + * Fix changelog + * Add twist output + * Add missing files to clang-formatting + * Merge branch 'dev/next' + * Merge branch 'master' of https://github.com/thomasemter/septentrio_gnss_driver + * Format according to clang-format + * Change log level of local frame tf insertion + * Always register ReceiverSetup + * Add firmware checks + * Add log and info to README + * Add insertion of local frame + * Update README and CHANGELOG + * Use leap seconds from receiver + * Update changelog + * Add config files for GNSS and INS + * Add ReceiverTime, WIP + * Add configs for GNSS and INS + * Contributors: Thomas Emter, Tibor Dome, septentrio-users, tibordome + +1.3.1 (2023-07-06) +------------------ +* New Features + * Recovery from connection interruption + * Add option to bypass configuration of Rx + * OSNMA + * Latency compensation for ROS timestamps + * Output of SBF block VelCovCartesian + * Support for UDP and TCP via IP server + * New VSM handling allows for unknown variances (INS firmware >= 1.4.1) + * Add heading angle to GPSFix msg (by diverting dip field, cf. readme) +* Improvements + * Rework IO core and message handling + * Unified stream processing + * Internal data queue + * Prevent message loss in file reading + * Add some explanatory warnings for parameter mismatches + * Add units to message definitions +* Fixes + * navsatfix for INS + * Empty headers + * Single antenna receiver setup +* Preliminary Features + * Output of localization and tf in ECEF frame, testing and feedback welcome + 1.2.3 (2022-11-09) ------------------ * New Features @@ -298,8 +600,8 @@ Changelog for package septentrio_gnss_driver * Move more global settings to settings struct * Move more global settings to settings struct * Move global settings to settings struct - * Move more functions to Comm_IO - * Move settings to struct and configuration to Comm_IO + * Move more functions to CommIo + * Move settings to struct and configuration to CommIo * Merge branch 'dev/change_utc_calculation' into dev/refactor * Remove obsolete global variables * Move g_unix_time to class diff --git a/CMakeLists.txt b/CMakeLists.txt index f4356e83..20b40e67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,17 @@ cmake_minimum_required(VERSION 3.0.2) project(septentrio_gnss_driver) -## Compile as C++14, supported in ROS Melodic and newer -add_compile_options(-std=c++14) +## Compile as C++17, supported in ROS Melodic and newer +add_compile_options(-std=c++17) + +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) +message(STATUS "Setting build type to Release as none was specified.") +set(CMAKE_BUILD_TYPE "Release" CACHE + STRING "Choose the type of build." FORCE) +# Set the possible values of build type for cmake-gui +set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS + "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -72,9 +81,13 @@ endif () ## Generate messages in the 'msg' folder add_message_files( FILES + AIMPlusStatus.msg BaseVectorCart.msg BaseVectorGeod.msg BlockHeader.msg + GALAuthStatus.msg + RFBand.msg + RFStatus.msg MeasEpoch.msg MeasEpochChannelType1.msg MeasEpochChannelType2.msg @@ -176,21 +189,19 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node - src/septentrio_gnss_driver/node/main.cpp - src/septentrio_gnss_driver/node/rosaic_node.cpp - src/septentrio_gnss_driver/communication/circular_buffer.cpp - src/septentrio_gnss_driver/parsers/parsing_utilities.cpp - src/septentrio_gnss_driver/parsers/string_utilities.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp - src/septentrio_gnss_driver/crc/crc.cpp - src/septentrio_gnss_driver/communication/communication_core.cpp - src/septentrio_gnss_driver/communication/rx_message.cpp - src/septentrio_gnss_driver/communication/callback_handlers.cpp - src/septentrio_gnss_driver/communication/pcap_reader.cpp +add_executable(${PROJECT_NAME}_node + src/septentrio_gnss_driver/communication/communication_core.cpp + src/septentrio_gnss_driver/communication/message_handler.cpp + src/septentrio_gnss_driver/communication/telegram_handler.cpp + src/septentrio_gnss_driver/crc/crc.cpp + src/septentrio_gnss_driver/node/main.cpp + src/septentrio_gnss_driver/node/rosaic_node.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp + src/septentrio_gnss_driver/parsers/parsing_utilities.cpp + src/septentrio_gnss_driver/parsers/string_utilities.cpp ) ## Rename C++ executable without prefix diff --git a/README.md b/README.md index b772ee09..a0752e65 100644 --- a/README.md +++ b/README.md @@ -3,12 +3,13 @@ ## Overview -This repository hosts a ROS Melodic and Noetic driver (i.e. for Linux only) - written in C++ - that works with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. Since Noetic will only be supported until 2025, a ROS2 version is available in the branch `ros2`. +This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers - Supports serial, TCP/IP and USB connections, the latter being compatible with both serial (RNDIS) and TCP/IP protocols - Supports several ASCII (including key NMEA ones) messages and SBF (Septentrio Binary Format) blocks +- Reports status of AIM+ (Advanced Interference Mitigation including OSNMA) anti-jamming and anti-spoofing. - Can publish `nav_msgs/Odometry` message for INS receivers - Can blend SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic` and `DOP` in order to publish `gps_common/GPSFix` and `sensor_msgs/NavSatFix` messages - Supports axis convention conversion as Septentrio follows the NED convention, whereas ROS is ENU. @@ -67,14 +68,18 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo + Once the catkin build or binary installation is finished, adapt the `config/rover.yaml` file according to your needs. The `launch/rover.launch` need not be modified. Specify the communication parameters, the ROS messages to be published, the frequency at which the latter should happen etc.:
+ Note for setting `ant_serial_nr` and `ant_aux1_serial_nr`: This is a string parameter, numeric-only serial numbers should be put in quotes. If this is not done a warning will be issued and the driver tries to parse it as integer. + Besides the aforementioned config file `rover.yaml` containing all parameters, specialized launch files for GNSS `config/gnss.yaml` and INS `config/ins.yaml` respectively contain only the relevant parameters in each case. + - NOTE: Unless `configure_rx` is set to `false`, this driver will overwrite the previous values of the parameters, even if the value is left to zero in the "yaml" file. + The driver was developed and tested with firmware versions >= 4.10.0 for GNSS and >= 1.3.2 for INS. Receivers with older firmware versions are supported but some features may not be available. Known limitations are: * GNSS with firmware < 4.10.0 does not support IP over USB. + * GNSS with firmware < 4.12.1 does not support OSNMA. * INS with firmware < 1.3.2 does not support NTP. + * INS with firmware < 1.4 does not support OSNMA. + * INS with firmware < 1.4.1 does not support improved VSM handling allowing for unknown variances. * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + + Known issues: + * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4. For GNSS it is fixed in version 4.14 (released on June 15th 2023), for INS it will be fixed in to-be-released 1.4.1 (expected in November 2023). + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. - + An INS can be used in GNSS mode but some features may not be supported. Known limitations are: - * Antenna types cannot be set, leading to an error messages. The receiver still works, but precision may be degraded by a few mm. :
``` # Example configuration Settings for the Rover Rx @@ -84,13 +89,28 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: off + + stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" + + configure_rx: true login: user: "" password: "" + osnma: + mode: "off" + ntp_server: "" + keep_open: true + frame_id: gnss imu_frame_id: imu @@ -139,6 +159,8 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo use_gnss_time: false + latency_compensation: false + rtk_settings: ntrip_1: id: "NTR1" @@ -205,12 +227,15 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: false atteuler: true attcoveuler: true pose: false twist: false diagnostics: false + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false @@ -224,7 +249,9 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo exteventinsnavgeod: false imu: false localization: false - tf: false + tf: false + localization_ecef: false + tf_ecef: false # INS-Specific Parameters @@ -310,7 +337,6 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo - Alternatively the lever arms may be specified via tf. Set `get_spatial_config_from_tf`to `true` in this case. - If the point of interest is neither the IMU nor the ARP of the main GNSS antenna, the vector between the IMU and the point of interest can be provided with the `ins_solution/poi_lever_arm` parameter. - - NOTE: This driver will always overwrite the previous value of the above mentioned parameters, also if the value is left to zero in the "yaml" file. - For further more information about Septentrio receivers, visit Septentrio [support resources](https://www.septentrio.com/en/supportresources) or check out the [user manual](https://www.septentrio.com/system/files/support/asterx_sbi3_user_manual_v1.0_0.pdf) and [reference guide](https://www.septentrio.com/system/files/support/asterx_sbi3_pro_firmware_v1.3.0_reference_guide.pdf) of the AsteRx SBi3 receiver. # ROSaic Parameters @@ -319,25 +345,51 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Connectivity Specs - + `device`: location of device connection - + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyUSB0` + + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. Incoming data streams of SBF blocks and NMEA sentences are recevied either via this interface or a static IP server for TCP and/or UDP. The former will be utilized if section `stream_device/tcp` and `stream_device/udp` are not configured. + + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. + Regarding the file path, ROS_HOME=\`pwd\` in front of `roslaunch septentrio...` might be useful to specify that the node should be started using the executable's directory as its working-directory. + `tcp://host:port` format for TCP/IP connections + `28784` should be used as the default (command) port for TCP/IP connections. If another port is specified, the receiver needs to be (re-)configured via the Web Interface before ROSaic can be used. + An RNDIS IP interface is provided via USB, assigning the address `192.168.3.1` to the receiver. This should work on most modern Linux distributions. To verify successful connection, open a web browser to access the web interface of the receiver using the IP address `192.168.3.1`. - + default: `tcp://192.168.3.1:28784 ` + + default: `tcp://192.168.3.1:28784` + `serial`: specifications for serial communication + `baudrate`: serial baud rate to be used in a serial connection. Ensure the provided rate is sufficient for the chosen SBF blocks. For example, activating MeasEpoch (also necessary for /gpsfix) may require up to almost 400 kBit/s. - + `rx_serial_port`: determines to which (virtual) serial port of the Rx we want to get connected to, e.g. USB1 or COM1 - + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART HW flow control enabled or not - + `off` to disable UART HW flow control, `RTS|CTS` to enable it + + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` + + `stream_device`: If left unconfigured, by default `device` is utilized for the data streams. Within `stream_device` static IP servers may be defined instead. In config mode (`configure_rx` set to `true`), TCP will be prioritized over UDP. If Rx is pre-configured, both may be set simultaneously. + + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password
+ +
+ OSNMA + + + `osnma`: Configuration of the Open Service Navigation Message Authentication (OSNMA) feature. + + `mode`: Three operating modes are supported: `off` where OSNMA authentication is disabled, `loose` where satellites are included in the PVT if they are successfully authenticated or if their authentication status is unknown, and `strict` where only successfully-authenticated satellites are included in the PVT. In case of `strict` synchronization via NTP is mandatory. + + default: off + + `ntp_server`: In `strict` mode, OSNMA authentication requires the availability of external time information. In `loose` mode, this is optional but recommended for enhanced security. The receiver can connect to an NTP time server for this purpose. Options are `default` to let the receiver choose an NTP server or specify one like `pool.ntp.org` for example. + + default: "" + + `keep_open`: Wether OSNMA shall be kept active on driver shutdown. + + default: true +
+ +
+ Receiver Configuration + + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`stream_device/tcp/ip_server` and `stream_device/tcp/port`), since dynamic connections (`device` is tcp) are not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + default: true +
Receiver Type @@ -345,14 +397,13 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `receiver_type`: This parameter is to select the type of the Septentrio receiver + `gnss` for GNSS receivers. + `ins` for INS receivers. - + `ins_in_gnss_mode` INS receivers in GNSS mode. + default: `gnss` + `multi_antenna`: Whether or not the Rx has multiple antennas. + default: `false`
- Frame ID + Frame IDs + `frame_id`: name of the ROS tf frame for the Rx, placed in the header of published GNSS messages. It corresponds to the frame of the main antenna. + In ROS, the [tf package](https://wiki.ros.org/tf) lets you keep track of multiple coordinate frames over time. The frame ID will be resolved by [`tf_prefix`](http://wiki.ros.org/geometry/CoordinateFrameConventions) if defined. If a ROS message has a header (all of those we publish do), the frame ID can be found via `rostopic echo /topic`, where `/topic` is the topic into which the message is being published. @@ -445,6 +496,8 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `use_gnss_time`: `true` if the ROS message headers' unix epoch time field shall be constructed from the TOW/WNC (in the SBF case) and UTC (in the NMEA case) data, `false` if those times shall be taken by the driver from ROS time. If `use_gnss_time` is set to `true`, make sure the ROS system is synchronized to an NTP time server either via internet or ideally via the Septentrio receiver since the latter serves as a Stratum 1 time server not dependent on an internet connection. The NTP server of the receiver is automatically activated on the Septentrio receiver (for INS/GNSS a firmware >= 1.3.3 is needed). + default: `true` + + `latency_compensation`: Rx reports processing latency in PVT and INS blocks. If set to `true`this latency is subtracted from ROS timestamps in related blocks (i.e., `use_gnss_time` set to `false`). Related blocks are INS, PVT, Covariances, and BaseVectors. In case of `use_gnss_time` set to `true`, the latency is already compensated within the RX and included in the reported timestamps. + + default: `false`
@@ -538,7 +591,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + default: "" + `config`: Defines which measurements belonging to the respective axes are forwarded to the INS. In addition, non-holonomic constraints may be introduced for directions known to be restricted in movement. For example, a vehicle with Ackermann steering is limited in its sidewards and upwards movement. So, even if only motion in x-direction may be measured, zero-velocities for y and z may be sent. + default: [] - + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values inside the messaged are used. + + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values from inside the ROS messages are used. + default: false + `variances`: Variances of the respective axes. Only have to be set if `ins_vsm/ros/variances_by_parameter` is set to `true`. Values must be > 0.0, else measurements cannot not be used. + default: [] @@ -573,12 +626,15 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/gpgsa`: `true` to publish `nmea_msgs/GPGSA.msg` messages into the topic `/gpgsa` + `publish/gpgsv`: `true` to publish `nmea_msgs/GPGSV.msg` messages into the topic `/gpgsv` + `publish/measepoch`: `true` to publish `septentrio_gnss_driver/MeasEpoch.msg` messages into the topic `/measepoch` + + `publish/galauthstatus`: `true` to publish `septentrio_gnss_driver/GALAuthStatus.msg` messages into the topic `/galauthstatus` and corresponding `/diganostics` + + `publish/aimplusstatus`: `true` to publish `septentrio_gnss_driver/RFStatus.msg` messages into the topic `/rfstatus`, `septentrio_gnss_driver/AIMPlusStatus.msg` messages into `/aimplusstatus` and corresponding `/diganostics`. Some information is only available with active OSNMA. + `publish/pvtcartesian`: `true` to publish `septentrio_gnss_driver/PVTCartesian.msg` messages into the topic `/pvtcartesian` + `publish/pvtgeodetic`: `true` to publish `septentrio_gnss_driver/PVTGeodetic.msg` messages into the topic `/pvtgeodetic` + `publish/basevectorcart`: `true` to publish `septentrio_gnss_driver/BaseVectorCart.msg` messages into the topic `/basevectorcart` + `publish/basevectorgeod`: `true` to publish `septentrio_gnss_driver/BaseVectorGeod.msg` messages into the topic `/basevectorgeod` + `publish/poscovcartesian`: `true` to publish `septentrio_gnss_driver/PosCovCartesian.msg` messages into the topic `/poscovcartesian` + `publish/poscovgeodetic`: `true` to publish `septentrio_gnss_driver/PosCovGeodetic.msg` messages into the topic `/poscovgeodetic` + + `publish/velcovcartesian`: `true` to publish `septentrio_gnss_driver/VelCovCartesian.msg` messages into the topic `/velcovcartesian` + `publish/velcovgeodetic`: `true` to publish `septentrio_gnss_driver/VelCovGeodetic.msg` messages into the topic `/velcovgeodetic` + `publish/atteuler`: `true` to publish `septentrio_gnss_driver/AttEuler.msg` messages into the topic `/atteuler` + `publish/attcoveuler`: `true` to publish `septentrio_gnss_driver/AttCovEuler.msg` messages into the topic `/attcoveuler` @@ -597,32 +653,38 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/exteventinsnavgeod`: `true` to publish `septentrio_gnss_driver/ExtEventINSNavGeod.msgs` message into the topic`/exteventinsnavgeod` + `publish/imu`: `true` to publish `sensor_msgs/Imu.msg` message into the topic`/imu` + `publish/localization`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` - + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. + + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`. + + `publish/localization_ecef`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` related to ECEF frame. + + `publish/tf_ecef`: `true` to broadcast tf of localization related to ECEF frame. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`.
## ROS Topic Publications A selection of NMEA sentences, the majority being standardized sentences, and proprietary SBF blocks is translated into ROS messages, partly generic and partly custom, and can be published at the discretion of the user into the following ROS topics. All published ROS messages, even custom ones, start with a ROS generic header [`std_msgs/Header.msg`](https://docs.ros.org/melodic/api/std_msgs/html/msg/Header.html), which includes the receiver time stamp as well as the frame ID, the latter being specified in the ROS parameter `frame_id`.
Available ROS Topics - + + `/gpgga`: publishes [`nmea_msgs/Gpgga.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgga.html) - converted from the NMEA sentence GGA. + `/gprmc`: publishes [`nmea_msgs/Gprmc.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gprmc.html) - converted from the NMEA sentence RMC. + `/gpgsa`: publishes [`nmea_msgs/Gpgsa.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsa.html) - converted from the NMEA sentence GSA. + `/gpgsv`: publishes [`nmea_msgs/Gpgsv.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsv.html) - converted from the NMEA sentence GSV. + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. + + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. + + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, compiled from the SBF block `RFStatus`. + + `/aimplusstatus`: publishes custom ROS message `septentrio_gnss_driver/AIMPlusStatus.msg`, reporting status of AIM+. Converted from SBF blocks `RFStatus` and optionally `GALAuthStatus`. For the latter OSNMA has to be activated. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. + `/basevectorgeod`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorGeod.msg`, corresponding to the SBF block `BaseVectorGeod`. + `/poscovcartesian`: publishes custom ROS message `septentrio_gnss_driver/PosCovCartesian.msg`, corresponding to SBF block `PosCovCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/poscovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PosCovGeodetic.msg`, corresponding to SBF block `PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + + `/velcovcartesian`: publishes custom ROS message `septentrio_gnss_driver/VelCovCartesian.msg`, corresponding to SBF block `VelCovCartesian` (GNSS case). + `/velcovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/VelCovGeodetic.msg`, corresponding to SBF block `VelCovGeodetic` (GNSS case). + `/atteuler`: publishes custom ROS message `septentrio_gnss_driver/AttEuler.msg`, corresponding to SBF block `AttEuler`. + `/attcoveuler`: publishes custom ROS message `septentrio_gnss_driver/AttCovEuler.msg`, corresponding to the SBF block `AttCovEuler`. - + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its header, or - if `use_gnss_time` is set to `false` - from the systems's wall-clock time. + + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its block header. + `/navsatfix`: publishes generic ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`,`PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + The ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) can be fed directly into the [`navsat_transform_node`](https://docs.ros.org/melodic/api/robot_localization/html/navsat_transform_node.html) of the ROS navigation stack. - + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). + + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). In order to publish heading information, the field *dip* is diverted from its intended meaning an populated with the heading angle and *err_dip* with its uncertainty. + `/pose`: publishes generic ROS message [`geometry_msgs/PoseWithCovarianceStamped.msg`](https://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `AttEuler`, `AttCovEuler` (GNSS case) or `INSNavGeod` (INS case). + Note that GNSS provides absolute positioning, while robots are often localized within a local level cartesian frame. The pose field of this ROS message contains position with respect to the absolute ENU frame (longitude, latitude, height), i.e. not a cartesian frame, while the orientation is with respect to a vehicle-fixed (e.g. for mosaic-x5 in moving base mode via the command `setAttitudeOffset`, ...) !local! NED frame or ENU frame if `use_ros_axis_directions` is set `true`. Thus the orientation is !not! given with respect to the same frame as the position is given in. The cross-covariances are hence set to 0. + `/twist`: publishes generic ROS message [`geometry_msgs/TwistWithCovarianceStamped.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/TwistWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic` and `VelCovGeodetic`. @@ -639,13 +701,14 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + The ROS message [`sensor_msgs/Imu.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + `/localization`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + + `/localization_ecef`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF blocks `INSNavCart` and `INSNavGeod`. + + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention.
## Suggestions for Improvements
Some Ideas - + Automatic Search: If the host address of the receiver is omitted in the `host:port` specification, the driver could automatically search and establish a connection on the specified port. + Equip ROSaic with an NTRIP client such that it can forward corrections to the receiver independently of `Data Link`.
@@ -655,16 +718,16 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr Is there an SBF or NMEA message that is not being addressed while being important to your application? If yes, follow these steps: 1. Find the log reference of interest in the publicly accessible, official documentation. Hence select the reference guide file, e.g. for mosaic-x5 in the [product support section for mosaic-X5](https://www.septentrio.com/en/support/mosaic/mosaic-x5), Chapter 4, of Septentrio's homepage. - 2. Add a new `.msg` file to the `septentrio_gnss_driver/msg` folder. - 3. SBF: Add the new struct definition to the `sbf_structs.hpp` file. - 4. Parsing/Processing the message/block: - - Both: Add a new include guard to let the compiler know about the existence of the header file (such as `septentrio_gnss_driver/PVTGeodetic.h`) that gets compiler-generated from the `.msg` file constructed in step 3. - - SBF: Extend the `NMEA_ID_Enum` enumeration in the `rx_message.hpp` file with a new entry. - - SBF: Extend the initialization of the `RxIDMap` map in the `rx_message.cpp` file with a new pair. - - SBF: Add a new callback function declaration, a new method, to the `io_comm_rx::RxMessage class` in the `rx_message.hpp` file. - - SBF: Add the latter's definition to the `rx_message.cpp` file. - - SBF: Add a new C++ "case" (part of the C++ switch-case structure) in the `rx_message.hpp` file. It should be modeled on the existing `evPVTGeodetic` case, e.g. one needs a static counter variable declaration. - - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `septentrio_gnss_driver/src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `septentrio_gnss_driver/include/septentrio_gnss_driver/parsers/nmea_parsers` folder. - 5. Create a new `publish/..` ROSaic parameter in the `septentrio_gnss_driver/config/rover.yaml` file, create a global boolean variable `publish_...` in the `septentrio_gnss_driver/src/septentrio_gnss_driver/node/rosaic_node.cpp` file, insert the publishing callback function to the C++ "multimap" `IO.handlers_.callbackmap_` - which is already storing all the others - in the `rosaic_node::ROSaicNode::defineMessages()` method in the same file and add an `extern bool publish_...;` line to the `septentrio_gnss_driver/include/septentrio_gnss_driver/node/rosaic_node.hpp` file. - 6. Modify the `septentrio_gnss_driver/CMakeLists.txt` file by adding a new entry to the `add_message_files` section. + 2. SBF: Add a new `.msg` file to the `../msg` folder. And modify the `../CMakeLists.txt` file by adding a new entry to the `add_message_files` section. + 3. Add msg header and typedef to `typedefs.hpp`. + 4. Parsers: + - SBF: Add a parser to the `sbf_blocks.hpp` file. + - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `../src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `../include/septentrio_gnss_driver/parsers/nmea_parsers` folder. + 5. Processing the message/block: + - SBF: Extend the `SbfId` enumeration in the `message_handler.hpp` file with a new entry. + - SBF: Extend the SBF switch-case in `message_handler.cpp` file with a new case. + - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. + - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. + 6. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. + 7. Add SBF block or NMEA to data stream setup in `communication_core.cpp` (function `configureRx()`). diff --git a/config/gnss.yaml b/config/gnss.yaml index e77e1a11..f422930a 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -6,13 +6,28 @@ device: tcp://192.168.3.1:28784 serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: "off" +stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" + +configure_rx: true + login: user: "" password: "" +osnma: + mode: "loose" + ntp_server: "" + keep_open: true + frame_id: gnss aux1_frame_id: aux1 @@ -42,6 +57,8 @@ polling_period: use_gnss_time: false +latency_compensation: false + rtk_settings: ntrip_1: id: "" @@ -83,12 +100,15 @@ publish: basevectorgeod: false poscovcartesian: true poscovgeodetic: true + velcovcartesian: false velcovgeodetic: true atteuler: true attcoveuler: true pose: true twist: false diagnostics: true + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false diff --git a/config/ins.yaml b/config/ins.yaml index 8248caa8..8d9076e0 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -6,13 +6,28 @@ device: tcp://192.168.3.1:28784 serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: "off" +stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" + +configure_rx: true + login: user: "" password: "" +osnma: + mode: "loose" + ntp_server: "" + keep_open: true + frame_id: gnss imu_frame_id: imu @@ -56,6 +71,8 @@ polling_period: use_gnss_time: false +latency_compensation: false + rtk_settings: keep_open: true ntrip_1: @@ -95,12 +112,15 @@ publish: basevectorgeod: false poscovcartesian: false poscovgeodetic: false + velcovcartesian: false velcovgeodetic: false atteuler: false attcoveuler: false pose: false twist: true diagnostics: true + aimplusstatus: true + galauthstatus: false # For INS Rx only insnavcart: true insnavgeod: true @@ -112,6 +132,8 @@ publish: imu: true localization: true tf: true + localization_ecef: false + tf_ecef: false # INS-Specific Parameters diff --git a/config/rover.yaml b/config/rover.yaml index 2ffe118e..ee81113c 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -6,13 +6,28 @@ device: tcp://192.168.3.1:28784 serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: off +stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" + +configure_rx: true + login: user: "" password: "" +osnma: + mode: "loose" + ntp_server: "" + keep_open: true + frame_id: gnss imu_frame_id: imu @@ -63,6 +78,8 @@ polling_period: use_gnss_time: false +latency_compensation: false + rtk_settings: ntrip_1: id: "" @@ -104,12 +121,15 @@ publish: basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: true atteuler: true attcoveuler: true pose: false twist: false diagnostics: false + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false @@ -124,6 +144,8 @@ publish: imu: false localization: false tf: false + localization_ecef: false + tf_ecef: false # INS-Specific Parameters diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 1513ff0e..875e8430 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -52,11 +52,13 @@ #include #include // GNSS msg includes +#include #include #include #include #include #include +#include #include #include #include @@ -64,6 +66,8 @@ #include #include #include +#include +#include #include #include #include @@ -81,8 +85,8 @@ #include #include // Rosaic includes -#include -#include +#include +#include // Timestamp in nanoseconds (Unix epoch) typedef uint64_t Timestamp; @@ -96,18 +100,22 @@ typedef geometry_msgs::Quaternion QuaternionMsg; typedef geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg; typedef geometry_msgs::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg; typedef geometry_msgs::TransformStamped TransformStampedMsg; -typedef gps_common::GPSFix GPSFixMsg; -typedef gps_common::GPSStatus GPSStatusMsg; +typedef gps_common::GPSFix GpsFixMsg; +typedef gps_common::GPSStatus GpsStatusMsg; typedef sensor_msgs::NavSatFix NavSatFixMsg; typedef sensor_msgs::NavSatStatus NavSatStatusMsg; typedef sensor_msgs::TimeReference TimeReferenceMsg; typedef sensor_msgs::Imu ImuMsg; -typedef nav_msgs::Odometry LocalizationUtmMsg; +typedef nav_msgs::Odometry LocalizationMsg; // Septentrio GNSS SBF messages +typedef septentrio_gnss_driver::AIMPlusStatus AimPlusStatusMsg; typedef septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg; typedef septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg; typedef septentrio_gnss_driver::BlockHeader BlockHeaderMsg; +typedef septentrio_gnss_driver::GALAuthStatus GalAuthStatusMsg; +typedef septentrio_gnss_driver::RFStatus RfStatusMsg; +typedef septentrio_gnss_driver::RFBand RfBandMsg; typedef septentrio_gnss_driver::MeasEpoch MeasEpochMsg; typedef septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg; typedef septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg; @@ -158,14 +166,16 @@ inline Timestamp timestampFromRos(const TimestampRos& tsr) { return tsr.toNSec() /** * @brief Log level for ROS logging */ -enum LogLevel -{ - DEBUG, - INFO, - WARN, - ERROR, - FATAL -}; +namespace log_level { + enum LogLevel + { + DEBUG, + INFO, + WARN, + ERROR, + FATAL + }; +} // namespace log_level /** * @class ROSaicNodeBase @@ -174,30 +184,45 @@ enum LogLevel class ROSaicNodeBase { public: - ROSaicNodeBase() : pNh_(new ros::NodeHandle("~")), tfListener_(tfBuffer_) {} + ROSaicNodeBase() : + pNh_(new ros::NodeHandle("~")), tfListener_(tfBuffer_), lastTfStamp_(0) + { + } virtual ~ROSaicNodeBase() {} + const Settings* settings() const { return &settings_; } + void registerSubscriber() { - ros::NodeHandle nh; - if (settings_.ins_vsm_ros_source == "odometry") - odometrySubscriber_ = nh.subscribe( - "odometry_vsm", 10, &ROSaicNodeBase::callbackOdometry, this); - else if (settings_.ins_vsm_ros_source == "twist") - twistSubscriber_ = nh.subscribe( - "twist_vsm", 10, &ROSaicNodeBase::callbackTwist, this); + try + { + ros::NodeHandle nh; + if (settings_.ins_vsm_ros_source == "odometry") + odometrySubscriber_ = nh.subscribe( + "odometry_vsm", 10, &ROSaicNodeBase::callbackOdometry, this); + else if (settings_.ins_vsm_ros_source == "twist") + twistSubscriber_ = nh.subscribe( + "twist_vsm", 10, &ROSaicNodeBase::callbackTwist, this); + } catch (const std::runtime_error& ex) + { + this->log(log_level::ERROR, "Subscriber initialization failed due to: " + + std::string(ex.what()) + "."); + } } /** - * @brief Gets an integer or unsigned integer value from the parameter server - * @param[in] name The key to be used in the parameter server's dictionary - * @param[out] val Storage for the retrieved value, of type U, which can be - * either unsigned int or int - * @param[in] defaultVal Value to use if the server doesn't contain this - * parameter + * @brief Gets an integer or unsigned integer value from the + * parameter server + * @param[in] name The key to be used in the parameter server's + * dictionary + * @param[out] val Storage for the retrieved value, of type U, which + * can be either unsigned int or int + * @param[in] defaultVal Value to use if the server doesn't contain + * this parameter */ - bool getUint32Param(const std::string& name, uint32_t& val, uint32_t defaultVal) + bool getUint32Param(const std::string& name, uint32_t& val, + uint32_t defaultVal) const { int32_t tempVal; if ((!pNh_->getParam(name, tempVal)) || (tempVal < 0)) @@ -211,14 +236,15 @@ class ROSaicNodeBase /** * @brief Gets parameter of type T from the parameter server - * @param[in] name The key to be used in the parameter server's dictionary + * @param[in] name The key to be used in the parameter server's + * dictionary * @param[out] val Storage for the retrieved value, of type T - * @param[in] defaultVal Value to use if the server doesn't contain this - * parameter + * @param[in] defaultVal Value to use if the server doesn't contain + * this parameter * @return True if it could be retrieved, false if not */ template - bool param(const std::string& name, T& val, const T& defaultVal) + bool param(const std::string& name, T& val, const T& defaultVal) const { return pNh_->param(name, val, defaultVal); }; @@ -228,23 +254,23 @@ class ROSaicNodeBase * @param[in] logLevel Log level * @param[in] s String to log */ - void log(LogLevel logLevel, const std::string& s) + void log(log_level::LogLevel logLevel, const std::string& s) const { switch (logLevel) { - case LogLevel::DEBUG: + case log_level::DEBUG: ROS_DEBUG_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::INFO: + case log_level::INFO: ROS_INFO_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::WARN: + case log_level::WARN: ROS_WARN_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::ERROR: + case log_level::ERROR: ROS_ERROR_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::FATAL: + case log_level::FATAL: ROS_FATAL_STREAM(ros::this_node::getName() << ": " << s); break; default: @@ -256,7 +282,7 @@ class ROSaicNodeBase * @brief Gets current timestamp * @return Timestamp */ - Timestamp getTime() { return ros::Time::now().toNSec(); } + Timestamp getTime() const { return ros::Time::now().toNSec(); } /** * @brief Publishing function @@ -282,7 +308,7 @@ class ROSaicNodeBase * @brief Publishing function for tf * @param[in] msg ROS localization message to be converted to tf */ - void publishTf(const LocalizationUtmMsg& loc) + void publishTf(const LocalizationMsg& loc) { if (std::isnan(loc.pose.pose.orientation.w)) return; @@ -347,6 +373,36 @@ class ROSaicNodeBase tf2Publisher_.sendTransform(transformStamped); } + /** + * @brief Set INS to true + */ + void setIsIns() { capabilities_.is_ins = true; } + + /** + * @brief Set has heading to true + */ + void setHasHeading() { capabilities_.has_heading = true; } + + /** + * @brief Set improved VSM handling to true + */ + void setImprovedVsmHandling() { capabilities_.has_improved_vsm_handling = true; } + + /** + * @brief Check if Rx is INS + */ + bool isIns() { return capabilities_.is_ins; } + + /** + * @brief Check if Rx has heading + */ + bool hasHeading() { return capabilities_.has_heading; } + + /** + * @brief Check if Rx has improved VSM handling + */ + bool hasImprovedVsmHandling() { return capabilities_.has_improved_vsm_handling; } + private: void callbackOdometry(const nav_msgs::Odometry::ConstPtr& odo) { @@ -365,93 +421,121 @@ class ROSaicNodeBase void processTwist(Timestamp stamp, const geometry_msgs::TwistWithCovariance& twist) { - time_t epochSeconds = stamp / 1000000000; - struct tm* tm_temp = std::gmtime(&epochSeconds); - std::stringstream timeUtc; - timeUtc << std::setfill('0') << std::setw(2) - << std::to_string(tm_temp->tm_hour) << std::setw(2) - << std::to_string(tm_temp->tm_min) << std::setw(2) - << std::to_string(tm_temp->tm_sec) << "." << std::setw(3) - << std::to_string((stamp - (stamp / 1000000000) * 1000000000) / - 1000000); - - std::string v_x; - std::string v_y; - std::string v_z; - std::string std_x; - std::string std_y; - std::string std_z; - if (settings_.ins_vsm_ros_config[0]) + // in case stamp was not set + if (stamp == 0) + stamp = getTime(); + + static Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + static Eigen::Vector3d var = Eigen::Vector3d::Zero(); + static uint64_t ctr = 0; + static Timestamp lastStamp = 0; + + ++ctr; + vel[0] += twist.twist.linear.x; + vel[1] += twist.twist.linear.y; + vel[2] += twist.twist.linear.z; + var[0] += twist.covariance[0]; + var[1] += twist.covariance[7]; + var[2] += twist.covariance[14]; + + // Rx expects averaged velocity at a rate of 2 Hz + if ((stamp - lastStamp) >= 495000000) // allow for 5 ms jitter { - v_x = string_utilities::trimDecimalPlaces(twist.twist.linear.x); - if (settings_.ins_vsm_ros_variances_by_parameter) - std_x = string_utilities::trimDecimalPlaces( - settings_.ins_vsm_ros_variances[0]); - else if (twist.covariance[0] > 0.0) - std_x = string_utilities::trimDecimalPlaces( - std::sqrt(twist.covariance[0])); - else + vel /= ctr; + var /= ctr; + time_t epochSeconds = stamp / 1000000000; + struct tm* tm_temp = std::gmtime(&epochSeconds); + std::stringstream timeUtc; + timeUtc << std::setfill('0') << std::setw(2) + << std::to_string(tm_temp->tm_hour) << std::setw(2) + << std::to_string(tm_temp->tm_min) << std::setw(2) + << std::to_string(tm_temp->tm_sec) << "." << std::setw(3) + << std::to_string((stamp - (stamp / 1000000000) * 1000000000) / + 1000000); + + std::string v_x; + std::string v_y; + std::string v_z; + std::string std_x; + std::string std_y; + std::string std_z; + if (settings_.ins_vsm_ros_config[0]) { - ROS_ERROR_STREAM("Invalid covariance value for v_x: " - << std::to_string(twist.covariance[0]) - << ". Ignoring measurement."); - } - } else - std_x = std::to_string(1000000.0); - if (settings_.ins_vsm_ros_config[1]) - { - if (settings_.use_ros_axis_orientation) - v_y = "-"; - v_y += string_utilities::trimDecimalPlaces(twist.twist.linear.y); - if (settings_.ins_vsm_ros_variances_by_parameter) - std_y = string_utilities::trimDecimalPlaces( - settings_.ins_vsm_ros_variances[1]); - else if (twist.covariance[7] > 0.0) - std_y = string_utilities::trimDecimalPlaces( - std::sqrt(twist.covariance[7])); - else + v_x = string_utilities::trimDecimalPlaces(vel[0]); + if (settings_.ins_vsm_ros_variances_by_parameter) + std_x = string_utilities::trimDecimalPlaces( + settings_.ins_vsm_ros_variances[0]); + else if (var[0] > 0.0) + std_x = string_utilities::trimDecimalPlaces(std::sqrt(var[0])); + else if (!capabilities_.has_improved_vsm_handling) + { + log(log_level::ERROR, "Invalid covariance value for v_x: " + + std::to_string(var[0]) + + ". Ignoring measurement."); + v_x = ""; + std_x = string_utilities::trimDecimalPlaces(1000000.0); + } + } else + std_x = std::to_string(1000000.0); + if (settings_.ins_vsm_ros_config[1]) { - ROS_ERROR_STREAM("Invalid covariance value for v_y: " - << std::to_string(twist.covariance[1]) - << ". Ignoring measurement."); - v_y = ""; + if (settings_.use_ros_axis_orientation) + v_y = "-"; + v_y += string_utilities::trimDecimalPlaces(vel[1]); + if (settings_.ins_vsm_ros_variances_by_parameter) + std_y = string_utilities::trimDecimalPlaces( + settings_.ins_vsm_ros_variances[1]); + else if (var[1] > 0.0) + std_y = string_utilities::trimDecimalPlaces(std::sqrt(var[1])); + else if (!capabilities_.has_improved_vsm_handling) + { + log(log_level::ERROR, "Invalid covariance value for v_y: " + + std::to_string(var[1]) + + ". Ignoring measurement."); + v_y = ""; + std_y = string_utilities::trimDecimalPlaces(1000000.0); + } + } else std_y = string_utilities::trimDecimalPlaces(1000000.0); - } - } else - std_y = string_utilities::trimDecimalPlaces(1000000.0); - if (settings_.ins_vsm_ros_config[2]) - { - if (settings_.use_ros_axis_orientation) - v_z = "-"; - v_z += string_utilities::trimDecimalPlaces(twist.twist.linear.z); - if (settings_.ins_vsm_ros_variances_by_parameter) - std_z = string_utilities::trimDecimalPlaces( - settings_.ins_vsm_ros_variances[2]); - else if (twist.covariance[14] > 0.0) - std_z = string_utilities::trimDecimalPlaces( - std::sqrt(twist.covariance[14])); - else + if (settings_.ins_vsm_ros_config[2]) { - ROS_ERROR_STREAM("Invalid covariance value for v_z: " - << std::to_string(twist.covariance[2]) - << ". Ignoring measurement."); - v_z = ""; + if (settings_.use_ros_axis_orientation) + v_z = "-"; + v_z += string_utilities::trimDecimalPlaces(vel[2]); + if (settings_.ins_vsm_ros_variances_by_parameter) + std_z = string_utilities::trimDecimalPlaces( + settings_.ins_vsm_ros_variances[2]); + else if (var[2] > 0.0) + std_z = string_utilities::trimDecimalPlaces(std::sqrt(var[2])); + else if (!capabilities_.has_improved_vsm_handling) + { + log(log_level::ERROR, "Invalid covariance value for v_z: " + + std::to_string(var[2]) + + ". Ignoring measurement."); + v_z = ""; + std_z = string_utilities::trimDecimalPlaces(1000000.0); + } + } else std_z = string_utilities::trimDecimalPlaces(1000000.0); - } - } else - std_z = string_utilities::trimDecimalPlaces(1000000.0); - std::string velNmea = "$PSSN,VSM," + timeUtc.str() + "," + v_x + "," + v_y + - "," + std_x + "," + std_y + "," + v_z + "," + std_z; + std::string velNmea = "$PSSN,VSM," + timeUtc.str() + "," + v_x + "," + + v_y + "," + std_x + "," + std_y + "," + v_z + "," + + std_z; - char crc = std::accumulate(velNmea.begin() + 1, velNmea.end(), 0, - [](char sum, char ch) { return sum ^ ch; }); + char crc = std::accumulate(velNmea.begin() + 1, velNmea.end(), 0, + [](char sum, char ch) { return sum ^ ch; }); - std::stringstream crcss; - crcss << std::hex << static_cast(crc); + std::stringstream crcss; + crcss << std::hex << static_cast(crc); - velNmea += "*" + crcss.str() + "\r\n"; - sendVelocity(velNmea); + velNmea += "*" + crcss.str() + "\r\n"; + sendVelocity(velNmea); + + vel = Eigen::Vector3d::Zero(); + var = Eigen::Vector3d::Zero(); + ctr = 0; + lastStamp = stamp; + } } protected: @@ -479,4 +563,6 @@ class ROSaicNodeBase tf2_ros::Buffer tfBuffer_; // tf listener tf2_ros::TransformListener tfListener_; + // Capabilities of Rx + Capabilities capabilities_; }; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index dab22a34..fad018b8 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -56,21 +56,20 @@ // // ***************************************************************************** +#pragma once + // Boost includes -#include #include #include -#include -#include -#include -#include -#include +#include // ROSaic includes -#include +#include +#include -#ifndef ASYNC_MANAGER_HPP -#define ASYNC_MANAGER_HPP +// local includes +#include +#include /** * @file async_manager.hpp @@ -81,28 +80,21 @@ * commands to serial port or via TCP/IP. */ -namespace io_comm_rx { +namespace io { /** - * @class Manager + * @class AsyncManagerBase * @brief Interface (in C++ terms), that could be used for any I/O manager, * synchronous and asynchronous alike */ - class Manager + class AsyncManagerBase { public: - typedef boost::function - Callback; - virtual ~Manager() {} - //! Sets the callback function - virtual void setCallback(const Callback& callback) = 0; + virtual ~AsyncManagerBase() {} + //! Connects the stream + [[nodiscard]] virtual bool connect() = 0; //! Sends commands to the receiver - virtual bool send(const std::string& cmd) = 0; - //! Waits count seconds before throwing ROS_INFO message in case no message - //! from the receiver arrived - virtual void wait(uint16_t* count) = 0; - //! Determines whether or not the connection is open - virtual bool isOpen() const = 0; + virtual void send(const std::string& cmd) = 0; }; /** @@ -110,360 +102,529 @@ namespace io_comm_rx { * @brief This is the central interface between ROSaic and the Rx(s), managing * I/O operations such as reading messages and sending commands.. * - * StreamT is either boost::asio::serial_port or boost::asio::tcp::ip + * IoType is either boost::asio::serial_port or boost::asio::tcp::ip */ - template - class AsyncManager : public Manager + template + class AsyncManager : public AsyncManagerBase { public: /** * @brief Class constructor - * @param stream Whether TCP/IP or serial communication, either - * boost::asio::serial_port or boost::asio::tcp::ip - * @param io_service The io_context object. The io_context represents your - * program's link to the operating system's I/O services - * @param[in] buffer_size Size of the circular buffer in bytes + * @param[in] node Pointer to node + * @param[in] telegramQueue Telegram queue */ - AsyncManager(ROSaicNodeBase* node, boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size = 16384); - virtual ~AsyncManager(); + AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue); - /** - * @brief Allows to connect to the CallbackHandlers class - * @param callback The function that becomes our callback, typically the - * readCallback() method of CallbackHandlers - */ - void setCallback(const Callback& callback) { read_callback_ = callback; } + ~AsyncManager(); - void wait(uint16_t* count); + [[nodiscard]] bool connect(); - /** - * @brief Sends commands via the I/O stream. - * @param cmd The command to be sent - */ - bool send(const std::string& cmd); + void setPort(const std::string& port); - bool isOpen() const { return stream_->is_open(); } + void send(const std::string& cmd); private: - //! Pointer to the node - ROSaicNodeBase* node_; - - protected: - //! Reads in via async_read_some and hands certain number of bytes - //! (bytes_transferred) over to async_read_some_handler - void read(); - - //! Handler for async_read_some (Boost library).. - void asyncReadSomeHandler(const boost::system::error_code& error, - std::size_t bytes_transferred); - - //! Sends command "cmd" to the Rx - void write(const std::string& cmd); - - //! Closes stream "stream_" + void receive(); void close(); + void runIoService(); + void runWatchdog(); + void write(const std::string& cmd); + void resync(); + template + void readSync(); + void readSbfHeader(); + void readSbf(std::size_t length); + void readUnknown(); + void readString(); + void readStringElements(); - //! Tries parsing SBF/NMEA whenever the boolean class variable "try_parsing" - //! is true - void tryParsing(); - - //! Mutex to control changes of class variable "try_parsing" - boost::mutex parse_mutex_; - - //! Determines when the tryParsing() method will attempt parsing SBF/NMEA - bool try_parsing_; - - //! Determines when the asyncReadSomeHandler() method should write SBF/NMEA - //! into the circular buffer - bool allow_writing_; - - //! Condition variable complementing "parse_mutex" - boost::condition_variable parsing_condition_; - - //! Stream, represents either serial or TCP/IP connection - boost::shared_ptr stream_; - - //! io_context object - boost::shared_ptr io_service_; - - //! Buffer for async_read_some() to read continuous SBF/NMEA stream - std::vector in_; + //! Pointer to the node + ROSaicNodeBase* node_; + std::shared_ptr ioService_; + IoType ioInterface_; + std::atomic running_; + std::thread ioThread_; + std::thread watchdogThread_; - //! Circular buffer to avoid unsuccessful SBF/NMEA parsing due to incomplete - //! messages - CircularBuffer circular_buffer_; + std::array buf_; + //! Timestamp of receiving buffer + Timestamp recvStamp_; + //! Telegram + std::shared_ptr telegram_; + //! TelegramQueue + TelegramQueue* telegramQueue_; + }; - //! New thread for receiving incoming messages - boost::shared_ptr async_background_thread_; + template + AsyncManager::AsyncManager(ROSaicNodeBase* node, + TelegramQueue* telegramQueue) : + node_(node), + ioService_(new boost::asio::io_service), ioInterface_(node, ioService_), + telegramQueue_(telegramQueue) + { + node_->log(log_level::DEBUG, "AsyncManager created."); + } - //! New thread for receiving incoming messages - boost::shared_ptr parsing_thread_; + template + AsyncManager::~AsyncManager() + { + running_ = false; + close(); + node_->log(log_level::DEBUG, "AsyncManager shutting down threads"); + ioService_->stop(); + ioThread_.join(); + watchdogThread_.join(); + node_->log(log_level::DEBUG, "AsyncManager threads stopped"); + } - //! New thread for receiving incoming messages - boost::shared_ptr waiting_thread_; + template + [[nodiscard]] bool AsyncManager::connect() + { + running_ = true; - //! Callback to be called once message arrives - Callback read_callback_; + if (!ioInterface_.connect()) + { + return false; + } + receive(); - //! Whether or not we want to sever the connection to the Rx - bool stopping_; + return true; + } - /// Size of in_ buffers - const std::size_t buffer_size_; + template + void AsyncManager::setPort(const std::string& port) + { + ioInterface_.setPort(port); + } - //! Boost timer for throwing ROS_INFO message once timed out due to lack of - //! incoming messages - boost::asio::deadline_timer timer_; + template + void AsyncManager::send(const std::string& cmd) + { + if (cmd.size() == 0) + { + node_->log(log_level::ERROR, + "AsyncManager message size to be sent to the Rx would be 0"); + return; + } - //! Number of seconds before ROS_INFO message is thrown (if no incoming - //! message) - const uint16_t count_max_; + ioService_->post(boost::bind(&AsyncManager::write, this, cmd)); + } - //! Handles the ROS_INFO throwing (if no incoming message) - void callAsyncWait(uint16_t* count); + template + void AsyncManager::receive() + { + resync(); + ioThread_ = + std::thread(std::bind(&AsyncManager::runIoService, this)); + if (!watchdogThread_.joinable()) + watchdogThread_ = + std::thread(std::bind(&AsyncManager::runWatchdog, this)); + } - //! Number of times the DoRead() method has been called (only counts - //! initially) - uint16_t do_read_count_; + template + void AsyncManager::close() + { + ioService_->post([this]() { ioInterface_.close(); }); + } - //! Timestamp of receiving buffer - Timestamp recvTime_; - }; + template + void AsyncManager::runIoService() + { + ioService_->run(); + node_->log(log_level::DEBUG, "AsyncManager ioService terminated."); + } - template - void AsyncManager::tryParsing() + template + void AsyncManager::runWatchdog() { - uint8_t* to_be_parsed = new uint8_t[buffer_size_ * 16]; - uint8_t* to_be_parsed_index = to_be_parsed; - bool timed_out = false; - std::size_t shift_bytes = 0; - std::size_t arg_for_read_callback = 0; - - while (!timed_out && - !stopping_) // Loop will stop if condition variable timed out + while (running_) { - boost::mutex::scoped_lock lock(parse_mutex_); - parsing_condition_.wait_for(lock, boost::chrono::seconds(10), - [this]() { return try_parsing_; }); - bool timed_out = !try_parsing_; - if (timed_out) - break; - try_parsing_ = false; - allow_writing_ = true; - std::size_t current_buffer_size = circular_buffer_.size(); - arg_for_read_callback += current_buffer_size; - circular_buffer_.read(to_be_parsed + shift_bytes, current_buffer_size); - Timestamp revcTime = recvTime_; - lock.unlock(); - parsing_condition_.notify_one(); - - try + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + if (running_ && ioService_->stopped()) { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - std::to_string(arg_for_read_callback)); - read_callback_(revcTime, to_be_parsed_index, arg_for_read_callback); - } catch (std::size_t& parsing_failed_here) - { - to_be_parsed_index += parsing_failed_here; - arg_for_read_callback -= parsing_failed_here; - node_->log(LogLevel::DEBUG, "Current buffer size is " + - std::to_string(current_buffer_size) + - " and parsing_failed_here is " + - std::to_string(parsing_failed_here)); - if (arg_for_read_callback < 0) // In case some parsing error was not - // caught, which should never happen.. + if (node_->settings()->read_from_sbf_log || + node_->settings()->read_from_pcap) + { + node_->log( + log_level::INFO, + "AsyncManager finished reading file. Node will continue to publish queued messages."); + break; + } else { - to_be_parsed_index = to_be_parsed; - shift_bytes = 0; - arg_for_read_callback = 0; - continue; + node_->log(log_level::ERROR, + "AsyncManager connection lost. Trying to reconnect."); + ioService_->reset(); + ioThread_.join(); + while (!ioInterface_.connect()) + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + receive(); } - shift_bytes += current_buffer_size; - continue; + } else if (running_ && std::is_same::value) + { + // Send to check if TCP connection still alive + std::string empty = " "; + boost::asio::async_write( + *(ioInterface_.stream_), boost::asio::buffer(empty.data(), 1), + [](boost::system::error_code ec, std::size_t /*length*/) {}); } - to_be_parsed_index = to_be_parsed; - shift_bytes = 0; - arg_for_read_callback = 0; } - node_->log( - LogLevel::INFO, - "TryParsing() method finished since it did not receive anything to parse for 10 seconds.."); - delete[] to_be_parsed; // Freeing memory } - template - bool AsyncManager::send(const std::string& cmd) + template + void AsyncManager::write(const std::string& cmd) { - if (cmd.size() == 0) - { - node_->log(LogLevel::ERROR, - "Message size to be sent to the Rx would be 0"); - return true; - } - - io_service_->post(boost::bind(&AsyncManager::write, this, cmd)); - return true; + boost::asio::async_write( + *(ioInterface_.stream_), boost::asio::buffer(cmd.data(), cmd.size()), + [this, cmd](boost::system::error_code ec, std::size_t /*length*/) { + if (!ec) + { + // Prints the data that was sent + node_->log(log_level::DEBUG, "AsyncManager sent the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); + } else + { + node_->log(log_level::ERROR, + "AsyncManager was unable to send the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); + } + }); } - template - void AsyncManager::write(const std::string& cmd) + template + void AsyncManager::resync() { - boost::asio::write(*stream_, boost::asio::buffer(cmd.data(), cmd.size())); - // Prints the data that was sent - node_->log(LogLevel::DEBUG, "Sent the following " + - std::to_string(cmd.size()) + - " bytes to the Rx: \n" + cmd); + telegram_.reset(new Telegram); + readSync<0>(); } - template - void AsyncManager::callAsyncWait(uint16_t* count) + template + template + void AsyncManager::readSync() { - timer_.async_wait(boost::bind(&AsyncManager::wait, this, count)); + static_assert(index < 3); + + boost::asio::async_read( + *(ioInterface_.stream_), + boost::asio::buffer(telegram_->message.data() + index, 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + Timestamp stamp = node_->getTime(); + + if (!ec) + { + if (numBytes == 1) + { + uint8_t& currByte = telegram_->message[index]; + + if (currByte == SYNC_BYTE_1) + { + telegram_->stamp = stamp; + readSync<1>(); + } else + { + switch (index) + { + case 0: + { + telegram_->type = telegram_type::UNKNOWN; + readUnknown(); + break; + } + case 1: + { + switch (currByte) + { + case SBF_SYNC_BYTE_2: + { + telegram_->type = telegram_type::SBF; + readSbfHeader(); + break; + } + case NMEA_SYNC_BYTE_2: + { + telegram_->type = telegram_type::NMEA; + readSync<2>(); + break; + } + case NMEA_INS_SYNC_BYTE_2: + { + telegram_->type = telegram_type::NMEA_INS; + readSync<2>(); + break; + } + case RESPONSE_SYNC_BYTE_2: + { + telegram_->type = telegram_type::RESPONSE; + readSync<2>(); + break; + } + default: + { + std::stringstream ss; + ss << std::hex << currByte; + node_->log( + log_level::DEBUG, + "AsyncManager sync byte 2 read fault, should never come here.. Received byte was " + + ss.str()); + resync(); + break; + } + } + break; + } + case 2: + { + switch (currByte) + { + case NMEA_SYNC_BYTE_3: + { + if (telegram_->type == telegram_type::NMEA) + readString(); + else + resync(); + break; + } + case NMEA_INS_SYNC_BYTE_3: + { + if (telegram_->type == telegram_type::NMEA_INS) + readString(); + else + resync(); + break; + } + case RESPONSE_SYNC_BYTE_3: + { + if (telegram_->type == telegram_type::RESPONSE) + readString(); + else + resync(); + break; + } + case RESPONSE_SYNC_BYTE_3a: + { + if (telegram_->type == telegram_type::RESPONSE) + readString(); + else + resync(); + break; + } + case ERROR_SYNC_BYTE_3: + { + if (telegram_->type == telegram_type::RESPONSE) + { + telegram_->type = + telegram_type::ERROR_RESPONSE; + readString(); + } else + resync(); + break; + } + default: + { + std::stringstream ss; + ss << std::hex << currByte; + node_->log( + log_level::DEBUG, + "AsyncManager sync byte 3 read fault, should never come here. Received byte was " + + ss.str()); + resync(); + break; + } + } + break; + } + default: + { + node_->log( + log_level::DEBUG, + "AsyncManager sync read fault, should never come here."); + resync(); + break; + } + } + } + } else + { + node_->log( + log_level::DEBUG, + "AsyncManager sync read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(log_level::DEBUG, + "AsyncManager sync read error: " + ec.message()); + } + }); } - template - AsyncManager::AsyncManager( - ROSaicNodeBase* node, boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size) : - node_(node), - timer_(*(io_service.get()), boost::posix_time::seconds(1)), stopping_(false), - try_parsing_(false), allow_writing_(true), do_read_count_(0), - buffer_size_(buffer_size), count_max_(6), circular_buffer_(node, buffer_size) - // Since buffer_size = 16384 in declaration, no need in definition anymore (even - // yields error message, due to "overwrite"). - { - node_->log( - LogLevel::DEBUG, - "Setting the private stream variable of the AsyncManager instance."); - stream_ = stream; - io_service_ = io_service; - in_.resize(buffer_size_); - - io_service_->post(boost::bind(&AsyncManager::read, this)); - // This function is used to ask the io_service to execute the given handler, - // but without allowing the io_service to call the handler from inside this - // function. The function signature of the handler must be: void handler(); - // The io_service guarantees that the handler (given as parameter) will only - // be called in a thread in which the run(), run_one(), poll() or poll_one() - // member functions is currently being invoked. So the fundamental difference - // is that dispatch will execute the work right away if it can and queue it - // otherwise while post queues the work no matter what. - async_background_thread_.reset(new boost::thread( - boost::bind(&boost::asio::io_service::run, io_service_))); - // Note that io_service_ is already pointer, hence need dereferencing - // operator & (ampersand). If the value of the pointer for the current thread - // is changed using reset(), then the previous value is destroyed by calling - // the cleanup routine. Alternatively, the stored value can be reset to NULL - // and the prior value returned by calling the release() member function, - // allowing the application to take back responsibility for destroying the - // object. - uint16_t count = 0; - waiting_thread_.reset(new boost::thread( - boost::bind(&AsyncManager::callAsyncWait, this, &count))); - - node_->log(LogLevel::DEBUG, "Launching tryParsing() thread.."); - parsing_thread_.reset( - new boost::thread(boost::bind(&AsyncManager::tryParsing, this))); - } // Calls std::terminate() on thread just created - - template - AsyncManager::~AsyncManager() + template + void AsyncManager::readSbfHeader() { - close(); - io_service_->stop(); - try_parsing_ = true; - parsing_condition_.notify_one(); - parsing_thread_->join(); - waiting_thread_->join(); - async_background_thread_->join(); + telegram_->message.resize(SBF_HEADER_SIZE); + + boost::asio::async_read( + *(ioInterface_.stream_), + boost::asio::buffer(telegram_->message.data() + 2, SBF_HEADER_SIZE - 2), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (SBF_HEADER_SIZE - 2)) + { + uint16_t length = + parsing_utilities::getLength(telegram_->message); + if (length > MAX_SBF_SIZE) + { + node_->log( + log_level::DEBUG, + "AsyncManager SBF header read fault, length of block exceeds " + + std::to_string(MAX_SBF_SIZE) + ": " + + std::to_string(length)); + } else + readSbf(length); + } else + { + node_->log( + log_level::DEBUG, + "AsyncManager SBF header read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(log_level::DEBUG, + "AsyncManager SBF header read error: " + + ec.message()); + } + }); } - template - void AsyncManager::read() + template + void AsyncManager::readSbf(std::size_t length) { - stream_->async_read_some( - boost::asio::buffer(in_.data(), in_.size()), - boost::bind(&AsyncManager::asyncReadSomeHandler, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); - // The handler is async_read_some_handler, whose call is postponed to - // when async_read_some completes. - if (do_read_count_ < 5) - ++do_read_count_; + telegram_->message.resize(length); + + boost::asio::async_read( + *(ioInterface_.stream_), + boost::asio::buffer(telegram_->message.data() + SBF_HEADER_SIZE, + length - SBF_HEADER_SIZE), + [this, length](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (length - SBF_HEADER_SIZE)) + { + if (crc::isValid(telegram_->message)) + { + telegramQueue_->push(telegram_); + } else + node_->log(log_level::DEBUG, + "AsyncManager crc failed for SBF " + + std::to_string(parsing_utilities::getId( + telegram_->message)) + + "."); + } else + { + node_->log( + log_level::DEBUG, + "AsyncManager SBF read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + } + resync(); + } else + { + node_->log(log_level::DEBUG, + "AsyncManager SBF read error: " + ec.message()); + } + }); } - template - void AsyncManager::asyncReadSomeHandler( - const boost::system::error_code& error, std::size_t bytes_transferred) + template + void AsyncManager::readUnknown() { - if (error) - { - node_->log(LogLevel::ERROR, - "Rx ASIO input buffer read error: " + error.message() + ", " + - std::to_string(bytes_transferred)); - } else if (bytes_transferred > 0) - { - Timestamp inTime = node_->getTime(); - if (read_callback_ && - !stopping_) // Will be false in InitializeSerial (first call) - // since read_callback_ not added yet.. - { - boost::mutex::scoped_lock lock(parse_mutex_); - parsing_condition_.wait(lock, [this]() { return allow_writing_; }); - circular_buffer_.write(in_.data(), bytes_transferred); - allow_writing_ = false; - try_parsing_ = true; - recvTime_ = inTime; - lock.unlock(); - parsing_condition_.notify_one(); - } - } - - if (!stopping_) - io_service_->post(boost::bind(&AsyncManager::read, this)); + telegram_->message.resize(1); + telegram_->message.reserve(256); + readStringElements(); } - template - void AsyncManager::close() + template + void AsyncManager::readString() { - stopping_ = true; - boost::system::error_code error; - stream_->close(error); - if (error) - { - node_->log(LogLevel::ERROR, - "Error while closing the AsyncManager: " + error.message()); - } + telegram_->message.resize(3); + telegram_->message.reserve(256); + readStringElements(); } - template - void AsyncManager::wait(uint16_t* count) + template + void AsyncManager::readStringElements() { - if (*count < count_max_) - { - ++(*count); - timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1)); - if (!(*count == count_max_)) - { - timer_.async_wait(boost::bind(&AsyncManager::wait, this, count)); - } - } - if ((*count == count_max_) && (do_read_count_ < 3)) - // Why 3? Even if there are no incoming messages, read() is called once. - // It will be called a second time in TCP/IP mode since (just example) - // "IP10<" is transmitted. - { - node_->log( - LogLevel::INFO, - "No incoming messages, driver stopped, ros::spin() will spin forever unless you hit Ctrl+C."); - async_background_thread_->interrupt(); - } + boost::asio::async_read( + *(ioInterface_.stream_), boost::asio::buffer(buf_.data(), 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == 1) + { + telegram_->message.push_back(buf_[0]); + /*node_->log(log_level::DEBUG, + "Buffer: " + + std::string(telegram_->message.begin(), + telegram_->message.end()));*/ + + switch (buf_[0]) + { + case SYNC_BYTE_1: + { + telegram_.reset(new Telegram); + telegram_->message[0] = buf_[0]; + telegram_->stamp = node_->getTime(); + node_->log( + log_level::DEBUG, + "AsyncManager string read fault, sync 1 found."); + readSync<1>(); + break; + } + case LF: + { + if (telegram_->message[telegram_->message.size() - 2] == + CR) + telegramQueue_->push(telegram_); + else + node_->log( + log_level::DEBUG, + "LF wo CR: " + + std::string(telegram_->message.begin(), + telegram_->message.end())); + resync(); + break; + } + case CONNECTION_DESCRIPTOR_FOOTER: + { + telegram_->type = telegram_type::CONNECTION_DESCRIPTOR; + telegramQueue_->push(telegram_); + resync(); + break; + } + default: + { + readStringElements(); + break; + } + } + } else + { + node_->log( + log_level::DEBUG, + "AsyncManager string read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(log_level::DEBUG, + "AsyncManager string read error: " + ec.message()); + } + }); } -} // namespace io_comm_rx - -#endif // for ASYNC_MANAGER_HPP +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/callback_handlers.hpp b/include/septentrio_gnss_driver/communication/callback_handlers.hpp deleted file mode 100644 index 5d202f58..00000000 --- a/include/septentrio_gnss_driver/communication/callback_handlers.hpp +++ /dev/null @@ -1,353 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ***************************************************************************** -// -// Boost Software License - Version 1.0 - August 17th, 2003 -// -// Permission is hereby granted, free of charge, to any person or organization -// obtaining a copy of the software and accompanying documentation covered by -// this license (the "Software") to use, reproduce, display, distribute, -// execute, and transmit the Software, and to prepare derivative works of the -// Software, and to permit third-parties to whom the Software is furnished to -// do so, all subject to the following: - -// The copyright notices in the Software and this entire statement, including -// the above license grant, this restriction and the following disclaimer, -// must be included in all copies of the Software, in whole or in part, and -// all derivative works of the Software, unless such copies or derivative -// works are solely in the form of machine-executable object code generated by -// a source language processor. -// -// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT -// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE -// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, -// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. -// -// ***************************************************************************** - -#ifndef CALLBACK_HANDLERS_HPP -#define CALLBACK_HANDLERS_HPP - -// Boost includes -#include -// In C++, writing a loop that iterates over a sequence is tedious --> -// BOOST_FOREACH(char ch, "Hello World") -#include -// E.g. boost::function f = std::atoi;defines a pointer f that can -// point to functions that expect a parameter of type const char* and return a value -// of type int Generally, any place in which a function pointer would be used to -// defer a call or make a callback, Boost.Function can be used instead to allow the -// user greater flexibility in the implementation of the target. -#include -// Boost's thread enables the use of multiple threads of execution with shared data -// in portable C++ code. It provides classes and functions for managing the threads -// themselves, along with others for synchronizing data between the threads or -// providing separate copies of data specific to individual threads. -#include -#include -// The tokenizer class provides a container view of a series of tokens contained in a -// sequence, e.g. if you are not interested in non-words... -#include -// Join algorithm is a counterpart to split algorithms. It joins strings from a -// 'list' by adding user defined separator. -#include -// The class boost::posix_time::ptime that we will use defines a location-independent -// time. It uses the type boost::gregorian::date, yet also stores a time. -#include -// Boost.Asio may be used to perform both synchronous and asynchronous operations on -// I/O objects such as sockets. -#include -#include -#include -#include - -// ROSaic and C++ includes -#include -#include - -/** - * @file callback_handlers.hpp - * @date 22/08/20 - * @brief Handles callbacks when reading NMEA/SBF messages - */ - -extern bool g_response_received; -extern boost::mutex g_response_mutex; -extern boost::condition_variable g_response_condition; -extern bool g_cd_received; -extern boost::mutex g_cd_mutex; -extern boost::condition_variable g_cd_condition; -extern uint32_t g_cd_count; -extern std::string g_rx_tcp_port; - -namespace io_comm_rx { - /** - * @class CallbackHandler - * @brief Abstract class representing a generic callback handler, includes - * high-level functionality such as wait - */ - class AbstractCallbackHandler - { - public: - virtual void handle(RxMessage& rx_message, std::string message_key) = 0; - - bool Wait(const boost::posix_time::time_duration& timeout) - { - boost::mutex::scoped_lock lock(mutex_); - return condition_.timed_wait(lock, timeout); - } - - protected: - boost::mutex mutex_; - boost::condition_variable condition_; - }; - - /** - * @class CallbackHandler - * @brief Derived class operating on a ROS message level - */ - template - class CallbackHandler : public AbstractCallbackHandler - { - public: - virtual const T& Get() { return message_; } - - void handle(RxMessage& rx_message, std::string message_key) - { - boost::mutex::scoped_lock lock(mutex_); - try - { - if (!rx_message.read(message_key)) - { - std::ostringstream ss; - ss << "Rx decoder error for message with ID (empty field if non-determinable)" - << rx_message.messageID() << ". Reason unknown."; - throw std::runtime_error(ss.str()); - return; - } - } catch (std::runtime_error& e) - { - std::ostringstream ss; - ss << "Rx decoder error for message with ID " - << rx_message.messageID() << ".\n" - << e.what(); - throw std::runtime_error(ss.str()); - return; - } - - condition_.notify_all(); - } - - private: - T message_; - }; - - /** - * @class CallbackHandlers - * @brief Represents ensemble of (to be constructed) ROS messages, to be handled - * at once by this class - */ - class CallbackHandlers - { - - public: - //! Key is std::string and represents the ROS message key, value is - //! boost::shared_ptr - typedef std::multimap> - CallbackMap; - - CallbackHandlers(ROSaicNodeBase* node, Settings* settings) : - node_(node), - rx_message_(node, settings), - settings_(settings) - {} - - /** - * @brief Adds a pair to the multimap "callbackmap_", with the message_key - * being the key - * - * This method is called by "handlers_" in rosaic_node.cpp. - * T would be a (custom or not) ROS message, e.g. - * PVTGeodeticMsg, or nmea_msgs::GPGGA. Note that - * "typename" could be omitted in the argument. - * @param message_key The pair's key - * @return The modified multimap "callbackmap_" - */ - template - CallbackMap insert(std::string message_key) - { - boost::mutex::scoped_lock lock(callback_mutex_); - // Adding typename might be cleaner, but is optional again - CallbackHandler* handler = new CallbackHandler(); - callbackmap_.insert(std::make_pair( - message_key, boost::shared_ptr(handler))); - CallbackMap::key_type key = message_key; - node_->log(LogLevel::DEBUG, "Key " + message_key + " successfully inserted into multimap: " + - (((unsigned int)callbackmap_.count(key)) ? "true" : "false")); - return callbackmap_; - } - - /** - * @brief Called every time rx_message is found to contain some potentially - * useful message - */ - void handle(); - - /** - * @brief Searches for Rx messages that could potentially be - * decoded/parsed/published - * @param[in] recvTimestamp Timestamp of buffer reception passed on from AsyncManager class - * @param[in] data Buffer passed on from AsyncManager class - * @param[in] size Size of the buffer - */ - void readCallback(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size); - - //! Callback handlers multimap for Rx messages; it needs to be public since - //! we copy-assign (did not work otherwise) new callbackmap_, after inserting - //! a pair to the multimap within the DefineMessages() method of the - //! ROSaicNode class, onto its old version. - CallbackMap callbackmap_; - - private: - //! Pointer to Node - ROSaicNodeBase* node_; - - //! RxMessage parser - RxMessage rx_message_; - - //! Settings - Settings* settings_; - - //! The "static" keyword resolves construct-by-copying issues related to this - //! mutex by making it available throughout the code unit. The mutex - //! constructor list contains "mutex (const mutex&) = delete", hence - //! construct-by-copying a mutex is explicitly prohibited. The get_handlers() - //! method of the Comm_IO class hence forces us to make this mutex static. - static boost::mutex callback_mutex_; - - //! Determines which of the SBF blocks necessary for the gps_common::GPSFix - //! ROS message arrives last and thus launches its construction - static std::string do_gpsfix_; - - //! Determines which of the INS integrated SBF blocks necessary for the gps_common::GPSFix - //! ROS message arrives last and thus launches its construction - static std::string do_insgpsfix_; - - //! Determines which of the SBF blocks necessary for the - //! NavSatFixMsg ROS message arrives last and thus launches its - //! construction - static std::string do_navsatfix_; - - //! Determines which of the INS integrated SBF blocks necessary for the - //! NavSatFixMsg ROS message arrives last and thus launches its construction - static std::string do_insnavsatfix_; - - //! Determines which of the SBF blocks necessary for the - //! geometry_msgs/PoseWithCovarianceStamped ROS message arrives last and thus - //! launches its construction - static std::string do_pose_; - - //! Determines which of the INS integrated SBF blocks necessary for the - //! geometry_msgs/PoseWithCovarianceStamped ROS message arrives last and thus - //! launches its construction - static std::string do_inspose_; - - //! Determines which of the SBF blocks necessary for the - //! diagnostic_msgs/DiagnosticArray ROS message arrives last and thus - //! launches its construction - static std::string do_diagnostics_; - - //! Determines which of the SBF blocks necessary for the - //! sensor_msgs/Imu ROS message arrives last and thus - //! launches its construction - static std::string do_imu_; - - //! Determines which of the SBF blocks necessary for the - //! nav_msgs/Odometry ROS message arrives last and thus - //! launches its construction - static std::string do_inslocalization_; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for GPSFix to a uint32_t - typedef std::unordered_map GPSFixMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static GPSFixMap gpsfix_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for NavSatFix to a uint32_t - typedef std::unordered_map NavSatFixMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static NavSatFixMap navsatfix_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for PoseWithCovarianceStamped to a uint32_t - typedef std::unordered_map PoseWithCovarianceStampedMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static PoseWithCovarianceStampedMap pose_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for DiagnosticArray to a uint32_t - typedef std::unordered_map DiagnosticArrayMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static DiagnosticArrayMap diagnosticarray_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for Imu to a uint32_t - typedef std::unordered_map ImuMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static ImuMap imu_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for Localization to a uint32_t - typedef std::unordered_map LocalizationMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static LocalizationMap localization_map; - }; - -} // namespace io_comm_rx - -#endif \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/circular_buffer.hpp b/include/septentrio_gnss_driver/communication/circular_buffer.hpp deleted file mode 100644 index 188906de..00000000 --- a/include/septentrio_gnss_driver/communication/circular_buffer.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ROS includes -#include - -// C++ library includes -#include -#include - -#ifndef CIRCULAR_BUFFER_HPP -#define CIRCULAR_BUFFER_HPP - -/** - * @file circular_buffer.hpp - * @brief Declares a class for creating, writing to and reading from a circular - * bufffer - * @date 25/09/20 - */ - -/** - * @class CircularBuffer - * @brief Class for creating, writing to and reading from a circular buffer - */ -class CircularBuffer -{ -public: - //! Constructor of CircularBuffer - explicit CircularBuffer(ROSaicNodeBase* node, std::size_t capacity); - //! Destructor of CircularBuffer - ~CircularBuffer(); - //! Returns size_ - std::size_t size() const { return size_; } - //! Returns capacity_ - std::size_t capacity() const { return capacity_; } - //! Returns number of bytes written. - std::size_t write(const uint8_t* data, std::size_t bytes); - //! Returns number of bytes read. - std::size_t read(uint8_t* data, std::size_t bytes); - -private: - //! Pointer to the node - ROSaicNodeBase* node_; - //! Specifies where we start writing - std::size_t head_; - //! Specifies where we start reading - std::size_t tail_; - //! Number of bytes that have been written but not yet read - std::size_t size_; - //! Capacity of the circular buffer - std::size_t capacity_; - //! Pointer that always points to the same memory address, hence could be const - //! pointer - uint8_t* data_; -}; - -#endif // for CIRCULAR_BUFFER_HPP \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 336367fc..320f9a5b 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -56,28 +56,18 @@ // // ***************************************************************************** -#ifndef COMMUNICATION_CORE_HPP // This block is called a conditional group. The - // controlled text will get included in the - // preprocessor output iff the macroname is not - // defined. -#define COMMUNICATION_CORE_HPP // Include guards help to avoid the double inclusion - // of header files, by defining a token = macro. +#pragma once // Boost includes #include #include -#include -#include -#include // dealing with bad file descriptor error -#include // C++ library includes #include #include #include -#include // for usleep() // ROSaic includes #include -#include +#include /** * @file communication_core.hpp @@ -86,11 +76,11 @@ */ /** - * @namespace io_comm_rx + * @namespace io * This namespace is for the communication interface, handling all aspects related to * serial and TCP/IP communication.. */ -namespace io_comm_rx { +namespace io { //! Possible baudrates for the Rx const static uint32_t BAUDRATES[] = { @@ -99,41 +89,34 @@ namespace io_comm_rx { 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; /** - * @class Comm_IO + * @class CommunicationCore * @brief Handles communication with and configuration of the mosaic (and beyond) * receiver(s) */ - class Comm_IO + class CommunicationCore { public: /** - * @brief Constructor of the class Comm_IO + * @brief Constructor of the class CommunicationCore * @param[in] node Pointer to node */ - Comm_IO(ROSaicNodeBase* node, Settings* settings); + CommunicationCore(ROSaicNodeBase* node); /** - * @brief Default destructor of the class Comm_IO + * @brief Default destructor of the class CommunicationCore */ - ~Comm_IO(); + ~CommunicationCore(); /** - * @brief Initializes the I/O handling + * @brief Connects the data stream */ - void initializeIO(); + void connect(); /** * @brief Configures Rx: Which SBF/NMEA messages it should output and later * correction settings - * @param[in] settings The device's settings * */ void configureRx(); - /** - * @brief Defines which Rx messages to read and which ROS messages to publish - * @param[in] settings The device's settings - * */ - void defineMessages(); - /** * @brief Hands over NMEA velocity message over to the send() method of * manager_ @@ -143,72 +126,23 @@ namespace io_comm_rx { private: /** - * @brief Reset main port so it can receive commands - */ - void resetMainPort(); - - /** - * @brief Sets up the stage for SBF file reading - * @param[in] file_name The name of (or path to) the SBF file, e.g. "xyz.sbf" + * @brief Resets Rx settings */ - void prepareSBFFileReading(std::string file_name); + void resetSettings(); /** - * @brief Sets up the stage for PCAP file reading - * @param[in] file_name The path to PCAP file, e.g. "/tmp/capture.sbf" - */ - void preparePCAPFileReading(std::string file_name); - - /** - * @brief Attempts to (re)connect every reconnect_delay_s_ seconds - */ - void reconnect(); - - /** - * @brief Calls the reconnect() method - */ - void connect(); - - /** - * @brief Initializes the serial port - * @param[in] port The device's port address - * @param[in] baudrate The chosen baud rate of the port - * @param[in] flowcontrol Default is "None", set variable (not yet checked) - * to "RTS|CTS" to activate hardware flow control (only for serial ports - * COM1, COM2 and COM3 (for mosaic)) - * @return True if connection could be established, false otherwise - */ - bool initializeSerial(std::string port, uint32_t baudrate = 115200, - std::string flowcontrol = "None"); - - /** - * @brief Initializes the TCP I/O - * @param[in] host The TCP host - * @param[in] port The TCP port - * @return True if connection could be established, false otherwise - */ - bool initializeTCP(std::string host, std::string port); - - /** - * @brief Initializes SBF file reading and reads SBF file by repeatedly - * calling read_callback_() - * @param[in] file_name The name of (or path to) the SBF file, e.g. "xyz.sbf" + * @brief Initializes the I/O handling + * * @return Wether connection was successful */ - void initializeSBFFileReading(std::string file_name); + [[nodiscard]] bool initializeIo(); /** - * @brief Initializes PCAP file reading and reads PCAP file by repeatedly - * calling read_callback_() - * @param[in] file_name The name of (or path to) the PCAP file, e.g. - * "/tmp/capture.pcap" + * @brief Reset main connection so it can receive commands + * @return Main connection descriptor */ - void initializePCAPFileReading(std::string file_name); + std::string resetMainConnection(); - /** - * @brief Set the I/O manager - * @param[in] manager An I/O handler - */ - void setManager(const boost::shared_ptr& manager); + void processTelegrams(); /** * @brief Hands over to the send() method of manager_ @@ -218,56 +152,31 @@ namespace io_comm_rx { //! Pointer to Node ROSaicNodeBase* node_; - //! Callback handlers for the inwards streaming messages - CallbackHandlers handlers_; //! Settings - Settings* settings_; - //! Whether connecting to Rx was successful - bool connected_ = false; - //! Since the configureRx() method should only be called once the connection - //! was established, we need the threads to communicate this to each other. - //! Associated mutex.. - boost::mutex connection_mutex_; - //! Since the configureRx() method should only be called once the connection - //! was established, we need the threads to communicate this to each other. - //! Associated condition variable.. - boost::condition_variable connection_condition_; - //! Host name of TCP server - std::string tcp_host_; - //! TCP port number - std::string tcp_port_; - //! Whether yet-to-be-established connection to Rx will be serial or TCP - bool serial_; - //! Saves the port description - std::string serial_port_; + const Settings* settings_; + //! TelegramQueue + TelegramQueue telegramQueue_; + //! TelegramHandler + TelegramHandler telegramHandler_; + //! Processing thread + std::thread processingThread_; + //! Whether connecting was successful + bool initializedIo_ = false; //! Processes I/O stream data //! This declaration is deliberately stream-independent (Serial or TCP). - boost::shared_ptr manager_; - //! Baudrate at the moment, unless InitializeSerial or ResetSerial fail - uint32_t baudrate_; + std::unique_ptr manager_; - bool nmeaActivated_ = false; + std::unique_ptr> tcpClient_; + std::unique_ptr udpClient_; - //! Connection or reading thread - std::unique_ptr connectionThread_; - //! Indicator for threads to exit - std::atomic stopping_; - - friend class CallbackHandlers; - friend class RxMessage; + bool nmeaActivated_ = false; - //! Communication ports - std::string mainPort_; + //! Indicator for threads to run + std::atomic running_; - //! Host currently connected to - std::string host_; - //! Port over which TCP/IP connection is currently established - std::string port_; - //! Sleep time in microseconds (there is no Unix command for milliseconds) - //! after setting the baudrate to certain value (important between - //! increments) - const static unsigned int SET_BAUDRATE_SLEEP_ = 500000; + //! Main communication port + std::string mainConnectionPort_; + // Port for receiving data streams + std::string streamPort_; }; -} // namespace io_comm_rx - -#endif // for COMMUNICATION_CORE_HPP +} // namespace io diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp new file mode 100644 index 00000000..d3125e9a --- /dev/null +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -0,0 +1,601 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#pragma once + +// C++ +#include + +// Linux +#include +#include + +// Boost +#include + +// pcap +#include + +// ROSaic +#include +#include + +//! Possible baudrates for the Rx +const static std::array baudrates = { + 1200, 2400, 4800, 9600, 19200, 38400, 57600, + 115200, 230400, 460800, 500000, 576000, 921600, 1000000, + 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; + +namespace io { + + class UdpClient + { + public: + UdpClient(ROSaicNodeBase* node, int16_t port, TelegramQueue* telegramQueue) : + node_(node), running_(true), port_(port), telegramQueue_(telegramQueue) + { + connect(); + watchdogThread_ = + std::thread(boost::bind(&UdpClient::runWatchdog, this)); + } + + ~UdpClient() + { + running_ = false; + + node_->log(log_level::INFO, "UDP client shutting down threads"); + ioService_.stop(); + ioThread_.join(); + watchdogThread_.join(); + node_->log(log_level::INFO, " UDP client threads stopped"); + } + + private: + void connect() + { + socket_.reset(new boost::asio::ip::udp::socket( + ioService_, + boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), port_))); + + asyncReceive(); + + ioThread_ = std::thread(boost::bind(&UdpClient::runIoService, this)); + + node_->log(log_level::INFO, + "Listening on UDP port " + std::to_string(port_)); + } + + void asyncReceive() + + { + socket_->async_receive_from( + boost::asio::buffer(buffer_, MAX_UDP_PACKET_SIZE), eP_, + boost::bind(&UdpClient::handleReceive, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); + } + + void handleReceive(const boost::system::error_code& error, + size_t bytes_recvd) + { + Timestamp stamp = node_->getTime(); + size_t idx = 0; + + if (!error && (bytes_recvd > 0)) + { + while ((bytes_recvd - idx) > 2) + { + std::shared_ptr telegram(new Telegram); + telegram->stamp = stamp; + /*node_->log(log_level::DEBUG, + "Buffer: " + std::string(telegram->message.begin(), + telegram->message.end()));*/ + if (buffer_[idx] == SYNC_BYTE_1) + { + if (buffer_[idx + 1] == SBF_SYNC_BYTE_2) + { + if ((bytes_recvd - idx) > SBF_HEADER_SIZE) + { + uint16_t length = parsing_utilities::parseUInt16( + &buffer_[idx + 6]); + telegram->message.assign(&buffer_[idx], + &buffer_[idx + length]); + if (crc::isValid(telegram->message)) + { + telegram->type = telegram_type::SBF; + telegramQueue_->push(telegram); + } else + node_->log( + log_level::DEBUG, + "AsyncManager crc failed for SBF " + + std::to_string(parsing_utilities::getId( + telegram->message)) + + "."); + + idx += length; + } + + } else if ((buffer_[idx + 1] == NMEA_SYNC_BYTE_2) && + (buffer_[idx + 2] == NMEA_SYNC_BYTE_3)) + { + size_t idx_end = findNmeaEnd(idx, bytes_recvd); + telegram->message.assign(&buffer_[idx], + &buffer_[idx_end + 1]); + telegram->type = telegram_type::NMEA; + telegramQueue_->push(telegram); + idx = idx_end + 1; + + } else if ((buffer_[idx + 1] == NMEA_INS_SYNC_BYTE_2) && + (buffer_[idx + 2] == NMEA_INS_SYNC_BYTE_3)) + { + size_t idx_end = findNmeaEnd(idx, bytes_recvd); + telegram->message.assign(&buffer_[idx], + &buffer_[idx_end + 1]); + telegram->type = telegram_type::NMEA_INS; + telegramQueue_->push(telegram); + idx = idx_end + 1; + } else + { + node_->log(log_level::DEBUG, + "head: " + + std::string(std::string( + telegram->message.begin(), + telegram->message.begin() + 2))); + } + } else + { + node_->log(log_level::DEBUG, "UDP msg resync."); + ++idx; + } + } + } else + { + node_->log(log_level::ERROR, + "UDP client receive error: " + error.message()); + } + + asyncReceive(); + } + + void runIoService() + { + ioService_.run(); + node_->log(log_level::INFO, "UDP client ioService terminated."); + } + + void runWatchdog() + { + while (running_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + if (running_ && ioService_.stopped()) + { + node_->log(log_level::ERROR, + "UDP client connection lost. Trying to reconnect."); + ioService_.reset(); + ioThread_.join(); + connect(); + } + } + } + + private: + size_t findNmeaEnd(size_t idx, size_t bytes_recvd) + { + size_t idx_end = idx + 2; + + while (idx_end < bytes_recvd) + { + if ((buffer_[idx_end] == LF) && (buffer_[idx_end - 1] == CR)) + break; + + ++idx_end; + } + return idx_end; + } + //! Pointer to the node + ROSaicNodeBase* node_; + std::atomic running_; + int16_t port_; + boost::asio::io_service ioService_; + std::thread ioThread_; + std::thread watchdogThread_; + boost::asio::ip::udp::endpoint eP_; + std::unique_ptr socket_; + std::array buffer_; + TelegramQueue* telegramQueue_; + }; + + class TcpIo + { + public: + TcpIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService) + { + port_ = node_->settings()->device_tcp_port; + } + + ~TcpIo() { stream_->close(); } + + void close() { stream_->close(); } + + void setPort(const std::string& port) { port_ = port; } + + [[nodiscard]] bool connect() + { + boost::asio::ip::tcp::resolver::iterator endpointIterator; + + try + { + boost::asio::ip::tcp::resolver resolver(*ioService_); + boost::asio::ip::tcp::resolver::query query( + node_->settings()->device_tcp_ip, port_); + endpointIterator = resolver.resolve(query); + } catch (std::runtime_error& e) + { + node_->log(log_level::ERROR, + "Could not resolve " + node_->settings()->device_tcp_ip + + " on port " + port_ + ": " + e.what()); + return false; + } + + stream_.reset(new boost::asio::ip::tcp::socket(*ioService_)); + + node_->log(log_level::INFO, "Connecting to tcp://" + + node_->settings()->device_tcp_ip + ":" + + port_ + "..."); + + try + { + stream_->connect(*endpointIterator); + + stream_->set_option(boost::asio::ip::tcp::no_delay(true)); + + node_->log(log_level::INFO, + "Connected to " + endpointIterator->host_name() + ":" + + endpointIterator->service_name() + "."); + } catch (std::runtime_error& e) + { + node_->log(log_level::ERROR, + "Could not connect to " + endpointIterator->host_name() + + ": " + endpointIterator->service_name() + ": " + + e.what()); + return false; + } + return true; + } + + private: + ROSaicNodeBase* node_; + std::shared_ptr ioService_; + + std::string port_; + + public: + std::unique_ptr stream_; + }; + + class SerialIo + { + public: + SerialIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService), flowcontrol_(node->settings()->hw_flow_control), + baudrate_(node->settings()->baudrate) + { + stream_.reset(new boost::asio::serial_port(*ioService_)); + } + + ~SerialIo() { stream_->close(); } + + void close() { stream_->close(); } + + [[nodiscard]] bool connect() + { + if (stream_->is_open()) + { + stream_->close(); + } + + bool opened = false; + + while (!opened) + { + try + { + node_->log(log_level::INFO, + "Connecting serially to device " + + node_->settings()->device + + ", targeted baudrate: " + + std::to_string(node_->settings()->baudrate)); + stream_->open(node_->settings()->device); + opened = true; + } catch (const boost::system::system_error& err) + { + node_->log(log_level::ERROR, "Could not open serial port " + + node_->settings()->device + + ". Error: " + err.what() + + ". Will retry every second."); + + using namespace std::chrono_literals; + std::this_thread::sleep_for(1s); + } + } + + // No Parity, 8bits data, 1 stop Bit + stream_->set_option(boost::asio::serial_port_base::baud_rate(baudrate_)); + stream_->set_option(boost::asio::serial_port_base::parity( + boost::asio::serial_port_base::parity::none)); + stream_->set_option(boost::asio::serial_port_base::character_size(8)); + stream_->set_option(boost::asio::serial_port_base::stop_bits( + boost::asio::serial_port_base::stop_bits::one)); + + // Hardware flow control settings + if (flowcontrol_ == "RTS|CTS") + { + stream_->set_option(boost::asio::serial_port_base::flow_control( + boost::asio::serial_port_base::flow_control::hardware)); + } else + { + stream_->set_option(boost::asio::serial_port_base::flow_control( + boost::asio::serial_port_base::flow_control::none)); + } + + // Set low latency + int fd = stream_->native_handle(); + struct serial_struct serialInfo; + ioctl(fd, TIOCGSERIAL, &serialInfo); + serialInfo.flags |= ASYNC_LOW_LATENCY; + ioctl(fd, TIOCSSERIAL, &serialInfo); + + return setBaudrate(); + } + + [[nodiscard]] bool setBaudrate() + { + // Setting the baudrate, incrementally.. + node_->log(log_level::DEBUG, + "Gradually increasing the baudrate to the desired value..."); + boost::asio::serial_port_base::baud_rate current_baudrate; + node_->log(log_level::DEBUG, "Initiated current_baudrate object..."); + try + { + stream_->get_option(current_baudrate); // Note that this sets + // current_baudrate.value() + // often to 115200, since by + // default, all Rx COM ports, + // at least for mosaic Rxs, are set to a baudrate of 115200 baud, + // using 8 data-bits, no parity and 1 stop-bit. + } catch (boost::system::system_error& e) + { + + node_->log(log_level::ERROR, "get_option failed due to " + + static_cast(e.what())); + node_->log(log_level::INFO, "Additional info about error is " + + static_cast(e.what())); + /* + boost::system::error_code e_loop; + do // Caution: Might cause infinite loop.. + { + stream_->get_option(current_baudrate, e_loop); + } while(e_loop); + */ + return false; + } + // Gradually increase the baudrate to the desired value + // The desired baudrate can be lower or larger than the + // current baudrate; the for loop takes care of both scenarios. + node_->log(log_level::DEBUG, + "Current baudrate is " + + std::to_string(current_baudrate.value())); + for (uint8_t i = 0; i < baudrates.size(); i++) + { + if (current_baudrate.value() == baudrate_) + { + break; // Break if the desired baudrate has been reached. + } + if (current_baudrate.value() >= baudrates[i] && + baudrate_ > baudrates[i]) + { + continue; + } + // Increment until Baudrate[i] matches current_baudrate. + try + { + stream_->set_option( + boost::asio::serial_port_base::baud_rate(baudrates[i])); + } catch (boost::system::system_error& e) + { + + node_->log(log_level::ERROR, + "set_option failed due to " + + static_cast(e.what())); + node_->log(log_level::INFO, + "Additional info about error is " + + static_cast(e.what())); + return false; + } + using namespace std::chrono_literals; + std::this_thread::sleep_for(500ms); + + try + { + stream_->get_option(current_baudrate); + } catch (boost::system::system_error& e) + { + + node_->log(log_level::ERROR, + "get_option failed due to " + + static_cast(e.what())); + node_->log(log_level::INFO, + "Additional info about error is " + + static_cast(e.what())); + /* + boost::system::error_code e_loop; + do // Caution: Might cause infinite loop.. + { + stream_->get_option(current_baudrate, e_loop); + } while(e_loop); + */ + return false; + } + node_->log(log_level::DEBUG, + "Set ASIO baudrate to " + + std::to_string(current_baudrate.value())); + } + node_->log(log_level::INFO, + "Set ASIO baudrate to " + + std::to_string(current_baudrate.value()) + + ", leaving InitializeSerial() method"); + + // clear io + ::tcflush(stream_->native_handle(), TCIOFLUSH); + + return true; + } + + private: + ROSaicNodeBase* node_; + std::shared_ptr ioService_; + std::string flowcontrol_; + uint32_t baudrate_; + + public: + std::unique_ptr stream_; + }; + + class SbfFileIo + { + public: + SbfFileIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService) + { + } + + ~SbfFileIo() { stream_->close(); } + + void close() { stream_->close(); } + + [[nodiscard]] bool connect() + { + node_->log(log_level::INFO, "Opening SBF file stream" + + node_->settings()->device + "..."); + + int fd = open(node_->settings()->device.c_str(), O_RDONLY); + if (fd == -1) + { + node_->log(log_level::ERROR, "open SBF file failed."); + return false; + } + + try + { + stream_.reset( + new boost::asio::posix::stream_descriptor(*ioService_)); + stream_->assign(fd); + + } catch (std::runtime_error& e) + { + node_->log(log_level::ERROR, "assigning SBF file failed due to " + + static_cast(e.what())); + return false; + } + return true; + } + + private: + ROSaicNodeBase* node_; + std::shared_ptr ioService_; + + public: + std::unique_ptr stream_; + }; + + class PcapFileIo + { + public: + PcapFileIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService) + { + } + + ~PcapFileIo() + { + pcap_close(pcap_); + stream_->close(); + } + + void close() + { + pcap_close(pcap_); + stream_->close(); + } + + [[nodiscard]] bool connect() + { + try + { + node_->log(log_level::INFO, "Opening pcap file stream" + + node_->settings()->device + "..."); + + stream_.reset( + new boost::asio::posix::stream_descriptor(*ioService_)); + + pcap_ = pcap_open_offline(node_->settings()->device.c_str(), + errBuff_.data()); + stream_->assign(pcap_get_selectable_fd(pcap_)); + + } catch (std::runtime_error& e) + { + node_->log(log_level::ERROR, "assigning PCAP file failed due to " + + static_cast(e.what())); + return false; + } + return true; + } + + private: + ROSaicNodeBase* node_; + std::shared_ptr ioService_; + std::array errBuff_; + pcap_t* pcap_; + + public: + std::unique_ptr stream_; + }; +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp new file mode 100644 index 00000000..4dd46f66 --- /dev/null +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -0,0 +1,430 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +// ***************************************************************************** +// +// Boost Software License - Version 1.0 - August 17th, 2003 +// +// Permission is hereby granted, free of charge, to any person or organization +// obtaining a copy of the software and accompanying documentation covered by +// this license (the "Software") to use, reproduce, display, distribute, +// execute, and transmit the Software, and to prepare derivative works of the +// Software, and to permit third-parties to whom the Software is furnished to +// do so, all subject to the following: + +// The copyright notices in the Software and this entire statement, including +// the above license grant, this restriction and the following disclaimer, +// must be included in all copies of the Software, in whole or in part, and +// all derivative works of the Software, unless such copies or derivative +// works are solely in the form of machine-executable object code generated by +// a source language processor. +// +// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT +// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// ***************************************************************************** + +#pragma once + +// C++ libraries +#include // for assert +#include +#include +#include +// Boost includes +#include +#include +#include +#include +// ROSaic includes +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @file message_parser.hpp + * @brief Defines a class that reads messages handed over from the circular buffer + */ + +//! Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's +//! Mode field +enum TypeOfPVT_Enum +{ + evNoPVT, + evStandAlone, + evDGPS, + evFixed, + evRTKFixed, + evRTKFloat, + evSBAS, + evMovingBaseRTKFixed, + evMovingBaseRTKFloat, + evPPP +}; + +enum SbfId +{ + PVT_CARTESIAN = 4006, + PVT_GEODETIC = 4007, + BASE_VECTOR_CART = 4043, + BASE_VECTOR_GEOD = 4028, + POS_COV_CARTESIAN = 5905, + POS_COV_GEODETIC = 5906, + ATT_EULER = 5938, + ATT_COV_EULER = 5939, + CHANNEL_STATUS = 4013, + MEAS_EPOCH = 4027, + DOP = 4001, + VEL_COV_CARTESIAN = 5907, + VEL_COV_GEODETIC = 5908, + RECEIVER_STATUS = 4014, + QUALITY_IND = 4082, + RECEIVER_SETUP = 5902, + INS_NAV_CART = 4225, + INS_NAV_GEOD = 4226, + EXT_EVENT_INS_NAV_GEOD = 4230, + EXT_EVENT_INS_NAV_CART = 4229, + IMU_SETUP = 4224, + VEL_SENSOR_SETUP = 4244, + EXT_SENSOR_MEAS = 4050, + RECEIVER_TIME = 5914, + GAL_AUTH_STATUS = 4245, + RF_STATUS = 4092 +}; + +namespace io { + + /** + * @class MessageHandler + * @brief Can search buffer for messages, read/parse them, and so on + */ + class MessageHandler + { + public: + /** + * @brief Constructor of the MessageHandler class + * @param[in] node Pointer to the node) + */ + MessageHandler(ROSaicNodeBase* node) : + node_(node), settings_(node->settings()), unix_time_(0) + { + } + + void setLeapSeconds() + { + // set leap seconds to paramter if reading from file + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + current_leap_seconds_ = settings_->leap_seconds; + } + + /** + * @brief Parse SBF block + * @param[in] telegram Telegram to be parsed + */ + void parseSbf(const std::shared_ptr& telegram); + + /** + * @brief Parse NMEA block + * @param[in] telegram Telegram to be parsed + */ + void parseNmea(const std::shared_ptr& telegram); + + private: + /** + * @brief Header assembling + * @param[in] frameId String of frame ID + * @param[in] telegram telegram from which the msg was assembled + * @param[in] msg ROS msg for which the header is assembled + */ + template + void assembleHeader(const std::string& frameId, + const std::shared_ptr& telegram, T& msg) const; + /** + * @brief Publishing function + * @param[in] topic String of topic + * @param[in] msg ROS message to be published + */ + template + void publish(const std::string& topic, const M& msg); + + /** + * @brief Publishing function + * @param[in] msg Localization message + */ + + void publishTf(const LocalizationMsg& msg); + + /** + * @brief Pointer to the node + */ + ROSaicNodeBase* node_; + + /** + * @brief Pointer to settings struct + */ + const Settings* settings_; + + /** + * @brief Map of NMEA messgae IDs and uint8_t + */ + std::unordered_map nmeaMap_{ + {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPRMC", 1}, {"$INRMC", 1}, + {"$GPGSA", 2}, {"$INGSA", 2}, {"$GAGSV", 3}, {"$INGSV", 3}}; + + /** + * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks + * need to be stored + */ + PVTGeodeticMsg last_pvtgeodetic_; + + /** + * @brief Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic + * blocks need to be stored + */ + PosCovGeodeticMsg last_poscovgeodetic_; + + /** + * @brief Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to + * be stored + */ + AttEulerMsg last_atteuler_; + + /** + * @brief Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks + * need to be stored + */ + AttCovEulerMsg last_attcoveuler_; + + /** + * @brief Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming + * INSNavGeod blocks need to be stored + */ + INSNavGeodMsg last_insnavgeod_; + + /** + * @brief Since LoclaizationEcef needs INSNavCart, incoming + * INSNavCart blocks need to be stored + */ + INSNavCartMsg last_insnavcart_; + + /** + * @brief Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks + * need to be stored + */ + ExtSensorMeasMsg last_extsensmeas_; + + /** + * @brief Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks + * need to be stored + */ + ChannelStatus last_channelstatus_; + + /** + * @brief Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks + * need to be stored + */ + MeasEpochMsg last_measepoch_; + + /** + * @brief Since GPSFix needs DOP, incoming DOP blocks need to be stored + */ + Dop last_dop_; + + /** + * @brief Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks + * need to be stored + */ + VelCovGeodeticMsg last_velcovgeodetic_; + + /** + * @brief Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus + * blocks need to be stored + */ + ReceiverStatus last_receiverstatus_; + + /** + * @brief Since DiagnosticArray needs QualityInd, incoming QualityInd blocks + * need to be stored + */ + QualityInd last_qualityind_; + + /** + * @brief Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup + * blocks need to be stored + */ + ReceiverSetup last_receiversetup_; + + /** + * @brief Stores incoming GALAuthStatus block + */ + GalAuthStatusMsg last_gal_auth_status_; + + /** + * @brief Wether OSNMA info has been received + */ + bool osnma_info_available_ = false; + + /** + * @brief Stores incoming RFStatus block + */ + RfStatusMsg last_rf_status_; + + //! When reading from an SBF file, the ROS publishing frequency is governed + //! by the time stamps found in the SBF blocks therein. + Timestamp unix_time_; + + //! Last reported PVT processing latency + mutable uint64_t last_pvt_latency_ = 0; + + //! Current leap seconds as received, do not use value is -128 + int32_t current_leap_seconds_ = -128; + + /** + * @brief "Callback" function when constructing NavSatFix messages + */ + void assembleNavSatFix(); + + /** + * @brief "Callback" function when constructing GPSFix messages + */ + void assembleGpsFix(); + + /** + * @brief "Callback" function when constructing PoseWithCovarianceStamped + * messages + */ + void assemblePoseWithCovarianceStamped(); + + /** + * @brief "Callback" function when constructing + * DiagnosticArrayMsg messages + * @param[in] telegram telegram from which the msg was assembled + */ + void assembleDiagnosticArray(const std::shared_ptr& telegram); + + /** + * @brief "Callback" function when constructing + * OSNMA DiagnosticArrayMsg messages + */ + void assembleOsnmaDiagnosticArray(); + + /** + * @brief "Callback" function when constructing + * RFStatus DiagnosticArrayMsg messages + */ + void assembleAimAndDiagnosticArray(); + + /** + * @brief "Callback" function when constructing + * ImuMsg messages + */ + void assembleImu(); + + /** + * @brief "Callback" function when constructing + * LocalizationMsg messages in UTM + */ + void assembleLocalizationUtm(); + + /** + * @brief "Callback" function when constructing + * LocalizationMsg messages in ECEF + */ + void assembleLocalizationEcef(); + + /** + * @brief function to fill twist part of LocalizationMsg + * @param[in] roll roll [rad] + * @param[in] pitch pitch [rad] + * @param[in] yaw yaw [rad] + * @param[inout] msg LocalizationMsg to be filled + */ + void assembleLocalizationMsgTwist(double roll, double pitch, double yaw, + LocalizationMsg& msg) const; + + /** + * @brief "Callback" function when constructing + * TwistWithCovarianceStampedMsg messages + * @param[in] fromIns Wether to contruct message from INS data + */ + void assembleTwist(bool fromIns = false); + + /** + * @brief function when constructing + * TimeReferenceMsg messages + * @param[in] telegram telegram from which the msg was assembled + */ + void assembleTimeReference(const std::shared_ptr& telegram); + + /** + * @brief Waits according to time when reading from file + * @param[in] time_obj wait until time + */ + void wait(Timestamp time_obj); + + /** + * @brief Fixed UTM zone + */ + std::shared_ptr fixedUtmZone_; + + /** + * @brief Calculates the timestamp, in the Unix Epoch time format + * This is either done using the TOW as transmitted with the SBF block (if + * "use_gnss" is true), or using the current time. + * @param[in] data Pointer to the buffer + * @return Timestamp object containing seconds and nanoseconds since last + * epoch + */ + Timestamp timestampSBF(const std::vector& message) const; + + /** + * @brief Calculates the timestamp, in the Unix Epoch time format + * This is either done using the TOW as transmitted with the SBF block (if + * "use_gnss" is true), or using the current time. + * @param[in] tow (Time of Week) Number of milliseconds that elapsed since + * the beginning of the current GPS week as transmitted by the SBF block + * @param[in] wnc (Week Number Counter) counts the number of complete weeks + * elapsed since January 6, 1980 + * @return Timestamp object containing seconds and nanoseconds since last + * epoch + */ + Timestamp timestampSBF(uint32_t tow, uint16_t wnc) const; + }; +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/pcap_reader.hpp b/include/septentrio_gnss_driver/communication/pcap_reader.hpp deleted file mode 100644 index 7ee5bdea..00000000 --- a/include/septentrio_gnss_driver/communication/pcap_reader.hpp +++ /dev/null @@ -1,121 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#ifndef PCAP_READER_H -#define PCAP_READER_H - -#include -#include -#include -#include - -/** - * @file pcap_reader.hpp - * @date 07/05/2021 - * @author Ashwin A Nayar - * - * @brief Declares a class for handling pcap files. - */ - -namespace pcapReader { - - using buffer_t = std::vector; - - //! Read operation status - enum ReadResult - { - /// Data read successfully - READ_SUCCESS = 0, - READ_INSUFFICIENT_DATA = 1, - READ_TIMEOUT = 2, - READ_INTERRUPTED = 3, - READ_ERROR = -1, - /// Unable to parse data, parsing error - READ_PARSE_FAILED = -2 - - }; - - /** - * @class PcapDevice - * @brief Class for handling a pcap file - */ - class PcapDevice - { - public: - static const size_t BUFFSIZE = 100; - - /** - * @brief Constructor for PcapDevice - * @param[out] buffer Buffer to write read raw data to - */ - explicit PcapDevice(ROSaicNodeBase* node, buffer_t& buffer); - - /** - * @brief Try to open a pcap file - * @param[in] device Path to pcap file - * @return True if success, false otherwise - */ - bool connect(const char* device); - - /** - * @brief Close connected file - */ - void disconnect(); - - /** - * @brief Check if file is open and healthy - * @return True if file is open, false otherwise - */ - bool isConnected() const; - - /** - * @brief Attempt to read a packet and store data to buffer - * @return Result of read operation - */ - ReadResult read(); - - //! Destructor for PcapDevice - ~PcapDevice(); - - private: - //! Pointer to the node - ROSaicNodeBase* node_; - //! Reference to raw data buffer to write to - buffer_t& m_dataBuff; - //! File handle to pcap file - pcap_t* m_device{nullptr}; - bpf_program m_pktFilter{}; - char m_errBuff[BUFFSIZE]{}; - char* m_deviceName; - buffer_t m_lastPkt; - }; -} // namespace pcapReader - -#endif // PCAP_READER_H diff --git a/include/septentrio_gnss_driver/communication/rx_message.hpp b/include/septentrio_gnss_driver/communication/rx_message.hpp deleted file mode 100644 index 4dbfde04..00000000 --- a/include/septentrio_gnss_driver/communication/rx_message.hpp +++ /dev/null @@ -1,760 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ***************************************************************************** -// -// Boost Software License - Version 1.0 - August 17th, 2003 -// -// Permission is hereby granted, free of charge, to any person or organization -// obtaining a copy of the software and accompanying documentation covered by -// this license (the "Software") to use, reproduce, display, distribute, -// execute, and transmit the Software, and to prepare derivative works of the -// Software, and to permit third-parties to whom the Software is furnished to -// do so, all subject to the following: - -// The copyright notices in the Software and this entire statement, including -// the above license grant, this restriction and the following disclaimer, -// must be included in all copies of the Software, in whole or in part, and -// all derivative works of the Software, unless such copies or derivative -// works are solely in the form of machine-executable object code generated by -// a source language processor. -// -// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT -// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE -// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, -// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. -// -// ***************************************************************************** - -//! 0x24 is ASCII for $ - 1st byte in each message -#ifndef NMEA_SYNC_BYTE_1 -#define NMEA_SYNC_BYTE_1 0x24 -#endif -//! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message -#ifndef NMEA_SYNC_BYTE_2_1 -#define NMEA_SYNC_BYTE_2_1 0x47 -#endif -//! 0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message -#ifndef NMEA_SYNC_BYTE_2_2 -#define NMEA_SYNC_BYTE_2_2 0x50 -#endif -//! 0x24 is ASCII for $ - 1st byte in each response from the Rx -#ifndef RESPONSE_SYNC_BYTE_1 -#define RESPONSE_SYNC_BYTE_1 0x24 -#endif -//! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx -#ifndef RESPONSE_SYNC_BYTE_2 -#define RESPONSE_SYNC_BYTE_2 0x52 -#endif -//! 0x0D is ASCII for "Carriage Return", i.e. "Enter" -#ifndef CARRIAGE_RETURN -#define CARRIAGE_RETURN 0x0D -#endif -//! 0x0A is ASCII for "Line Feed", i.e. "New Line" -#ifndef LINE_FEED -#define LINE_FEED 0x0A -#endif -//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the -//! command was invalid -#ifndef RESPONSE_SYNC_BYTE_3 -#define RESPONSE_SYNC_BYTE_3 0x3F -#endif -//! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after -//! initiating TCP connection -#ifndef CONNECTION_DESCRIPTOR_BYTE_1 -#define CONNECTION_DESCRIPTOR_BYTE_1 0x49 -#endif -//! 0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after -//! initiating TCP connection -#ifndef CONNECTION_DESCRIPTOR_BYTE_2 -#define CONNECTION_DESCRIPTOR_BYTE_2 0x50 -#endif - -// C++ libraries -#include // for assert -#include -#include -#include -// Boost includes -#include -#include -#include -#include -// ROSaic includes -#include -#include -#include -#include -#include -#include -#include - -#ifndef RX_MESSAGE_HPP -#define RX_MESSAGE_HPP - -/** - * @file rx_message.hpp - * @date 20/08/20 - * @brief Defines a class that reads messages handed over from the circular buffer - */ - -extern bool g_read_cd; -extern uint32_t g_cd_count; - -//! Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's -//! Mode field -enum TypeOfPVT_Enum -{ - evNoPVT, - evStandAlone, - evDGPS, - evFixed, - evRTKFixed, - evRTKFloat, - evSBAS, - evMovingBaseRTKFixed, - evMovingBaseRTKFloat, - evPPP -}; - -//! Since switch only works with int (yet NMEA message IDs are strings), we need -//! enum. Note drawbacks: No variable can have a name which is already in some -//! enumeration, enums are not type safe etc.. -enum RxID_Enum -{ - evNavSatFix, - evINSNavSatFix, - evGPSFix, - evINSGPSFix, - evPoseWithCovarianceStamped, - evINSPoseWithCovarianceStamped, - evGPGGA, - evGPRMC, - evGPGSA, - evGPGSV, - evGLGSV, - evGAGSV, - evPVTCartesian, - evPVTGeodetic, - evBaseVectorCart, - evBaseVectorGeod, - evPosCovCartesian, - evPosCovGeodetic, - evAttEuler, - evAttCovEuler, - evINSNavCart, - evINSNavGeod, - evIMUSetup, - evVelSensorSetup, - evExtEventINSNavGeod, - evExtEventINSNavCart, - evExtSensorMeas, - evGPST, - evChannelStatus, - evMeasEpoch, - evDOP, - evVelCovGeodetic, - evDiagnosticArray, - evLocalization, - evReceiverStatus, - evQualityInd, - evReceiverTime, - evReceiverSetup -}; - -namespace io_comm_rx { - - /** - * @class RxMessage - * @brief Can search buffer for messages, read/parse them, and so on - */ - class RxMessage - { - public: - /** - * @brief Constructor of the RxMessage class - * - * One can always provide a non-const value where a const one was expected. - * The const-ness of the argument just means the function promises not to - * change it.. Recall: static_cast by the way can remove or add const-ness, - * no other C++ cast is capable of removing it (not even reinterpret_cast) - * @param[in] data Pointer to the buffer that is about to be analyzed - * @param[in] size Size of the buffer (as handed over by async_read_some) - */ - RxMessage(ROSaicNodeBase* node, Settings* settings) : - node_(node), settings_(settings), unix_time_(0) - { - found_ = false; - crc_check_ = false; - message_size_ = 0; - - //! Pair of iterators to facilitate initialization of the map - std::pair type_of_pvt_pairs[] = { - std::make_pair(static_cast(0), evNoPVT), - std::make_pair(static_cast(1), evStandAlone), - std::make_pair(static_cast(2), evDGPS), - std::make_pair(static_cast(3), evFixed), - std::make_pair(static_cast(4), evRTKFixed), - std::make_pair(static_cast(5), evRTKFloat), - std::make_pair(static_cast(6), evSBAS), - std::make_pair(static_cast(7), evMovingBaseRTKFixed), - std::make_pair(static_cast(8), evMovingBaseRTKFloat), - std::make_pair(static_cast(10), evPPP)}; - - type_of_pvt_map = - TypeOfPVTMap(type_of_pvt_pairs, type_of_pvt_pairs + evPPP + 1); - - //! Pair of iterators to facilitate initialization of the map - std::pair rx_id_pairs[] = { - std::make_pair("NavSatFix", evNavSatFix), - std::make_pair("INSNavSatFix", evINSNavSatFix), - std::make_pair("GPSFix", evGPSFix), - std::make_pair("INSGPSFix", evINSGPSFix), - std::make_pair("PoseWithCovarianceStamped", - evPoseWithCovarianceStamped), - std::make_pair("INSPoseWithCovarianceStamped", - evINSPoseWithCovarianceStamped), - std::make_pair("$GPGGA", evGPGGA), - std::make_pair("$GPRMC", evGPRMC), - std::make_pair("$GPGSA", evGPGSA), - std::make_pair("$GPGSV", evGPGSV), - std::make_pair("$GLGSV", evGLGSV), - std::make_pair("$GAGSV", evGAGSV), - std::make_pair("4006", evPVTCartesian), - std::make_pair("4007", evPVTGeodetic), - std::make_pair("4043", evBaseVectorCart), - std::make_pair("4028", evBaseVectorGeod), - std::make_pair("5905", evPosCovCartesian), - std::make_pair("5906", evPosCovGeodetic), - std::make_pair("5938", evAttEuler), - std::make_pair("5939", evAttCovEuler), - std::make_pair("GPST", evGPST), - std::make_pair("4013", evChannelStatus), - std::make_pair("4027", evMeasEpoch), - std::make_pair("4001", evDOP), - std::make_pair("5908", evVelCovGeodetic), - std::make_pair("DiagnosticArray", evDiagnosticArray), - std::make_pair("Localization", evLocalization), - std::make_pair("4014", evReceiverStatus), - std::make_pair("4082", evQualityInd), - std::make_pair("5902", evReceiverSetup), - std::make_pair("4225", evINSNavCart), - std::make_pair("4226", evINSNavGeod), - std::make_pair("4230", evExtEventINSNavGeod), - std::make_pair("4229", evExtEventINSNavCart), - std::make_pair("4224", evIMUSetup), - std::make_pair("4244", evVelSensorSetup), - std::make_pair("4050", evExtSensorMeas), - std::make_pair("5914", evReceiverTime)}; - - rx_id_map = RxIDMap(rx_id_pairs, rx_id_pairs + evReceiverSetup + 1); - } - - /** - * @brief Put new data - * @param[in] recvTimestamp Timestamp of receiving buffer - * @param[in] data Pointer to the buffer that is about to be analyzed - * @param[in] size Size of the buffer (as handed over by async_read_some) - */ - void newData(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size) - { - recvTimestamp_ = recvTimestamp; - data_ = data; - count_ = size; - found_ = false; - crc_check_ = false; - message_size_ = 0; - } - - //! Determines whether data_ points to the SBF block with ID "ID", e.g. 5003 - bool isMessage(const uint16_t ID); - //! Determines whether data_ points to the NMEA message with ID "ID", e.g. - //! "$GPGGA" - bool isMessage(std::string ID); - //! Determines whether data_ currently points to an SBF block - bool isSBF(); - //! Determines whether data_ currently points to an NMEA message - bool isNMEA(); - //! Determines whether data_ currently points to an NMEA message - bool isResponse(); - //! Determines whether data_ currently points to a connection descriptor - //! (right after initiating a TCP connection) - bool isConnectionDescriptor(); - //! Determines whether data_ currently points to an error message reply from - //! the Rx - bool isErrorMessage(); - //! Determines size of the message (also command reply) that data_ is - //! currently pointing at - std::size_t messageSize(); - //! Returns the message ID of the message where data_ is pointing at at the - //! moment, SBF identifiers embellished with inverted commas, e.g. "5003" - std::string messageID(); - - /** - * @brief Returns the count_ variable - * @return The variable count_ - */ - std::size_t getCount() { return count_; }; - /** - * @brief Searches the buffer for the beginning of the next message (NMEA or - * SBF) - * @return A pointer to the start of the next message. - */ - const uint8_t* search(); - - /** - * @brief Gets the length of the SBF block - * - * It determines the length from the header of the SBF block. The block - * length thus includes the header length. - * @return The length of the SBF block - */ - uint16_t getBlockLength(); - - /** - * @brief Gets the current position in the read buffer - * @return The current position of the read buffer - */ - const uint8_t* getPosBuffer(); - - /** - * @brief Gets the end position in the read buffer - * @return A pointer pointing to just after the read buffer (matches - * search()'s final pointer in case no valid message is found) - */ - const uint8_t* getEndBuffer(); - - /** - * @brief Has an NMEA message, SBF block or command reply been found in the - * buffer? - * @returns True if a message with the correct header & length has been found - */ - bool found(); - - /** - * @brief Goes to the start of the next message based on the calculated - * length of current message - */ - void next(); - - /** - * @brief Publishing function - * @param[in] topic String of topic - * @param[in] msg ROS message to be published - */ - template - void publish(const std::string& topic, const M& msg); - - /** - * @brief Publishing function - * @param[in] msg Localization message - */ - void publishTf(const LocalizationUtmMsg& msg); - - /** - * @brief Performs the CRC check (if SBF) and publishes ROS messages - * @return True if read was successful, false otherwise - */ - bool read(std::string message_key, bool search = false); - - /** - * @brief Whether or not a message has been found - */ - bool found_; - - /** - * @brief Wether all blocks from GNSS have arrived for GpsFix Message - */ - bool gnss_gpsfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for GpsFix Message - */ - bool ins_gpsfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from GNSS have arrived for NavSatFix Message - */ - bool gnss_navsatfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for NavSatFix Message - */ - bool ins_navsatfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from GNSS have arrived for Pose Message - */ - bool gnss_pose_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for Pose Message - */ - bool ins_pose_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Diagnostics Message - */ - bool diagnostics_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Localization Message - */ - bool ins_localization_complete(uint32_t id); - - private: - /** - * @brief Pointer to the node - */ - ROSaicNodeBase* node_; - - /** - * @brief Timestamp of receiving buffer - */ - Timestamp recvTimestamp_; - - /** - * @brief Pointer to the buffer of messages - */ - const uint8_t* data_; - - /** - * @brief Number of unread bytes in the buffer - */ - std::size_t count_; - - /** - * @brief Whether the CRC check as evaluated in the read() method was - * successful or not is stored here - */ - bool crc_check_; - - /** - * @brief Helps to determine size of response message / NMEA message / SBF - * block - */ - std::size_t message_size_; - - /** - * @brief Number of times the GPSFixMsg message has been published - */ - uint32_t count_gpsfix_ = 0; - - /** - * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks - * need to be stored - */ - PVTGeodeticMsg last_pvtgeodetic_; - - /** - * @brief Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic - * blocks need to be stored - */ - PosCovGeodeticMsg last_poscovgeodetic_; - - /** - * @brief Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to - * be stored - */ - AttEulerMsg last_atteuler_; - - /** - * @brief Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks - * need to be stored - */ - AttCovEulerMsg last_attcoveuler_; - - /** - * @brief Since NavSatFix, GPSFix, Imu and Pose. need INSNavGeod, incoming - * INSNavGeod blocks need to be stored - */ - INSNavGeodMsg last_insnavgeod_; - - /** - * @brief Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks - * need to be stored - */ - ExtSensorMeasMsg last_extsensmeas_; - - /** - * @brief Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks - * need to be stored - */ - ChannelStatus last_channelstatus_; - - /** - * @brief Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks - * need to be stored - */ - MeasEpochMsg last_measepoch_; - - /** - * @brief Since GPSFix needs DOP, incoming DOP blocks need to be stored - */ - DOP last_dop_; - - /** - * @brief Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks - * need to be stored - */ - VelCovGeodeticMsg last_velcovgeodetic_; - - /** - * @brief Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus - * blocks need to be stored - */ - ReceiverStatus last_receiverstatus_; - - /** - * @brief Since DiagnosticArray needs QualityInd, incoming QualityInd blocks - * need to be stored - */ - QualityInd last_qualityind_; - - /** - * @brief Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup - * blocks need to be stored - */ - ReceiverSetup last_receiversetup_; - - //! Shorthand for the map responsible for matching PVTGeodetic's Mode field - //! to an enum value - typedef std::unordered_map TypeOfPVTMap; - - /** - * @brief All instances of the RxMessage class shall have access to the map - * without reinitializing it, hence static - */ - TypeOfPVTMap type_of_pvt_map; - - //! Shorthand for the map responsible for matching ROS message identifiers to - //! an enum value - typedef std::unordered_map RxIDMap; - - /** - * @brief All instances of the RxMessage class shall have access to the map - * without reinitializing it, hence static - * - * This map is for mapping ROS message, SBF and NMEA identifiers to an - * enumeration and makes the switch-case formalism in rx_message.hpp more - * explicit. - */ - RxIDMap rx_id_map; - - //! When reading from an SBF file, the ROS publishing frequency is governed - //! by the time stamps found in the SBF blocks therein. - Timestamp unix_time_; - - //! Current leap seconds as received, do not use value is -128 - int8_t current_leap_seconds_ = -128; - - //! For GPSFix: Whether the ChannelStatus block of the current epoch has - //! arrived or not - bool channelstatus_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the MeasEpoch block of the current epoch has arrived - //! or not - bool measepoch_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the DOP block of the current epoch has arrived or not - bool dop_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the PVTGeodetic block of the current epoch has - //! arrived or not - bool pvtgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the PosCovGeodetic block of the current epoch has - //! arrived or not - bool poscovgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the VelCovGeodetic block of the current epoch has - //! arrived or not - bool velcovgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the AttEuler block of the current epoch has arrived - //! or not - bool atteuler_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the AttCovEuler block of the current epoch has - //! arrived or not - bool attcoveuler_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the INSNavGeod block of the current epoch has arrived - //! or not - bool insnavgeod_has_arrived_gpsfix_ = false; - - //! For NavSatFix: Whether the PVTGeodetic block of the current epoch has - //! arrived or not - bool pvtgeodetic_has_arrived_navsatfix_ = false; - - //! For NavSatFix: Whether the PosCovGeodetic block of the current epoch has - //! arrived or not - bool poscovgeodetic_has_arrived_navsatfix_ = false; - - //! For NavSatFix: Whether the INSNavGeod block of the current epoch has - //! arrived or not - bool insnavgeod_has_arrived_navsatfix_ = false; - //! For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the - //! current epoch has arrived or not - bool pvtgeodetic_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the - //! current epoch has arrived or not - bool poscovgeodetic_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the AttEuler block of the current - //! epoch has arrived or not - bool atteuler_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the AttCovEuler block of the - //! current epoch has arrived or not - bool attcoveuler_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the INSNavGeod block of the - //! current epoch has arrived or not - bool insnavgeod_has_arrived_pose_ = false; - - //! For DiagnosticArray: Whether the ReceiverStatus block of the current - //! epoch has arrived or not - bool receiverstatus_has_arrived_diagnostics_ = false; - - //! For DiagnosticArray: Whether the QualityInd block of the current epoch - //! has arrived or not - bool qualityind_has_arrived_diagnostics_ = false; - - //! For Localization: Whether the INSNavGeod block of the current epoch - //! has arrived or not - bool insnavgeod_has_arrived_localization_ = false; - - /** - * @brief "Callback" function when constructing NavSatFix messages - * @return A smart pointer to the ROS message NavSatFix just created - */ - NavSatFixMsg NavSatFixCallback(); - - /** - * @brief "Callback" function when constructing GPSFix messages - * @return A smart pointer to the ROS message GPSFix just created - */ - GPSFixMsg GPSFixCallback(); - - /** - * @brief "Callback" function when constructing PoseWithCovarianceStamped - * messages - * @return A smart pointer to the ROS message PoseWithCovarianceStamped just - * created - */ - PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback(); - - /** - * @brief "Callback" function when constructing - * DiagnosticArrayMsg messages - * @return A ROS message - * DiagnosticArrayMsg just created - */ - DiagnosticArrayMsg DiagnosticArrayCallback(); - - /** - * @brief "Callback" function when constructing - * ImuMsg messages - * @return A ROS message - * ImuMsg just created - */ - ImuMsg ImuCallback(); - - /** - * @brief "Callback" function when constructing - * LocalizationUtmMsg messages - * @return A ROS message - * LocalizationUtmMsg just created - */ - LocalizationUtmMsg LocalizationUtmCallback(); - - /** - * @brief "Callback" function when constructing - * TwistWithCovarianceStampedMsg messages - * @param[in] fromIns Wether to contruct message from INS data - * @return A ROS message - * TwistWithCovarianceStampedMsg just created - */ - TwistWithCovarianceStampedMsg TwistCallback(bool fromIns = false); - - /** - * @brief Waits according to time when reading from file - */ - void wait(Timestamp time_obj); - - /** - * @brief Wether all elements are true - */ - bool allTrue(std::vector& vec, uint32_t id); - - /** - * @brief Settings struct - */ - Settings* settings_; - - /** - * @brief Fixed UTM zone - */ - std::shared_ptr fixedUtmZone_; - - /** - * @brief Calculates the timestamp, in the Unix Epoch time format - * This is either done using the TOW as transmitted with the SBF block (if - * "use_gnss" is true), or using the current time. - * @param[in] data Pointer to the buffer - * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is - * used, otherwise the current time - * @return Timestamp object containing seconds and nanoseconds since last - * epoch - */ - Timestamp timestampSBF(const uint8_t* data, bool use_gnss_time); - - /** - * @brief Calculates the timestamp, in the Unix Epoch time format - * This is either done using the TOW as transmitted with the SBF block (if - * "use_gnss" is true), or using the current time. - * @param[in] tow (Time of Week) Number of milliseconds that elapsed since - * the beginning of the current GPS week as transmitted by the SBF block - * @param[in] wnc (Week Number Counter) counts the number of complete weeks - * elapsed since January 6, 1980 - * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is - * used, otherwise the current time - * @return Timestamp object containing seconds and nanoseconds since last - * epoch - */ - Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time); - }; -} // namespace io_comm_rx -#endif // for RX_MESSAGE_HPP \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/settings.h b/include/septentrio_gnss_driver/communication/settings.hpp similarity index 85% rename from include/septentrio_gnss_driver/communication/settings.h rename to include/septentrio_gnss_driver/communication/settings.hpp index 02f50b60..47ef6f6f 100644 --- a/include/septentrio_gnss_driver/communication/settings.h +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -34,6 +34,16 @@ #include #include +struct Osnma +{ + //! OSNMA mode + std::string mode; + //! Server for NTP synchronization + std::string ntp_server; + //! Wether OSNMA shall be kept open on shutdown + bool keep_open; +}; + struct RtkNtrip { //! Id of the NTRIP port @@ -92,20 +102,48 @@ struct RtkSerial bool keep_open; }; -struct RtkSettings +struct Rtk { std::vector ntrip; std::vector ip_server; std::vector serial; }; +namespace device_type { + enum DeviceType + { + TCP, + SERIAL, + SBF_FILE, + PCAP_FILE + }; +} // namespace device_type + //! Settings struct struct Settings { //! Set logger level to DEBUG bool activate_debug_log; - //! Device port + //! Device std::string device; + //! Device type + device_type::DeviceType device_type; + //! TCP IP + std::string device_tcp_ip; + //! TCP port + std::string device_tcp_port; + //! UDP port + uint32_t udp_port; + //! UDP unicast destination ip + std::string udp_unicast_ip; + //! UDP IP server id + std::string udp_ip_server; + //! TCP port + uint32_t tcp_port; + //! TCP IP server id + std::string tcp_ip_server; + //! Filename + std::string file_name; //! Username for login std::string login_user; //! Password for login @@ -117,9 +155,8 @@ struct Settings uint32_t baudrate; //! HW flow control std::string hw_flow_control; - //! In case of serial communication to Rx, rx_serial_port specifies Rx's - //! serial port connected to, e.g. USB1 or COM1 - std::string rx_serial_port; + // Wether to configure Rx + bool configure_rx; //! Datum to be used std::string datum; //! Polling period for PVT-related SBF blocks @@ -183,7 +220,9 @@ struct Settings //! Position deviation mask float pos_std_dev; //! RTK corrections settings - RtkSettings rtk_settings; + Rtk rtk; + //! OSNMA settings + Osnma osnma; //! Whether or not to publish the GGA message bool publish_gpgga; //! Whether or not to publish the RMC message @@ -194,6 +233,11 @@ struct Settings bool publish_gpgsv; //! Whether or not to publish the MeasEpoch message bool publish_measepoch; + //! Whether or not to publish the RFStatus and AIMPlusStatus message and + //! diagnostics + bool publish_aimplusstatus; + //! Whether or not to publish the GALAuthStatus message and diagnostics + bool publish_galauthstatus; //! Whether or not to publish the PVTCartesianMsg //! message bool publish_pvtcartesian; @@ -210,6 +254,9 @@ struct Settings //! Whether or not to publish the PosCovGeodeticMsg //! message bool publish_poscovgeodetic; + //! Whether or not to publish the VelCovCartesianMsg + //! message + bool publish_velcovcartesian; //! Whether or not to publish the VelCovGeodeticMsg //! message bool publish_velcovgeodetic; @@ -235,7 +282,7 @@ struct Settings bool publish_gpst; //! Whether or not to publish the NavSatFixMsg message bool publish_navsatfix; - //! Whether or not to publish the GPSFixMsg message + //! Whether or not to publish the GpsFixMsg message bool publish_gpsfix; //! Whether or not to publish the PoseWithCovarianceStampedMsg message bool publish_pose; @@ -245,22 +292,26 @@ struct Settings bool publish_imu; //! Whether or not to publish the LocalizationMsg message bool publish_localization; + //! Whether or not to publish the LocalizationMsg message + bool publish_localization_ecef; //! Whether or not to publish the TwistWithCovarianceStampedMsg message bool publish_twist; //! Whether or not to publish the tf of the localization bool publish_tf; + //! Whether or not to publish the tf of the localization + bool publish_tf_ecef; //! Wether local frame should be inserted into tf bool insert_local_frame = false; //! Frame id of the local frame to be inserted std::string local_frame_id; //! Septentrio receiver type, either "gnss" or "ins" std::string septentrio_receiver_type; - //! Handle the case when an INS is used in GNSS mode - bool ins_in_gnss_mode = false; //! If true, the ROS message headers' unix time field is constructed from the TOW //! (in the SBF case) and UTC (in the NMEA case) data. If false, times are //! constructed within the driver via time(NULL) of the \ library. bool use_gnss_time; + //! Wether processing latency shall be compensated for in ROS timestamp + bool latency_compensation; //! The frame ID used in the header of every published ROS message std::string frame_id; //! The frame ID used in the header of published ROS Imu message @@ -277,7 +328,7 @@ struct Settings //! Wether the UTM zone of the localization is locked bool lock_utm_zone; //! The number of leap seconds that have been inserted into the UTC time - int32_t leap_seconds; + int32_t leap_seconds = -128; //! Whether or not we are reading from an SBF file bool read_from_sbf_log = false; //! Whether or not we are reading from a PCAP file @@ -302,4 +353,15 @@ struct Settings uint32_t ins_vsm_serial_baud_rate; //! Wether VSM shall be kept open om shutdown bool ins_vsm_serial_keep_open; +}; + +//! Capabilities struct +struct Capabilities +{ + //! Wether Rx is INS + bool is_ins = false; + //! Wether Rx has heading + bool has_heading = false; + //! Wether Rx has improved VSM handling + bool has_improved_vsm_handling = false; }; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp new file mode 100644 index 00000000..56987e2d --- /dev/null +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -0,0 +1,199 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#pragma once + +// C++ +#include +#include +#include +#include +#include + +// ROSaic +#include + +//! 0x24 is ASCII for $ - 1st byte in each message +static const uint8_t SYNC_BYTE_1 = 0x24; +//! 0x40 is ASCII for @ - 2nd byte to indicate SBF block +static const uint8_t SBF_SYNC_BYTE_2 = 0x40; +//! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message +static const uint8_t NMEA_SYNC_BYTE_2 = 0x47; +//! 0x50 is ASCII for P - 3rd byte to indicate NMEA-type ASCII message +static const uint8_t NMEA_SYNC_BYTE_3 = 0x50; +//! 0x49 is ASCII for I - 2nd byte to indicate INS NMEA-type ASCII message +static const uint8_t NMEA_INS_SYNC_BYTE_2 = 0x49; +//! 0x4E is ASCII for N - 3rd byte to indicate NMEA-type ASCII message +static const uint8_t NMEA_INS_SYNC_BYTE_3 = 0x4E; +//! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx +static const uint8_t RESPONSE_SYNC_BYTE_2 = 0x52; +//! 0x3A is ASCII for : - 3rd byte in the response message from the Rx +static const uint8_t RESPONSE_SYNC_BYTE_3 = 0x3A; +//! 0x21 is ASCII for ! 3rd byte in the response message from the Rx +static const uint8_t RESPONSE_SYNC_BYTE_3a = 0x21; +//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the +//! command was invalid +static const uint8_t ERROR_SYNC_BYTE_3 = 0x3F; +//! 0x0D is ASCII for "Carriage Return", i.e. "Enter" +static const uint8_t CR = 0x0D; +//! 0x0A is ASCII for "Line Feed", i.e. "New Line" +static const uint8_t LF = 0x0A; +//! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after +//! initiating IP connection +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_I = 0x49; +//! 0x43 is ASCII for C - 1st character of connection descriptor sent by the Rx after +//! initiating COM connection +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_C = 0x43; +//! 0x55 is ASCII for U - 1st character of connection descriptor sent by the Rx after +//! initiating USB connection +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_U = 0x55; +//! 0x4E is ASCII for N - 1st character of connection descriptor sent by the Rx after +//! initiating NTRIP connection +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_N = 0x4E; +//! 0x44 is ASCII for D - 1st character of connection descriptor sent by the Rx after +//! initiating DSK connection +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_D = 0x44; +//! 0x3E is ASCII for > - end character of connection descriptor +static const uint8_t CONNECTION_DESCRIPTOR_FOOTER = 0x3E; + +static const uint16_t SBF_HEADER_SIZE = 8; +static const uint16_t MAX_SBF_SIZE = 65535; +static const uint16_t MAX_UDP_PACKET_SIZE = 65535; + +namespace telegram_type { + enum TelegramType + { + EMPTY, + SBF, + NMEA, + NMEA_INS, + RESPONSE, + ERROR_RESPONSE, + CONNECTION_DESCRIPTOR, + UNKNOWN + }; +} + +struct Telegram +{ + Timestamp stamp; + telegram_type::TelegramType type; + std::vector message; + + Telegram(size_t preallocate = 3) noexcept : + stamp(0), type(telegram_type::EMPTY), + message(std::vector(preallocate)) + { + } + + ~Telegram() {} + + Telegram(const Telegram& other) noexcept : + stamp(other.stamp), type(other.type), message(other.message) + { + } + + Telegram(Telegram&& other) noexcept : + stamp(other.stamp), type(other.type), message(other.message) + { + } + + Telegram& operator=(const Telegram& other) noexcept + { + if (this != &other) + { + this->stamp = other.stamp; + this->type = other.type; + this->message = other.message; + } + return *this; + } + + Telegram& operator=(Telegram&& other) noexcept + { + if (this != &other) + { + this->stamp = other.stamp; + this->type = other.type; + this->message = other.message; + } + return *this; + } +}; + +template +class ConcurrentQueue +{ +public: + [[nodiscard]] bool empty() const noexcept; + [[nodiscard]] size_t size() const noexcept; + void push(const T& input) noexcept; + void pop(T& output) noexcept; + +private: + std::queue queue_; + std::condition_variable cond_; + mutable std::mutex mtx_; +}; + +template +[[nodiscard]] bool ConcurrentQueue::empty() const noexcept +{ + std::lock_guard lck(mtx_); + return queue_.empty(); +} + +template +[[nodiscard]] size_t ConcurrentQueue::size() const noexcept +{ + std::lock_guard lck(mtx_); + return queue_.size(); +} + +template +void ConcurrentQueue::push(const T& input) noexcept +{ + { + std::lock_guard lck(mtx_); + queue_.push(input); + } + cond_.notify_one(); +} + +template +void ConcurrentQueue::pop(T& output) noexcept +{ + std::unique_lock lck(mtx_); + cond_.wait(lck, [this] { return !queue_.empty(); }); + output = queue_.front(); + queue_.pop(); +} + +typedef ConcurrentQueue> TelegramQueue; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp new file mode 100644 index 00000000..6e4ceecd --- /dev/null +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -0,0 +1,166 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +// ***************************************************************************** +// +// Boost Software License - Version 1.0 - August 17th, 2003 +// +// Permission is hereby granted, free of charge, to any person or organization +// obtaining a copy of the software and accompanying documentation covered by +// this license (the "Software") to use, reproduce, display, distribute, +// execute, and transmit the Software, and to prepare derivative works of the +// Software, and to permit third-parties to whom the Software is furnished to +// do so, all subject to the following: + +// The copyright notices in the Software and this entire statement, including +// the above license grant, this restriction and the following disclaimer, +// must be included in all copies of the Software, in whole or in part, and +// all derivative works of the Software, unless such copies or derivative +// works are solely in the form of machine-executable object code generated by +// a source language processor. +// +// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT +// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// ***************************************************************************** + +#pragma once + +// C++ includes +#include + +// ROSaic includes +#include +#include +#include + +/** + * @file telegram_handler.hpp + * @brief Handles messages when reading NMEA/SBF/response/error/connection descriptor + * messages + */ + +namespace io { + class Semaphore + { + public: + Semaphore() : block_(true) {} + + void notify() + { + std::unique_lock lock(mtx_); + block_ = false; + cv_.notify_one(); + } + + void wait() + { + std::unique_lock lock(mtx_); + while (block_) + { + cv_.wait(lock); + } + block_ = true; + } + + private: + std::mutex mtx_; + std::condition_variable cv_; + bool block_; + }; + + /** + * @class TelegramHandler + * @brief Represents ensemble of (to be constructed) ROS messages, to be handled + * at once by this class + */ + class TelegramHandler + { + + public: + TelegramHandler(ROSaicNodeBase* node) : node_(node), messageHandler_(node) {} + + ~TelegramHandler() + { + cdSemaphore_.notify(); + responseSemaphore_.notify(); + } + + void clearSemaphores() + { + cdSemaphore_.notify(); + responseSemaphore_.notify(); + } + + /** + * @brief Called every time a telegram is received + */ + void handleTelegram(const std::shared_ptr& telegram); + + //! Returns the connection descriptor + void resetWaitforMainCd() { mainConnectionDescriptor_ = std::string(); } + + //! Returns the connection descriptor + [[nodiscard]] std::string getMainCd() + { + cdSemaphore_.wait(); + return mainConnectionDescriptor_; + } + + //! Waits for response + void waitForResponse() { responseSemaphore_.wait(); } + + //! Waits for capabilities + void waitForCapabilities() { capabilitiesSemaphore_.wait(); } + + private: + void handleSbf(const std::shared_ptr& telegram); + void handleNmea(const std::shared_ptr& telegram); + void handleResponse(const std::shared_ptr& telegram); + void handleError(const std::shared_ptr& telegram); + void handleCd(const std::shared_ptr& telegram); + //! Pointer to Node + ROSaicNodeBase* node_; + + //! MessageHandler parser + MessageHandler messageHandler_; + + Semaphore cdSemaphore_; + Semaphore responseSemaphore_; + Semaphore capabilitiesSemaphore_; + std::string mainConnectionDescriptor_ = std::string(); + }; + +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/crc/crc.h b/include/septentrio_gnss_driver/crc/crc.hpp similarity index 68% rename from include/septentrio_gnss_driver/crc/crc.h rename to include/septentrio_gnss_driver/crc/crc.hpp index 62332fcb..d0ad4ec7 100644 --- a/include/septentrio_gnss_driver/crc/crc.h +++ b/include/septentrio_gnss_driver/crc/crc.hpp @@ -28,39 +28,39 @@ // // ***************************************************************************** -#ifndef CRC_H -#define CRC_H +#pragma once // ROSaic includes // The following imports structs into which SBF blocks can be unpacked then shipped // to handler functions -#include +#include // C++ libary includes #include #include #include -/** - * @file crc.h - * @brief Declares the functions to compute and validate the CRC of a buffer - * @date 17/08/20 - */ +namespace crc { + /** + * @file crc.hpp + * @brief Declares the functions to compute and validate the CRC of a buffer + * @date 17/08/20 + */ -/** - * @brief This function computes the CRC-8-CCITT (Cyclic Redundancy Check) of a - * buffer "buf" of "buf_length" bytes - * @param[in] buf The buffer at hand - * @param[in] buf_length Number of bytes in "buf" - * @return The calculated CRC - */ -uint16_t compute16CCITT(const uint8_t* buf, size_t buf_length); + /** + * @brief This function computes the CRC-8-CCITT (Cyclic Redundancy Check) of a + * buffer "buf" of "buf_length" bytes + * @param[in] buf The buffer at hand + * @param[in] buf_length Number of bytes in "buf" + * @return The calculated CRC + */ + uint16_t compute16CCITT(const uint8_t* buf, size_t buf_length); -/** - * @brief Validates whether the calculated CRC of the SBF block at hand matches the - * CRC field of the streamed SBF block - * @param block The SBF block that we are interested in - * @return True if the CRC check of the SBFBlock has passed, false otherwise - */ -bool isValid(const uint8_t* block); + /** + * @brief Validates whether the calculated CRC of the SBF block at hand matches + * the CRC field of the streamed SBF block + * @param block The SBF block that we are interested in + * @return True if the CRC check of the SBFBlock has passed, false otherwise + */ + bool isValid(const std::vector& message); -#endif // CRC_H +} // namespace crc diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp index b232c51f..3fbd98ae 100644 --- a/include/septentrio_gnss_driver/node/rosaic_node.hpp +++ b/include/septentrio_gnss_driver/node/rosaic_node.hpp @@ -56,8 +56,7 @@ // // ***************************************************************************** -#ifndef ROSAIC_NODE_HPP -#define ROSAIC_NODE_HPP +#pragma once /** * @file rosaic_node.hpp @@ -98,14 +97,14 @@ namespace rosaic_node { * * The other ROSaic parameters are specified via the command line. */ - bool getROSParams(); + [[nodiscard]] bool getROSParams(); /** * @brief Checks if the period has a valid value * @param[in] period period [ms] * @param[in] isIns wether recevier is an INS * @return wether the period is valid */ - bool validPeriod(uint32_t period, bool isIns); + [[nodiscard]] bool validPeriod(uint32_t period, bool isIns) const; /** * @brief Gets transforms from tf2 * @param[in] targetFrame traget frame id @@ -114,7 +113,7 @@ namespace rosaic_node { */ void getTransform(const std::string& targetFrame, const std::string& sourceFrame, - TransformStampedMsg& T_s_t); + TransformStampedMsg& T_s_t) const; /** * @brief Gets Euler angles from quaternion message * @param[in] qm quaternion message @@ -123,16 +122,14 @@ namespace rosaic_node { * @param[out] yaw yaw angle */ void getRPY(const QuaternionMsg& qm, double& roll, double& pitch, - double& yaw); + double& yaw) const; void sendVelocity(const std::string& velNmea); //! Handles communication with the Rx - io_comm_rx::Comm_IO IO_; + io::CommunicationCore IO_; //! tf2 buffer and listener tf2_ros::Buffer tfBuffer_; std::unique_ptr tfListener_; }; -} // namespace rosaic_node - -#endif // for ROSAIC_NODE_HPP \ No newline at end of file +} // namespace rosaic_node \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp index 086fd7d6..91c2e642 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp @@ -56,14 +56,11 @@ // // ***************************************************************************** -#ifndef GPGGA_HPP -#define GPGGA_HPP +#pragma once // ROSaic includes #include -#include -// Boost and ROS includes -#include +#include /** * @file gpgga.hpp @@ -116,6 +113,4 @@ class GpggaParser : public BaseParser * was valid */ bool was_last_gpgga_valid_; -}; - -#endif // GPGGA_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp index 1fd22495..95f510a3 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp @@ -56,12 +56,11 @@ // // ***************************************************************************** -#ifndef GPGSA_HPP -#define GPGSA_HPP +#pragma once // ROSaic includes #include -#include +#include // Boost and ROS includes #include @@ -103,6 +102,4 @@ class GpgsaParser : public BaseParser * @brief Declares the string MESSAGE_ID */ static const std::string MESSAGE_ID; -}; - -#endif // GPGSA_HPP \ No newline at end of file +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp index 0698cd14..761b4341 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp @@ -56,14 +56,11 @@ // // ***************************************************************************** -#ifndef GPGSV_HPP -#define GPGSV_HPP +#pragma once // ROSaic includes #include -#include -// Boost and ROS includes -#include +#include /** * @file gpgsv.hpp @@ -103,6 +100,4 @@ class GpgsvParser : public BaseParser * @brief Declares the string MESSAGE_ID */ static const std::string MESSAGE_ID; -}; - -#endif // GPGSV_HPP \ No newline at end of file +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp index 87445ccd..b6069ef1 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp @@ -56,12 +56,9 @@ // // ***************************************************************************** -#ifndef GPRMC_HPP -#define GPRMC_HPP - // ROSaic includes #include -#include +#include // Boost and ROS includes #include @@ -123,6 +120,4 @@ class GprmcParser : public BaseParser * was valid */ bool was_last_gprmc_valid_; -}; - -#endif // GPRMC_HPP \ No newline at end of file +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp b/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp index f6032228..a61d489b 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef NMEA_SENTENCE_HPP -#define NMEA_SENTENCE_HPP +#pragma once // C++ library includes #include @@ -65,6 +64,4 @@ class NMEASentence protected: std::string id_; std::vector body_; -}; - -#endif // NMEA_SENTENCE_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/parse_exception.hpp b/include/septentrio_gnss_driver/parsers/parse_exception.hpp index 0e6e23fa..b9c8ae95 100644 --- a/include/septentrio_gnss_driver/parsers/parse_exception.hpp +++ b/include/septentrio_gnss_driver/parsers/parse_exception.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PARSE_EXCEPTION_HPP -#define PARSE_EXCEPTION_HPP +#pragma once // C++ library includes #include @@ -88,6 +87,4 @@ class ParseException : public std::runtime_error { public: explicit ParseException(const std::string& error) : std::runtime_error(error) {} -}; - -#endif // PARSE_EXCEPTION_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/parser_base_class.hpp b/include/septentrio_gnss_driver/parsers/parser_base_class.hpp index 871a7b1e..a5e57b08 100644 --- a/include/septentrio_gnss_driver/parsers/parser_base_class.hpp +++ b/include/septentrio_gnss_driver/parsers/parser_base_class.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PARSER_BASE_CLASS_HPP -#define PARSER_BASE_CLASS_HPP +#pragma once // ROSaic includes #include "nmea_sentence.hpp" @@ -47,8 +46,7 @@ * @brief Base class for parsing NMEA messages and SBF blocks * * Subclasses that parse NMEA messages should implement - * ParseASCII(const NMEASentence&); subclasses that parse SBF blocks - * should implement ParseBinary(const SBFBlock&). The base class is implemented + * ParseASCII(const NMEASentence&); The base class is implemented * as a template, which is a simple and yet very powerful tool in C++. The * simple idea is to pass data type as a parameter so that we don’t need to * write the same code for different data types. Like function templates, class @@ -97,22 +95,6 @@ class BaseParser */ virtual const std::string getMessageID() const = 0; - /** - * @brief Converts bin_msg into a ROS message pointer (e.g. nmea_msgs::GpggaPtr) - * and returns it - * - * The returned value should not be NULL. ParseException will be thrown - * if there are any issues parsing the block. - * - * @param[in] bin_msg The message to convert, of type const SBFStructT - * @return A valid ROS message pointer - */ - template - T parseBinary(const SBFStructT& bin_msg) noexcept(false) - { - throw ParseException("ParseBinary not implemented."); - }; - /** * @brief Converts an NMEA sentence - both standardized and proprietary ones - * into a ROS message pointer (e.g. nmea_msgs::GpggaPtr) and returns it @@ -128,6 +110,4 @@ class BaseParser { throw ParseException("ParseASCII not implemented."); }; -}; - -#endif // PARSER_BASE_CLASS_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index 3c38055c..b4e95e62 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PARSING_UTILITIES_HPP -#define PARSING_UTILITIES_HPP +#pragma once // C++ library includes #include // C++ header, corresponds to in C @@ -58,7 +57,7 @@ namespace parsing_utilities { * Square value **********************************************************************/ template - inline T square(T val) + [[nodiscard]] inline T square(T val) { return val * val; } @@ -67,7 +66,7 @@ namespace parsing_utilities { * Convert degrees to radians **********************************************************************/ template - inline T deg2rad(T deg) + [[nodiscard]] inline T deg2rad(T deg) { return deg * boost::math::constants::degree(); } @@ -76,7 +75,7 @@ namespace parsing_utilities { * Convert degrees^2 to radians^2 **********************************************************************/ template - inline T deg2radSq(T deg) + [[nodiscard]] inline T deg2radSq(T deg) { return deg * boost::math::constants::degree() * boost::math::constants::degree(); @@ -86,7 +85,7 @@ namespace parsing_utilities { * Convert radians to degree **********************************************************************/ template - inline T rad2deg(T rad) + [[nodiscard]] inline T rad2deg(T rad) { return rad * boost::math::constants::radian(); } @@ -94,7 +93,7 @@ namespace parsing_utilities { /*********************************************************************** * Convert Euler angles to rotation matrix **********************************************************************/ - inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw) + [[nodiscard]] inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw) { Eigen::Matrix3d M; double sa, ca, sb, cb, sc, cc; @@ -114,14 +113,14 @@ namespace parsing_utilities { * @brief Wraps an angle between -180 and 180 degrees * @param[in] angle The angle to be wrapped */ - double wrapAngle180to180(double angle); + [[nodiscard]] double wrapAngle180to180(double angle); /** * @brief Converts an 8-byte-buffer into a double * @param[in] buffer A pointer to a buffer containing 8 bytes of data * @return The double extracted from the data in the buffer */ - double parseDouble(const uint8_t* buffer); + [[nodiscard]] double parseDouble(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a floating point number of type @@ -135,14 +134,14 @@ namespace parsing_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool parseDouble(const std::string& string, double& value); + [[nodiscard]] bool parseDouble(const std::string& string, double& value); /** * @brief Converts a 4-byte-buffer into a float * @param[in] buffer A pointer to a buffer containing 4 bytes of data * @return The float extracted from the data in the buffer */ - float parseFloat(const uint8_t* buffer); + [[nodiscard]] float parseFloat(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a floating point number of type @@ -156,14 +155,14 @@ namespace parsing_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool parseFloat(const std::string& string, float& value); + [[nodiscard]] bool parseFloat(const std::string& string, float& value); /** * @brief Converts a 2-byte-buffer into a signed 16-bit integer * @param[in] buffer A pointer to a buffer containing 2 bytes of data * @return The int16_t value extracted from the data in the buffer */ - int16_t parseInt16(const uint8_t* buffer); + [[nodiscard]] int16_t parseInt16(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a integer number of type @@ -179,14 +178,14 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10); + [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10); /** * @brief Converts a 4-byte-buffer into a signed 32-bit integer * @param[in] buffer A pointer to a buffer containing 4 bytes of data * @return The int32_t value extracted from the data in the buffer */ - int32_t parseInt32(const uint8_t* buffer); + [[nodiscard]] int32_t parseInt32(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a integer number of type @@ -202,7 +201,7 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10); + [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -218,14 +217,14 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10); /** * @brief Converts a 2-byte-buffer into an unsigned 16-bit integer * @param[in] buffer A pointer to a buffer containing 2 bytes of data * @return The uint16_t value extracted from the data in the buffer */ - uint16_t parseUInt16(const uint8_t* buffer); + [[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -241,14 +240,14 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10); /** * @brief Converts a 4-byte-buffer into an unsigned 32-bit integer * @param[in] buffer A pointer to a buffer containing 4 bytes of data * @return The uint32_t value extracted from the data in the buffer */ - uint32_t parseUInt32(const uint8_t* buffer); + [[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -264,7 +263,7 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10); /** * @brief Converts UTC time from the without-colon-delimiter format to the @@ -273,7 +272,7 @@ namespace parsing_utilities { * format * @return Represents UTC time in the number-of-seconds-since-midnight format */ - double convertUTCDoubleToSeconds(double utc_double); + [[nodiscard]] double convertUTCDoubleToSeconds(double utc_double); /** * @brief Converts UTC time from the without-colon-delimiter format to Unix Epoch @@ -285,7 +284,7 @@ namespace parsing_utilities { * format * @return The time_t variable representing Unix Epoch time */ - std::time_t convertUTCtoUnix(double utc_double); + [[nodiscard]] std::time_t convertUTCtoUnix(double utc_double); /** * @brief Converts latitude or longitude from the DMS notation (in the @@ -296,16 +295,65 @@ namespace parsing_utilities { * without-colon-delimiter format) * @return Represents latitude or longitude in the pure degree notation */ - double convertDMSToDegrees(double dms); + [[nodiscard]] double convertDMSToDegrees(double dms); /** * @brief Transforms Euler angles to a quaternion - * @param[in] yaw Yaw, i.e. heading, about the Up-axis - * @param[in] pitch Pitch about the new North-axis - * @param[in] roll Roll about the new East-axis + * @param[in] yaw Yaw, i.e. heading, about the z-axis [rad] + * @param[in] pitch Pitch about the new y-axis [rad] + * @param[in] roll Roll about the new y-axis [rad] + * @return quaternion + */ + [[nodiscard]] Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, + double yaw); + + /** + * @brief Transforms Euler angles to a QuaternionMsg + * @param[in] yaw Yaw, i.e. heading, about the z-axis [rad] + * @param[in] pitch Pitch about the new y-axis [rad] + * @param[in] roll Roll about the new x-axis [rad] + * @return ROS message representing a quaternion + */ + [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw); + + /** + * @brief Convert Eigen quaternion to a QuaternionMsg + * @param[in] q Eigen quaternion * @return ROS message representing a quaternion */ - QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll); + [[nodiscard]] QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q); + + /** + * @brief Generates the quaternion to rotate from local ENU to ECEF + * @param[in] lat gedoetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return quaternion + */ + [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon); + + /** + * @brief Generates the quaternion to rotate from local NED to ECEF + * @param[in] lat geodetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return rotation matrix + */ + [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon); + + /** + * @brief Generates the matrix to rotate from local ENU to ECEF + * @param[in] lat geodetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return rotation matrix + */ + [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon); + + /** + * @brief Generates the matrix to rotate from local NED to ECEF + * @param[in] lat geodetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return rotation matrix + */ + [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon); /** * @brief Transforms the input polling period [milliseconds] into a std::string @@ -315,7 +363,7 @@ namespace parsing_utilities { * @return Number to be appended to either msec, sec or min when sending commands * to the Rx */ - std::string convertUserPeriodToRxCommand(uint32_t period_user); + [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user); /** * @brief Get the CRC of the SBF message @@ -323,7 +371,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message CRC */ - uint16_t getCrc(const uint8_t* buffer); + [[nodiscard]] uint16_t getCrc(const std::vector& message); /** * @brief Get the ID of the SBF message @@ -331,7 +379,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message ID */ - uint16_t getId(const uint8_t* buffer); + [[nodiscard]] uint16_t getId(const std::vector& message); /** * @brief Get the length of the SBF message @@ -339,7 +387,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message length */ - uint16_t getLength(const uint8_t* buffer); + [[nodiscard]] uint16_t getLength(const std::vector& message); /** * @brief Get the time of week in ms of the SBF message @@ -347,7 +395,7 @@ namespace parsing_utilities { * @param[in] buffer A pointer to a buffer containing an SBF message * @return SBF time of week in ms */ - uint32_t getTow(const uint8_t* buffer); + [[nodiscard]] uint32_t getTow(const std::vector& message); /** * @brief Get the GPS week counter of the SBF message @@ -355,7 +403,5 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF GPS week counter */ - uint16_t getWnc(const uint8_t* buffer); -} // namespace parsing_utilities - -#endif // PARSING_UTILITIES_HPP + [[nodiscard]] uint16_t getWnc(const std::vector& message); +} // namespace parsing_utilities \ No newline at end of file diff --git a/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp similarity index 77% rename from include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp rename to include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index e3f44e23..29d4693d 100644 --- a/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -59,14 +59,15 @@ static const uint16_t MAXSB_MEASEPOCH_T2 = static const uint8_t MAXSB_NBVECTORINFO = 30; //! 0x24 is ASCII for $ - 1st byte in each message -static const uint8_t SBF_SYNC_BYTE_1 = 0x24; +static const uint8_t SBF_SYNC_1 = 0x24; //! 0x40 is ASCII for @ - 2nd byte to indicate SBF block -static const uint8_t SBF_SYNC_BYTE_2 = 0x40; +static const uint8_t SBF_SYNC_2 = 0x40; // C++ #include // Boost #include +// ROSaic #include #include @@ -141,7 +142,7 @@ struct ChannelStatus * @class DOP * @brief Struct for the SBF block "DOP" */ -struct DOP +struct Dop { BlockHeader block_header; @@ -268,10 +269,10 @@ namespace qi = boost::spirit::qi; /** * validValue - * @brief Check if value is not set to Do-Not-Use -2e10 + * @brief Check if value is not set to Do-Not-Use */ template -bool validValue(T s) +[[nodiscard]] bool validValue(T s) { static_assert(std::is_same::value || std::is_same::value || @@ -293,19 +294,30 @@ bool validValue(T s) /** * setDoNotUse - * @brief Sets scalar to Do-Not-Use value -2e10 + * @brief Sets scalar to Do-Not-Use value */ -template -void setDoNotUse(T& s) +template +void setDoNotUse(Val& s) { - static_assert(std::is_same::value || std::is_same::value); - if (std::is_same::value) + static_assert( + std::is_same::value || std::is_same::value || + std::is_same::value || std::is_same::value || + std::is_same::value); + + if (std::is_same::value) + { + s = 65535; + } else if (std::is_same::value) + { + s = 4294967295ul; + } else if (std::is_same::value) { s = -2e10f; - } else if (std::is_same::value) + } else if (std::is_same::value) { s = -2e10; } + // TODO add more } /** @@ -313,7 +325,7 @@ void setDoNotUse(T& s) * @brief Qi little endian parsers for numeric values */ template -bool qiLittleEndianParser(It& it, Val& val) +void qiLittleEndianParser(It& it, Val& val) { static_assert( std::is_same::value || std::is_same::value || @@ -324,28 +336,28 @@ bool qiLittleEndianParser(It& it, Val& val) if (std::is_same::value) { - return qi::parse(it, it + 1, qi::char_, val); + qi::parse(it, it + 1, qi::char_, val); } else if (std::is_same::value) { - return qi::parse(it, it + 1, qi::byte_, val); + qi::parse(it, it + 1, qi::byte_, val); } else if ((std::is_same::value) || (std::is_same::value)) { - return qi::parse(it, it + 2, qi::little_word, val); + qi::parse(it, it + 2, qi::little_word, val); } else if ((std::is_same::value) || (std::is_same::value)) { - return qi::parse(it, it + 4, qi::little_dword, val); + qi::parse(it, it + 4, qi::little_dword, val); } else if ((std::is_same::value) || (std::is_same::value)) { - return qi::parse(it, it + 8, qi::little_qword, val); + qi::parse(it, it + 8, qi::little_qword, val); } else if (std::is_same::value) { - return qi::parse(it, it + 4, qi::little_bin_float, val); + qi::parse(it, it + 4, qi::little_bin_float, val); } else if (std::is_same::value) { - return qi::parse(it, it + 8, qi::little_bin_double, val); + qi::parse(it, it + 8, qi::little_bin_double, val); } } @@ -354,14 +366,12 @@ bool qiLittleEndianParser(It& it, Val& val) * @brief Qi parser for char array to string */ template -bool qiCharsToStringParser(It& it, std::string& val, std::size_t num) +void qiCharsToStringParser(It& it, std::string& val, std::size_t num) { - bool success = false; val.clear(); - success = qi::parse(it, it + num, qi::repeat(num)[qi::char_], val); + qi::parse(it, it + num, qi::repeat(num)[qi::char_], val); // remove string termination characters '\0' val.erase(std::remove(val.begin(), val.end(), '\0'), val.end()); - return success; } /** @@ -369,18 +379,18 @@ bool qiCharsToStringParser(It& it, std::string& val, std::size_t num) * @brief Qi based parser for the SBF block "BlockHeader" plus receiver time stamp */ template -bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header) +[[nodiscard]] bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header) { qiLittleEndianParser(it, block_header.sync_1); - if (block_header.sync_1 != SBF_SYNC_BYTE_1) + if (block_header.sync_1 != SBF_SYNC_1) { - node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 1."); + node->log(log_level::ERROR, "Parse error: Wrong sync byte 1."); return false; } qiLittleEndianParser(it, block_header.sync_2); - if (block_header.sync_2 != SBF_SYNC_BYTE_2) + if (block_header.sync_2 != SBF_SYNC_2) { - node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 2."); + node->log(log_level::ERROR, "Parse error: Wrong sync byte 2."); return false; } qiLittleEndianParser(it, block_header.crc); @@ -414,8 +424,9 @@ void ChannelStateInfoParser(It& it, ChannelStateInfo& msg, uint8_t sb2_length) * @brief Qi based parser or the SBF sub-block "ChannelSatInfo" */ template -bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, - uint8_t sb1_length, uint8_t sb2_length) +[[nodiscard]] bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, + ChannelSatInfo& msg, uint8_t sb1_length, + uint8_t sb2_length) { qiLittleEndianParser(it, msg.sv_id); qiLittleEndianParser(it, msg.freq_nr); @@ -426,8 +437,8 @@ bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, qiLittleEndianParser(it, msg.n2); if (msg.n2 > MAXSB_CHANNELSTATEINFO) { - node->log(LogLevel::ERROR, "Parse error: Too many ChannelStateInfo " + - std::to_string(msg.n2)); + node->log(log_level::ERROR, "Parse error: Too many ChannelStateInfo " + + std::to_string(msg.n2)); return false; } qiLittleEndianParser(it, msg.rx_channel); @@ -446,20 +457,21 @@ bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, * @brief Qi based parser for the SBF block "ChannelStatus" */ template -bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& msg) +[[nodiscard]] bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, + ChannelStatus& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4013) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_CHANNELSATINFO) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many ChannelSatInfo " + std::to_string(msg.n)); return false; } @@ -474,7 +486,7 @@ bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& m } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -485,15 +497,15 @@ bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& m * @brief Qi based parser for the SBF block "DOP" */ template -bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, DOP& msg) +[[nodiscard]] bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4001) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.nr_sv); @@ -511,7 +523,7 @@ bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, DOP& msg) qiLittleEndianParser(it, msg.vpl); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -538,13 +550,14 @@ void MeasEpochChannelType2Parser(It& it, MeasEpochChannelType2Msg& msg, }; /** - * @class MeasEpochChannelType1Parser + * MeasEpochChannelType1Parser * @brief Qi based parser for the SBF sub-block "MeasEpochChannelType1" */ template -bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, - MeasEpochChannelType1Msg& msg, uint8_t sb1_length, - uint8_t sb2_length) +[[nodiscard]] bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, + MeasEpochChannelType1Msg& msg, + uint8_t sb1_length, + uint8_t sb2_length) { qiLittleEndianParser(it, msg.rx_channel); qiLittleEndianParser(it, msg.type); @@ -561,8 +574,8 @@ bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, std::advance(it, sb1_length - 20); // skip padding if (msg.n2 > MAXSB_MEASEPOCH_T2) { - node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType2 " + - std::to_string(msg.n2)); + node->log(log_level::ERROR, "Parse error: Too many MeasEpochChannelType2 " + + std::to_string(msg.n2)); return false; } msg.type2.resize(msg.n2); @@ -574,25 +587,26 @@ bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, }; /** - * @class MeasEpoch + * MeasEpochParser * @brief Qi based parser for the SBF block "MeasEpoch" */ template -bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) +[[nodiscard]] bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, + MeasEpochMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4027) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_MEASEPOCH_T1) { - node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType1 " + - std::to_string(msg.n)); + node->log(log_level::ERROR, "Parse error: Too many MeasEpochChannelType1 " + + std::to_string(msg.n)); return false; } qiLittleEndianParser(it, msg.sb1_length); @@ -610,7 +624,83 @@ bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); + return false; + } + return true; +}; + +/** + * GALAuthStatus + * @brief Qi based parser for the SBF block "GALAuthStatus" + */ +template +[[nodiscard]] bool GalAuthStatusParser(ROSaicNodeBase* node, It it, It itEnd, + GalAuthStatusMsg& msg) +{ + if (!BlockHeaderParser(node, it, msg.block_header)) + return false; + if (msg.block_header.id != 4245) + { + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); + return false; + } + qiLittleEndianParser(it, msg.osnma_status); + qiLittleEndianParser(it, msg.trusted_time_delta); + qiLittleEndianParser(it, msg.gal_active_mask); + qiLittleEndianParser(it, msg.gal_authentic_mask); + qiLittleEndianParser(it, msg.gps_active_mask); + qiLittleEndianParser(it, msg.gps_authentic_mask); + if (it > itEnd) + { + node->log(log_level::ERROR, "Parse error: iterator past end."); + return false; + } + return true; +}; + +/** + * RFBandParser + * @brief Qi based parser for the SBF sub-block "RFBand" + */ +template +void RfBandParser(It& it, RfBandMsg& msg, uint8_t sb_length) +{ + qiLittleEndianParser(it, msg.frequency); + qiLittleEndianParser(it, msg.bandwidth); + qiLittleEndianParser(it, msg.info); + std::advance(it, sb_length - 7); // skip padding +}; + +/** + * RFStatusParser + * @brief Qi based parser for the SBF block "RFStatus" + */ +template +[[nodiscard]] bool RfStatusParser(ROSaicNodeBase* node, It it, It itEnd, + RfStatusMsg& msg) +{ + if (!BlockHeaderParser(node, it, msg.block_header)) + return false; + if (msg.block_header.id != 4092) + { + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); + return false; + } + qiLittleEndianParser(it, msg.n); + qiLittleEndianParser(it, msg.sb_length); + qiLittleEndianParser(it, msg.flags); + std::advance(it, 3); // reserved + msg.rfband.resize(msg.n); + for (auto& rfband : msg.rfband) + { + RfBandParser(it, rfband, msg.sb_length); + } + if (it > itEnd) + { + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -621,14 +711,15 @@ bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) * @brief Qi based parser for the SBF block "ReceiverSetup" */ template -bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& msg) +[[nodiscard]] bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverSetup& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5902) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } std::advance(it, 2); // reserved @@ -667,7 +758,7 @@ bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& m } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -678,14 +769,15 @@ bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& m * @brief Struct for the SBF block "ReceiverTime" */ template -bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& msg) +[[nodiscard]] bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverTimeMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5914) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.utc_year); @@ -698,7 +790,7 @@ bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& qiLittleEndianParser(it, msg.sync_level); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -709,14 +801,15 @@ bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& * @brief Qi based parser for the SBF block "PVTCartesian" */ template -bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& msg) +[[nodiscard]] bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, + PVTCartesianMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4006) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -753,7 +846,7 @@ bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -764,14 +857,15 @@ bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& * @brief Qi based parser for the SBF block "PVTGeodetic" */ template -bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& msg) +[[nodiscard]] bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, + PVTGeodeticMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4007) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -808,7 +902,7 @@ bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& ms } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -819,15 +913,15 @@ bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& ms * @brief Qi based parser for the SBF block "AttEuler" */ template -bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, + AttEulerMsg& msg, bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5938) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.nr_sv); @@ -853,7 +947,7 @@ bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -864,15 +958,16 @@ bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, * @brief Qi based parser for the SBF block "AttCovEuler" */ template -bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, + AttCovEulerMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5939) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } ++it; // reserved @@ -892,7 +987,7 @@ bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& ms } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -924,25 +1019,25 @@ void VectorInfoCartParser(It& it, VectorInfoCartMsg& msg, uint8_t sb_length) }; /** - * @class BaseVectorCart + * BaseVectorCartParser * @brief Qi based parser for the SBF block "BaseVectorCart" */ template -bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, - BaseVectorCartMsg& msg) +[[nodiscard]] bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, + BaseVectorCartMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4043) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_NBVECTORINFO) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many VectorInfoCart " + std::to_string(msg.n)); return false; } @@ -954,7 +1049,7 @@ bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -986,25 +1081,25 @@ void VectorInfoGeodParser(It& it, VectorInfoGeodMsg& msg, uint8_t sb_length) }; /** - * @class BaseVectorGeod + * BaseVectorGeodParser * @brief Qi based parser for the SBF block "BaseVectorGeod" */ template -bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, - BaseVectorGeodMsg& msg) +[[nodiscard]] bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, + BaseVectorGeodMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4028) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_NBVECTORINFO) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many VectorInfoGeod " + std::to_string(msg.n)); return false; } @@ -1016,7 +1111,7 @@ bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1027,15 +1122,16 @@ bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "INSNavCart" */ template -bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, + INSNavCartMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if ((msg.block_header.id != 4225) && (msg.block_header.id != 4229)) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.gnss_mode); @@ -1154,7 +1250,7 @@ bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1165,15 +1261,15 @@ bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, * @brief Qi based parser for the SBF block "PosCovCartesian" */ template -bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, - PosCovCartesianMsg& msg) +[[nodiscard]] bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, + PosCovCartesianMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5905) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1190,7 +1286,7 @@ bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_zb); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1201,15 +1297,15 @@ bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "PosCovGeodetic" */ template -bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, - PosCovGeodeticMsg& msg) +[[nodiscard]] bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, + PosCovGeodeticMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5906) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1226,7 +1322,7 @@ bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_hb); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1237,15 +1333,15 @@ bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "VelCovCartesian" */ template -bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, - VelCovCartesianMsg& msg) +[[nodiscard]] bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, + VelCovCartesianMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5907) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1262,7 +1358,7 @@ bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_vzdt); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1273,15 +1369,15 @@ bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "VelCovGeodetic" */ template -bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, - VelCovGeodeticMsg& msg) +[[nodiscard]] bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, + VelCovGeodeticMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5908) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1298,7 +1394,7 @@ bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_vudt); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1309,20 +1405,21 @@ bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, * @brief @brief Qi based parser for the SBF block "QualityInd" */ template -bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg) +[[nodiscard]] bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, + QualityInd& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4082) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > 40) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many indicators " + std::to_string(msg.n)); return false; } @@ -1335,7 +1432,7 @@ bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg) } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1360,14 +1457,15 @@ void AgcStateParser(It it, AgcState& msg, uint8_t sb_length) * @brief Struct for the SBF block "ReceiverStatus" */ template -bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& msg) +[[nodiscard]] bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverStatus& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4014) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.cpu_load); @@ -1378,7 +1476,7 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& qiLittleEndianParser(it, msg.n); if (msg.n > 18) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many AGCState " + std::to_string(msg.n)); return false; } @@ -1392,7 +1490,7 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1403,14 +1501,15 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& * @brief Struct for the SBF block "ReceiverTime" */ template -bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& msg) +[[nodiscard]] bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverTimeMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 5914) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.utc_year); @@ -1423,7 +1522,7 @@ bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& qiLittleEndianParser(it, msg.sync_level); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1434,15 +1533,16 @@ bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& * @brief Qi based parser for the SBF block "INSNavGeod" */ template -bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, + INSNavGeodMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if ((msg.block_header.id != 4226) && (msg.block_header.id != 4230)) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.gnss_mode); @@ -1562,7 +1662,7 @@ bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1573,15 +1673,15 @@ bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, * @brief Qi based parser for the SBF block "IMUSetup" */ template -bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, + IMUSetupMsg& msg, bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4224) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } ++it; // reserved @@ -1600,7 +1700,7 @@ bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1611,15 +1711,16 @@ bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, * @brief Qi based parser for the SBF block "VelSensorSetup" */ template -bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, - VelSensorSetupMsg& msg, bool use_ros_axis_orientation) +[[nodiscard]] bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, + VelSensorSetupMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4244) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } ++it; // reserved @@ -1634,7 +1735,7 @@ bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1645,23 +1746,23 @@ bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "ExtSensorMeas" */ template -bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, - ExtSensorMeasMsg& msg, bool use_ros_axis_orientation, - bool& hasImuMeas) +[[nodiscard]] bool +ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg, + bool use_ros_axis_orientation, bool& hasImuMeas) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; if (msg.block_header.id != 4050) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); qiLittleEndianParser(it, msg.sb_length); if (msg.sb_length != 28) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Wrong sb_length " + std::to_string(msg.sb_length)); return false; } @@ -1682,7 +1783,7 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, msg.std_dev_y = std::numeric_limits::quiet_NaN(); msg.std_dev_z = std::numeric_limits::quiet_NaN(); - msg.sensor_temperature = -32768.0f; // do not use value + msg.sensor_temperature = std::numeric_limits::quiet_NaN(); msg.zero_velocity_flag = std::numeric_limits::quiet_NaN(); msg.source.resize(msg.n); @@ -1706,13 +1807,6 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.acceleration_x); qiLittleEndianParser(it, msg.acceleration_y); qiLittleEndianParser(it, msg.acceleration_z); - if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi - { - if (validValue(msg.acceleration_y)) - msg.acceleration_y = -msg.acceleration_y; - if (validValue(msg.acceleration_z)) - msg.acceleration_z = -msg.acceleration_z; - } hasAcc = true; break; } @@ -1721,20 +1815,17 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.angular_rate_x); qiLittleEndianParser(it, msg.angular_rate_y); qiLittleEndianParser(it, msg.angular_rate_z); - if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi - { - if (validValue(msg.angular_rate_y)) - msg.angular_rate_y = -msg.angular_rate_y; - if (validValue(msg.angular_rate_z)) - msg.angular_rate_z = -msg.angular_rate_z; - } hasOmega = true; break; } case 3: { - qi::parse(it, it + 2, qi::little_word, msg.sensor_temperature); - msg.sensor_temperature /= 100.0f; + int16_t temp; + qiLittleEndianParser(it, temp); + if (temp != -32768) + msg.sensor_temperature = temp / 100.0f; + else + msg.sensor_temperature = std::numeric_limits::quiet_NaN(); std::advance(it, 22); // reserved break; } @@ -1764,8 +1855,9 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, default: { node->log( - LogLevel::ERROR, - "Unknown external sensor measurement type in SBF ExtSensorMeas."); + log_level::DEBUG, + "Unknown external sensor measurement type in SBF ExtSensorMeas: " + + std::to_string(msg.type[i])); std::advance(it, 24); break; } @@ -1773,7 +1865,7 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } hasImuMeas = hasAcc && hasOmega; diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.h b/include/septentrio_gnss_driver/parsers/string_utilities.hpp similarity index 88% rename from include/septentrio_gnss_driver/parsers/string_utilities.h rename to include/septentrio_gnss_driver/parsers/string_utilities.hpp index 0ce07c7e..f7625a04 100644 --- a/include/septentrio_gnss_driver/parsers/string_utilities.h +++ b/include/septentrio_gnss_driver/parsers/string_utilities.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef STRING_UTILITIES_H -#define STRING_UTILITIES_H +#pragma once // C and C++ library includes #include @@ -37,15 +36,11 @@ #include /** - * @file string_utilities.h + * @file string_utilities.hpp * @brief Declares lower-level string utility functions used when parsing messages * @date 13/08/20 */ -#ifdef __cplusplus -extern "C" { -#endif - /** * @namespace string_utilities * This namespace is for the functions that encapsulate basic string manipulation and @@ -62,7 +57,7 @@ namespace string_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool toDouble(const std::string& string, double& value); + [[nodiscard]] bool toDouble(const std::string& string, double& value); /** * @brief Interprets the contents of "string" as a floating point number of type @@ -76,7 +71,7 @@ namespace string_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool toFloat(const std::string& string, float& value); + [[nodiscard]] bool toFloat(const std::string& string, float& value); /** * @brief Interprets the contents of "string" as a floating point number of @@ -91,7 +86,7 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return True if all went fine, false if not */ - bool toInt32(const std::string& string, int32_t& value, int32_t base = 10); + [[nodiscard]] bool toInt32(const std::string& string, int32_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a floating point number of @@ -106,7 +101,7 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return True if all went fine, false if not */ - bool toUInt32(const std::string& string, uint32_t& value, int32_t base = 10); + [[nodiscard]] bool toUInt32(const std::string& string, uint32_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a floating point number of @@ -120,7 +115,7 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return The value found in "string" */ - int8_t toInt8(const std::string& string, int8_t& value, int32_t base = 10); + [[nodiscard]] int8_t toInt8(const std::string& string, int8_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a floating point number of @@ -134,25 +129,19 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return The value found in "string" */ - uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base = 10); + [[nodiscard]] uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base = 10); /** - * @brief Trims decimal places to two + * @brief Trims decimal places to three * @param[in] num The double who shall be trimmed * @return The string */ - std::string trimDecimalPlaces(double num); + [[nodiscard]] std::string trimDecimalPlaces(double num); /** * @brief Checks if a string contains spaces * @param[in] str the string to be analyzed * @return true if string contains space */ - bool containsSpace(const std::string str); -} // namespace string_utilities - -#ifdef __cplusplus -} -#endif - -#endif // STRING_UTILITIES_H + [[nodiscard]] bool containsSpace(const std::string str); +} // namespace string_utilities \ No newline at end of file diff --git a/include/septentrio_gnss_driver/plugin/BlockHeader.h b/include/septentrio_gnss_driver/plugin/BlockHeader.h new file mode 100644 index 00000000..9a269329 --- /dev/null +++ b/include/septentrio_gnss_driver/plugin/BlockHeader.h @@ -0,0 +1,8 @@ +// Custom constructor macro to initalize SBF msgs with do_not_use values in block +// header +#define SEPTENTRIO_GNSS_DRIVER_MESSAGE_BLOCKHEADER_PLUGIN_CONSTRUCTOR \ + BlockHeader_() : \ + sync_1(0x24), sync_2(0x40), crc(0), id(0), revision(0), length(0), \ + tow(4294967295UL), wnc(65535) \ + { \ + } diff --git a/msg/AIMPlusStatus.msg b/msg/AIMPlusStatus.msg new file mode 100644 index 00000000..f6b3ae70 --- /dev/null +++ b/msg/AIMPlusStatus.msg @@ -0,0 +1,27 @@ +# AIMPlusStatus message +# ROS message header +std_msgs/Header header + +uint32 tow # ms since week start +uint16 wnc # weeks since Jan 06, 1980 at 00:00:00 UTC + +uint8 interference +#------------------------------- +uint8 SPECTRUM_CLEAN = 0 +uint8 INTERFERENCE_MITIGATED = 1 +uint8 INTERFERENCE_PRESENT = 2 +#------------------------------- + +uint8 spoofing +#-------------------------------------------------------- +uint8 NONE_DETECTED = 0 +uint8 SPOOFING_DETECTED_BY_OSNMA = 1 +uint8 SPOOFING_DETECTED_BY_AUTHENTCITY_TEST = 2 +uint8 SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST = 3 +#-------------------------------------------------------- + +bool osnma_authenticating +uint8 galileo_authentic +uint8 galileo_spoofed +uint8 gps_authentic +uint8 gps_spoofed \ No newline at end of file diff --git a/msg/AttCovEuler.msg b/msg/AttCovEuler.msg index 6b5e4a9a..64ef1119 100644 --- a/msg/AttCovEuler.msg +++ b/msg/AttCovEuler.msg @@ -6,9 +6,9 @@ std_msgs/Header header BlockHeader block_header uint8 error -float32 cov_headhead -float32 cov_pitchpitch -float32 cov_rollroll -float32 cov_headpitch -float32 cov_headroll -float32 cov_pitchroll +float32 cov_headhead # deg^2 +float32 cov_pitchpitch # deg^2 +float32 cov_rollroll # deg^2 +float32 cov_headpitch # deg^2 +float32 cov_headroll # deg^2 +float32 cov_pitchroll # deg^2 diff --git a/msg/AttEuler.msg b/msg/AttEuler.msg index 33857a4f..320f7280 100644 --- a/msg/AttEuler.msg +++ b/msg/AttEuler.msg @@ -8,9 +8,9 @@ BlockHeader block_header uint8 nr_sv uint8 error uint16 mode -float32 heading -float32 pitch -float32 roll -float32 pitch_dot -float32 roll_dot -float32 heading_dot +float32 heading # deg +float32 pitch # deg +float32 roll # deg +float32 pitch_dot # deg/s +float32 roll_dot # deg/s +float32 heading_dot # deg/s diff --git a/msg/BlockHeader.msg b/msg/BlockHeader.msg index d299dfd3..e4af0bf4 100644 --- a/msg/BlockHeader.msg +++ b/msg/BlockHeader.msg @@ -7,5 +7,5 @@ uint16 id uint8 revision uint16 length -uint32 tow -uint16 wnc \ No newline at end of file +uint32 tow # ms since week start +uint16 wnc # weeks since Jan 06, 1980 at 00:00:00 UTC \ No newline at end of file diff --git a/msg/ExtSensorMeas.msg b/msg/ExtSensorMeas.msg index 9dece5fe..6ed338bb 100644 --- a/msg/ExtSensorMeas.msg +++ b/msg/ExtSensorMeas.msg @@ -16,22 +16,22 @@ uint8[] type uint8[] obs_info #ExtSensorMeasAcceleration -float64 acceleration_x -float64 acceleration_y -float64 acceleration_z +float64 acceleration_x # m/s^2 +float64 acceleration_y # m/s^2 +float64 acceleration_z # m/s^2 #ExtSensorMeasAngularRate -float64 angular_rate_x -float64 angular_rate_y -float64 angular_rate_z +float64 angular_rate_x # deg/s +float64 angular_rate_y # deg/s +float64 angular_rate_z # deg/s #ExtSensorMeasVelocity -float32 velocity_x -float32 velocity_y -float32 velocity_z -float32 std_dev_x -float32 std_dev_y -float32 std_dev_z +float32 velocity_x # m/s +float32 velocity_y # m/s +float32 velocity_z # m/s +float32 std_dev_x # m/s +float32 std_dev_y # m/s +float32 std_dev_z # m/s #ExtSensorMeasInfo float32 sensor_temperature # deg C diff --git a/msg/GALAuthStatus.msg b/msg/GALAuthStatus.msg new file mode 100644 index 00000000..5acd0c70 --- /dev/null +++ b/msg/GALAuthStatus.msg @@ -0,0 +1,14 @@ +# GALAuthStatus block +# Block_Number 4245 + +std_msgs/Header header + +# SBF block header including time header +BlockHeader block_header + +uint16 osnma_status +float32 trusted_time_delta # s +uint64 gal_active_mask +uint64 gal_authentic_mask +uint64 gps_active_mask +uint64 gps_authentic_mask \ No newline at end of file diff --git a/msg/IMUSetup.msg b/msg/IMUSetup.msg index d19a5865..71d9bae3 100644 --- a/msg/IMUSetup.msg +++ b/msg/IMUSetup.msg @@ -7,9 +7,9 @@ std_msgs/Header header BlockHeader block_header uint8 serial_port -float32 ant_lever_arm_x -float32 ant_lever_arm_y -float32 ant_lever_arm_z -float32 theta_x -float32 theta_y -float32 theta_z \ No newline at end of file +float32 ant_lever_arm_x # m +float32 ant_lever_arm_y # m +float32 ant_lever_arm_z # m +float32 theta_x # deg +float32 theta_y # deg +float32 theta_z # deg \ No newline at end of file diff --git a/msg/INSNavCart.msg b/msg/INSNavCart.msg index ec921161..bb8db689 100644 --- a/msg/INSNavCart.msg +++ b/msg/INSNavCart.msg @@ -9,60 +9,60 @@ BlockHeader block_header uint8 gnss_mode uint8 error uint16 info -uint16 gnss_age -float64 x -float64 y -float64 z -uint16 accuracy -uint16 latency -uint8 datum +uint16 gnss_age # 0.01 s +float64 x # m +float64 y # m +float64 z # m +uint16 accuracy # 0.01 m +uint16 latency # 0.1 ms +uint8 datum #uint8 reserved uint16 sb_list # INSNavCartPosStdDev sub-block definition: # If the Position StdDev field is 1 then this sub block is available: -float32 x_std_dev -float32 y_std_dev -float32 z_std_dev +float32 x_std_dev # m +float32 y_std_dev # m +float32 z_std_dev # m # INSNavCartPosCov sub-block definition: # If the Position Cov field is 1 then this sub block is available: -float32 xy_cov -float32 xz_cov -float32 yz_cov +float32 xy_cov # m^2 +float32 xz_cov # m^2 +float32 yz_cov # m^2 # INSNavCartAtt sub-block definition: # If the Attitude field is 1 then this sub block is available: -float32 heading -float32 pitch -float32 roll +float32 heading # deg +float32 pitch # deg +float32 roll # deg # INSNavCartAttStdDev sub-block definition: # If the Attitude StdDev field is 1 then this sub block is available: -float32 heading_std_dev -float32 pitch_std_dev -float32 roll_std_dev +float32 heading_std_dev # deg +float32 pitch_std_dev # deg +float32 roll_std_dev # deg # INSNavCartAttCov sub-block definition: # If the Attitude Cov field is 1 then this sub block is available: -float32 heading_pitch_cov -float32 heading_roll_cov -float32 pitch_roll_cov +float32 heading_pitch_cov # deg^2 +float32 heading_roll_cov # deg^2 +float32 pitch_roll_cov # deg^2 # INSNavCartVel sub-block definition: # If the Velocity field is 1 then this sub block is available: -float32 vx -float32 vy -float32 vz +float32 vx # m/s +float32 vy # m/S +float32 vz # m/s # INSNavCartVelStdDev sub-block definition: # If the Velocity StdDev field is 1 then this sub block is available: -float32 vx_std_dev -float32 vy_std_dev -float32 vz_std_dev +float32 vx_std_dev # m/s +float32 vy_std_dev # m/s +float32 vz_std_dev # m/s # INSNavCartVelCov sub-block definition: # If the Velocity Cov field is 1 then this sub block is available: -float32 vx_vy_cov -float32 vx_vz_cov -float32 vy_vz_cov \ No newline at end of file +float32 vx_vy_cov # m^2/s^2 +float32 vx_vz_cov # m^2/s^2 +float32 vy_vz_cov # m^2/s^2 \ No newline at end of file diff --git a/msg/INSNavGeod.msg b/msg/INSNavGeod.msg index 71f2e11d..9bb7e2b4 100644 --- a/msg/INSNavGeod.msg +++ b/msg/INSNavGeod.msg @@ -9,61 +9,61 @@ BlockHeader block_header uint8 gnss_mode uint8 error uint16 info -uint16 gnss_age -float64 latitude -float64 longitude -float64 height -float32 undulation -uint16 accuracy -uint16 latency +uint16 gnss_age # 0.01 s +float64 latitude # rad +float64 longitude # rad +float64 height # m (ellipsoidal) +float32 undulation # m +uint16 accuracy # 0.01 m +uint16 latency # 0.0001 s uint8 datum #uint8 reserved uint16 sb_list # INSNavGeodPosStdDev sub-block definition: # If the Position StdDev field is 1 then this sub block is available: -float32 latitude_std_dev -float32 longitude_std_dev -float32 height_std_dev +float32 latitude_std_dev # m +float32 longitude_std_dev # m +float32 height_std_dev # m # INSNavGeodPosCov sub-block definition: # If the Position Cov field is 1 then this sub block is available: -float32 latitude_longitude_cov -float32 latitude_height_cov -float32 longitude_height_cov +float32 latitude_longitude_cov # m^2 +float32 latitude_height_cov # m^2 +float32 longitude_height_cov # m^2 # INSNavGeodAtt sub-block definition: # If the Attitude field is 1 then this sub block is available: -float32 heading -float32 pitch -float32 roll +float32 heading # deg +float32 pitch # deg +float32 roll # deg # INSNavGeodAttStdDev sub-block definition: # If the Attitude StdDev field is 1 then this sub block is available: -float32 heading_std_dev -float32 pitch_std_dev -float32 roll_std_dev +float32 heading_std_dev # deg +float32 pitch_std_dev # deg +float32 roll_std_dev # deg # INSNavGeodAttCov sub-block definition: # If the Attitude Cov field is 1 then this sub block is available: -float32 heading_pitch_cov -float32 heading_roll_cov -float32 pitch_roll_cov +float32 heading_pitch_cov # deg^2 +float32 heading_roll_cov # deg^2 +float32 pitch_roll_cov # deg^2 # INSNavGeodVel sub-block definition: # If the Velocity field is 1 then this sub block is available: -float32 ve -float32 vn -float32 vu +float32 ve # m/s +float32 vn # m/s +float32 vu # m/s # INSNavGeodVelStdDev sub-block definition: # If the Velocity StdDev field is 1 then this sub block is available: -float32 ve_std_dev -float32 vn_std_dev -float32 vu_std_dev +float32 ve_std_dev # m/s +float32 vn_std_dev # m/s +float32 vu_std_dev # m/s # INSNavGeodVelCov sub-block definition: # If the Velocity Cov field is 1 then this sub block is available: -float32 ve_vn_cov -float32 ve_vu_cov -float32 vn_vu_cov +float32 ve_vn_cov # m^2/s^2 +float32 ve_vu_cov # m^2/s^2 +float32 vn_vu_cov # m^2/s^2 diff --git a/msg/MeasEpoch.msg b/msg/MeasEpoch.msg index 7ce4175f..b0e3d522 100644 --- a/msg/MeasEpoch.msg +++ b/msg/MeasEpoch.msg @@ -9,6 +9,6 @@ uint8 n uint8 sb1_length uint8 sb2_length uint8 common_flags -uint8 cum_clk_jumps +uint8 cum_clk_jumps # 0.001 s MeasEpochChannelType1[] type1 \ No newline at end of file diff --git a/msg/MeasEpochChannelType1.msg b/msg/MeasEpochChannelType1.msg index 00e6c68b..01dad32f 100644 --- a/msg/MeasEpochChannelType1.msg +++ b/msg/MeasEpochChannelType1.msg @@ -3,12 +3,12 @@ uint8 rx_channel uint8 type uint8 sv_id -uint8 misc -uint32 code_lsb -int32 doppler -uint16 carrier_lsb -int8 carrier_msb -uint8 cn0 +uint8 misc # 4294967.296 m +uint32 code_lsb # 0.001 m +int32 doppler # 0.0001 Hz +uint16 carrier_lsb # 0.001 cycles +int8 carrier_msb # 65.536 cycles +uint8 cn0 # 0.25 dB-Hz uint16 lock_time uint8 obs_info uint8 n2 diff --git a/msg/MeasEpochChannelType2.msg b/msg/MeasEpochChannelType2.msg index f79b781d..83cb96e4 100644 --- a/msg/MeasEpochChannelType2.msg +++ b/msg/MeasEpochChannelType2.msg @@ -1,11 +1,11 @@ # MeasEpochChannelType2 block uint8 type -uint8 lock_time -uint8 cn0 -uint8 offsets_msb -int8 carrier_msb +uint8 lock_time # s +uint8 cn0 # 0.25 dB-Hz +uint8 offsets_msb # 65.536 m or 65.536 Hz +int8 carrier_msb # 65.536 cycles uint8 obs_info -uint16 code_offset_lsb -uint16 carrier_lsb -uint16 doppler_offset_lsb \ No newline at end of file +uint16 code_offset_lsb # 0.001 m +uint16 carrier_lsb # 0.001 cycles +uint16 doppler_offset_lsb # 0.0001 Hz \ No newline at end of file diff --git a/msg/PVTCartesian.msg b/msg/PVTCartesian.msg index 526ebe8a..9a48aa81 100644 --- a/msg/PVTCartesian.msg +++ b/msg/PVTCartesian.msg @@ -7,27 +7,27 @@ BlockHeader block_header uint8 mode uint8 error -float64 x -float64 y -float64 z -float32 undulation -float32 vx -float32 vy -float32 vz -float32 cog -float64 rx_clk_bias -float32 rx_clk_drift +float64 x # m +float64 y # m +float64 z # m +float32 undulation # m +float32 vx # m/s +float32 vy # m/s +float32 vz # m/s +float32 cog # deg +float64 rx_clk_bias # ms +float32 rx_clk_drift # ppm uint8 time_system uint8 datum uint8 nr_sv uint8 wa_corr_info uint16 reference_id -uint16 mean_corr_age +uint16 mean_corr_age # 0.01s uint32 signal_info uint8 alert_flag uint8 nr_bases -uint16 ppp_info -uint16 latency -uint16 h_accuracy -uint16 v_accuracy +uint16 ppp_info # s +uint16 latency # 0.0001 s +uint16 h_accuracy # 0.01 m +uint16 v_accuracy # 0.01 m uint8 misc diff --git a/msg/PVTGeodetic.msg b/msg/PVTGeodetic.msg index 6c174ff5..78c6e4c1 100644 --- a/msg/PVTGeodetic.msg +++ b/msg/PVTGeodetic.msg @@ -7,27 +7,27 @@ BlockHeader block_header uint8 mode uint8 error -float64 latitude -float64 longitude -float64 height -float32 undulation -float32 vn -float32 ve -float32 vu -float32 cog -float64 rx_clk_bias -float32 rx_clk_drift +float64 latitude # rad +float64 longitude # rad +float64 height # m (ellipsoidal) +float32 undulation # m +float32 vn # m/s +float32 ve # m/s +float32 vu # m/s +float32 cog # deg +float64 rx_clk_bias # ms +float32 rx_clk_drift # ppm uint8 time_system uint8 datum uint8 nr_sv uint8 wa_corr_info uint16 reference_id -uint16 mean_corr_age +uint16 mean_corr_age # 0.01s uint32 signal_info uint8 alert_flag uint8 nr_bases -uint16 ppp_info -uint16 latency -uint16 h_accuracy -uint16 v_accuracy +uint16 ppp_info # s +uint16 latency # 0.0001 s +uint16 h_accuracy # 0.01 m +uint16 v_accuracy # 0.01 m uint8 misc \ No newline at end of file diff --git a/msg/PosCovCartesian.msg b/msg/PosCovCartesian.msg index 08ce8e3b..3b517687 100644 --- a/msg/PosCovCartesian.msg +++ b/msg/PosCovCartesian.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_xx -float32 cov_yy -float32 cov_zz -float32 cov_bb -float32 cov_xy -float32 cov_xz -float32 cov_xb -float32 cov_yz -float32 cov_yb -float32 cov_zb \ No newline at end of file +float32 cov_xx # m^2 +float32 cov_yy # m^2 +float32 cov_zz # m^2 +float32 cov_bb # m^2 +float32 cov_xy # m^2 +float32 cov_xz # m^2 +float32 cov_xb # m^2 +float32 cov_yz # m^2 +float32 cov_yb # m^2 +float32 cov_zb # m^2 \ No newline at end of file diff --git a/msg/PosCovGeodetic.msg b/msg/PosCovGeodetic.msg index 8f9f8c16..2d69400e 100644 --- a/msg/PosCovGeodetic.msg +++ b/msg/PosCovGeodetic.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_latlat -float32 cov_lonlon -float32 cov_hgthgt -float32 cov_bb -float32 cov_latlon -float32 cov_lathgt -float32 cov_latb -float32 cov_lonhgt -float32 cov_lonb -float32 cov_hb \ No newline at end of file +float32 cov_latlat # m^2 +float32 cov_lonlon # m^2 +float32 cov_hgthgt # m^2 +float32 cov_bb # m^2 +float32 cov_latlon # m^2 +float32 cov_lathgt # m^2 +float32 cov_latb # m^2 +float32 cov_lonhgt # m^2 +float32 cov_lonb # m^2 +float32 cov_hb # m^2 \ No newline at end of file diff --git a/msg/RFBand.msg b/msg/RFBand.msg new file mode 100644 index 00000000..69c977e6 --- /dev/null +++ b/msg/RFBand.msg @@ -0,0 +1,5 @@ +# RFband block + +uint32 frequency # Hz +uint16 bandwidth # kHz +uint8 info \ No newline at end of file diff --git a/msg/RFStatus.msg b/msg/RFStatus.msg new file mode 100644 index 00000000..ff7853fc --- /dev/null +++ b/msg/RFStatus.msg @@ -0,0 +1,12 @@ +# RFStatus block +# ROS message header +std_msgs/Header header + +# SBF block header including time header +BlockHeader block_header + +uint8 n +uint8 sb_length +uint8 flags +RFBand[] rfband + diff --git a/msg/ReceiverTime.msg b/msg/ReceiverTime.msg index 8566f056..d2667190 100644 --- a/msg/ReceiverTime.msg +++ b/msg/ReceiverTime.msg @@ -5,11 +5,11 @@ std_msgs/Header header # SBF block header including time header BlockHeader block_header -int8 utc_year -int8 utc_month -int8 utc_day -int8 utc_hour -int8 utc_min -int8 utc_second -int8 delta_ls +int8 utc_year # year +int8 utc_month # month +int8 utc_day # day +int8 utc_hour # hour +int8 utc_min # minute +int8 utc_second # s +int8 delta_ls # s uint8 sync_level \ No newline at end of file diff --git a/msg/VelCovCartesian.msg b/msg/VelCovCartesian.msg index 35ada65d..5cb77ea1 100644 --- a/msg/VelCovCartesian.msg +++ b/msg/VelCovCartesian.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_vxvx -float32 cov_vyvy -float32 cov_vzvz -float32 cov_dtdt -float32 cov_vxvy -float32 cov_vxvz -float32 cov_vxdt -float32 cov_vyvz -float32 cov_vydt -float32 cov_vzdt \ No newline at end of file +float32 cov_vxvx # m^2/s^2 +float32 cov_vyvy # m^2/s^2 +float32 cov_vzvz # m^2/s^2 +float32 cov_dtdt # m^2/s^2 +float32 cov_vxvy # m^2/s^2 +float32 cov_vxvz # m^2/s^2 +float32 cov_vxdt # m^2/s^2 +float32 cov_vyvz # m^2/s^2 +float32 cov_vydt # m^2/s^2 +float32 cov_vzdt # m^2/s^2 \ No newline at end of file diff --git a/msg/VelCovGeodetic.msg b/msg/VelCovGeodetic.msg index 0c65d8e4..396586fe 100644 --- a/msg/VelCovGeodetic.msg +++ b/msg/VelCovGeodetic.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_vnvn -float32 cov_veve -float32 cov_vuvu -float32 cov_dtdt -float32 cov_vnve -float32 cov_vnvu -float32 cov_vndt -float32 cov_vevu -float32 cov_vedt -float32 cov_vudt \ No newline at end of file +float32 cov_vnvn # m^2/s^2 +float32 cov_veve # m^2/s^2 +float32 cov_vuvu # m^2/s^2 +float32 cov_dtdt # m^2/s^2 +float32 cov_vnve # m^2/s^2 +float32 cov_vnvu # m^2/s^2 +float32 cov_vndt # m^2/s^2 +float32 cov_vevu # m^2/s^2 +float32 cov_vedt # m^2/s^2 +float32 cov_vudt # m^2/s^2 \ No newline at end of file diff --git a/msg/VelSensorSetup.msg b/msg/VelSensorSetup.msg index f673096b..42de4c29 100644 --- a/msg/VelSensorSetup.msg +++ b/msg/VelSensorSetup.msg @@ -7,6 +7,6 @@ std_msgs/Header header BlockHeader block_header uint8 port -float32 lever_arm_x -float32 lever_arm_y -float32 lever_arm_z \ No newline at end of file +float32 lever_arm_x # m +float32 lever_arm_y # m +float32 lever_arm_z # m \ No newline at end of file diff --git a/package.xml b/package.xml index 3f8601b4..75ce0cc4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.2.3 + 1.3.2 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond diff --git a/src/septentrio_gnss_driver/communication/callback_handlers.cpp b/src/septentrio_gnss_driver/communication/callback_handlers.cpp deleted file mode 100644 index 7f708217..00000000 --- a/src/septentrio_gnss_driver/communication/callback_handlers.cpp +++ /dev/null @@ -1,661 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include - -/** - * @file callback_handlers.cpp - * @date 22/08/20 - * @brief Handles callbacks when reading NMEA/SBF messages - */ - -std::pair gpsfix_pairs[] = { - std::make_pair("4013", 0), std::make_pair("4027", 1), std::make_pair("4001", 2), - std::make_pair("4007", 3), std::make_pair("5906", 4), std::make_pair("5908", 5), - std::make_pair("5938", 6), std::make_pair("5939", 7), std::make_pair("4226", 8)}; - -std::pair navsatfix_pairs[] = { - std::make_pair("4007", 0), std::make_pair("5906", 1), std::make_pair("4226", 2)}; - -std::pair pose_pairs[] = { - std::make_pair("4007", 0), std::make_pair("5906", 1), std::make_pair("5938", 2), - std::make_pair("5939", 3), std::make_pair("4226", 4)}; - -std::pair diagnosticarray_pairs[] = { - std::make_pair("4014", 0), std::make_pair("4082", 1)}; - -std::pair imu_pairs[] = {std::make_pair("4226", 0), - std::make_pair("4050", 1)}; - -std::pair localization_pairs[] = {std::make_pair("4226", 0)}; - -namespace io_comm_rx { - boost::mutex CallbackHandlers::callback_mutex_; - - CallbackHandlers::GPSFixMap CallbackHandlers::gpsfix_map(gpsfix_pairs, - gpsfix_pairs + 9); - CallbackHandlers::NavSatFixMap - CallbackHandlers::navsatfix_map(navsatfix_pairs, navsatfix_pairs + 3); - CallbackHandlers::PoseWithCovarianceStampedMap - CallbackHandlers::pose_map(pose_pairs, pose_pairs + 5); - CallbackHandlers::DiagnosticArrayMap - CallbackHandlers::diagnosticarray_map(diagnosticarray_pairs, - diagnosticarray_pairs + 2); - CallbackHandlers::ImuMap CallbackHandlers::imu_map(imu_pairs, imu_pairs + 2); - - CallbackHandlers::LocalizationMap - CallbackHandlers::localization_map(localization_pairs, - localization_pairs + 1); - - std::string CallbackHandlers::do_gpsfix_ = "4007"; - std::string CallbackHandlers::do_navsatfix_ = "4007"; - std::string CallbackHandlers::do_pose_ = "4007"; - std::string CallbackHandlers::do_diagnostics_ = "4014"; - - std::string CallbackHandlers::do_insgpsfix_ = "4226"; - std::string CallbackHandlers::do_insnavsatfix_ = "4226"; - std::string CallbackHandlers::do_inspose_ = "4226"; - std::string CallbackHandlers::do_imu_ = "4226"; - std::string CallbackHandlers::do_inslocalization_ = "4226"; - - //! The for loop forwards to a ROS message specific handle if the latter was - //! added via callbackmap_.insert at some earlier point. - void CallbackHandlers::handle() - { - // Find the ROS message callback handler for the equivalent Rx message - // (SBF/NMEA) at hand & call it - boost::mutex::scoped_lock lock(callback_mutex_); - CallbackMap::key_type key = rx_message_.messageID(); - std::string ID_temp = rx_message_.messageID(); - if (!(ID_temp == "4013" || ID_temp == "4001" || ID_temp == "4014" || - ID_temp == "4082")) - // We only want to handle ChannelStatus, MeasEpoch, DOP, ReceiverStatus, - // QualityInd blocks in case GPSFix and DiagnosticArray - // messages are to be published, respectively, see few lines below. - { - for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - - // Call NavSatFix callback function if it was added for GNSS - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_navsatfix) - { - CallbackMap::key_type key = "NavSatFix"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_navsatfix_) - // The last incoming block PVTGeodetic triggers - // the publishing of NavSatFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_navsatfix_ = std::string(); - } - } - } - // Call NavSatFix callback function if it was added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_navsatfix) - { - CallbackMap::key_type key = "INSNavSatFix"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_insnavsatfix_) - // The last incoming block INSNavGeod triggers - // the publishing of NavSatFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_insnavsatfix_ = std::string(); - } - } - } - // Call PoseWithCovarianceStampedMsg callback function if it was - // added for GNSS - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_pose) - { - CallbackMap::key_type key = "PoseWithCovarianceStamped"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_pose_) - // The last incoming block among PVTGeodetic, PosCovGeodetic, - // AttEuler and AttCovEuler triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_pose_ = std::string(); - } - } - } - // Call PoseWithCovarianceStampedMsg callback function if it was - // added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_pose) - { - CallbackMap::key_type key = "INSPoseWithCovarianceStamped"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_inspose_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_inspose_ = std::string(); - } - } - } - // Call DiagnosticArrayMsg callback function if it was added - // for the both type of receivers - if (settings_->publish_diagnostics) - { - CallbackMap::key_type key1 = rx_message_.messageID(); - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == "4014" || ID_temp == "4082" || ID_temp == "5902") - { - for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - CallbackMap::key_type key2 = "DiagnosticArray"; - if (ID_temp == do_diagnostics_) - // The last incoming block among ReceiverStatus, QualityInd and - // ReceiverSetup triggers the publishing of DiagnosticArray. - { - for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2); - callback != callbackmap_.upper_bound(key2); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_diagnostics_ = std::string(); - } - } - // Call ImuMsg callback function if it was - // added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_imu) - { - CallbackMap::key_type key = "Imu"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_imu_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_imu_ = std::string(); - } - } - } - // Call LocalizationMsg callback function if it was - // added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_localization || settings_->publish_tf) - { - CallbackMap::key_type key = "Localization"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_inslocalization_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_inslocalization_ = std::string(); - } - } - } - // Call TimeReferenceMsg (with GPST) callback function if it was - // added - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpst) - { - CallbackMap::key_type key1 = "GPST"; - std::string ID_temp = rx_message_.messageID(); - // If no new PVTGeodetic block is coming in, there is no need to - // publish TimeReferenceMsg (with GPST) anew. - if (ID_temp == "4007") - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpst) - { - CallbackMap::key_type key1 = "GPST"; - std::string ID_temp = rx_message_.messageID(); - // If no new INSNavGeod block is coming in, there is no need to - // publish TimeReferenceMsg (with GPST) anew. - if (ID_temp == "4226") - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - } - } - // Call GPSFix callback function if it was added for GNSS - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpsfix) - { - std::string ID_temp = rx_message_.messageID(); - CallbackMap::key_type key1 = rx_message_.messageID(); - if (ID_temp == "4013" || ID_temp == "4001") - // Even though we are not interested in publishing ChannelStatus - // (4013) and DOP (4001) ROS messages, we have to save some contents - // of these incoming blocks in order to publish the GPSFix message. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - CallbackMap::key_type key2 = "GPSFix"; - if (ID_temp == do_gpsfix_) - // The last incoming block among ChannelStatus (4013), MeasEpoch - // (4027), and DOP (4001) triggers the publishing of GPSFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key2); - callback != callbackmap_.upper_bound(key2); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_gpsfix_ = std::string(); - } - } - } - // Call GPSFix callback function if it was added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpsfix) - { - std::string ID_temp = rx_message_.messageID(); - CallbackMap::key_type key1 = rx_message_.messageID(); - if (ID_temp == "4013" || ID_temp == "4001") - // Even though we are not interested in publishing ChannelStatus - // (4013) and DOP (4001) ROS messages, we have to save some contents - // of these incoming blocks in order to publish the GPSFix message. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - CallbackMap::key_type key2 = "INSGPSFix"; - if (ID_temp == do_insgpsfix_) - // The last incoming block among ChannelStatus (4013), MeasEpoch - // (4027) and DOP (4001) triggers the publishing of INSGPSFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key2); - callback != callbackmap_.upper_bound(key2); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_insgpsfix_ = std::string(); - } - } - } - } - - void CallbackHandlers::readCallback(Timestamp recvTimestamp, const uint8_t* data, - std::size_t& size) - { - rx_message_.newData(recvTimestamp, data, size); - // Read !all! (there might be many) messages in the buffer - while (rx_message_.search() != rx_message_.getEndBuffer() && - rx_message_.found()) - { - // Print the found message (if NMEA) or just show messageID (if SBF).. - if (rx_message_.isSBF()) - { - std::size_t sbf_block_length; - std::string ID_temp = rx_message_.messageID(); - sbf_block_length = - static_cast(rx_message_.getBlockLength()); - node_->log(LogLevel::DEBUG, - "ROSaic reading SBF block " + ID_temp + " made up of " + - std::to_string(sbf_block_length) + " bytes..."); - // If full message did not yet arrive, throw an error message. - if (sbf_block_length > rx_message_.getCount()) - { - node_->log( - LogLevel::DEBUG, - "Not a valid SBF block, parts of the SBF block are yet to be received. Ignore.."); - throw( - static_cast(rx_message_.getPosBuffer() - data)); - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpsfix == true && - (ID_temp == "4013" || ID_temp == "4027" || - ID_temp == "4001" || ID_temp == "4007" || - ID_temp == "5906" || ID_temp == "5908" || - ID_temp == "5938" || ID_temp == "5939")) - { - if (rx_message_.gnss_gpsfix_complete(gpsfix_map[ID_temp])) - { - do_gpsfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpsfix == true && - (ID_temp == "4013" || ID_temp == "4027" || - ID_temp == "4001" || ID_temp == "4226")) - { - if (rx_message_.ins_gpsfix_complete(gpsfix_map[ID_temp])) - { - do_insgpsfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_navsatfix == true && - (ID_temp == "4007" || ID_temp == "5906")) - { - if (rx_message_.gnss_navsatfix_complete( - navsatfix_map[ID_temp])) - { - do_navsatfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_navsatfix == true && (ID_temp == "4226")) - { - if (rx_message_.ins_navsatfix_complete( - navsatfix_map[ID_temp])) - { - do_insnavsatfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_pose == true && - (ID_temp == "4007" || ID_temp == "5906" || - ID_temp == "5938" || ID_temp == "5939")) - { - if (rx_message_.gnss_pose_complete(pose_map[ID_temp])) - { - do_pose_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_pose == true && (ID_temp == "4226")) - { - if (rx_message_.ins_pose_complete(pose_map[ID_temp])) - { - do_inspose_ = ID_temp; - } - } - } - if (settings_->publish_diagnostics == true && - (ID_temp == "4014" || ID_temp == "4082")) - { - if (rx_message_.diagnostics_complete( - diagnosticarray_map[ID_temp])) - { - do_diagnostics_ = ID_temp; - } - } - if ((settings_->publish_localization || settings_->publish_tf) && - (ID_temp == "4226")) - { - if (rx_message_.ins_localization_complete( - localization_map[ID_temp])) - { - do_inslocalization_ = ID_temp; - } - } - } - if (rx_message_.isNMEA()) - { - boost::char_separator sep("\r"); // Carriage Return (CR) - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = rx_message_.messageSize(); - // Syntax: new_string_name (const char* s, size_t n); size_t is - // either 2 or 8 bytes, depending on your system - std::string block_in_string( - reinterpret_cast(rx_message_.getPosBuffer()), - nmea_size); - tokenizer tokens(block_in_string, sep); - node_->log(LogLevel::DEBUG, - "The NMEA message contains " + std::to_string(nmea_size) + - " bytes and is ready to be parsed. It reads: " + - *tokens.begin()); - } - if (rx_message_.isResponse()) // If the response is not sent at once, - // only first part is ROS_DEBUG-printed - { - std::size_t response_size = rx_message_.messageSize(); - std::string block_in_string( - reinterpret_cast(rx_message_.getPosBuffer()), - response_size); - node_->log(LogLevel::DEBUG, "The Rx's response contains " + - std::to_string(response_size) + - " bytes and reads:\n " + - block_in_string); - { - boost::mutex::scoped_lock lock(g_response_mutex); - g_response_received = true; - lock.unlock(); - g_response_condition.notify_one(); - } - if (rx_message_.isErrorMessage()) - { - node_->log( - LogLevel::ERROR, - "Invalid command just sent to the Rx! The Rx's response contains " + - std::to_string(response_size) + " bytes and reads:\n " + - block_in_string); - } - continue; - } - if (rx_message_.isConnectionDescriptor()) - { - std::string cd( - reinterpret_cast(rx_message_.getPosBuffer()), 4); - g_rx_tcp_port = cd; - if (g_cd_count == 0) - { - node_->log( - LogLevel::INFO, - "The connection descriptor for the TCP connection is " + cd); - } - if (g_cd_count < 3) - ++g_cd_count; - if (g_cd_count == 2) - { - boost::mutex::scoped_lock lock(g_cd_mutex); - g_cd_received = true; - lock.unlock(); - g_cd_condition.notify_one(); - } - continue; - } - try - { - handle(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "Incomplete message: " + std::string(e.what())); - throw(static_cast(rx_message_.getPosBuffer() - data)); - } - } - } -} // namespace io_comm_rx \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/circular_buffer.cpp b/src/septentrio_gnss_driver/communication/circular_buffer.cpp deleted file mode 100644 index 2699fbb2..00000000 --- a/src/septentrio_gnss_driver/communication/circular_buffer.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include - -/** - * @file circular_buffer.cpp - * @brief Defines a class for creating, writing and reading from a circular bufffer - * @date 25/09/20 - */ - -CircularBuffer::CircularBuffer(ROSaicNodeBase* node, std::size_t capacity) : - node_(node), head_(0), tail_(0), size_(0), capacity_(capacity) -{ - data_ = new uint8_t[capacity]; -} - -//! The destructor frees memory (first line) and points the dangling pointer to NULL -//! (second line). -CircularBuffer::~CircularBuffer() -{ - delete[] data_; - data_ = NULL; -} - -std::size_t CircularBuffer::write(const uint8_t* data, std::size_t bytes) -{ - if (bytes == 0) - return 0; - - std::size_t capacity = capacity_; - std::size_t bytes_to_write = std::min(bytes, capacity - size_); - if (bytes_to_write != bytes) - { - node_->log( - LogLevel::ERROR, - "You are trying to overwrite parts of the circular buffer that have not yet been read!"); - } - - // Writes in a single step - if (bytes_to_write <= capacity - head_) - { - - memcpy(data_ + head_, data, bytes_to_write); - head_ += bytes_to_write; - if (head_ == capacity) - head_ = 0; - } - // Writes in two steps. Here the circular nature comes to the surface - else - { - std::size_t size_1 = capacity - head_; - memcpy(data_ + head_, data, size_1); - std::size_t size_2 = bytes_to_write - size_1; - memcpy(data_, data + size_1, size_2); - head_ = - size_2; // Hence setting head_ = 0 three lines above was not necessary. - } - size_ += bytes_to_write; - return bytes_to_write; -} - -std::size_t CircularBuffer::read(uint8_t* data, std::size_t bytes) -{ - if (bytes == 0) - return 0; - std::size_t capacity = capacity_; - std::size_t bytes_to_read = std::min(bytes, size_); - if (bytes_to_read != bytes) - { - node_->log( - LogLevel::ERROR, - "You are trying to read parts of the circular buffer that have not yet been written!"); - } - - // Read in a single step - if (bytes_to_read <= - capacity - tail_) // Note that it is not size_ - tail_: - // If write() hasn't written something into all of capacity - // yet (first round of writing), we would still read those - // unknown bytes.. - { - memcpy(data, data_ + tail_, bytes_to_read); - tail_ += bytes_to_read; - if (tail_ == capacity) - tail_ = 0; // Same here? - } - - // Read in two steps - else - { - std::size_t size_1 = capacity - tail_; - memcpy(data, data_ + tail_, size_1); - std::size_t size_2 = bytes_to_read - size_1; - memcpy(data + size_1, data_, size_2); - tail_ = size_2; - } - - size_ -= bytes_to_read; - return bytes_to_read; -} \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index d8ac0f80..2f5c593f 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -34,63 +34,21 @@ // Boost includes #include #include -#include -#ifndef ANGLE_MAX -#define ANGLE_MAX 180 -#endif - -#ifndef ANGLE_MIN -#define ANGLE_MIN -180 -#endif - -#ifndef THETA_Y_MAX -#define THETA_Y_MAX 90 -#endif - -#ifndef THETA_Y_MIN -#define THETA_Y_MIN -90 -#endif - -#ifndef LEVER_ARM_MAX -#define LEVER_ARM_MAX 100 -#endif - -#ifndef LEVER_ARM_MIN -#define LEVER_ARM_MIN -100 -#endif - -#ifndef HEADING_MAX -#define HEADING_MAX 360 -#endif - -#ifndef HEADING_MIN -#define HEADING_MIN -360 -#endif - -#ifndef PITCH_MAX -#define PITCH_MAX 90 -#endif - -#ifndef PITCH_MIN -#define PITCH_MIN -90 -#endif - -#ifndef ATTSTD_DEV_MIN -#define ATTSTD_DEV_MIN 0 -#endif - -#ifndef ATTSTD_DEV_MAX -#define ATTSTD_DEV_MAX 5 -#endif - -#ifndef POSSTD_DEV_MIN -#define POSSTD_DEV_MIN 0 -#endif - -#ifndef POSSTD_DEV_MAX -#define POSSTD_DEV_MAX 100 -#endif +static const int16_t ANGLE_MAX = 180; +static const int16_t ANGLE_MIN = -180; +static const int8_t THETA_Y_MAX = 90; +static const int8_t THETA_Y_MIN = -90; +static const int8_t LEVER_ARM_MAX = 100; +static const int8_t LEVER_ARM_MIN = -100; +static const int16_t HEADING_MAX = 360; +static const int16_t HEADING_MIN = -360; +static const int8_t PITCH_MAX = 90; +static const int8_t PITCH_MIN = -90; +static const int8_t ATTSTD_DEV_MIN = 0; +static const int8_t ATTSTD_DEV_MAX = 5; +static const int8_t POSSTD_DEV_MIN = 0; +static const int8_t POSSTD_DEV_MAX = 100; /** * @file communication_core.cpp @@ -98,1464 +56,923 @@ * @brief Highest-Level view on communication services */ -//! Mutex to control changes of global variable "g_response_received" -boost::mutex g_response_mutex; -//! Determines whether a command reply was received from the Rx -bool g_response_received; -//! Condition variable complementing "g_response_mutex" -boost::condition_variable g_response_condition; -//! Mutex to control changes of global variable "g_cd_received" -boost::mutex g_cd_mutex; -//! Determines whether the connection descriptor was received from the Rx -bool g_cd_received; -//! Condition variable complementing "g_cd_mutex" -boost::condition_variable g_cd_condition; -//! Whether or not we still want to read the connection descriptor, which we only -//! want in the very beginning to know whether it is IP10, IP11 etc. -bool g_read_cd; -//! Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to -std::string g_rx_tcp_port; -//! Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have -//! to count the connection descriptors -uint32_t g_cd_count; - -io_comm_rx::Comm_IO::Comm_IO(ROSaicNodeBase* node, Settings* settings) : - node_(node), handlers_(node, settings), settings_(settings), stopping_(false) -{ - g_response_received = false; - g_cd_received = false; - g_read_cd = true; - g_cd_count = 0; -} - -io_comm_rx::Comm_IO::~Comm_IO() -{ - if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) +namespace io { + + CommunicationCore::CommunicationCore(ROSaicNodeBase* node) : + node_(node), settings_(node->settings()), telegramHandler_(node), + running_(true) { - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); - manager_.get()->send(cmd); - send("sdio, " + mainPort_ + ", auto, none\x0D"); - for (auto ntrip : settings_->rtk_settings.ntrip) - { - if (!ntrip.id.empty() && !ntrip.keep_open) - { - send("snts, " + ntrip.id + ", off \x0D"); - } - } - for (auto ip_server : settings_->rtk_settings.ip_server) - { - if (!ip_server.id.empty() && !ip_server.keep_open) - { - send("sdio, " + ip_server.id + ", auto, none\x0D"); - send("siss, " + ip_server.id + ", 0\x0D"); - } - } - for (auto serial : settings_->rtk_settings.serial) - { - if (!serial.port.empty() && !serial.keep_open) - { - send("sdio, " + serial.port + ", auto, none\x0D"); - if (serial.port.rfind("COM", 0) == 0) - send("scs, " + serial.port + - ", baud115200, bits8, No, bit1, none\x0D"); - } - } - if (!settings_->ins_vsm_ip_server_id.empty()) - { - if (!settings_->ins_vsm_ip_server_keep_open) - { - send("sdio, " + settings_->ins_vsm_ip_server_id + - ", auto, none\x0D"); - send("siss, " + settings_->ins_vsm_ip_server_id + ", 0\x0D"); - } - } - if (!settings_->ins_vsm_serial_port.empty()) - { - if (!settings_->ins_vsm_serial_keep_open) - { - if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) - send("scs, " + settings_->ins_vsm_serial_port + - ", baud115200, bits8, No, bit1, none\x0D"); - send("sdio, " + settings_->ins_vsm_serial_port + - ", auto, none\x0D"); - } - } + running_ = true; - send("logout \x0D"); + processingThread_ = + std::thread(std::bind(&CommunicationCore::processTelegrams, this)); } - stopping_ = true; - connectionThread_->join(); -} - -void io_comm_rx::Comm_IO::resetMainPort() -{ - // It is imperative to hold a lock on the mutex "g_cd_mutex" while - // modifying the variable and "g_cd_received". - boost::mutex::scoped_lock lock_cd(g_cd_mutex); - // Escape sequence (escape from correction mode), ensuring that we can send - // our real commands afterwards... - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); - manager_.get()->send(cmd); - // We wait for the connection descriptor before we send another command, - // otherwise the latter would not be processed. - g_cd_condition.wait(lock_cd, []() { return g_cd_received; }); - g_cd_received = false; -} - -void io_comm_rx::Comm_IO::initializeIO() -{ - node_->log(LogLevel::DEBUG, "Called initializeIO() method"); - boost::smatch match; - // In fact: smatch is a typedef of match_results - if (boost::regex_match(settings_->device, match, - boost::regex("(tcp)://(.+):(\\d+)"))) - // C++ needs \\d instead of \d: Both mean decimal. - // Note that regex_match can be used with a smatch object to store results, or - // without. In any case, true is returned if and only if it matches the - // !complete! string. - { - // The first sub_match (index 0) contained in a match_result always - // represents the full match within a target sequence made by a regex, and - // subsequent sub_matches represent sub-expression matches corresponding in - // sequence to the left parenthesis delimiting the sub-expression in the - // regex, i.e. $n Perl is equivalent to m[n] in boost regex. - tcp_host_ = match[2]; - tcp_port_ = match[3]; - - serial_ = false; - connectionThread_.reset( - new boost::thread(boost::bind(&Comm_IO::connect, this))); - } else if (boost::regex_match(settings_->device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) - { - serial_ = false; - settings_->read_from_sbf_log = true; - settings_->use_gnss_time = true; - connectionThread_.reset(new boost::thread( - boost::bind(&Comm_IO::prepareSBFFileReading, this, match[2]))); - - } else if (boost::regex_match( - settings_->device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) - { - serial_ = false; - settings_->read_from_pcap = true; - settings_->use_gnss_time = true; - connectionThread_.reset(new boost::thread( - boost::bind(&Comm_IO::preparePCAPFileReading, this, match[2]))); - - } else if (boost::regex_match(settings_->device, match, - boost::regex("(serial):(.+)"))) + CommunicationCore::~CommunicationCore() { - serial_ = true; - std::string proto(match[2]); - std::stringstream ss; - ss << "Searching for serial port" << proto; - settings_->device = proto; - node_->log(LogLevel::DEBUG, ss.str()); - connectionThread_.reset( - new boost::thread(boost::bind(&Comm_IO::connect, this))); - } else - { - std::stringstream ss; - ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; - node_->log(LogLevel::ERROR, ss.str()); - } - node_->log(LogLevel::DEBUG, "Leaving initializeIO() method"); -} + telegramHandler_.clearSemaphores(); -void io_comm_rx::Comm_IO::prepareSBFFileReading(std::string file_name) -{ - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from" << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializeSBFFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "Comm_IO::initializeSBFFileReading() failed for SBF File" << file_name - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } -} + resetSettings(); -void io_comm_rx::Comm_IO::preparePCAPFileReading(std::string file_name) -{ - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializePCAPFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "CommIO::initializePCAPFileReading() failed for SBF File " << file_name - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); + running_ = false; + std::shared_ptr telegram(new Telegram); + telegramQueue_.push(telegram); + processingThread_.join(); } -} - -void io_comm_rx::Comm_IO::connect() -{ - node_->log(LogLevel::DEBUG, "Called connect() method"); - node_->log( - LogLevel::DEBUG, - "Started timer for calling reconnect() method until connection succeeds"); - - boost::asio::io_service io; - boost::posix_time::millisec wait_ms( - static_cast(settings_->reconnect_delay_s * 1000)); - while (!connected_ && !stopping_) - { - boost::asio::deadline_timer t(io, wait_ms); - reconnect(); - t.wait(); - } - node_->log(LogLevel::DEBUG, "Successully connected. Leaving connect() method"); -} - -//! In serial mode (not USB, since the Rx port is then called USB1 or USB2), please -//! ensure that you are connected to the Rx's COM1, COM2 or COM3 port, !if! you -//! employ UART hardware flow control. -void io_comm_rx::Comm_IO::reconnect() -{ - node_->log(LogLevel::DEBUG, "Called reconnect() method"); - if (serial_) + + void CommunicationCore::resetSettings() { - bool initialize_serial_return = false; - try - { - node_->log( - LogLevel::INFO, - "Connecting serially to device" + settings_->device + - ", targeted baudrate: " + std::to_string(settings_->baudrate)); - initialize_serial_return = initializeSerial( - settings_->device, settings_->baudrate, settings_->hw_flow_control); - } catch (std::runtime_error& e) + if (settings_->configure_rx && !settings_->read_from_sbf_log && + !settings_->read_from_pcap) { + resetMainConnection(); + send("sdio, " + mainConnectionPort_ + ", auto, none\x0D"); + // Turning off all current SBF/NMEA output + send("sso, all, none, none, off \x0D"); + send("sno, all, none, none, off \x0D"); + if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { - std::stringstream ss; - ss << "initializeSerial() failed for device " << settings_->device - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); + send("siss, " + settings_->udp_ip_server + ", 0\x0D"); } - } - if (initialize_serial_return) - { - boost::mutex::scoped_lock lock(connection_mutex_); - connected_ = true; - lock.unlock(); - connection_condition_.notify_one(); - } - } else - { - bool initialize_tcp_return = false; - try - { - node_->log(LogLevel::INFO, - "Connecting to tcp://" + tcp_host_ + ":" + tcp_port_ + "..."); - initialize_tcp_return = initializeTCP(tcp_host_, tcp_port_); - } catch (std::runtime_error& e) - { + for (auto ntrip : settings_->rtk.ntrip) + { + if (!ntrip.id.empty() && !ntrip.keep_open) + { + send("snts, " + ntrip.id + ", off \x0D"); + } + } + for (auto ip_server : settings_->rtk.ip_server) + { + if (!ip_server.id.empty() && !ip_server.keep_open) + { + send("sdio, " + ip_server.id + ", auto, none\x0D"); + send("siss, " + ip_server.id + ", 0\x0D"); + } + } + for (auto serial : settings_->rtk.serial) + { + if (!serial.port.empty() && !serial.keep_open) + { + send("sdio, " + serial.port + ", auto, none\x0D"); + if (serial.port.rfind("COM", 0) == 0) + send("scs, " + serial.port + + ", baud115200, bits8, No, bit1, none\x0D"); + } + } + if (!settings_->ins_vsm_ip_server_id.empty()) + { + if (!settings_->ins_vsm_ip_server_keep_open) + { + send("sdio, " + settings_->ins_vsm_ip_server_id + + ", auto, none\x0D"); + send("siss, " + settings_->ins_vsm_ip_server_id + ", 0\x0D"); + } + } + if (!settings_->ins_vsm_serial_port.empty()) + { + if (!settings_->ins_vsm_serial_keep_open) + { + if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) + send("scs, " + settings_->ins_vsm_serial_port + + ", baud115200, bits8, No, bit1, none\x0D"); + send("sdio, " + settings_->ins_vsm_serial_port + + ", auto, none\x0D"); + } + } + if (!settings_->osnma.keep_open && (settings_->osnma.mode == "loose" || + settings_->osnma.mode == "strict")) { std::stringstream ss; - ss << "initializeTCP() failed for host " << tcp_host_ << " on port " - << tcp_port_ << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); + ss << "sou, off \x0D"; + send(ss.str()); + + if (!settings_->osnma.ntp_server.empty()) + { + std::stringstream ss; + ss << "snc, off \x0D"; + send(ss.str()); + } } - } - if (initialize_tcp_return) - { - boost::mutex::scoped_lock lock(connection_mutex_); - connected_ = true; - lock.unlock(); - connection_condition_.notify_one(); + + if (!settings_->login_user.empty() && !settings_->login_password.empty()) + send("logout \x0D"); } } - node_->log(LogLevel::DEBUG, "Leaving reconnect() method"); -} - -//! The send() method of AsyncManager class is paramount for this purpose. -//! Note that std::to_string() is from C++11 onwards only. -//! Since ROSaic can be launched before booting the Rx, we have to watch out for -//! escape characters that are sent by the Rx to indicate that it is in upgrade mode. -//! Those characters would then be mingled with the first command we send to it in -//! this method and could result in an invalid command. Hence we first enter command -//! mode via "SSSSSSSSSS". -void io_comm_rx::Comm_IO::configureRx() -{ - node_->log(LogLevel::DEBUG, "Called configureRx() method"); - { - // wait for connection - boost::mutex::scoped_lock lock(connection_mutex_); - connection_condition_.wait(lock, [this]() { return connected_; }); - } - // Determining communication mode: TCP vs USB/Serial - unsigned stream = 1; - boost::smatch match; - boost::regex_match(settings_->device, match, - boost::regex("(tcp)://(.+):(\\d+)")); - std::string proto(match[1]); - resetMainPort(); - if (proto == "tcp") - { - mainPort_ = g_rx_tcp_port; - } else + void CommunicationCore::connect() { - mainPort_ = settings_->rx_serial_port; - // After booting, the Rx sends the characters "x?" to all ports, which could - // potentially mingle with our first command. Hence send a safeguard command - // "lif", whose potentially false processing is harmless. - send("lif, Identification \x0D"); - } + node_->log(log_level::DEBUG, "Called connect() method"); + node_->log( + log_level::DEBUG, + "Started timer for calling connect() method until connection succeeds"); - std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand( - settings_->polling_period_pvt); + boost::asio::io_service io; + boost::posix_time::millisec wait_ms( + static_cast(settings_->reconnect_delay_s * 1000)); + if (initializeIo()) + { + while (running_) + { + boost::asio::deadline_timer t(io, wait_ms); - std::string rest_interval = parsing_utilities::convertUserPeriodToRxCommand( - settings_->polling_period_rest); + if (manager_->connect()) + { + initializedIo_ = true; + break; + } - // Credentials for login - if (!settings_->login_user.empty() && !settings_->login_password.empty()) - { - if (string_utilities::containsSpace(settings_->login_password)) - send("login, " + settings_->login_user + ", \"" + - settings_->login_password + "\" \x0D"); - else - send("login, " + settings_->login_user + ", " + - settings_->login_password + " \x0D"); - } + t.wait(); + } + } - // Turning off all current SBF/NMEA output - send("sso, all, none, none, off \x0D"); - send("sno, all, none, none, off \x0D"); + // Sends commands to the Rx regarding which SBF/NMEA messages it should + // output + // and sets all its necessary corrections-related parameters + if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) + { + node_->log(log_level::DEBUG, "Configure Rx."); + if (settings_->configure_rx) + configureRx(); + } - // Activate NTP server - if (settings_->use_gnss_time) - send("sntp, on \x0D"); + node_->log(log_level::INFO, "Setup complete."); - // Setting the datum to be used by the Rx (not the NMEA output though, which only - // provides MSL and undulation (by default with respect to WGS84), but not - // ellipsoidal height) - { - std::stringstream ss; - // WGS84 is equivalent to Default and kept for backwards compatibility - if (settings_->datum == "Default") - settings_->datum = "WGS84"; - ss << "sgd, " << settings_->datum << "\x0D"; - send(ss.str()); + node_->log(log_level::DEBUG, + "Successully connected. Leaving connect() method"); } - // Setting up SBF blocks with rx_period_pvt + [[nodiscard]] bool CommunicationCore::initializeIo() { - std::stringstream blocks; - if (settings_->use_gnss_time) - { - blocks << " +ReceiverTime"; - } - if (settings_->publish_pvtcartesian) + bool client = false; + node_->log(log_level::DEBUG, "Called initializeIo() method"); + if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty())) { - blocks << " +PVTCartesian"; + tcpClient_.reset(new AsyncManager(node_, &telegramQueue_)); + tcpClient_->setPort(std::to_string(settings_->tcp_port)); + if (!settings_->configure_rx) + tcpClient_->connect(); + client = true; } - if (settings_->publish_pvtgeodetic || settings_->publish_twist || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) + if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { - blocks << " +PVTGeodetic"; + udpClient_.reset( + new UdpClient(node_, settings_->udp_port, &telegramQueue_)); + client = true; } - if (settings_->publish_basevectorcart) - { - blocks << " +BaseVectorCart"; - } - if (settings_->publish_basevectorgeod) - { - blocks << " +BaseVectorGeod"; - } - if (settings_->publish_poscovcartesian) - { - blocks << " +PosCovCartesian"; - } - if (settings_->publish_poscovgeodetic || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +PosCovGeodetic"; - } - if (settings_->publish_velcovgeodetic || settings_->publish_twist || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss"))) + + switch (settings_->device_type) { - blocks << " +VelCovGeodetic"; - } - if (settings_->publish_atteuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) + case device_type::TCP: { - blocks << " +AttEuler"; - } - if (settings_->publish_attcoveuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +AttCovEuler"; + manager_.reset(new AsyncManager(node_, &telegramQueue_)); + break; } - if (settings_->publish_measepoch || settings_->publish_gpsfix) + case device_type::SERIAL: { - blocks << " +MeasEpoch"; + manager_.reset(new AsyncManager(node_, &telegramQueue_)); + break; } - if (settings_->publish_gpsfix) + case device_type::SBF_FILE: { - blocks << " +ChannelStatus +DOP"; + manager_.reset(new AsyncManager(node_, &telegramQueue_)); + break; } - // Setting SBF output of Rx depending on the receiver type - // If INS then... - if (settings_->septentrio_receiver_type == "ins") + case device_type::PCAP_FILE: { - if (settings_->publish_insnavcart) - { - blocks << " +INSNavCart"; - } - if (settings_->publish_insnavgeod || settings_->publish_navsatfix || - settings_->publish_gpsfix || settings_->publish_pose || - settings_->publish_imu || settings_->publish_localization || - settings_->publish_tf || settings_->publish_twist) - { - blocks << " +INSNavGeod"; - } - if (settings_->publish_exteventinsnavgeod) - { - blocks << " +ExtEventINSNavGeod"; - } - if (settings_->publish_exteventinsnavcart) - { - blocks << " +ExtEventINSNavCart"; - } - if (settings_->publish_extsensormeas || settings_->publish_imu) - { - blocks << " +ExtSensorMeas"; - } + manager_.reset(new AsyncManager(node_, &telegramQueue_)); + break; } - std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; - send(ss.str()); - ++stream; - } - // Setting up SBF blocks with rx_period_rest - { - std::stringstream blocks; - if (settings_->septentrio_receiver_type == "ins") + default: { - if (settings_->publish_imusetup) + if (!client || settings_->configure_rx || + (settings_->ins_vsm_ros_source == "odometry") || + (settings_->ins_vsm_ros_source == "twist")) { - blocks << " +IMUSetup"; - } - if (settings_->publish_velsensorsetup) - { - blocks << " +VelSensorSetup"; + node_->log(log_level::DEBUG, "Unsupported device."); + return false; } + break; } - if (settings_->publish_diagnostics) - { - blocks << " +ReceiverStatus +QualityInd"; } - - blocks << " +ReceiverSetup"; - - std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << rest_interval << "\x0D"; - send(ss.str()); - ++stream; + return true; } - // Setting up NMEA streams + //! The send() method of AsyncManager class is paramount for this purpose. + //! Note that std::to_string() is from C++11 onwards only. + //! Since ROSaic can be launched before booting the Rx, we have to watch out for + //! escape characters that are sent by the Rx to indicate that it is in upgrade + //! mode. Those characters would then be mingled with the first command we send + //! to it in this method and could result in an invalid command. Hence we first + //! enter command mode via "SSSSSSSSSS". + void CommunicationCore::configureRx() { - send("snti, GP\x0D"); + node_->log(log_level::DEBUG, "Called configureRx() method"); - std::stringstream blocks; - if (settings_->publish_gpgga) - { - blocks << " +GGA"; - } - if (settings_->publish_gprmc) + if (!initializedIo_) { - blocks << " +RMC"; + node_->log(log_level::DEBUG, + "Called configureRx() method but IO is not initialized."); + return; } - if (settings_->publish_gpgsa) + + uint8_t stream = 1; + // Determining communication mode: TCP vs USB/Serial + boost::smatch match; + boost::regex_match(settings_->device, match, + boost::regex("(tcp)://(.+):(\\d+)")); + std::string proto(match[1]); + mainConnectionPort_ = resetMainConnection(); + node_->log(log_level::INFO, + "The connection descriptor is " + mainConnectionPort_); + streamPort_ = mainConnectionPort_; + if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty())) { - blocks << " +GSA"; - } - if (settings_->publish_gpgsv) + streamPort_ = settings_->tcp_ip_server; + send("siss, " + streamPort_ + ", " + + std::to_string(settings_->tcp_port) + ", TCP, " + "\x0D"); + tcpClient_->connect(); + } else if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { - blocks << " +GSV"; + streamPort_ = settings_->udp_ip_server; + std::string destination; + if (!settings_->udp_unicast_ip.empty()) + destination = settings_->udp_unicast_ip; + else + destination = "255.255.255.255"; + send("siss, " + streamPort_ + ", " + + std::to_string(settings_->udp_port) + ", UDP, " + destination + + "\x0D"); } - std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; - send(ss.str()); - ++stream; - } + node_->log(log_level::INFO, "Setting up Rx."); - if ((settings_->septentrio_receiver_type == "ins") || - settings_->ins_in_gnss_mode) - { - { - std::stringstream ss; - ss << "sat, Main, \"" << settings_->ant_type << "\"" - << "\x0D"; - send(ss.str()); - } + std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand( + settings_->polling_period_pvt); - // Configure Aux1 antenna - { - std::stringstream ss; - ss << "sat, Aux1, \"" << settings_->ant_type << "\"" - << "\x0D"; - send(ss.str()); - } - } else if (settings_->septentrio_receiver_type == "gnss") - { - // Setting the marker-to-ARP offsets. This comes after the "sso, ..., - // ReceiverSetup, ..." command, since the latter is only generated when a - // user-command is entered to change one or more values in the block. + std::string rest_interval = parsing_utilities::convertUserPeriodToRxCommand( + settings_->polling_period_rest); + + // Credentials for login + if (!settings_->login_user.empty() && !settings_->login_password.empty()) { - std::stringstream ss; - ss << "sao, Main, " - << string_utilities::trimDecimalPlaces(settings_->delta_e) << ", " - << string_utilities::trimDecimalPlaces(settings_->delta_n) << ", " - << string_utilities::trimDecimalPlaces(settings_->delta_u) << ", \"" - << settings_->ant_type << "\", " << settings_->ant_serial_nr - << "\x0D"; - send(ss.str()); + if (string_utilities::containsSpace(settings_->login_password)) + send("login, " + settings_->login_user + ", \"" + + settings_->login_password + "\" \x0D"); + else + send("login, " + settings_->login_user + ", " + + settings_->login_password + " \x0D"); } - // Configure Aux1 antenna + // Turning off all current SBF/NMEA output + send("sso, all, none, none, off \x0D"); + send("sno, all, none, none, off \x0D"); + + // Get Rx capabilities + send("grc \x0D"); + telegramHandler_.waitForCapabilities(); + + // Activate NTP server + if (settings_->use_gnss_time) + send("sntp, on \x0D"); + + // Setting the datum to be used by the Rx (not the NMEA output though, which + // only provides MSL and undulation (by default with respect to WGS84), but + // not ellipsoidal height) { std::stringstream ss; - ss << "sao, Aux1, " << string_utilities::trimDecimalPlaces(0.0) << ", " - << string_utilities::trimDecimalPlaces(0.0) << ", " - << string_utilities::trimDecimalPlaces(0.0) << ", \"" - << settings_->ant_aux1_type << "\", " << settings_->ant_aux1_serial_nr - << "\x0D"; + ss << "sgd, " << settings_->datum << "\x0D"; send(ss.str()); } - } - // Configuring the corrections connection - for (auto ntrip : settings_->rtk_settings.ntrip) - { - if (!ntrip.id.empty()) + if ((settings_->septentrio_receiver_type == "ins") || node_->isIns()) { - // First disable any existing NTRIP connection on NTR1 - send("snts, " + ntrip.id + ", off \x0D"); { std::stringstream ss; - ss << "snts, " << ntrip.id << ", Client, " << ntrip.caster << ", " - << std::to_string(ntrip.caster_port) << ", " << ntrip.username - << ", " << ntrip.password << ", " << ntrip.mountpoint << ", " - << ntrip.version << ", " << ntrip.send_gga << " \x0D"; + ss << "sat, Main, \"" << settings_->ant_type << "\"" + << "\x0D"; send(ss.str()); } - if (ntrip.tls) - { - std::stringstream ss; - ss << "sntt, " << ntrip.id << ", on, \"" << ntrip.fingerprint - << "\" \x0D"; - send(ss.str()); - } else + + // Configure Aux1 antenna + if (settings_->multi_antenna) { std::stringstream ss; - ss << "sntt, " << ntrip.id << ", off \x0D"; + ss << "sat, Aux1, \"" << settings_->ant_type << "\"" + << "\x0D"; send(ss.str()); } - } - } - - for (auto ip_server : settings_->rtk_settings.ip_server) - { - if (!ip_server.id.empty()) - // Since the Rx does not have internet (and you will not - // be able to share it via USB), we need to forward the - // corrections ourselves, though not on the same port. + } else if (settings_->septentrio_receiver_type == "gnss") { + // Setting the marker-to-ARP offsets. This comes after the "sso, ..., + // ReceiverSetup, ..." command, since the latter is only generated when a + // user-command is entered to change one or more values in the block. { std::stringstream ss; - // In case this IP server was used before, old configuration is lost - // of course. - ss << "siss, " << ip_server.id << ", " - << std::to_string(ip_server.port) << ", TCP2Way \x0D"; + ss << "sao, Main, " + << string_utilities::trimDecimalPlaces(settings_->delta_e) << ", " + << string_utilities::trimDecimalPlaces(settings_->delta_n) << ", " + << string_utilities::trimDecimalPlaces(settings_->delta_u) + << ", \"" << settings_->ant_type << "\", " + << settings_->ant_serial_nr << "\x0D"; send(ss.str()); } + + // Configure Aux1 antenna + if (settings_->multi_antenna) { std::stringstream ss; - ss << "sdio, " << ip_server.id << ", " << ip_server.rtk_standard - << ", +SBF+NMEA \x0D"; + ss << "sao, Aux1, " << string_utilities::trimDecimalPlaces(0.0) + << ", " << string_utilities::trimDecimalPlaces(0.0) << ", " + << string_utilities::trimDecimalPlaces(0.0) << ", \"" + << settings_->ant_aux1_type << "\", " + << settings_->ant_aux1_serial_nr << "\x0D"; send(ss.str()); } - if (ip_server.send_gga != "off") + } + + // Configuring the corrections connection + for (auto ntrip : settings_->rtk.ntrip) + { + if (!ntrip.id.empty()) { - std::string rate = ip_server.send_gga; - if (ip_server.send_gga == "auto") - rate = "sec1"; - std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << ip_server.id - << ", GGA, " << rate << " \x0D"; - ++stream; - send(ss.str()); + // First disable any existing NTRIP connection on NTR1 + send("snts, " + ntrip.id + ", off \x0D"); + { + std::stringstream ss; + ss << "snts, " << ntrip.id << ", Client, " << ntrip.caster + << ", " << std::to_string(ntrip.caster_port) << ", " + << ntrip.username << ", " << ntrip.password << ", " + << ntrip.mountpoint << ", " << ntrip.version << ", " + << ntrip.send_gga << " \x0D"; + send(ss.str()); + } + if (ntrip.tls) + { + std::stringstream ss; + ss << "sntt, " << ntrip.id << ", on, \"" << ntrip.fingerprint + << "\" \x0D"; + send(ss.str()); + } else + { + std::stringstream ss; + ss << "sntt, " << ntrip.id << ", off \x0D"; + send(ss.str()); + } } } - } - for (auto serial : settings_->rtk_settings.serial) - { - if (!serial.port.empty()) - { - if (serial.port.rfind("COM", 0) == 0) - send("scs, " + serial.port + ", baud" + - std::to_string(serial.baud_rate) + - ", bits8, No, bit1, none\x0D"); + for (auto ip_server : settings_->rtk.ip_server) + { + if (!ip_server.id.empty()) + // Since the Rx does not have internet (and you will not + // be able to share it via USB), we need to forward the + // corrections ourselves, though not on the same port. + { + { + std::stringstream ss; + // In case this IP server was used before, old + // configuration is lost of course. + ss << "siss, " << ip_server.id << ", " + << std::to_string(ip_server.port) << ", TCP2Way \x0D"; + send(ss.str()); + } + { + std::stringstream ss; + ss << "sdio, " << ip_server.id << ", " << ip_server.rtk_standard + << ", +SBF+NMEA \x0D"; + send(ss.str()); + } + if (ip_server.send_gga != "off") + { + std::string rate = ip_server.send_gga; + if (ip_server.send_gga == "auto") + rate = "sec1"; + std::stringstream ss; + ss << "sno, Stream" << std::to_string(stream) << ", " + << ip_server.id << ", GGA, " << rate << " \x0D"; + ++stream; + send(ss.str()); + } + } + } - std::stringstream ss; - ss << "sdio, " << serial.port << ", " << serial.rtk_standard - << ", +SBF+NMEA \x0D"; - send(ss.str()); - if (serial.send_gga != "off") + for (auto serial : settings_->rtk.serial) + { + if (!serial.port.empty()) { - std::string rate = serial.send_gga; - if (serial.send_gga == "auto") - rate = "sec1"; + if (serial.port.rfind("COM", 0) == 0) + send("scs, " + serial.port + ", baud" + + std::to_string(serial.baud_rate) + + ", bits8, No, bit1, none\x0D"); + std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << serial.port - << ", GGA, " << rate << " \x0D"; - ++stream; + ss << "sdio, " << serial.port << ", " << serial.rtk_standard + << ", +SBF+NMEA \x0D"; send(ss.str()); + if (serial.send_gga != "off") + { + std::string rate = serial.send_gga; + if (serial.send_gga == "auto") + rate = "sec1"; + std::stringstream ss; + ss << "sno, Stream" << std::to_string(stream) << ", " + << serial.port << ", GGA, " << rate << " \x0D"; + ++stream; + send(ss.str()); + } } } - } - // Setting multi antenna - if (settings_->multi_antenna) - { - send("sga, MultiAntenna \x0D"); - } else - { - send("sga, none \x0D"); - } - - // Setting the Attitude Determination - { - if (settings_->heading_offset >= HEADING_MIN && - settings_->heading_offset <= HEADING_MAX && - settings_->pitch_offset >= PITCH_MIN && - settings_->pitch_offset <= PITCH_MAX) + // Setting multi antenna + if (settings_->multi_antenna) { - std::stringstream ss; - ss << "sto, " - << string_utilities::trimDecimalPlaces(settings_->heading_offset) - << ", " - << string_utilities::trimDecimalPlaces(settings_->pitch_offset) - << " \x0D"; - send(ss.str()); + if (node_->hasHeading()) + send("sga, MultiAntenna \x0D"); + else + node_->log(log_level::WARN, + "Multi antenna requested but Rx does not support it."); } else { - node_->log(LogLevel::ERROR, - "Please specify a valid parameter for heading and pitch"); + send("sga, none \x0D"); } - } - // Setting the INS-related commands - if (settings_->septentrio_receiver_type == "ins") - { - // IMU orientation + // Setting the Attitude Determination { - std::stringstream ss; - if (settings_->theta_x >= ANGLE_MIN && settings_->theta_x <= ANGLE_MAX && - settings_->theta_y >= THETA_Y_MIN && - settings_->theta_y <= THETA_Y_MAX && - settings_->theta_z >= ANGLE_MIN && settings_->theta_z <= ANGLE_MAX) + if (settings_->heading_offset >= HEADING_MIN && + settings_->heading_offset <= HEADING_MAX && + settings_->pitch_offset >= PITCH_MIN && + settings_->pitch_offset <= PITCH_MAX) { - ss << " sio, " - << "manual" - << ", " << string_utilities::trimDecimalPlaces(settings_->theta_x) - << ", " << string_utilities::trimDecimalPlaces(settings_->theta_y) - << ", " << string_utilities::trimDecimalPlaces(settings_->theta_z) + std::stringstream ss; + ss << "sto, " + << string_utilities::trimDecimalPlaces(settings_->heading_offset) + << ", " + << string_utilities::trimDecimalPlaces(settings_->pitch_offset) << " \x0D"; send(ss.str()); } else { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for IMU orientation angles"); + node_->log(log_level::ERROR, + "Please specify a valid parameter for heading and pitch"); } } - // Setting the INS antenna lever arm offset + // Setting the INS-related commands + if (settings_->septentrio_receiver_type == "ins") { - if (settings_->ant_lever_x >= LEVER_ARM_MIN && - settings_->ant_lever_x <= LEVER_ARM_MAX && - settings_->ant_lever_y >= LEVER_ARM_MIN && - settings_->ant_lever_y <= LEVER_ARM_MAX && - settings_->ant_lever_z >= LEVER_ARM_MIN && - settings_->ant_lever_z <= LEVER_ARM_MAX) + // IMU orientation { std::stringstream ss; - ss << "sial, " - << string_utilities::trimDecimalPlaces(settings_->ant_lever_x) - << ", " - << string_utilities::trimDecimalPlaces(settings_->ant_lever_y) - << ", " - << string_utilities::trimDecimalPlaces(settings_->ant_lever_z) - << " \x0D"; - send(ss.str()); - } else - { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for x, y and z in the config file under ant_lever_arm"); + if (settings_->theta_x >= ANGLE_MIN && + settings_->theta_x <= ANGLE_MAX && + settings_->theta_y >= THETA_Y_MIN && + settings_->theta_y <= THETA_Y_MAX && + settings_->theta_z >= ANGLE_MIN && + settings_->theta_z <= ANGLE_MAX) + { + ss << " sio, " + << "manual" + << ", " + << string_utilities::trimDecimalPlaces(settings_->theta_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->theta_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->theta_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + log_level::ERROR, + "Please specify a correct value for IMU orientation angles"); + } } - } - // Setting the user defined point offset - { - if (settings_->poi_x >= LEVER_ARM_MIN && - settings_->poi_x <= LEVER_ARM_MAX && - settings_->poi_y >= LEVER_ARM_MIN && - settings_->poi_y <= LEVER_ARM_MAX && - settings_->poi_z >= LEVER_ARM_MIN && - settings_->poi_z <= LEVER_ARM_MAX) + // Setting the INS antenna lever arm offset + { + if (settings_->ant_lever_x >= LEVER_ARM_MIN && + settings_->ant_lever_x <= LEVER_ARM_MAX && + settings_->ant_lever_y >= LEVER_ARM_MIN && + settings_->ant_lever_y <= LEVER_ARM_MAX && + settings_->ant_lever_z >= LEVER_ARM_MIN && + settings_->ant_lever_z <= LEVER_ARM_MAX) + { + std::stringstream ss; + ss << "sial, " + << string_utilities::trimDecimalPlaces(settings_->ant_lever_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->ant_lever_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->ant_lever_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + log_level::ERROR, + "Please specify a correct value for x, y and z in the config file under ant_lever_arm"); + } + } + + // Setting the user defined point offset + { + if (settings_->poi_x >= LEVER_ARM_MIN && + settings_->poi_x <= LEVER_ARM_MAX && + settings_->poi_y >= LEVER_ARM_MIN && + settings_->poi_y <= LEVER_ARM_MAX && + settings_->poi_z >= LEVER_ARM_MIN && + settings_->poi_z <= LEVER_ARM_MAX) + { + std::stringstream ss; + ss << "sipl, POI1, " + << string_utilities::trimDecimalPlaces(settings_->poi_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->poi_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->poi_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + log_level::ERROR, + "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm"); + } + } + + // Setting the Velocity sensor lever arm offset + { + if (settings_->vsm_x >= LEVER_ARM_MIN && + settings_->vsm_x <= LEVER_ARM_MAX && + settings_->vsm_y >= LEVER_ARM_MIN && + settings_->vsm_y <= LEVER_ARM_MAX && + settings_->vsm_z >= LEVER_ARM_MIN && + settings_->vsm_z <= LEVER_ARM_MAX) + { + std::stringstream ss; + ss << "sivl, VSM1, " + << string_utilities::trimDecimalPlaces(settings_->vsm_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->vsm_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->vsm_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + log_level::ERROR, + "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vsm_lever_arm"); + } + } + + // Setting the INS Solution Reference Point: MainAnt or POI1 + // First disable any existing INS sub-block connection { std::stringstream ss; - ss << "sipl, POI1, " - << string_utilities::trimDecimalPlaces(settings_->poi_x) << ", " - << string_utilities::trimDecimalPlaces(settings_->poi_y) << ", " - << string_utilities::trimDecimalPlaces(settings_->poi_z) - << " \x0D"; + ss << "sinc, off, all, MainAnt \x0D"; send(ss.str()); - } else - { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm"); } - } - // Setting the Velocity sensor lever arm offset - { - if (settings_->vsm_x >= LEVER_ARM_MIN && - settings_->vsm_x <= LEVER_ARM_MAX && - settings_->vsm_y >= LEVER_ARM_MIN && - settings_->vsm_y <= LEVER_ARM_MAX && - settings_->vsm_z >= LEVER_ARM_MIN && - settings_->vsm_z <= LEVER_ARM_MAX) + // INS solution reference point { std::stringstream ss; - ss << "sivl, VSM1, " - << string_utilities::trimDecimalPlaces(settings_->vsm_x) << ", " - << string_utilities::trimDecimalPlaces(settings_->vsm_y) << ", " - << string_utilities::trimDecimalPlaces(settings_->vsm_z) - << " \x0D"; - send(ss.str()); - } else + if (settings_->ins_use_poi) + { + ss << "sinc, on, all, " + << "POI1" + << " \x0D"; + send(ss.str()); + } else + { + ss << "sinc, on, all, " + << "MainAnt" + << " \x0D"; + send(ss.str()); + } + } + + // Setting the INS heading { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vsm_lever_arm"); + std::stringstream ss; + if (settings_->ins_initial_heading == "auto") + { + ss << "siih, " << settings_->ins_initial_heading << " \x0D"; + send(ss.str()); + } else if (settings_->ins_initial_heading == "stored") + { + ss << "siih, " << settings_->ins_initial_heading << " \x0D"; + send(ss.str()); + } else + { + node_->log(log_level::ERROR, + "Invalid mode specified for ins_initial_heading."); + } + } + + // Setting the INS navigation filter + { + if (settings_->att_std_dev >= ATTSTD_DEV_MIN && + settings_->att_std_dev <= ATTSTD_DEV_MAX && + settings_->pos_std_dev >= POSSTD_DEV_MIN && + settings_->pos_std_dev <= POSSTD_DEV_MAX) + { + std::stringstream ss; + ss << "sism, " + << string_utilities::trimDecimalPlaces(settings_->att_std_dev) + << ", " + << string_utilities::trimDecimalPlaces(settings_->pos_std_dev) + << " \x0D"; + send(ss.str()); + } else + { + node_->log(log_level::ERROR, + "Please specify a valid AttStsDev and PosStdDev"); + } } } - // Setting the INS Solution Reference Point: MainAnt or POI1 - // First disable any existing INS sub-block connection + // OSNMA + if (settings_->osnma.mode == "loose" || settings_->osnma.mode == "strict") { std::stringstream ss; - ss << "sinc, off, all, MainAnt \x0D"; + ss << "sou, " << settings_->osnma.mode << " \x0D"; send(ss.str()); - } - // INS solution reference point - { - std::stringstream ss; - if (settings_->ins_use_poi) + if (!settings_->osnma.ntp_server.empty()) { - ss << "sinc, on, all, " - << "POI1" - << " \x0D"; + std::stringstream ss; + ss << "snc, on, " << settings_->osnma.ntp_server << " \x0D"; send(ss.str()); } else { - ss << "sinc, on, all, " - << "MainAnt" - << " \x0D"; - send(ss.str()); + if (settings_->osnma.mode == "strict") + node_->log( + log_level::ERROR, + "OSNMA mode set to strict but no NTP server provided. In Strict mode an NTP server is mandatory!"); } } - // Setting the INS heading + // Setting up SBF blocks with rx_period_rest { - std::stringstream ss; - if (settings_->ins_initial_heading == "auto") - { - ss << "siih, " << settings_->ins_initial_heading << " \x0D"; - send(ss.str()); - } else if (settings_->ins_initial_heading == "stored") + std::stringstream blocks; + if (settings_->septentrio_receiver_type == "ins") { - ss << "siih, " << settings_->ins_initial_heading << " \x0D"; - send(ss.str()); - } else + if (settings_->publish_imusetup) + { + blocks << " +IMUSetup"; + } + if (settings_->publish_velsensorsetup) + { + blocks << " +VelSensorSetup"; + } + } + if (settings_->publish_diagnostics) { - node_->log(LogLevel::ERROR, - "Invalid mode specified for ins_initial_heading."); + blocks << " +ReceiverStatus +QualityInd"; } - } - - // Setting the INS navigation filter - { - if (settings_->att_std_dev >= ATTSTD_DEV_MIN && - settings_->att_std_dev <= ATTSTD_DEV_MAX && - settings_->pos_std_dev >= POSSTD_DEV_MIN && - settings_->pos_std_dev <= POSSTD_DEV_MAX) + if (settings_->publish_aimplusstatus) { - std::stringstream ss; - ss << "sism, " - << string_utilities::trimDecimalPlaces(settings_->att_std_dev) - << ", " - << string_utilities::trimDecimalPlaces(settings_->pos_std_dev) - << " \x0D"; - send(ss.str()); - } else + blocks << " +RFStatus"; + } + if (settings_->publish_galauthstatus || + (settings_->osnma.mode == "loose") || + (settings_->osnma.mode == "strict")) { - node_->log(LogLevel::ERROR, - "Please specify a valid AttStsDev and PosStdDev"); + blocks << " +GALAuthStatus"; } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (!settings_->ins_vsm_ip_server_id.empty()) - { - send("siss, " + settings_->ins_vsm_ip_server_id + ", " + - std::to_string(settings_->ins_vsm_ip_server_port) + - ", TCP2Way \x0D"); - send("sdio, IPS2, NMEA, none\x0D"); - } - if (!settings_->ins_vsm_serial_port.empty()) - { - if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) - send("scs, " + settings_->ins_vsm_serial_port + ", baud" + - std::to_string(settings_->ins_vsm_serial_baud_rate) + - ", bits8, No, bit1, none\x0D"); - send("sdio, " + settings_->ins_vsm_serial_port + ", NMEA\x0D"); - } - if ((settings_->ins_vsm_ros_source == "odometry") || - (settings_->ins_vsm_ros_source == "twist")) - { - std::string s; - s = "sdio, " + mainPort_ + ", NMEA, +NMEA +SBF\x0D"; - send(s); - nmeaActivated_ = true; - } - } - node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); -} - -//! initializeSerial is not self-contained: The for loop in Callbackhandlers' handle -//! method would never open a specific handler unless the handler is added -//! (=inserted) to the C++ map via this function. This way, the specific handler can -//! be called, in which in turn RxMessage's read() method is called, which publishes -//! the ROS message. -void io_comm_rx::Comm_IO::defineMessages() -{ - node_->log(LogLevel::DEBUG, "Called defineMessages() method"); - - if (settings_->use_gnss_time || settings_->publish_gpst) - { - handlers_.callbackmap_ = handlers_.insert("5914"); - } - if (settings_->publish_gpgga) - { - handlers_.callbackmap_ = handlers_.insert("$GPGGA"); - } - if (settings_->publish_gprmc) - { - handlers_.callbackmap_ = handlers_.insert("$GPRMC"); - } - if (settings_->publish_gpgsa) - { - handlers_.callbackmap_ = handlers_.insert("$GPGSA"); - } - if (settings_->publish_gpgsv) - { - handlers_.callbackmap_ = handlers_.insert("$GPGSV"); - handlers_.callbackmap_ = handlers_.insert("$GLGSV"); - handlers_.callbackmap_ = handlers_.insert("$GAGSV"); - handlers_.callbackmap_ = handlers_.insert("$GBGSV"); - } - if (settings_->publish_pvtcartesian) - { - handlers_.callbackmap_ = handlers_.insert("4006"); - } - if (settings_->publish_pvtgeodetic || settings_->publish_twist || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("4007"); - } - if (settings_->publish_basevectorcart) - { - handlers_.callbackmap_ = handlers_.insert("4043"); - } - if (settings_->publish_basevectorgeod) - { - handlers_.callbackmap_ = handlers_.insert("4028"); - } - if (settings_->publish_poscovcartesian) - { - handlers_.callbackmap_ = handlers_.insert("5905"); - } - if (settings_->publish_poscovgeodetic || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5906"); - } - if (settings_->publish_velcovgeodetic || settings_->publish_twist || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5908"); - } - if (settings_->publish_atteuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5938"); - } - if (settings_->publish_attcoveuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5939"); - } - if (settings_->publish_measepoch || settings_->publish_gpsfix) - { - handlers_.callbackmap_ = - handlers_.insert("4027"); // MeasEpoch block - } + blocks << " +ReceiverSetup"; - // INS-related SBF blocks - if (settings_->publish_insnavcart) - { - handlers_.callbackmap_ = handlers_.insert("4225"); - } - if (settings_->publish_insnavgeod || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_imu && (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_localization && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_twist && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_tf && (settings_->septentrio_receiver_type == "ins"))) - { - handlers_.callbackmap_ = handlers_.insert("4226"); - } - if (settings_->publish_imusetup) - { - handlers_.callbackmap_ = handlers_.insert("4224"); - } - if (settings_->publish_extsensormeas || settings_->publish_imu) - { - handlers_.callbackmap_ = handlers_.insert("4050"); - } - if (settings_->publish_exteventinsnavgeod) - { - handlers_.callbackmap_ = handlers_.insert("4230"); - } - if (settings_->publish_velsensorsetup) - { - handlers_.callbackmap_ = handlers_.insert("4244"); - } - if (settings_->publish_exteventinsnavcart) - { - handlers_.callbackmap_ = handlers_.insert("4229"); - } - if (settings_->publish_gpst) - { - handlers_.callbackmap_ = handlers_.insert("GPST"); - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_navsatfix) - { - handlers_.callbackmap_ = handlers_.insert("NavSatFix"); - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_navsatfix) - { - handlers_.callbackmap_ = handlers_.insert("INSNavSatFix"); - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpsfix) - { - handlers_.callbackmap_ = handlers_.insert("GPSFix"); - // The following blocks are never published, yet are needed for the - // construction of the GPSFix message, hence we have empty callbacks. - handlers_.callbackmap_ = - handlers_.insert("4013"); // ChannelStatus block - handlers_.callbackmap_ = handlers_.insert("4001"); // DOP block - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpsfix) - { - handlers_.callbackmap_ = handlers_.insert("INSGPSFix"); - handlers_.callbackmap_ = - handlers_.insert("4013"); // ChannelStatus block - handlers_.callbackmap_ = handlers_.insert("4001"); // DOP block - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_pose) - { - handlers_.callbackmap_ = handlers_.insert( - "PoseWithCovarianceStamped"); - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_pose) - { - handlers_.callbackmap_ = handlers_.insert( - "INSPoseWithCovarianceStamped"); - } - } - if (settings_->publish_diagnostics) - { - handlers_.callbackmap_ = - handlers_.insert("DiagnosticArray"); - handlers_.callbackmap_ = - handlers_.insert("4014"); // ReceiverStatus block - handlers_.callbackmap_ = - handlers_.insert("4082"); // QualityInd block - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_localization || settings_->publish_tf) - { - handlers_.callbackmap_ = - handlers_.insert("Localization"); + std::stringstream ss; + ss << "sso, Stream" << std::to_string(stream) << ", " << streamPort_ + << "," << blocks.str() << ", " << rest_interval << "\x0D"; + send(ss.str()); + ++stream; } - } - handlers_.callbackmap_ = - handlers_.insert("5902"); // ReceiverSetup block - // so on and so forth... - node_->log(LogLevel::DEBUG, "Leaving defineMessages() method"); -} - -void io_comm_rx::Comm_IO::send(const std::string& cmd) -{ - // It is imperative to hold a lock on the mutex "g_response_mutex" while - // modifying the variable "g_response_received". - boost::mutex::scoped_lock lock(g_response_mutex); - // Determine byte size of cmd and hand over to send() method of manager_ - manager_.get()->send(cmd); - g_response_condition.wait(lock, []() { return g_response_received; }); - g_response_received = false; -} - -void io_comm_rx::Comm_IO::sendVelocity(const std::string& velNmea) -{ - if (nmeaActivated_) - manager_.get()->send(velNmea); -} - -bool io_comm_rx::Comm_IO::initializeTCP(std::string host, std::string port) -{ - node_->log(LogLevel::DEBUG, "Calling initializeTCP() method.."); - host_ = host; - port_ = port; - // The io_context, of which io_service is a typedef of; it represents your - // program's link to the operating system's I/O services. - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::asio::ip::tcp::resolver::iterator endpoint; - - try - { - boost::asio::ip::tcp::resolver resolver(*io_service); - // Note that tcp::resolver::query takes the host to resolve or the IP as the - // first parameter and the name of the service (as defined e.g. in - // /etc/services on Unix hosts) as second parameter. For the latter, one can - // also use a numeric service identifier (aka port number). In any case, it - // returns a list of possible endpoints, as there might be several entries - // for a single host. - endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query( - host, port)); // Resolves query object.. - } catch (std::runtime_error& e) - { - throw std::runtime_error("Could not resolve " + host + " on port " + port + - ": " + e.what()); - return false; - } - boost::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); + // send command to trigger emission of receiver setup + send("sop, \"\", \"\" \x0D"); - try - { - // The list of endpoints obtained above may contain both IPv4 and IPv6 - // endpoints, so we need to try each of them until we find one that works. - // This keeps the client program independent of a specific IP version. The - // boost::asio::connect() function does this for us automatically. - socket->connect(*endpoint); - socket->set_option(boost::asio::ip::tcp::no_delay(true)); - } catch (std::runtime_error& e) - { - throw std::runtime_error("Could not connect to " + endpoint->host_name() + - ": " + endpoint->service_name() + ": " + e.what()); - return false; - } + // Setting up NMEA streams + { + if (settings_->septentrio_receiver_type == "ins") + send("snti, auto\x0D"); + else + send("snti, GP\x0D"); - node_->log(LogLevel::INFO, "Connected to " + endpoint->host_name() + ":" + - endpoint->service_name() + "."); + std::stringstream blocks; + if (settings_->publish_gpgga) + { + blocks << " +GGA"; + } + if (settings_->publish_gprmc) + { + blocks << " +RMC"; + } + if (settings_->publish_gpgsa) + { + blocks << " +GSA"; + } + if (settings_->publish_gpgsv) + { + blocks << " +GSV"; + } - if (manager_) - { - node_->log( - LogLevel::ERROR, - "You have called the InitializeTCP() method though an AsyncManager object is already available! Start all anew.."); - return false; - } - setManager(boost::shared_ptr( - new AsyncManager(node_, socket, io_service))); - node_->log(LogLevel::DEBUG, "Leaving initializeTCP() method.."); - return true; -} - -void io_comm_rx::Comm_IO::initializeSBFFileReading(std::string file_name) -{ - node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); - std::size_t buffer_size = 8192; - uint8_t* to_be_parsed; - to_be_parsed = new uint8_t[buffer_size]; - std::ifstream bin_file(file_name, std::ios::binary); - std::vector vec_buf; - if (bin_file.good()) - { - /* Reads binary data using streambuffer iterators. - Copies all SBF file content into bin_data. */ - std::vector v_buf((std::istreambuf_iterator(bin_file)), - (std::istreambuf_iterator())); - vec_buf = v_buf; - bin_file.close(); - } else - { - throw std::runtime_error("I could not find your file. Or it is corrupted."); - } - // The spec now guarantees that vectors store their elements contiguously. - to_be_parsed = vec_buf.data(); - std::stringstream ss; - ss << "Opened and copied over from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); + std::stringstream ss; + ss << "sno, Stream" << std::to_string(stream) << ", " << streamPort_ + << "," << blocks.str() << ", " << pvt_interval << "\x0D"; + send(ss.str()); + ++stream; + } - while (!stopping_) // Loop will stop if we are done reading the SBF file - { - try + // Setting up SBF blocks with rx_period_pvt { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - buffer_size); - handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); - } catch (std::size_t& parsing_failed_here) - { - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) + std::stringstream blocks; + if (settings_->use_gnss_time || settings_->publish_gpst) { - break; + blocks << " +ReceiverTime"; } - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; - } - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; + if (settings_->publish_pvtcartesian) + { + blocks << " +PVTCartesian"; + } + if (settings_->publish_pvtgeodetic || settings_->publish_twist || + (settings_->publish_gpst && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_navsatfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss")) || + settings_->latency_compensation) + { + blocks << " +PVTGeodetic"; + } + if (settings_->publish_basevectorcart) + { + blocks << " +BaseVectorCart"; + } + if (settings_->publish_basevectorgeod) + { + blocks << " +BaseVectorGeod"; + } + if (settings_->publish_poscovcartesian) + { + blocks << " +PosCovCartesian"; + } + if (settings_->publish_poscovgeodetic || + (settings_->publish_navsatfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +PosCovGeodetic"; + } + if (settings_->publish_velcovcartesian) + { + blocks << " +VelCovCartesian"; + } + if (settings_->publish_velcovgeodetic || settings_->publish_twist || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +VelCovGeodetic"; + } + if (settings_->publish_atteuler || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +AttEuler"; + } + if (settings_->publish_attcoveuler || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +AttCovEuler"; + } + if (settings_->publish_measepoch || settings_->publish_gpsfix) + { + blocks << " +MeasEpoch"; + } + if (settings_->publish_gpsfix) + { + blocks << " +ChannelStatus +DOP"; + } + // Setting SBF output of Rx depending on the receiver type + // If INS then... + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->publish_insnavcart || + settings_->publish_localization_ecef || + settings_->publish_tf_ecef) + { + blocks << " +INSNavCart"; + } + if (settings_->publish_insnavgeod || settings_->publish_navsatfix || + settings_->publish_gpsfix || settings_->publish_pose || + settings_->publish_imu || settings_->publish_localization || + settings_->publish_tf || settings_->publish_twist || + settings_->publish_localization_ecef || + settings_->publish_tf_ecef || settings_->publish_gpst) + { + blocks << " +INSNavGeod"; + } + if (settings_->publish_exteventinsnavgeod) + { + blocks << " +ExtEventINSNavGeod"; + } + if (settings_->publish_exteventinsnavcart) + { + blocks << " +ExtEventINSNavCart"; + } + if (settings_->publish_extsensormeas || settings_->publish_imu) + { + blocks << " +ExtSensorMeas"; + } + } + std::stringstream ss; + ss << "sso, Stream" << std::to_string(stream) << ", " << streamPort_ + << "," << blocks.str() << ", " << pvt_interval << "\x0D"; + send(ss.str()); + ++stream; } - to_be_parsed = to_be_parsed + buffer_size; - } - node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method.."); -} - -void io_comm_rx::Comm_IO::initializePCAPFileReading(std::string file_name) -{ - node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() method.."); - pcapReader::buffer_t vec_buf; - pcapReader::PcapDevice device(node_, vec_buf); - - if (!device.connect(file_name.c_str())) - { - node_->log(LogLevel::ERROR, "Unable to find file or either it is corrupted"); - return; - } - node_->log(LogLevel::INFO, "Reading ..."); - while (device.isConnected() && device.read() == pcapReader::READ_SUCCESS) - ; - device.disconnect(); - - std::size_t buffer_size = pcapReader::PcapDevice::BUFFSIZE; - uint8_t* to_be_parsed = new uint8_t[buffer_size]; - to_be_parsed = vec_buf.data(); - - while (!stopping_) // Loop will stop if we are done reading the SBF file - { - try - { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - buffer_size); - handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); - } catch (std::size_t& parsing_failed_here) + if (settings_->septentrio_receiver_type == "ins") { - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) + if (!settings_->ins_vsm_ip_server_id.empty()) { - break; + send("siss, " + settings_->ins_vsm_ip_server_id + ", " + + std::to_string(settings_->ins_vsm_ip_server_port) + + ", TCP2Way \x0D"); + send("sdio, IPS2, NMEA, none\x0D"); + } + if (!settings_->ins_vsm_serial_port.empty()) + { + if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) + send("scs, " + settings_->ins_vsm_serial_port + ", baud" + + std::to_string(settings_->ins_vsm_serial_baud_rate) + + ", bits8, No, bit1, none\x0D"); + send("sdio, " + settings_->ins_vsm_serial_port + ", NMEA\x0D"); + } + if ((settings_->ins_vsm_ros_source == "odometry") || + (settings_->ins_vsm_ros_source == "twist")) + { + std::string s; + s = "sdio, " + mainConnectionPort_ + ", NMEA, +NMEA +SBF\x0D"; + send(s); + nmeaActivated_ = true; } - if (!parsing_failed_here) - parsing_failed_here = 1; - - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; - } - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; } - to_be_parsed = to_be_parsed + buffer_size; - } - node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method.."); -} - -bool io_comm_rx::Comm_IO::initializeSerial(std::string port, uint32_t baudrate, - std::string flowcontrol) -{ - node_->log(LogLevel::DEBUG, "Calling initializeSerial() method.."); - serial_port_ = port; - baudrate_ = baudrate; - // The io_context, of which io_service is a typedef of; it represents your - // program's link to the operating system's I/O services. - boost::shared_ptr io_service( - new boost::asio::io_service); - // To perform I/O operations the program needs an I/O object, here "serial". - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); - - // We attempt the opening of the serial port.. - try - { - serial->open(serial_port_); - } catch (std::runtime_error& e) - { - // and return an error message in case it fails. - throw std::runtime_error("Could not open serial port : " + serial_port_ + - ": " + e.what()); - return false; + + node_->log(log_level::DEBUG, "Leaving configureRx() method"); } - node_->log(LogLevel::INFO, "Opened serial port " + serial_port_); - node_->log(LogLevel::DEBUG, - "Our boost version is " + std::to_string(BOOST_VERSION) + "."); - if (BOOST_VERSION < 106600) // E.g. for ROS melodic (i.e. Ubuntu 18.04), the - // version is 106501, standing for 1.65.1. + void CommunicationCore::sendVelocity(const std::string& velNmea) { - // Workaround to set some options for the port manually, - // cf. https://github.com/mavlink/mavros/pull/971/files - // This function native_handle() may be used to obtain - // the underlying representation of the serial port. - // Conversion from type native_handle_type to int is done implicitly. - int fd = serial->native_handle(); - termios tio; - // Get terminal attribute, follows the syntax - // int tcgetattr(int fd, struct termios *termios_p); - tcgetattr(fd, &tio); - - // Hardware flow control settings_->. - if (flowcontrol == "RTS|CTS") - { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag |= CRTSCTS; - } else - { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag &= ~CRTSCTS; - } - // Setting serial port to "raw" mode to prevent EOF exit.. - cfmakeraw(&tio); - - // Commit settings, syntax is - // int tcsetattr(int fd, int optional_actions, const struct termios - // *termios_p); - tcsetattr(fd, TCSANOW, &tio); - - // Set low latency - struct serial_struct serialInfo; - - ioctl(fd, TIOCGSERIAL, &serialInfo); - serialInfo.flags |= ASYNC_LOW_LATENCY; - ioctl(fd, TIOCSSERIAL, &serialInfo); + if (nmeaActivated_) + manager_.get()->send(velNmea); } - // Set the I/O manager - if (manager_) + std::string CommunicationCore::resetMainConnection() { - node_->log( - LogLevel::ERROR, - "You have called the initializeSerial() method though an AsyncManager object is already available! Start all anew.."); - return false; + // Escape sequence (escape from correction mode), ensuring that we + // can send our real commands afterwards... has to be sent multiple times. + std::string cmd("\x0DSSSSSSSSSS\x0D\x0D"); + telegramHandler_.resetWaitforMainCd(); + manager_.get()->send(cmd); + std::ignore = telegramHandler_.getMainCd(); + telegramHandler_.resetWaitforMainCd(); + manager_.get()->send(cmd); + std::ignore = telegramHandler_.getMainCd(); + telegramHandler_.resetWaitforMainCd(); + manager_.get()->send(cmd); + return telegramHandler_.getMainCd(); } - node_->log(LogLevel::DEBUG, "Creating new Async-Manager object.."); - setManager(boost::shared_ptr( - new AsyncManager(node_, serial, io_service))); - - // Setting the baudrate, incrementally.. - node_->log(LogLevel::DEBUG, - "Gradually increasing the baudrate to the desired value..."); - boost::asio::serial_port_base::baud_rate current_baudrate; - node_->log(LogLevel::DEBUG, "Initiated current_baudrate object..."); - try - { - serial->get_option( - current_baudrate); // Note that this sets current_baudrate.value() often - // to 115200, since by default, all Rx COM ports, - // at least for mosaic Rxs, are set to a baudrate of 115200 baud, using 8 - // data-bits, no parity and 1 stop-bit. - } catch (boost::system::system_error& e) - { - node_->log(LogLevel::ERROR, - "get_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); - /* - boost::system::error_code e_loop; - do // Caution: Might cause infinite loop.. - { - serial->get_option(current_baudrate, e_loop); - } while(e_loop); - */ - return false; - } - // Gradually increase the baudrate to the desired value - // The desired baudrate can be lower or larger than the - // current baudrate; the for loop takes care of both scenarios. - node_->log(LogLevel::DEBUG, - "Current baudrate is " + std::to_string(current_baudrate.value())); - for (uint8_t i = 0; i < sizeof(BAUDRATES) / sizeof(BAUDRATES[0]); i++) + void CommunicationCore::processTelegrams() { - if (current_baudrate.value() == baudrate_) - { - break; // Break if the desired baudrate has been reached. - } - if (current_baudrate.value() >= BAUDRATES[i] && baudrate_ > BAUDRATES[i]) - { - continue; - } - // Increment until Baudrate[i] matches current_baudrate. - try - { - serial->set_option( - boost::asio::serial_port_base::baud_rate(BAUDRATES[i])); - } catch (boost::system::system_error& e) + while (running_) { + std::shared_ptr telegram; + telegramQueue_.pop(telegram); - node_->log(LogLevel::ERROR, - "set_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); - return false; + if (telegram->type != telegram_type::EMPTY) + telegramHandler_.handleTelegram(telegram); } - usleep(SET_BAUDRATE_SLEEP_); - // boost::this_thread::sleep(boost::posix_time::milliseconds(SET_BAUDRATE_SLEEP_*1000)); - // Boost's sleep would yield an error message with exit code -7 the second - // time it is called, hence we use sleep() or usleep(). - try - { - serial->get_option(current_baudrate); - } catch (boost::system::system_error& e) - { + } - node_->log(LogLevel::ERROR, - "get_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); - /* - boost::system::error_code e_loop; - do // Caution: Might cause infinite loop.. - { - serial->get_option(current_baudrate, e_loop); - } while(e_loop); - */ - return false; - } - node_->log(LogLevel::DEBUG, "Set ASIO baudrate to " + - std::to_string(current_baudrate.value())); + void CommunicationCore::send(const std::string& cmd) + { + manager_.get()->send(cmd); + telegramHandler_.waitForResponse(); } - node_->log(LogLevel::INFO, "Set ASIO baudrate to " + - std::to_string(current_baudrate.value()) + - ", leaving InitializeSerial() method"); - return true; -} - -void io_comm_rx::Comm_IO::setManager(const boost::shared_ptr& manager) -{ - namespace bp = boost::placeholders; - - node_->log(LogLevel::DEBUG, "Called setManager() method"); - if (manager_) - return; - manager_ = manager; - manager_->setCallback(boost::bind(&CallbackHandlers::readCallback, &handlers_, - bp::_1, bp::_2, bp::_3)); - node_->log(LogLevel::DEBUG, "Leaving setManager() method"); -} \ No newline at end of file + +} // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp new file mode 100644 index 00000000..ab5136b1 --- /dev/null +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -0,0 +1,2903 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include +#include +#include + +/** + * The position_covariance array is populated in row-major order, where the basis of + * the correspond matrix is (E, N, U, Roll, Pitch, Heading). Important: The Euler + * angles (Roll, Pitch, Heading) are with respect to a vehicle-fixed (e.g. for + * mosaic-x5 in moving base mode via the command setAntennaLocation, ...) !local! NED + * frame or ENU frame if use_ros_axis_directions is set true Thus the orientation + * is !not! given with respect to the same frame as the position is given in. The + * cross-covariances are hence (apart from the fact that e.g. mosaic receivers do + * not calculate these quantities) set to zero. The position and the partial + * (with 2 antennas) or full (for INS receivers) orientation have covariance matrices + * available e.g. in the PosCovGeodetic or AttCovEuler blocks, yet those are separate + * computations. + */ + +using parsing_utilities::convertEulerToQuaternionMsg; +using parsing_utilities::deg2rad; +using parsing_utilities::deg2radSq; +using parsing_utilities::rad2deg; +using parsing_utilities::square; + +namespace io { + void MessageHandler::assemblePoseWithCovarianceStamped() + { + if (!settings_->publish_pose) + return; + + static auto last_ins_tow = last_insnavgeod_.block_header.tow; + + PoseWithCovarianceStampedMsg msg; + if (settings_->septentrio_receiver_type == "ins") + { + if (!validValue(last_insnavgeod_.block_header.tow) || + (last_insnavgeod_.block_header.tow == last_ins_tow)) + return; + last_ins_tow = last_insnavgeod_.block_header.tow; + + msg.header = last_insnavgeod_.header; + + msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude); + msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude); + msg.pose.pose.position.z = last_insnavgeod_.height; + + // Filling in the pose data + if ((last_insnavgeod_.sb_list & 1) != 0) + { + // Pos autocov + msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev); + msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev); + msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev); + } else + { + msg.pose.covariance[0] = -1.0; + msg.pose.covariance[7] = -1.0; + msg.pose.covariance[14] = -1.0; + } + if ((last_insnavgeod_.sb_list & 2) != 0) + { + double yaw = 0.0; + if (validValue(last_insnavgeod_.heading)) + yaw = last_insnavgeod_.heading; + double pitch = 0.0; + if (validValue(last_insnavgeod_.pitch)) + pitch = last_insnavgeod_.pitch; + double roll = 0.0; + if (validValue(last_insnavgeod_.roll)) + roll = last_insnavgeod_.roll; + // Attitude + msg.pose.pose.orientation = convertEulerToQuaternionMsg( + deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); + } else + { + msg.pose.pose.orientation.w = + std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = + std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = + std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = + std::numeric_limits::quiet_NaN(); + } + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocov + if (validValue(last_insnavgeod_.roll_std_dev)) + msg.pose.covariance[21] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + msg.pose.covariance[21] = -1.0; + if (validValue(last_insnavgeod_.pitch_std_dev)) + msg.pose.covariance[28] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + msg.pose.covariance[28] = -1.0; + if (validValue(last_insnavgeod_.heading_std_dev)) + msg.pose.covariance[35] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + msg.pose.covariance[35] = -1.0; + } else + { + msg.pose.covariance[21] = -1.0; + msg.pose.covariance[28] = -1.0; + msg.pose.covariance[35] = -1.0; + } + if ((last_insnavgeod_.sb_list & 32) != 0) + { + // Pos cov + msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; + msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; + msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; + } + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude cov + msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + + msg.pose.covariance[29] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + } + } else + { + if ((!validValue(last_pvtgeodetic_.block_header.tow)) || + (last_pvtgeodetic_.block_header.tow != + last_atteuler_.block_header.tow) || + (last_pvtgeodetic_.block_header.tow != + last_poscovgeodetic_.block_header.tow) || + (last_pvtgeodetic_.block_header.tow != + last_attcoveuler_.block_header.tow)) + return; + + msg.header = last_pvtgeodetic_.header; + + // Filling in the pose data + double yaw = 0.0; + if (validValue(last_atteuler_.heading)) + yaw = last_atteuler_.heading; + double pitch = 0.0; + if (validValue(last_atteuler_.pitch)) + pitch = last_atteuler_.pitch; + double roll = 0.0; + if (validValue(last_atteuler_.roll)) + roll = last_atteuler_.roll; + msg.pose.pose.orientation = convertEulerToQuaternionMsg( + deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); + msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); + msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); + msg.pose.pose.position.z = last_pvtgeodetic_.height; + // Filling in the covariance data in row-major order + msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon; + msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat; + msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt; + msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; + msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; + msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; + msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll); + msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll); + msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch); + msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch); + msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll); + msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); + msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); + } + publish("pose", msg); + }; + + void MessageHandler::assembleDiagnosticArray( + const std::shared_ptr& telegram) + { + if (!settings_->publish_diagnostics) + return; + + DiagnosticArrayMsg msg; + if (!validValue(last_receiverstatus_.block_header.tow) || + (last_receiverstatus_.block_header.tow != + last_qualityind_.block_header.tow)) + return; + std::string serialnumber; + if (validValue(last_receiversetup_.block_header.tow)) + serialnumber = last_receiversetup_.rx_serial_number; + else + serialnumber = "unknown"; + DiagnosticStatusMsg gnss_status; + // Constructing the "level of operation" field + uint16_t indicators_type_mask = static_cast(255); + uint16_t indicators_value_mask = static_cast(3840); + uint16_t qualityind_pos; + for (uint16_t i = static_cast(0); + i < last_qualityind_.indicators.size(); ++i) + { + if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(0)) + { + qualityind_pos = i; + if (((last_qualityind_.indicators[i] & indicators_value_mask) >> + 8) == static_cast(0)) + { + gnss_status.level = DiagnosticStatusMsg::STALE; + } else if (((last_qualityind_.indicators[i] & + indicators_value_mask) >> + 8) == static_cast(1) || + ((last_qualityind_.indicators[i] & + indicators_value_mask) >> + 8) == static_cast(2)) + { + gnss_status.level = DiagnosticStatusMsg::WARN; + } else + { + gnss_status.level = DiagnosticStatusMsg::OK; + } + break; + } + } + // If the ReceiverStatus's RxError field is not 0, then at least one error + // has been detected. + if (last_receiverstatus_.rx_error != static_cast(0)) + { + gnss_status.level = DiagnosticStatusMsg::ERROR; + } + // Creating an array of values associated with the GNSS status + gnss_status.values.resize(static_cast(last_qualityind_.n - 1)); + for (uint16_t i = static_cast(0); + i != static_cast(last_qualityind_.n); ++i) + { + if (i == qualityind_pos) + { + continue; + } + if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(1)) + { + gnss_status.values[i].key = "GNSS Signals, Main Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(2)) + { + gnss_status.values[i].key = "GNSS Signals, Aux1 Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(11)) + { + gnss_status.values[i].key = "RF Power, Main Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(12)) + { + gnss_status.values[i].key = "RF Power, Aux1 Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(21)) + { + gnss_status.values[i].key = "CPU Headroom"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(25)) + { + gnss_status.values[i].key = "OCXO Stability"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(30)) + { + gnss_status.values[i].key = "Base Station Measurements"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else + { + assert((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(31)); + gnss_status.values[i].key = "RTK Post-Processing"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } + } + gnss_status.hardware_id = serialnumber; + gnss_status.name = "septentrio_driver: Quality indicators"; + gnss_status.message = + "GNSS quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; + msg.status.push_back(gnss_status); + std::string frame_id; + if (settings_->septentrio_receiver_type == "gnss") + { + frame_id = settings_->frame_id; + } + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + } + assembleHeader(frame_id, telegram, msg); + publish("/diagnostics", msg); + }; + + void MessageHandler::assembleOsnmaDiagnosticArray() + { + DiagnosticArrayMsg msg; + DiagnosticStatusMsg diagOsnma; + + diagOsnma.hardware_id = last_receiversetup_.rx_serial_number; + diagOsnma.name = "septentrio_driver: OSNMA"; + diagOsnma.message = "Current status of the OSNMA authentication"; + + diagOsnma.values.resize(6); + diagOsnma.values[0].key = "status"; + switch (last_gal_auth_status_.osnma_status & 7) + { + case 0: + { + diagOsnma.values[0].value = "Disabled"; + break; + } + case 1: + { + uint16_t percent = (last_gal_auth_status_.osnma_status >> 3) & 127; + diagOsnma.values[0].value = + "Initializing " + std::to_string(percent) + " %"; + break; + } + case 2: + { + diagOsnma.values[0].value = "Waiting on NTP"; + break; + } + case 3: + { + diagOsnma.values[0].value = "Init failed - inconsistent time"; + break; + } + case 4: + { + diagOsnma.values[0].value = "Init failed - KROOT signature invalid"; + break; + } + case 5: + { + diagOsnma.values[0].value = "Init failed - invalid param received"; + break; + } + case 6: + { + diagOsnma.values[0].value = "Authenticating"; + break; + } + + default: + break; + } + + diagOsnma.values[1].key = "trusted_time_delta"; + if (validValue(last_gal_auth_status_.trusted_time_delta)) + diagOsnma.values[1].value = + std::to_string(last_gal_auth_status_.trusted_time_delta); + else + diagOsnma.values[1].value = "N/A"; + + std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask; + std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask; + uint8_t gal_authentic = (gal_auth & gal_active).count(); + uint8_t gal_spoofed = (~gal_auth & gal_active).count(); + diagOsnma.values[2].key = "Galileo authentic"; + diagOsnma.values[2].value = std::to_string(gal_authentic); + diagOsnma.values[3].key = "Galileo spoofed"; + diagOsnma.values[3].value = std::to_string(gal_spoofed); + + std::bitset<64> gps_active = last_gal_auth_status_.gps_active_mask; + std::bitset<64> gps_auth = last_gal_auth_status_.gps_authentic_mask; + uint8_t gps_authentic = (gps_auth & gps_active).count(); + uint8_t gps_spoofed = (~gps_auth & gps_active).count(); + diagOsnma.values[4].key = "GPS authentic"; + diagOsnma.values[4].value = std::to_string(gps_authentic); + diagOsnma.values[5].key = "GPS spoofed"; + diagOsnma.values[5].value = std::to_string(gps_spoofed); + + if ((gal_spoofed + gps_spoofed) == 0) + diagOsnma.level = DiagnosticStatusMsg::OK; + else if ((gal_authentic + gps_authentic) > 0) + diagOsnma.level = DiagnosticStatusMsg::WARN; + else + diagOsnma.level = DiagnosticStatusMsg::ERROR; + + msg.status.push_back(diagOsnma); + msg.header = last_gal_auth_status_.header; + + publish("/diagnostics", msg); + } + + void MessageHandler::assembleAimAndDiagnosticArray() + { + AimPlusStatusMsg aimMsg; + DiagnosticArrayMsg msg; + DiagnosticStatusMsg diagRf; + diagRf.hardware_id = last_receiversetup_.rx_serial_number; + diagRf.name = "septentrio_driver: AIM+ status"; + diagRf.message = + "Current status of the AIM+ interference and spoofing mitigation"; + + diagRf.values.resize(2); + diagRf.values[0].key = "interference"; + bool mitigated = false; + bool detected = false; + for (auto rfband : last_rf_status_.rfband) + { + std::bitset<8> info = rfband.info; + if (info.test(1)) + mitigated = true; + if (info.test(3)) + { + detected = true; + break; + } + } + if (detected) + { + diagRf.values[0].value = "present"; + aimMsg.interference = AimPlusStatusMsg::INTERFERENCE_PRESENT; + } else if (mitigated) + { + diagRf.values[0].value = "mitigated"; + aimMsg.interference = AimPlusStatusMsg::INTERFERENCE_MITIGATED; + } else + { + diagRf.values[0].value = "spectrum clean"; + aimMsg.interference = AimPlusStatusMsg::SPECTRUM_CLEAN; + } + + diagRf.values[1].key = "spoofing"; + bool spoofed = false; + std::bitset<8> flags = last_rf_status_.flags; + if (flags.test(0) && flags.test(1)) + { + diagRf.values[1].value = "detected by OSNMA and authenticity test"; + aimMsg.spoofing = + AimPlusStatusMsg::SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST; + spoofed = true; + } else if (flags.test(0)) + { + diagRf.values[1].value = "detected by authenticity test"; + aimMsg.spoofing = + AimPlusStatusMsg::SPOOFING_DETECTED_BY_AUTHENTCITY_TEST; + spoofed = true; + } else if (flags.test(1)) + { + diagRf.values[1].value = "detected by OSNMA"; + aimMsg.spoofing = AimPlusStatusMsg::SPOOFING_DETECTED_BY_OSNMA; + spoofed = true; + } else + { + diagRf.values[1].value = "none detected"; + aimMsg.spoofing = AimPlusStatusMsg::NONE_DETECTED; + } + if (osnma_info_available_) + { + aimMsg.osnma_authenticating = + ((last_gal_auth_status_.osnma_status & 7) == 6); + std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask; + std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask; + aimMsg.galileo_authentic = (gal_auth & gal_active).count(); + aimMsg.galileo_spoofed = (~gal_auth & gal_active).count(); + std::bitset<64> gps_active = last_gal_auth_status_.gps_active_mask; + std::bitset<64> gps_auth = last_gal_auth_status_.gps_authentic_mask; + aimMsg.gps_authentic = (gps_auth & gps_active).count(); + aimMsg.gps_spoofed = (~gps_auth & gps_active).count(); + } else + { + aimMsg.osnma_authenticating = false; + aimMsg.galileo_authentic = 0; + aimMsg.galileo_spoofed = 0; + aimMsg.gps_authentic = 0; + aimMsg.gps_spoofed = 0; + } + aimMsg.header = last_rf_status_.header; + aimMsg.tow = last_rf_status_.block_header.tow; + aimMsg.wnc = last_rf_status_.block_header.wnc; + publish("aimplusstatus", aimMsg); + + if (spoofed || detected) + diagRf.level = DiagnosticStatusMsg::ERROR; + else if (mitigated) + diagRf.level = DiagnosticStatusMsg::WARN; + else + diagRf.level = DiagnosticStatusMsg::OK; + + msg.status.push_back(diagRf); + msg.header = last_rf_status_.header; + + publish("/diagnostics", msg); + } + + void MessageHandler::assembleImu() + { + ImuMsg msg; + + msg.header = last_extsensmeas_.header; + + msg.linear_acceleration.x = last_extsensmeas_.acceleration_x; + msg.linear_acceleration.y = last_extsensmeas_.acceleration_y; + msg.linear_acceleration.z = last_extsensmeas_.acceleration_z; + + msg.angular_velocity.x = deg2rad(last_extsensmeas_.angular_rate_x); + msg.angular_velocity.y = deg2rad(last_extsensmeas_.angular_rate_y); + msg.angular_velocity.z = deg2rad(last_extsensmeas_.angular_rate_z); + + bool valid_orientation = false; + if (settings_->septentrio_receiver_type == "ins") + { + if (validValue(last_insnavgeod_.block_header.tow)) + { + // INS tow and extsens meas tow have the same time scale + Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow, + last_extsensmeas_.block_header.wnc); + Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc); + + static int64_t maxDt = (settings_->polling_period_pvt == 0) + ? 10000000 + : settings_->polling_period_pvt * 1000000; + if ((tsImu - tsIns) > maxDt) + { + valid_orientation = false; + } else + { + if ((last_insnavgeod_.sb_list & 2) != 0) + { + // Attitude + if (validValue(last_insnavgeod_.heading) && + validValue(last_insnavgeod_.pitch) && + validValue(last_insnavgeod_.roll)) + { + msg.orientation = convertEulerToQuaternionMsg( + deg2rad(last_insnavgeod_.roll), + deg2rad(last_insnavgeod_.pitch), + deg2rad(last_insnavgeod_.heading)); + } else + { + valid_orientation = false; + } + } else + { + valid_orientation = false; + } + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocov + if (validValue(last_insnavgeod_.roll_std_dev) && + validValue(last_insnavgeod_.pitch_std_dev) && + validValue(last_insnavgeod_.heading_std_dev)) + { + msg.orientation_covariance[0] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + msg.orientation_covariance[4] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + msg.orientation_covariance[8] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + } else + { + valid_orientation = false; + } + } else + { + valid_orientation = false; + } + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude cov + msg.orientation_covariance[1] = + deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.orientation_covariance[2] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.orientation_covariance[3] = + deg2radSq(last_insnavgeod_.pitch_roll_cov); + + msg.orientation_covariance[5] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.orientation_covariance[6] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.orientation_covariance[7] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + } + } + } else + { + valid_orientation = false; + } + } + + if (!valid_orientation) + { + msg.orientation.w = std::numeric_limits::quiet_NaN(); + msg.orientation.x = std::numeric_limits::quiet_NaN(); + msg.orientation.y = std::numeric_limits::quiet_NaN(); + msg.orientation.z = std::numeric_limits::quiet_NaN(); + msg.orientation_covariance[0] = -1.0; + msg.orientation_covariance[4] = -1.0; + msg.orientation_covariance[8] = -1.0; + } + + publish("imu", msg); + }; + + void MessageHandler::assembleTwist(bool fromIns /* = false*/) + { + if (!settings_->publish_twist) + return; + TwistWithCovarianceStampedMsg msg; + + if (fromIns) + { + msg.header = last_insnavgeod_.header; + + if ((last_insnavgeod_.sb_list & 8) != 0) + { + // Linear velocity in navigation frame + double ve = 0.0; + if (validValue(last_insnavgeod_.ve)) + ve = last_insnavgeod_.ve; + double vn = 0.0; + if (validValue(last_insnavgeod_.vn)) + vn = last_insnavgeod_.vn; + double vu = 0.0; + if (validValue(last_insnavgeod_.vu)) + vu = last_insnavgeod_.vu; + Eigen::Vector3d vel; + if (settings_->use_ros_axis_orientation) + { + // (ENU) + vel << ve, vn, vu; + } else + { + // (NED) + vel << vn, ve, -vu; + } + // Linear velocity + msg.twist.twist.linear.x = vel(0); + msg.twist.twist.linear.y = vel(1); + msg.twist.twist.linear.z = vel(2); + } else + { + msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + } + + if (((last_insnavgeod_.sb_list & 16) != 0) && + ((last_insnavgeod_.sb_list & 2) != 0) && + ((last_insnavgeod_.sb_list & 8) != 0)) + { + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); + if ((last_insnavgeod_.sb_list & 128) != 0) + { + // Linear velocity covariance + if (validValue(last_insnavgeod_.ve_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(0, 0) = -1.0; + if (validValue(last_insnavgeod_.vn_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(1, 1) = -1.0; + if (validValue(last_insnavgeod_.vu_std_dev)) + covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); + else + covVel_local(2, 2) = -1.0; + + if (validValue(last_insnavgeod_.ve_vn_cov)) + covVel_local(0, 1) = covVel_local(1, 0) = + last_insnavgeod_.ve_vn_cov; + if (settings_->use_ros_axis_orientation) + { + if (validValue(last_insnavgeod_.ve_vu_cov)) + covVel_local(0, 2) = covVel_local(2, 0) = + last_insnavgeod_.ve_vu_cov; + if (validValue(last_insnavgeod_.vn_vu_cov)) + covVel_local(2, 1) = covVel_local(1, 2) = + last_insnavgeod_.vn_vu_cov; + } else + { + if (validValue(last_insnavgeod_.vn_vu_cov)) + covVel_local(0, 2) = covVel_local(2, 0) = + -last_insnavgeod_.vn_vu_cov; + if (validValue(last_insnavgeod_.ve_vu_cov)) + covVel_local(2, 1) = covVel_local(1, 2) = + -last_insnavgeod_.ve_vu_cov; + } + } else + { + covVel_local(0, 0) = -1.0; + covVel_local(1, 1) = -1.0; + covVel_local(2, 2) = -1.0; + } + + msg.twist.covariance[0] = covVel_local(0, 0); + msg.twist.covariance[1] = covVel_local(0, 1); + msg.twist.covariance[2] = covVel_local(0, 2); + msg.twist.covariance[6] = covVel_local(1, 0); + msg.twist.covariance[7] = covVel_local(1, 1); + msg.twist.covariance[8] = covVel_local(1, 2); + msg.twist.covariance[12] = covVel_local(2, 0); + msg.twist.covariance[13] = covVel_local(2, 1); + msg.twist.covariance[14] = covVel_local(2, 2); + } else + { + msg.twist.covariance[0] = -1.0; + msg.twist.covariance[7] = -1.0; + msg.twist.covariance[14] = -1.0; + } + // Autocovariances of angular velocity + msg.twist.covariance[21] = -1.0; + msg.twist.covariance[28] = -1.0; + msg.twist.covariance[35] = -1.0; + + publish("twist_ins", msg); + } else + { + if ((!validValue(last_pvtgeodetic_.block_header.tow)) || + (last_pvtgeodetic_.block_header.tow != + last_velcovgeodetic_.block_header.tow)) + return; + msg.header = last_pvtgeodetic_.header; + + if (last_pvtgeodetic_.error == 0) + { + // Linear velocity in navigation frame + double ve = 0.0; + if (validValue(last_pvtgeodetic_.ve)) + ve = last_pvtgeodetic_.ve; + double vn = 0.0; + if (validValue(last_pvtgeodetic_.vn)) + vn = last_pvtgeodetic_.vn; + double vu = 0.0; + if (validValue(last_pvtgeodetic_.vu)) + vu = last_pvtgeodetic_.vu; + Eigen::Vector3d vel; + if (settings_->use_ros_axis_orientation) + { + // (ENU) + vel << ve, vn, vu; + } else + { + // (NED) + vel << vn, ve, -vu; + } + // Linear velocity + msg.twist.twist.linear.x = vel(0); + msg.twist.twist.linear.y = vel(1); + msg.twist.twist.linear.z = vel(2); + } else + { + msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + } + + if (last_velcovgeodetic_.error == 0) + { + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); + // Linear velocity covariance in navigation frame + if (validValue(last_velcovgeodetic_.cov_veve)) + if (settings_->use_ros_axis_orientation) + covVel_local(0, 0) = last_velcovgeodetic_.cov_veve; + else + covVel_local(1, 1) = last_velcovgeodetic_.cov_veve; + else + covVel_local(0, 0) = -1.0; + if (validValue(last_velcovgeodetic_.cov_vnvn)) + if (settings_->use_ros_axis_orientation) + covVel_local(1, 1) = last_velcovgeodetic_.cov_vnvn; + else + covVel_local(0, 0) = last_velcovgeodetic_.cov_vnvn; + else + covVel_local(1, 1) = -1.0; + if (validValue(last_velcovgeodetic_.cov_vuvu)) + covVel_local(2, 2) = last_velcovgeodetic_.cov_vuvu; + else + covVel_local(2, 2) = -1.0; + + covVel_local(0, 1) = covVel_local(1, 0) = + last_velcovgeodetic_.cov_vnve; + if (settings_->use_ros_axis_orientation) + { + covVel_local(0, 2) = covVel_local(2, 0) = + last_velcovgeodetic_.cov_vevu; + covVel_local(2, 1) = covVel_local(1, 2) = + last_velcovgeodetic_.cov_vnvu; + } else + { + covVel_local(0, 2) = covVel_local(2, 0) = + -last_velcovgeodetic_.cov_vnvu; + covVel_local(2, 1) = covVel_local(1, 2) = + -last_velcovgeodetic_.cov_vevu; + } + + msg.twist.covariance[0] = covVel_local(0, 0); + msg.twist.covariance[1] = covVel_local(0, 1); + msg.twist.covariance[2] = covVel_local(0, 2); + msg.twist.covariance[6] = covVel_local(1, 0); + msg.twist.covariance[7] = covVel_local(1, 1); + msg.twist.covariance[8] = covVel_local(1, 2); + msg.twist.covariance[12] = covVel_local(2, 0); + msg.twist.covariance[13] = covVel_local(2, 1); + msg.twist.covariance[14] = covVel_local(2, 2); + } else + { + msg.twist.covariance[0] = -1.0; + msg.twist.covariance[7] = -1.0; + msg.twist.covariance[14] = -1.0; + } + // Autocovariances of angular velocity + msg.twist.covariance[21] = -1.0; + msg.twist.covariance[28] = -1.0; + msg.twist.covariance[35] = -1.0; + + publish("twist_gnss", msg); + } + }; + + /** + * Localization in UTM coordinates. Yaw angle is converted from true north to + * grid north. Linear velocity of twist in body frame as per msg definition. + * Angular velocity not available, thus according autocovariances are set to + * -1.0. + */ + void MessageHandler::assembleLocalizationUtm() + { + if (!settings_->publish_localization && !settings_->publish_tf) + return; + + LocalizationMsg msg; + + int zone; + std::string zonestring; + bool northernHemisphere; + double easting; + double northing; + double meridian_convergence = 0.0; + if (fixedUtmZone_) + { + try + { + GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, + northernHemisphere); + double k; + GeographicLib::UTMUPS::Forward( + rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), zone, northernHemisphere, + easting, northing, meridian_convergence, k, zone); + } catch (const std::exception& e) + { + node_->log(log_level::DEBUG, + "UTMUPS conversion exception: " + std::string(e.what())); + return; + } + zonestring = *fixedUtmZone_; + } else + { + try + { + double k; + GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), + zone, northernHemisphere, easting, + northing, meridian_convergence, k); + zonestring = + GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); + } catch (const std::exception& e) + { + node_->log(log_level::DEBUG, + "UTMUPS conversion exception: " + std::string(e.what())); + return; + } + } + if (settings_->lock_utm_zone && !fixedUtmZone_) + fixedUtmZone_ = std::make_shared(zonestring); + + // UTM position (ENU) + if (settings_->use_ros_axis_orientation) + { + msg.pose.pose.position.x = easting; + msg.pose.pose.position.y = northing; + msg.pose.pose.position.z = last_insnavgeod_.height; + } else // (NED) + { + msg.pose.pose.position.x = northing; + msg.pose.pose.position.y = easting; + msg.pose.pose.position.z = -last_insnavgeod_.height; + } + + msg.header.frame_id = "utm_" + zonestring; + msg.header.stamp = last_insnavgeod_.header.stamp; + if (settings_->ins_use_poi) + msg.child_frame_id = settings_->poi_frame_id; + else + msg.child_frame_id = settings_->frame_id; + + Eigen::Matrix3d P_pos = -Eigen::Matrix3d::Identity(); + if ((last_insnavgeod_.sb_list & 1) != 0) + { + // Position autocovariance + P_pos(0, 0) = square(last_insnavgeod_.longitude_std_dev); + P_pos(1, 1) = square(last_insnavgeod_.latitude_std_dev); + P_pos(2, 2) = square(last_insnavgeod_.height_std_dev); + } + + // Euler angles + double roll = 0.0; + if (validValue(last_insnavgeod_.roll)) + roll = deg2rad(last_insnavgeod_.roll); + double pitch = 0.0; + if (validValue(last_insnavgeod_.pitch)) + pitch = deg2rad(last_insnavgeod_.pitch); + double yaw = 0.0; + if (validValue(last_insnavgeod_.heading)) + yaw = deg2rad(last_insnavgeod_.heading); + // meridian_convergence for conversion from true north to grid north + if (settings_->use_ros_axis_orientation) + yaw += deg2rad(meridian_convergence); + else + yaw -= deg2rad(meridian_convergence); + + if ((last_insnavgeod_.sb_list & 2) != 0) + { + // Attitude + msg.pose.pose.orientation = + convertEulerToQuaternionMsg(roll, pitch, yaw); + } else + { + msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + } + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocovariance + if (validValue(last_insnavgeod_.roll_std_dev)) + msg.pose.covariance[21] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + msg.pose.covariance[21] = -1.0; + if (validValue(last_insnavgeod_.pitch_std_dev)) + msg.pose.covariance[28] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + msg.pose.covariance[28] = -1.0; + if (validValue(last_insnavgeod_.heading_std_dev)) + msg.pose.covariance[35] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + msg.pose.covariance[35] = -1.0; + } else + { + msg.pose.covariance[21] = -1.0; + msg.pose.covariance[28] = -1.0; + msg.pose.covariance[35] = -1.0; + } + + if ((last_insnavgeod_.sb_list & 32) != 0) + { + // Position covariance + P_pos(0, 1) = last_insnavgeod_.latitude_longitude_cov; + P_pos(1, 0) = last_insnavgeod_.latitude_longitude_cov; + + if (settings_->use_ros_axis_orientation) + { + // (ENU) + P_pos(0, 2) = last_insnavgeod_.longitude_height_cov; + P_pos(1, 2) = last_insnavgeod_.latitude_height_cov; + P_pos(2, 0) = last_insnavgeod_.longitude_height_cov; + P_pos(2, 1) = last_insnavgeod_.latitude_height_cov; + } else + { + // (NED): down = -height + P_pos(0, 2) = -last_insnavgeod_.latitude_height_cov; + P_pos(1, 2) = -last_insnavgeod_.longitude_height_cov; + P_pos(2, 0) = -last_insnavgeod_.latitude_height_cov; + P_pos(2, 1) = -last_insnavgeod_.longitude_height_cov; + } + } + + if ((meridian_convergence != 0.0) && (last_insnavgeod_.sb_list & 1)) + { + double cg = std::cos(meridian_convergence); + double sg = std::sin(meridian_convergence); + Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); + R(0, 0) = cg; + R(0, 1) = -sg; + R(1, 0) = sg; + R(1, 1) = cg; + P_pos = (R * P_pos * R.transpose()).eval(); + } + + msg.pose.covariance[0] = P_pos(0, 0); + msg.pose.covariance[1] = P_pos(0, 1); + msg.pose.covariance[2] = P_pos(0, 2); + msg.pose.covariance[6] = P_pos(1, 0); + msg.pose.covariance[7] = P_pos(1, 1); + msg.pose.covariance[8] = P_pos(1, 2); + msg.pose.covariance[12] = P_pos(2, 0); + msg.pose.covariance[13] = P_pos(2, 1); + msg.pose.covariance[14] = P_pos(2, 2); + + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude covariancae + msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + + msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + } + + assembleLocalizationMsgTwist(roll, pitch, yaw, msg); + + if (settings_->publish_localization) + publish("localization", msg); + if (settings_->publish_tf) + publishTf(msg); + }; + + /** + * Localization in ECEF coordinates. Attitude is converted from local to ECEF + * Linear velocity of twist in body frame as per msg definition. Angular + * velocity not available, thus according autocovariances are set to -1.0. + */ + void MessageHandler::assembleLocalizationEcef() + { + if (!settings_->publish_localization_ecef && !settings_->publish_tf_ecef) + return; + + if ((!validValue(last_insnavcart_.block_header.tow)) || + (last_insnavcart_.block_header.tow != last_insnavgeod_.block_header.tow)) + return; + + LocalizationMsg msg; + + msg.header.frame_id = "ecef"; + msg.header.stamp = last_insnavcart_.header.stamp; + if (settings_->ins_use_poi) + msg.child_frame_id = settings_->poi_frame_id; + else + msg.child_frame_id = settings_->frame_id; + + // ECEF position + msg.pose.pose.position.x = last_insnavcart_.x; + msg.pose.pose.position.y = last_insnavcart_.y; + msg.pose.pose.position.z = last_insnavcart_.z; + + if ((last_insnavgeod_.sb_list & 1) != 0) + { + // Position autocovariance + msg.pose.covariance[0] = square(last_insnavcart_.x_std_dev); + msg.pose.covariance[7] = square(last_insnavcart_.y_std_dev); + msg.pose.covariance[14] = square(last_insnavcart_.z_std_dev); + } else + { + msg.pose.covariance[0] = -1.0; + msg.pose.covariance[7] = -1.0; + msg.pose.covariance[14] = -1.0; + } + if ((last_insnavcart_.sb_list & 32) != 0) + { + // Position covariance + msg.pose.covariance[1] = last_insnavcart_.xy_cov; + msg.pose.covariance[6] = last_insnavcart_.xy_cov; + msg.pose.covariance[2] = last_insnavcart_.xz_cov; + msg.pose.covariance[8] = last_insnavcart_.yz_cov; + msg.pose.covariance[12] = last_insnavcart_.xz_cov; + msg.pose.covariance[13] = last_insnavcart_.yz_cov; + } + + // Euler angles + double roll = 0.0; + if (validValue(last_insnavcart_.roll)) + roll = deg2rad(last_insnavcart_.roll); + double pitch = 0.0; + if (validValue(last_insnavcart_.pitch)) + pitch = deg2rad(last_insnavcart_.pitch); + double yaw = 0.0; + if (validValue(last_insnavcart_.heading)) + yaw = deg2rad(last_insnavcart_.heading); + + if ((last_insnavcart_.sb_list & 2) != 0) + { + // Attitude + Eigen::Quaterniond q_local_ecef; + if (settings_->use_ros_axis_orientation) + q_local_ecef = parsing_utilities::q_enu_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + else + q_local_ecef = parsing_utilities::q_ned_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + Eigen::Quaterniond q_b_local = + parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw); + + Eigen::Quaterniond q_b_ecef = q_local_ecef * q_b_local; + + msg.pose.pose.orientation = + parsing_utilities::quaternionToQuaternionMsg(q_b_ecef); + } else + { + msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + } + Eigen::Matrix3d covAtt_local = Eigen::Matrix3d::Zero(); + bool covAttValid = true; + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocovariance + if (validValue(last_insnavgeod_.roll_std_dev)) + covAtt_local(0, 0) = square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + { + covAtt_local(0, 0) = -1.0; + covAttValid = false; + } + if (validValue(last_insnavgeod_.pitch_std_dev)) + covAtt_local(1, 1) = square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + { + covAtt_local(1, 1) = -1.0; + covAttValid = false; + } + if (validValue(last_insnavgeod_.heading_std_dev)) + covAtt_local(2, 2) = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + { + covAtt_local(2, 2) = -1.0; + covAttValid = false; + } + } else + { + covAtt_local(0, 0) = -1.0; + covAtt_local(1, 1) = -1.0; + covAtt_local(2, 2) = -1.0; + covAttValid = false; + } + + if (covAttValid) + { + if ((last_insnavcart_.sb_list & 64) != 0) + { + // Attitude covariancae + covAtt_local(0, 1) = deg2radSq(last_insnavcart_.pitch_roll_cov); + covAtt_local(0, 2) = deg2radSq(last_insnavcart_.heading_roll_cov); + covAtt_local(1, 0) = deg2radSq(last_insnavcart_.pitch_roll_cov); + covAtt_local(2, 1) = deg2radSq(last_insnavcart_.heading_pitch_cov); + covAtt_local(2, 0) = deg2radSq(last_insnavcart_.heading_roll_cov); + covAtt_local(1, 2) = deg2radSq(last_insnavcart_.heading_pitch_cov); + } + + Eigen::Matrix3d R_local_ecef; + if (settings_->use_ros_axis_orientation) + R_local_ecef = parsing_utilities::R_enu_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + else + R_local_ecef = parsing_utilities::R_ned_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + // Rotate attitude covariance matrix to ecef coordinates + Eigen::Matrix3d covAtt_ecef = + R_local_ecef * covAtt_local * R_local_ecef.transpose(); + + msg.pose.covariance[21] = covAtt_ecef(0, 0); + msg.pose.covariance[22] = covAtt_ecef(0, 1); + msg.pose.covariance[23] = covAtt_ecef(0, 2); + msg.pose.covariance[27] = covAtt_ecef(1, 0); + msg.pose.covariance[28] = covAtt_ecef(1, 1); + msg.pose.covariance[29] = covAtt_ecef(1, 2); + msg.pose.covariance[33] = covAtt_ecef(2, 0); + msg.pose.covariance[34] = covAtt_ecef(2, 1); + msg.pose.covariance[35] = covAtt_ecef(2, 2); + } else + { + msg.pose.covariance[21] = -1.0; + msg.pose.covariance[28] = -1.0; + msg.pose.covariance[35] = -1.0; + } + + assembleLocalizationMsgTwist(roll, pitch, yaw, msg); + + if (settings_->publish_localization_ecef) + publish("localization_ecef", msg); + if (settings_->publish_tf_ecef) + publishTf(msg); + }; + + void MessageHandler::assembleLocalizationMsgTwist(double roll, double pitch, + double yaw, + LocalizationMsg& msg) const + { + Eigen::Matrix3d R_local_body = + parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); + if ((last_insnavgeod_.sb_list & 8) != 0) + { + // Linear velocity (ENU) + double ve = 0.0; + if (validValue(last_insnavgeod_.ve)) + ve = last_insnavgeod_.ve; + double vn = 0.0; + if (validValue(last_insnavgeod_.vn)) + vn = last_insnavgeod_.vn; + double vu = 0.0; + if (validValue(last_insnavgeod_.vu)) + vu = last_insnavgeod_.vu; + Eigen::Vector3d vel_local; + if (settings_->use_ros_axis_orientation) + { + // (ENU) + vel_local << ve, vn, vu; + } else + { + // (NED) + vel_local << vn, ve, -vu; + } + // Linear velocity, rotate to body coordinates + Eigen::Vector3d vel_body = R_local_body * vel_local; + msg.twist.twist.linear.x = vel_body(0); + msg.twist.twist.linear.y = vel_body(1); + msg.twist.twist.linear.z = vel_body(2); + } else + { + msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + } + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); + if ((last_insnavgeod_.sb_list & 16) != 0) + { + // Linear velocity autocovariance + if (validValue(last_insnavgeod_.ve_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(0, 0) = -1.0; + if (validValue(last_insnavgeod_.vn_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(1, 1) = -1.0; + if (validValue(last_insnavgeod_.vu_std_dev)) + covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); + else + covVel_local(2, 2) = -1.0; + } else + { + covVel_local(0, 0) = -1.0; + covVel_local(1, 1) = -1.0; + covVel_local(2, 2) = -1.0; + } + + if ((last_insnavgeod_.sb_list & 128) != 0) + { + covVel_local(0, 1) = covVel_local(1, 0) = last_insnavgeod_.ve_vn_cov; + if (settings_->use_ros_axis_orientation) + { + covVel_local(0, 2) = covVel_local(2, 0) = last_insnavgeod_.ve_vu_cov; + covVel_local(2, 1) = covVel_local(1, 2) = last_insnavgeod_.vn_vu_cov; + } else + { + covVel_local(0, 2) = covVel_local(2, 0) = + -last_insnavgeod_.vn_vu_cov; + covVel_local(2, 1) = covVel_local(1, 2) = + -last_insnavgeod_.ve_vu_cov; + } + } + + if (((last_insnavgeod_.sb_list & 16) != 0) && + ((last_insnavgeod_.sb_list & 2) != 0) && + ((last_insnavgeod_.sb_list & 8) != 0) && + validValue(last_insnavgeod_.ve_std_dev) && + validValue(last_insnavgeod_.vn_std_dev) && + validValue(last_insnavgeod_.vu_std_dev)) + { + // Rotate velocity covariance matrix to body coordinates + Eigen::Matrix3d covVel_body = + R_local_body * covVel_local * R_local_body.transpose(); + + msg.twist.covariance[0] = covVel_body(0, 0); + msg.twist.covariance[1] = covVel_body(0, 1); + msg.twist.covariance[2] = covVel_body(0, 2); + msg.twist.covariance[6] = covVel_body(1, 0); + msg.twist.covariance[7] = covVel_body(1, 1); + msg.twist.covariance[8] = covVel_body(1, 2); + msg.twist.covariance[12] = covVel_body(2, 0); + msg.twist.covariance[13] = covVel_body(2, 1); + msg.twist.covariance[14] = covVel_body(2, 2); + } else + { + msg.twist.covariance[0] = -1.0; + msg.twist.covariance[7] = -1.0; + msg.twist.covariance[14] = -1.0; + } + // Autocovariances of angular velocity + msg.twist.covariance[21] = -1.0; + msg.twist.covariance[28] = -1.0; + msg.twist.covariance[35] = -1.0; + } + + /** + * The position_covariance array is populated in row-major order, where the basis + * of the corresponding matrix is ENU (so Cov_lonlon is in location 11 of the + * matrix). The B2b signal type of BeiDou is not checked for usage, since the + * SignalInfo field of the PVTGeodetic block does not disclose it. For that, one + * would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block. + */ + void MessageHandler::assembleNavSatFix() + { + if (!settings_->publish_navsatfix) + return; + + static auto last_ins_tow = last_insnavgeod_.block_header.tow; + + NavSatFixMsg msg; + uint16_t mask = 15; // We extract the first four bits using this mask. + if (settings_->septentrio_receiver_type == "gnss") + { + if ((!validValue(last_pvtgeodetic_.block_header.tow)) || + (last_pvtgeodetic_.block_header.tow != + last_poscovgeodetic_.block_header.tow)) + return; + + msg.header = last_pvtgeodetic_.header; + + uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; + switch (type_of_pvt) + { + case evNoPVT: + { + msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = NavSatStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + { + msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; + break; + } + default: + { + node_->log( + log_level::DEBUG, + "PVTGeodetic's Mode field contains an invalid type of PVT solution."); + break; + } + } + bool gps_in_pvt = false; + bool glo_in_pvt = false; + bool com_in_pvt = false; + bool gal_in_pvt = false; + uint32_t mask_2 = 1; + for (int bit = 0; bit != 31; ++bit) + { + bool in_use = last_pvtgeodetic_.signal_info & mask_2; + if (bit <= 5 && in_use) + { + gps_in_pvt = true; + } + if (8 <= bit && bit <= 12 && in_use) + glo_in_pvt = true; + if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) + com_in_pvt = true; + if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) + gal_in_pvt = true; + mask_2 *= 2; + } + // Below, booleans will be promoted to integers automatically. + uint16_t service = + gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; + msg.status.service = service; + msg.latitude = rad2deg(last_pvtgeodetic_.latitude); + msg.longitude = rad2deg(last_pvtgeodetic_.longitude); + msg.altitude = last_pvtgeodetic_.height; + msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; + msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; + msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; + } else if (settings_->septentrio_receiver_type == "ins") + { + if ((!validValue(last_insnavgeod_.block_header.tow)) || + (last_insnavgeod_.block_header.tow == last_ins_tow)) + { + return; + } + last_ins_tow = last_insnavgeod_.block_header.tow; + + msg.header = last_insnavgeod_.header; + + uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; + switch (type_of_pvt) + { + case evNoPVT: + { + msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = NavSatStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + { + msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; + break; + } + default: + { + node_->log( + log_level::DEBUG, + "INSNavGeod's Mode field contains an invalid type of PVT solution."); + break; + } + } + bool gps_in_pvt = false; + bool glo_in_pvt = false; + bool com_in_pvt = false; + bool gal_in_pvt = false; + uint32_t mask_2 = 1; + for (int bit = 0; bit != 31; ++bit) + { + bool in_use = last_pvtgeodetic_.signal_info & mask_2; + if (bit <= 5 && in_use) + { + gps_in_pvt = true; + } + if (8 <= bit && bit <= 12 && in_use) + glo_in_pvt = true; + if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) + com_in_pvt = true; + if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) + gal_in_pvt = true; + mask_2 *= 2; + } + // Below, booleans will be promoted to integers automatically. + uint16_t service = + gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; + msg.status.service = service; + msg.latitude = rad2deg(last_insnavgeod_.latitude); + msg.longitude = rad2deg(last_insnavgeod_.longitude); + msg.altitude = last_insnavgeod_.height; + + if ((last_insnavgeod_.sb_list & 1) != 0) + { + msg.position_covariance[0] = + square(last_insnavgeod_.longitude_std_dev); + msg.position_covariance[4] = + square(last_insnavgeod_.latitude_std_dev); + msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); + } + if ((last_insnavgeod_.sb_list & 32) != 0) + { + msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; + msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; + } + msg.position_covariance_type = + NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; + } + publish("navsatfix", msg); + }; + + /** + * Note that the field "dip" denotes the local magnetic inclination in degrees + * (positive when the magnetic field points downwards (into the Earth)). + * This quantity cannot be calculated by most Septentrio + * receivers. We assume that for the ROS field "err_time", we are requested to + * provide the 2 sigma uncertainty on the clock bias estimate in square meters, + * not the clock drift estimate (latter would be + * "2*std::sqrt(last_velcovgeodetic_.Cov_DtDt)"). + * The "err_track" entry is calculated via the Gaussian error propagation formula + * from the eastward and the northward velocities. For the formula's usage we + * have to assume that the eastward and the northward velocities are independent + * variables. Note that elevations and azimuths of visible satellites are taken + * from the ChannelStatus block, which provides 1 degree precision, while the + * SatVisibility block could provide hundredths of degrees precision. Change if + * imperative for your application... Definition of "visible satellite" adopted + * here: We define a visible satellite as being !up to! "in sync" mode with the + * receiver, which corresponds to last_measepoch_.N (signal-to-noise ratios are + * thereby available for these), though not last_channelstatus_.N, which also + * includes those "in search". In case certain values appear unphysical, please + * consult the firmware, since those most likely refer to Do-Not-Use values. + */ + void MessageHandler::assembleGpsFix() + { + if (!settings_->publish_gpsfix) + return; + + if (settings_->septentrio_receiver_type == "gnss") + { + if (!validValue(last_measepoch_.block_header.tow) || + !validValue(last_channelstatus_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_pvtgeodetic_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_poscovgeodetic_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_atteuler_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_attcoveuler_.block_header.tow)) + return; + } else if (settings_->septentrio_receiver_type == "ins") + { + if (!validValue(last_measepoch_.block_header.tow) || + !validValue(last_channelstatus_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_insnavgeod_.block_header.tow)) + return; + } + + GpsFixMsg msg; + msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); + + // MeasEpoch Processing + std::vector cno_tracked; + std::vector svid_in_sync; + { + cno_tracked.reserve(last_measepoch_.type1.size()); + svid_in_sync.reserve(last_measepoch_.type1.size()); + for (const auto& measepoch_channel_type1 : last_measepoch_.type1) + { + // Define MeasEpochChannelType1 struct for the corresponding + // sub-block + svid_in_sync.push_back( + static_cast(measepoch_channel_type1.sv_id)); + uint8_t type_mask = + 15; // We extract the first four bits using this mask. + if (((measepoch_channel_type1.type & type_mask) == + static_cast(1)) || + ((measepoch_channel_type1.type & type_mask) == + static_cast(2))) + { + cno_tracked.push_back( + static_cast(measepoch_channel_type1.cn0) / 4); + } else + { + cno_tracked.push_back( + static_cast(measepoch_channel_type1.cn0) / 4 + + static_cast(10)); + } + } + } + + // ChannelStatus Processing + std::vector svid_in_sync_2; + std::vector elevation_tracked; + std::vector azimuth_tracked; + std::vector svid_pvt; + svid_pvt.clear(); + std::vector ordering; + { + svid_in_sync_2.reserve(last_channelstatus_.satInfo.size()); + elevation_tracked.reserve(last_channelstatus_.satInfo.size()); + azimuth_tracked.reserve(last_channelstatus_.satInfo.size()); + for (const auto& channel_sat_info : last_channelstatus_.satInfo) + { + bool to_be_added = false; + for (int32_t j = 0; j < static_cast(svid_in_sync.size()); + ++j) + { + if (svid_in_sync[j] == + static_cast(channel_sat_info.sv_id)) + { + ordering.push_back(j); + to_be_added = true; + break; + } + } + if (to_be_added) + { + svid_in_sync_2.push_back( + static_cast(channel_sat_info.sv_id)); + elevation_tracked.push_back( + static_cast(channel_sat_info.elev)); + static uint16_t azimuth_mask = 511; + azimuth_tracked.push_back(static_cast( + (channel_sat_info.az_rise_set & azimuth_mask))); + } + svid_pvt.reserve(channel_sat_info.stateInfo.size()); + for (const auto& channel_state_info : channel_sat_info.stateInfo) + { + // Define ChannelStateInfo struct for the corresponding sub-block + bool pvt_status = false; + uint16_t pvt_status_mask = std::pow(2, 15) + std::pow(2, 14); + for (int k = 15; k != -1; k -= 2) + { + uint16_t pvt_status_value = + (channel_state_info.pvt_status & pvt_status_mask) >> + k - 1; + if (pvt_status_value == 2) + { + pvt_status = true; + } + if (k > 1) + { + pvt_status_mask = pvt_status_mask - std::pow(2, k) - + std::pow(2, k - 1) + + std::pow(2, k - 2) + + std::pow(2, k - 3); + } + } + if (pvt_status) + { + svid_pvt.push_back( + static_cast(channel_sat_info.sv_id)); + } + } + } + } + msg.status.satellite_used_prn = + svid_pvt; // Entries such as int32[] in ROS messages are to be treated as + // std::vectors. + msg.status.satellites_visible = static_cast(svid_in_sync.size()); + msg.status.satellite_visible_prn = svid_in_sync_2; + msg.status.satellite_visible_z = elevation_tracked; + msg.status.satellite_visible_azimuth = azimuth_tracked; + + // Reordering CNO vector to that of all previous arrays + std::vector cno_tracked_reordered; + if (static_cast(last_channelstatus_.n) != 0) + { + for (int32_t k = 0; k < static_cast(ordering.size()); ++k) + { + cno_tracked_reordered.push_back(cno_tracked[ordering[k]]); + } + } + msg.status.satellite_visible_snr = cno_tracked_reordered; + msg.err_time = 2 * std::sqrt(last_poscovgeodetic_.cov_bb); + + if (settings_->septentrio_receiver_type == "gnss") + { + msg.header = last_pvtgeodetic_.header; + + // PVT Status Analysis + uint16_t status_mask = + 15; // We extract the first four bits using this mask. + uint16_t type_of_pvt = + ((uint16_t)(last_pvtgeodetic_.mode)) & status_mask; + switch (type_of_pvt) + { + case evNoPVT: + { + msg.status.status = GpsStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = GpsStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = GpsStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + { + uint16_t reference_id = last_pvtgeodetic_.reference_id; + // Here come the PRNs of the 4 WAAS satellites.. + if (reference_id == 131 || reference_id == 133 || + reference_id == 135 || reference_id == 135) + { + msg.status.status = GpsStatusMsg::STATUS_WAAS_FIX; + } else + { + msg.status.status = GpsStatusMsg::STATUS_SBAS_FIX; + } + break; + } + default: + { + node_->log( + log_level::DEBUG, + "PVTGeodetic's Mode field contains an invalid type of PVT solution."); + break; + } + } + // Doppler is not used when calculating the velocities of, say, + // mosaic-x5, hence: + msg.status.motion_source = GpsStatusMsg::SOURCE_POINTS; + // Doppler is not used when calculating the orientation of, say, + // mosaic-x5, hence: + msg.status.orientation_source = GpsStatusMsg::SOURCE_POINTS; + msg.status.position_source = GpsStatusMsg::SOURCE_GPS; + msg.latitude = rad2deg(last_pvtgeodetic_.latitude); + msg.longitude = rad2deg(last_pvtgeodetic_.longitude); + msg.altitude = last_pvtgeodetic_.height; + // Note that cog is of type float32 while track is of type float64. + msg.track = last_pvtgeodetic_.cog; + msg.speed = std::sqrt(square(last_pvtgeodetic_.vn) + + square(last_pvtgeodetic_.ve)); + msg.climb = last_pvtgeodetic_.vu; + + msg.roll = last_atteuler_.roll; + msg.pitch = last_atteuler_.pitch; + msg.dip = last_atteuler_.heading; + + if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) + { + msg.gdop = -1.0; + } else + { + msg.gdop = + std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); + } + if (last_dop_.pdop == 0.0) + { + msg.pdop = -1.0; + } else + { + msg.pdop = last_dop_.pdop; + } + if (last_dop_.hdop == 0.0) + { + msg.hdop = -1.0; + } else + { + msg.hdop = last_dop_.hdop; + } + if (last_dop_.vdop == 0.0) + { + msg.vdop = -1.0; + } else + { + msg.vdop = last_dop_.vdop; + } + if (last_dop_.tdop == 0.0) + { + msg.tdop = -1.0; + } else + { + msg.tdop = last_dop_.tdop; + } + msg.time = + static_cast(last_pvtgeodetic_.block_header.tow) / 1000 + + static_cast(last_pvtgeodetic_.block_header.wnc * 604800); + // position + msg.err = + 2 * + (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + + static_cast(last_poscovgeodetic_.cov_lonlon) + + static_cast(last_poscovgeodetic_.cov_hgthgt))); + msg.err_horz = + 2 * + (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + + static_cast(last_poscovgeodetic_.cov_lonlon))); + msg.err_vert = + 2 * std::sqrt(static_cast(last_poscovgeodetic_.cov_hgthgt)); + // motion + msg.err_track = + 2 * (std::sqrt(square(1.0 / (last_pvtgeodetic_.vn + + square(last_pvtgeodetic_.ve) / + last_pvtgeodetic_.vn)) * + last_poscovgeodetic_.cov_lonlon + + square((last_pvtgeodetic_.ve) / + (square(last_pvtgeodetic_.vn) + + square(last_pvtgeodetic_.ve))) * + last_poscovgeodetic_.cov_latlat)); + msg.err_speed = + 2 * (std::sqrt(static_cast(last_velcovgeodetic_.cov_vnvn) + + static_cast(last_velcovgeodetic_.cov_veve))); + msg.err_climb = + 2 * std::sqrt(static_cast(last_velcovgeodetic_.cov_vuvu)); + // attitude + msg.err_roll = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_rollroll)); + msg.err_pitch = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_pitchpitch)); + msg.err_dip = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_headhead)); + + msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; + msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; + msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; + } else if (settings_->septentrio_receiver_type == "ins") + { + msg.header = last_insnavgeod_.header; + + // PVT Status Analysis + uint16_t status_mask = + 15; // We extract the first four bits using this mask. + uint16_t type_of_pvt = + ((uint16_t)(last_insnavgeod_.gnss_mode)) & status_mask; + switch (type_of_pvt) + { + case evNoPVT: + { + msg.status.status = GpsStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = GpsStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = GpsStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + default: + { + node_->log( + log_level::DEBUG, + "INSNavGeod's Mode field contains an invalid type of PVT solution."); + break; + } + } + // Doppler is not used when calculating the velocities of, say, + // mosaic-x5, hence: + msg.status.motion_source = GpsStatusMsg::SOURCE_POINTS; + // Doppler is not used when calculating the orientation of, say, + // mosaic-x5, hence: + msg.status.orientation_source = GpsStatusMsg::SOURCE_POINTS; + msg.status.position_source = GpsStatusMsg::SOURCE_GPS; + msg.latitude = rad2deg(last_insnavgeod_.latitude); + msg.longitude = rad2deg(last_insnavgeod_.longitude); + msg.altitude = last_insnavgeod_.height; + // Note that cog is of type float32 while track is of type float64. + if ((last_insnavgeod_.sb_list & 2) != 0) + { + msg.pitch = last_insnavgeod_.pitch; + msg.roll = last_insnavgeod_.roll; + msg.dip = last_insnavgeod_.heading; + } + if ((last_insnavgeod_.sb_list & 8) != 0) + { + msg.speed = std::sqrt(square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve)); + + msg.climb = last_insnavgeod_.vu; + msg.track = std::atan2(last_insnavgeod_.vn, last_insnavgeod_.ve); + } + if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) + { + msg.gdop = -1.0; + } else + { + msg.gdop = + std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); + } + if (last_dop_.pdop == 0.0) + { + msg.pdop = -1.0; + } else + { + msg.pdop = last_dop_.pdop; + } + if (last_dop_.hdop == 0.0) + { + msg.hdop = -1.0; + } else + { + msg.hdop = last_dop_.hdop; + } + if (last_dop_.vdop == 0.0) + { + msg.vdop = -1.0; + } else + { + msg.vdop = last_dop_.vdop; + } + if (last_dop_.tdop == 0.0) + { + msg.tdop = -1.0; + } else + { + msg.tdop = last_dop_.tdop; + } + msg.time = + static_cast(last_insnavgeod_.block_header.tow) / 1000 + + static_cast(last_insnavgeod_.block_header.wnc * 604800); + if ((last_insnavgeod_.sb_list & 1) != 0) + { + msg.err = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + + square(last_insnavgeod_.longitude_std_dev) + + square(last_insnavgeod_.height_std_dev))); + msg.err_horz = + 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + + square(last_insnavgeod_.longitude_std_dev))); + msg.err_vert = 2 * last_insnavgeod_.height_std_dev; + } + if (((last_insnavgeod_.sb_list & 8) != 0) || + ((last_insnavgeod_.sb_list & 1) != 0)) + { + msg.err_track = + 2 * (std::sqrt(square(1.0 / (last_insnavgeod_.vn + + square(last_insnavgeod_.ve) / + last_insnavgeod_.vn)) * + square(last_insnavgeod_.longitude_std_dev) + + square((last_insnavgeod_.ve) / + (square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve))) * + square(last_insnavgeod_.latitude_std_dev))); + } + if ((last_insnavgeod_.sb_list & 8) != 0) + { + msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.ve_std_dev) + + square(last_insnavgeod_.vn_std_dev))); + msg.err_climb = 2 * last_insnavgeod_.vu_std_dev; + } + if ((last_insnavgeod_.sb_list & 2) != 0) + { + msg.err_pitch = 2 * last_insnavgeod_.pitch_std_dev; + msg.err_roll = 2 * last_insnavgeod_.roll_std_dev; + msg.err_dip = 2 * last_insnavgeod_.heading_std_dev; + } + if ((last_insnavgeod_.sb_list & 1) != 0) + { + msg.position_covariance[0] = + square(last_insnavgeod_.longitude_std_dev); + msg.position_covariance[4] = + square(last_insnavgeod_.latitude_std_dev); + msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); + } + if ((last_insnavgeod_.sb_list & 32) != 0) + { + msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; + msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; + } + msg.position_covariance_type = + NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; + } + publish("gpsfix", msg); + } + + void + MessageHandler::assembleTimeReference(const std::shared_ptr& telegram) + { + TimeReferenceMsg msg; + Timestamp time_obj = timestampSBF(telegram->message); + msg.time_ref = timestampToRos(time_obj); + msg.source = "GPST"; + assembleHeader(settings_->frame_id, telegram, msg); + publish("gpst", msg); + } + + template + void MessageHandler::assembleHeader(const std::string& frameId, + const std::shared_ptr& telegram, + T& msg) const + { + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; + msg.header.frame_id = frameId; + + if (!settings_->use_gnss_time && settings_->latency_compensation) + { + if constexpr (std::is_same::value || + std::is_same::value) + { + time_obj -= msg.latency * 100000ul; // from 0.0001 s to ns + } else if constexpr (std::is_same::value || + std::is_same::value) + { + last_pvt_latency_ = msg.latency * 100000ul; // from 0.0001 s to ns + time_obj -= last_pvt_latency_; + } else if constexpr (std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value) + { + time_obj -= last_pvt_latency_; + } + } + + msg.header.stamp = timestampToRos(time_obj); + } + + Timestamp MessageHandler::timestampSBF(const std::vector& message) const + { + uint32_t tow = parsing_utilities::getTow(message); + uint16_t wnc = parsing_utilities::getWnc(message); + + if (!validValue(tow) || !validValue(wnc)) + return 0; + + return timestampSBF(tow, wnc); + } + + /// If the current time shall be employed, it is calculated via the time(NULL) + /// function found in the \ library At the time of writing the code + /// (2020), the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the + /// settings_->leap_seconds ROSaic parameter accordingly as soon as the + /// next leap second is inserted into the UTC time. + Timestamp MessageHandler::timestampSBF(uint32_t tow, uint16_t wnc) const + { + Timestamp time_obj; + + // conversion from GPS time of week and week number to UTC taking leap + // seconds into account + static uint64_t secToNSec = 1000000000; + static uint64_t mSec2NSec = 1000000; + static uint64_t nsOfGpsStart = + 315964800 * + secToNSec; // GPS week counter starts at 1980-01-06 which is + // 315964800 seconds since Unix epoch (1970-01-01 UTC) + static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec; + + time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek; + + if (current_leap_seconds_ != -128) + time_obj -= current_leap_seconds_ * secToNSec; + // else: warn? + + return time_obj; + } + + /** + * If GNSS time is used, Publishing is only done with valid leap seconds + */ + template + void MessageHandler::publish(const std::string& topic, const M& msg) + { + // TODO: maybe publish only if wnc and tow is valid? + if (!settings_->use_gnss_time || + (settings_->use_gnss_time && (current_leap_seconds_ != -128))) + { + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(timestampFromRos(msg.header.stamp)); + } + node_->publishMessage(topic, msg); + } else + { + node_->log( + log_level::DEBUG, + "Not publishing message with GNSS time because no leap seconds are available yet."); + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + node_->log( + log_level::WARN, + "No leap seconds were set and none were received from log yet."); + setLeapSeconds(); + } + } + } + + /** + * If GNSS time is used, Publishing is only done with valid leap seconds + */ + void MessageHandler::publishTf(const LocalizationMsg& msg) + { + // TODO: maybe publish only if wnc and tow is valid? + if (!settings_->use_gnss_time || + (settings_->use_gnss_time && (current_leap_seconds_ != -128))) + { + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(timestampFromRos(msg.header.stamp)); + } + node_->publishTf(msg); + } else + { + node_->log( + log_level::DEBUG, + "Not publishing tf with GNSS time because no leap seconds are available yet."); + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + node_->log( + log_level::WARN, + "No leap seconds were set and none were received from log yet. "); + setLeapSeconds(); + }; + } + } + + void MessageHandler::parseSbf(const std::shared_ptr& telegram) + { + + uint16_t sbfId = parsing_utilities::getId(telegram->message); + + /*node_->log(log_level::DEBUG, "ROSaic reading SBF block " + + std::to_string(sbfId) + " made up of " + + std::to_string(telegram->message.size()) + + " bytes...");*/ + + switch (sbfId) + { + case PVT_CARTESIAN: // Position and velocity in XYZ + { + if (settings_->publish_pvtcartesian) + { + PVTCartesianMsg msg; + + if (!PVTCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in PVTCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("pvtcartesian", msg); + } + break; + } + case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame + // (ENU frame) + { + if (!PVTGeodeticParser(node_, telegram->message.begin(), + telegram->message.end(), last_pvtgeodetic_)) + { + node_->log(log_level::ERROR, "parse error in PVTGeodetic"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_pvtgeodetic_); + if (settings_->publish_pvtgeodetic) + publish("pvtgeodetic", last_pvtgeodetic_); + assembleTwist(); + assembleNavSatFix(); + assemblePoseWithCovarianceStamped(); + assembleGpsFix(); + if (settings_->publish_gpst && + (settings_->septentrio_receiver_type == "gnss")) + assembleTimeReference(telegram); + break; + } + case BASE_VECTOR_CART: + { + if (settings_->publish_basevectorcart) + { + BaseVectorCartMsg msg; + + if (!BaseVectorCartParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in BaseVectorCart"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("basevectorcart", msg); + } + break; + } + case BASE_VECTOR_GEOD: + { + if (settings_->publish_basevectorgeod) + { + BaseVectorGeodMsg msg; + + if (!BaseVectorGeodParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in BaseVectorGeod"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("basevectorgeod", msg); + } + break; + } + case POS_COV_CARTESIAN: + { + if (settings_->publish_poscovcartesian) + { + PosCovCartesianMsg msg; + + if (!PosCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in PosCovCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("poscovcartesian", msg); + } + break; + } + case POS_COV_GEODETIC: + { + if (!PosCovGeodeticParser(node_, telegram->message.begin(), + telegram->message.end(), last_poscovgeodetic_)) + { + node_->log(log_level::ERROR, "parse error in PosCovGeodetic"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_poscovgeodetic_); + if (settings_->publish_poscovgeodetic) + publish("poscovgeodetic", last_poscovgeodetic_); + assembleNavSatFix(); + assemblePoseWithCovarianceStamped(); + assembleGpsFix(); + break; + } + case ATT_EULER: + { + if (!AttEulerParser(node_, telegram->message.begin(), + telegram->message.end(), last_atteuler_, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in AttEuler"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_atteuler_); + if (settings_->publish_atteuler) + publish("atteuler", last_atteuler_); + assemblePoseWithCovarianceStamped(); + assembleGpsFix(); + break; + } + case ATT_COV_EULER: + { + if (!AttCovEulerParser(node_, telegram->message.begin(), + telegram->message.end(), last_attcoveuler_, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in AttCovEuler"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_attcoveuler_); + if (settings_->publish_attcoveuler) + publish("attcoveuler", last_attcoveuler_); + assemblePoseWithCovarianceStamped(); + assembleGpsFix(); + break; + } + case GAL_AUTH_STATUS: + { + if (!GalAuthStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_gal_auth_status_)) + { + node_->log(log_level::ERROR, "parse error in GalAuthStatus"); + break; + } + osnma_info_available_ = true; + assembleHeader(settings_->frame_id, telegram, last_gal_auth_status_); + if (settings_->publish_galauthstatus) + { + publish("galauthstatus", last_gal_auth_status_); + assembleOsnmaDiagnosticArray(); + } + break; + } + case RF_STATUS: + { + if (!RfStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_rf_status_)) + { + node_->log(log_level::ERROR, "parse error inRfStatus"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_rf_status_); + if (settings_->publish_aimplusstatus) + { + publish("rfstatus", last_rf_status_); + assembleAimAndDiagnosticArray(); + } + break; + } + case INS_NAV_CART: // Position, velocity and orientation in cartesian + // coordinate frame (ENU frame) + { + if (!INSNavCartParser(node_, telegram->message.begin(), + telegram->message.end(), last_insnavcart_, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in INSNavCart"); + break; + } + std::string frame_id; + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + assembleHeader(frame_id, telegram, last_insnavcart_); + if (settings_->publish_insnavcart) + publish("insnavcart", last_insnavcart_); + assembleLocalizationEcef(); + break; + } + case INS_NAV_GEOD: // Position, velocity and orientation in geodetic + // coordinate frame (ENU frame) + { + if (!INSNavGeodParser(node_, telegram->message.begin(), + telegram->message.end(), last_insnavgeod_, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in INSNavGeod"); + break; + } + std::string frame_id; + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + assembleHeader(frame_id, telegram, last_insnavgeod_); + if (settings_->publish_insnavgeod) + publish("insnavgeod", last_insnavgeod_); + assembleLocalizationUtm(); + assembleLocalizationEcef(); + assembleTwist(true); + assemblePoseWithCovarianceStamped(); + assembleNavSatFix(); + assembleGpsFix(); + if (settings_->publish_gpst) + assembleTimeReference(telegram); + break; + } + case IMU_SETUP: // IMU orientation and lever arm + { + if (settings_->publish_imusetup) + { + IMUSetupMsg msg; + + if (!IMUSetupParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in IMUSetup"); + break; + } + assembleHeader(settings_->vehicle_frame_id, telegram, msg); + publish("imusetup", msg); + } + break; + } + case VEL_SENSOR_SETUP: // Velocity sensor lever arm + { + if (settings_->publish_velcovgeodetic) + { + VelSensorSetupMsg msg; + + if (!VelSensorSetupParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in VelSensorSetup"); + break; + } + assembleHeader(settings_->vehicle_frame_id, telegram, msg); + publish("velsensorsetup", msg); + } + break; + } + case EXT_EVENT_INS_NAV_CART: // Position, velocity and orientation in + // cartesian coordinate frame (ENU frame) + { + if (settings_->publish_exteventinsnavcart) + { + INSNavCartMsg msg; + + if (!INSNavCartParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, + "parse error in ExtEventINSNavCart"); + break; + } + std::string frame_id; + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + assembleHeader(frame_id, telegram, msg); + publish("exteventinsnavcart", msg); + } + break; + } + case EXT_EVENT_INS_NAV_GEOD: + { + if (settings_->publish_exteventinsnavgeod) + { + INSNavGeodMsg msg; + + if (!INSNavGeodParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, + "parse error in ExtEventINSNavGeod"); + break; + } + std::string frame_id; + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + assembleHeader(frame_id, telegram, msg); + publish("exteventinsnavgeod", msg); + } + break; + } + case EXT_SENSOR_MEAS: + { + bool hasImuMeas = false; + if (!ExtSensorMeasParser(node_, telegram->message.begin(), + telegram->message.end(), last_extsensmeas_, + settings_->use_ros_axis_orientation, + hasImuMeas)) + { + node_->log(log_level::ERROR, "parse error in ExtSensorMeas"); + break; + } + assembleHeader(settings_->imu_frame_id, telegram, last_extsensmeas_); + if (settings_->publish_extsensormeas) + publish("extsensormeas", last_extsensmeas_); + if (settings_->publish_imu && hasImuMeas) + { + assembleImu(); + } + break; + } + case CHANNEL_STATUS: + { + if (!ChannelStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_channelstatus_)) + { + node_->log(log_level::ERROR, "parse error in ChannelStatus"); + break; + } + assembleGpsFix(); + break; + } + case MEAS_EPOCH: + { + if (!MeasEpochParser(node_, telegram->message.begin(), + telegram->message.end(), last_measepoch_)) + { + node_->log(log_level::ERROR, "parse error in MeasEpoch"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_measepoch_); + if (settings_->publish_measepoch) + publish("measepoch", last_measepoch_); + assembleGpsFix(); + break; + } + case DOP: + { + if (!DOPParser(node_, telegram->message.begin(), telegram->message.end(), + last_dop_)) + { + node_->log(log_level::ERROR, "parse error in DOP"); + break; + } + assembleGpsFix(); + break; + } + case VEL_COV_CARTESIAN: + { + if (settings_->publish_velcovcartesian) + { + VelCovCartesianMsg msg; + if (!VelCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in VelCovCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("velcovcartesian", msg); + } + break; + } + case VEL_COV_GEODETIC: + { + + if (!VelCovGeodeticParser(node_, telegram->message.begin(), + telegram->message.end(), last_velcovgeodetic_)) + { + node_->log(log_level::ERROR, "parse error in VelCovGeodetic"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_velcovgeodetic_); + if (settings_->publish_velcovgeodetic) + publish("velcovgeodetic", last_velcovgeodetic_); + assembleTwist(); + assembleGpsFix(); + break; + } + case RECEIVER_STATUS: + { + if (!ReceiverStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_receiverstatus_)) + { + node_->log(log_level::ERROR, "parse error in ReceiverStatus"); + break; + } + assembleDiagnosticArray(telegram); + break; + } + case QUALITY_IND: + { + if (!QualityIndParser(node_, telegram->message.begin(), + telegram->message.end(), last_qualityind_)) + { + node_->log(log_level::ERROR, "parse error in QualityInd"); + break; + } + assembleDiagnosticArray(telegram); + break; + } + case RECEIVER_SETUP: + { + if (!ReceiverSetupParser(node_, telegram->message.begin(), + telegram->message.end(), last_receiversetup_)) + { + node_->log(log_level::ERROR, "parse error in ReceiverSetup"); + break; + } + node_->log(log_level::DEBUG, + "receiver setup firmware: " + last_receiversetup_.rx_version); + + static const int32_t ins_major = 1; + static const int32_t ins_minor = 4; + static const int32_t ins_patch = 0; + static const int32_t gnss_major = 4; + static const int32_t gnss_minor = 12; + static const int32_t gnss_patch = 1; + boost::tokenizer<> tok(last_receiversetup_.rx_version); + boost::tokenizer<>::iterator it = tok.begin(); + std::vector major_minor_patch; + major_minor_patch.reserve(3); + for (boost::tokenizer<>::iterator it = tok.begin(); it != tok.end(); + ++it) + { + int32_t v = std::atoi(it->c_str()); + major_minor_patch.push_back(v); + } + if (major_minor_patch.size() < 3) + { + node_->log(log_level::ERROR, "parse error of firmware version."); + } else + { + if ((settings_->septentrio_receiver_type == "ins") || node_->isIns()) + { + if ((major_minor_patch[0] < ins_major) || + ((major_minor_patch[0] == ins_major) && + (major_minor_patch[1] < ins_minor)) || + ((major_minor_patch[0] == ins_major) && + (major_minor_patch[1] == ins_minor) && + (major_minor_patch[2] < ins_patch))) + { + node_->log( + log_level::INFO, + "INS receiver has firmware version: " + + last_receiversetup_.rx_version + + ", which does not support all features. Please update to at least " + + std::to_string(ins_major) + "." + + std::to_string(ins_minor) + "." + + std::to_string(ins_patch) + " or consult README."); + } else + node_->setImprovedVsmHandling(); + } else if (settings_->septentrio_receiver_type == "gnss") + { + if ((major_minor_patch[0] < gnss_major) || + ((major_minor_patch[0] == gnss_major) && + (major_minor_patch[1] < gnss_minor)) || + ((major_minor_patch[0] == gnss_major) && + (major_minor_patch[1] == gnss_minor) && + (major_minor_patch[2] < gnss_patch))) + { + node_->log( + log_level::INFO, + "GNSS receiver has firmware version: " + + last_receiversetup_.rx_version + + ", which may not support all features. Please update to at least " + + std::to_string(gnss_major) + "." + + std::to_string(gnss_minor) + "." + + std::to_string(gnss_patch) + " or consult README."); + } + } + } + + break; + } + case RECEIVER_TIME: + { + ReceiverTimeMsg msg; + + if (!ReceiverTimeParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in ReceiverTime"); + break; + } + current_leap_seconds_ = msg.delta_ls; + break; + } + default: + { + node_->log(log_level::DEBUG, "unhandled SBF block " + + std::to_string(sbfId) + " received."); + break; + } + // Many more to be implemented... + } + } + + void MessageHandler::wait(Timestamp time_obj) + { + Timestamp unix_old = unix_time_; + unix_time_ = time_obj; + if ((unix_old != 0) && (unix_time_ > unix_old)) + { + auto sleep_nsec = unix_time_ - unix_old; + + std::stringstream ss; + ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; + node_->log(log_level::DEBUG, ss.str()); + + std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); + } + } + + void MessageHandler::parseNmea(const std::shared_ptr& telegram) + { + std::string message(telegram->message.begin(), telegram->message.end()); + /*node_->log( + LogLevel::DEBUG, + "The NMEA message contains " + std::to_string(message.size()) + + " bytes and is ready to be parsed. It reads: " + message);*/ + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + boost::tokenizer> tokens(message, sep_2); + std::vector body; + body.reserve(20); + for (boost::tokenizer>::iterator tok_iter = + tokens.begin(); + tok_iter != tokens.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + + std::string id(body[0].begin() + 1, body[0].end()); + + auto it = nmeaMap_.find(body[0]); + if (it != nmeaMap_.end()) + { + switch (it->second) + { + case 0: + { + // Create NmeaSentence struct to pass to GpggaParser::parseASCII + NMEASentence gga_message(id, body); + GpggaMsg msg; + GpggaParser parser_obj; + try + { + msg = parser_obj.parseASCII(gga_message, settings_->frame_id, + settings_->use_gnss_time, + telegram->stamp); + } catch (ParseException& e) + { + node_->log(log_level::DEBUG, + "GpggaMsg: " + std::string(e.what())); + break; + } + publish("gpgga", msg); + break; + } + case 1: + { + // Create NmeaSentence struct to pass to GprmcParser::parseASCII + NMEASentence rmc_message(id, body); + GprmcMsg msg; + GprmcParser parser_obj; + try + { + msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, + settings_->use_gnss_time, + telegram->stamp); + } catch (ParseException& e) + { + node_->log(log_level::DEBUG, + "GprmcMsg: " + std::string(e.what())); + break; + } + publish("gprmc", msg); + break; + } + case 2: + { + // Create NmeaSentence struct to pass to GpgsaParser::parseASCII + NMEASentence gsa_message(id, body); + GpgsaMsg msg; + GpgsaParser parser_obj; + try + { + msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, + settings_->use_gnss_time, + node_->getTime()); + } catch (ParseException& e) + { + node_->log(log_level::DEBUG, + "GpgsaMsg: " + std::string(e.what())); + break; + } + if (settings_->use_gnss_time) + { + if (settings_->septentrio_receiver_type == "gnss") + { + Timestamp time_obj = + timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc); + msg.header.stamp = timestampToRos(time_obj); + } + if (settings_->septentrio_receiver_type == "ins") + { + Timestamp time_obj = + timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc); + msg.header.stamp = timestampToRos(time_obj); + } + } else + msg.header.stamp = timestampToRos(telegram->stamp); + publish("gpgsa", msg); + break; + } + case 4: + { + // Create NmeaSentence struct to pass to GpgsvParser::parseASCII + NMEASentence gsv_message(id, body); + GpgsvMsg msg; + GpgsvParser parser_obj; + try + { + msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, + settings_->use_gnss_time, + node_->getTime()); + } catch (ParseException& e) + { + node_->log(log_level::DEBUG, + "GpgsvMsg: " + std::string(e.what())); + break; + } + if (settings_->use_gnss_time) + { + + if (settings_->septentrio_receiver_type == "gnss") + { + Timestamp time_obj = + timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc); + msg.header.stamp = timestampToRos(time_obj); + } + if (settings_->septentrio_receiver_type == "ins") + { + Timestamp time_obj = + timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc); + msg.header.stamp = timestampToRos(time_obj); + } + } else + msg.header.stamp = timestampToRos(telegram->stamp); + publish("gpgsv", msg); + break; + } + } + } else + { + node_->log(log_level::DEBUG, "Unknown NMEA message: " + body[0]); + } + } + +} // namespace io diff --git a/src/septentrio_gnss_driver/communication/pcap_reader.cpp b/src/septentrio_gnss_driver/communication/pcap_reader.cpp deleted file mode 100644 index 87f77e60..00000000 --- a/src/septentrio_gnss_driver/communication/pcap_reader.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include "septentrio_gnss_driver/communication/pcap_reader.hpp" - -#include -#include -#include -#include -#include -#include - -/** - * @file pcap_reader.cpp - * @date 07/05/2021 - * @author Ashwin A Nayar - * - * @brief Implements auxiliary reader object for handling pcap files. - * - * Functions include connecting to the file, reading the contents to the - * specified data buffer and graceful exit. - */ - -namespace pcapReader { - - PcapDevice::PcapDevice(ROSaicNodeBase* node, buffer_t& buffer) : - node_(node), m_dataBuff{buffer} - { - } - - PcapDevice::~PcapDevice() { disconnect(); } - - bool PcapDevice::connect(const char* device) - { - if (isConnected()) - return true; - // Try to open pcap file - if ((m_device = pcap_open_offline(device, m_errBuff)) == nullptr) - return false; - - m_deviceName = (char*)device; - // Try to compile filter program - if (pcap_compile(m_device, &m_pktFilter, "tcp dst port 3001", 1, - PCAP_NETMASK_UNKNOWN) != 0) - return false; - - node_->log(LogLevel::INFO, "Connected to" + std::string(m_deviceName)); - return true; - } - - void PcapDevice::disconnect() - { - if (!isConnected()) - return; - - pcap_close(m_device); - m_device = nullptr; - node_->log(LogLevel::INFO, "Disconnected from " + std::string(m_deviceName)); - } - - bool PcapDevice::isConnected() const { return m_device; } - - ReadResult PcapDevice::read() - { - if (!isConnected()) - return READ_ERROR; - - struct pcap_pkthdr* header; - const u_char* pktData; - int result; - - result = pcap_next_ex(m_device, &header, &pktData); - - if (result >= 0) - { - auto ipHdr = - reinterpret_cast(pktData + sizeof(struct ethhdr)); - uint32_t ipHdrLen = ipHdr->ihl * 4u; - - switch (ipHdr->protocol) - { - case 6: - { - if (header->len == 54) - { - return READ_SUCCESS; - } - bool storePkt = true; - - if (!m_lastPkt.empty()) - { - auto tcpHdr = reinterpret_cast( - pktData + ipHdrLen + sizeof(struct ethhdr)); - - auto lastIpHdr = reinterpret_cast(&(m_lastPkt[0])); - uint32_t lastIpHdrLen = lastIpHdr->ihl * 4u; - auto lastTcpHdr = reinterpret_cast( - &(m_lastPkt[0]) + lastIpHdrLen); - uint16_t lastLen = - ntohs(static_cast(lastIpHdr->tot_len)); - uint16_t newLen = ntohs(static_cast(ipHdr->tot_len)); - uint32_t lastSeq = ntohl(lastTcpHdr->seq); - uint32_t newSeq = ntohl(tcpHdr->seq); - - if (newSeq != lastSeq) - { - uint32_t dataOffset = lastTcpHdr->doff * 4; - m_dataBuff.insert(m_dataBuff.end(), - m_lastPkt.begin() + lastIpHdrLen + - dataOffset, - m_lastPkt.end()); - } else if (newLen <= lastLen) - { - storePkt = false; - } - } - - if (storePkt) - { - m_lastPkt.clear(); - m_lastPkt.insert(m_lastPkt.end(), - pktData + sizeof(struct ethhdr), - pktData + header->len); - } - break; - } - - default: - { - node_->log(LogLevel::ERROR, - "Skipping protocol: " + std::to_string(result)); - return READ_ERROR; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - return READ_SUCCESS; - } else if (result == -2) - { - node_->log(LogLevel::INFO, - "Done reading from " + std::string(m_deviceName)); - if (!m_lastPkt.empty()) - { - auto lastIpHdr = reinterpret_cast(&(m_lastPkt[0])); - uint32_t ipHdrLength = lastIpHdr->ihl * 4u; - - auto lastTcpHdr = - reinterpret_cast(&(m_lastPkt[0]) + ipHdrLength); - uint32_t dataOffset = lastTcpHdr->doff * 4u; - - m_dataBuff.insert(m_dataBuff.end(), - m_lastPkt.begin() + ipHdrLength + dataOffset, - m_lastPkt.end()); - - m_lastPkt.clear(); - } - disconnect(); - return READ_SUCCESS; - } else - { - node_->log(LogLevel::ERROR, "Error reading data"); - return READ_ERROR; - } - } -} // namespace pcapReader \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp deleted file mode 100644 index 8ea09e94..00000000 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ /dev/null @@ -1,3133 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include -#include -#include -#include - -/** - * The position_covariance array is populated in row-major order, where the basis of - * the correspond matrix is (E, N, U, Roll, Pitch, Heading). Important: The Euler - * angles (Roll, Pitch, Heading) are with respect to a vehicle-fixed (e.g. for - * mosaic-x5 in moving base mode via the command setAntennaLocation, ...) !local! NED - * frame or ENU frame if use_ros_axis_directions is set true Thus the orientation - * is !not! given with respect to the same frame as the position is given in. The - * cross-covariances are hence (apart from the fact that e.g. mosaic receivers do - * not calculate these quantities) set to zero. The position and the partial - * (with 2 antennas) or full (for INS receivers) orientation have covariance matrices - * available e.g. in the PosCovGeodetic or AttCovEuler blocks, yet those are separate - * computations. - */ - -using parsing_utilities::deg2rad; -using parsing_utilities::deg2radSq; -using parsing_utilities::rad2deg; - -PoseWithCovarianceStampedMsg -io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() -{ - PoseWithCovarianceStampedMsg msg; - if (settings_->septentrio_receiver_type == "gnss") - { - // Filling in the pose data - double yaw = 0.0; - if (validValue(last_atteuler_.heading)) - yaw = last_atteuler_.heading; - double pitch = 0.0; - if (validValue(last_atteuler_.pitch)) - pitch = last_atteuler_.pitch; - double roll = 0.0; - if (validValue(last_atteuler_.roll)) - roll = last_atteuler_.roll; - msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion( - deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); - msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); - msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); - msg.pose.pose.position.z = last_pvtgeodetic_.height; - // Filling in the covariance data in row-major order - msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon; - msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat; - msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt; - msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; - msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; - msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; - msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll); - msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll); - msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch); - msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll); - msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); - } - if (settings_->septentrio_receiver_type == "ins") - { - msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude); - msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude); - msg.pose.pose.position.z = last_insnavgeod_.height; - - // Filling in the pose data - if ((last_insnavgeod_.sb_list & 1) != 0) - { - // Pos autocov - msg.pose.covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.pose.covariance[7] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.pose.covariance[14] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); - } else - { - msg.pose.covariance[0] = -1.0; - msg.pose.covariance[7] = -1.0; - msg.pose.covariance[14] = -1.0; - } - if ((last_insnavgeod_.sb_list & 2) != 0) - { - double yaw = 0.0; - if (validValue(last_insnavgeod_.heading)) - yaw = last_insnavgeod_.heading; - double pitch = 0.0; - if (validValue(last_insnavgeod_.pitch)) - pitch = last_insnavgeod_.pitch; - double roll = 0.0; - if (validValue(last_insnavgeod_.roll)) - roll = last_insnavgeod_.roll; - // Attitude - msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion( - deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); - } else - { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); - } - if ((last_insnavgeod_.sb_list & 4) != 0) - { - // Attitude autocov - if (validValue(last_insnavgeod_.roll_std_dev)) - msg.pose.covariance[21] = parsing_utilities::square( - deg2rad(last_insnavgeod_.roll_std_dev)); - else - msg.pose.covariance[21] = -1.0; - if (validValue(last_insnavgeod_.pitch_std_dev)) - msg.pose.covariance[28] = parsing_utilities::square( - deg2rad(last_insnavgeod_.pitch_std_dev)); - else - msg.pose.covariance[28] = -1.0; - if (validValue(last_insnavgeod_.heading_std_dev)) - msg.pose.covariance[35] = parsing_utilities::square( - deg2rad(last_insnavgeod_.heading_std_dev)); - else - msg.pose.covariance[35] = -1.0; - } else - { - msg.pose.covariance[21] = -1.0; - msg.pose.covariance[28] = -1.0; - msg.pose.covariance[35] = -1.0; - } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - // Pos cov - msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; - } - if ((last_insnavgeod_.sb_list & 64) != 0) - { - // Attitude cov - msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - } - } - return msg; -}; - -DiagnosticArrayMsg io_comm_rx::RxMessage::DiagnosticArrayCallback() -{ - DiagnosticArrayMsg msg; - std::string serialnumber(last_receiversetup_.rx_serial_number); - DiagnosticStatusMsg gnss_status; - // Constructing the "level of operation" field - uint16_t indicators_type_mask = static_cast(255); - uint16_t indicators_value_mask = static_cast(3840); - uint16_t qualityind_pos; - for (uint16_t i = static_cast(0); - i < last_qualityind_.indicators.size(); ++i) - { - if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(0)) - { - qualityind_pos = i; - if (((last_qualityind_.indicators[i] & indicators_value_mask) >> 8) == - static_cast(0)) - { - gnss_status.level = DiagnosticStatusMsg::STALE; - } else if (((last_qualityind_.indicators[i] & indicators_value_mask) >> - 8) == static_cast(1) || - ((last_qualityind_.indicators[i] & indicators_value_mask) >> - 8) == static_cast(2)) - { - gnss_status.level = DiagnosticStatusMsg::WARN; - } else - { - gnss_status.level = DiagnosticStatusMsg::OK; - } - break; - } - } - // If the ReceiverStatus's RxError field is not 0, then at least one error has - // been detected. - if (last_receiverstatus_.rx_error != static_cast(0)) - { - gnss_status.level = DiagnosticStatusMsg::ERROR; - } - // Creating an array of values associated with the GNSS status - gnss_status.values.resize(static_cast(last_qualityind_.n - 1)); - for (uint16_t i = static_cast(0); - i != static_cast(last_qualityind_.n); ++i) - { - if (i == qualityind_pos) - { - continue; - } - if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(1)) - { - gnss_status.values[i].key = "GNSS Signals, Main Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(2)) - { - gnss_status.values[i].key = "GNSS Signals, Aux1 Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(11)) - { - gnss_status.values[i].key = "RF Power, Main Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(12)) - { - gnss_status.values[i].key = "RF Power, Aux1 Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(21)) - { - gnss_status.values[i].key = "CPU Headroom"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(25)) - { - gnss_status.values[i].key = "OCXO Stability"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(30)) - { - gnss_status.values[i].key = "Base Station Measurements"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else - { - assert((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(31)); - gnss_status.values[i].key = "RTK Post-Processing"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } - } - gnss_status.hardware_id = serialnumber; - gnss_status.name = "gnss"; - gnss_status.message = - "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; - msg.status.push_back(gnss_status); - return msg; -}; - -ImuMsg io_comm_rx::RxMessage::ImuCallback() -{ - ImuMsg msg; - - msg.linear_acceleration.x = last_extsensmeas_.acceleration_x; - msg.linear_acceleration.y = last_extsensmeas_.acceleration_y; - msg.linear_acceleration.z = last_extsensmeas_.acceleration_z; - - msg.angular_velocity.x = last_extsensmeas_.angular_rate_x; - msg.angular_velocity.y = last_extsensmeas_.angular_rate_y; - msg.angular_velocity.z = last_extsensmeas_.angular_rate_z; - - bool valid_orientation = true; - if (settings_->septentrio_receiver_type == "ins") - { - if (validValue(last_insnavgeod_.block_header.tow)) - { - Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow, - last_extsensmeas_.block_header.wnc, true); - Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - true); // Filling in the oreintation data - - static int64_t maxDt = (settings_->polling_period_pvt == 0) - ? 10000000 - : settings_->polling_period_pvt * 1000000; - if ((tsImu - tsIns) > maxDt) - { - valid_orientation = false; - } else - { - if ((last_insnavgeod_.sb_list & 2) != 0) - { - // Attitude - if (validValue(last_insnavgeod_.heading) && - validValue(last_insnavgeod_.pitch) && - validValue(last_insnavgeod_.roll)) - { - msg.orientation = - parsing_utilities::convertEulerToQuaternion( - deg2rad(last_insnavgeod_.heading), - deg2rad(last_insnavgeod_.pitch), - deg2rad(last_insnavgeod_.roll)); - } else - { - valid_orientation = false; - } - } else - { - valid_orientation = false; - } - if ((last_insnavgeod_.sb_list & 4) != 0) - { - // Attitude autocov - if (validValue(last_insnavgeod_.roll_std_dev) && - validValue(last_insnavgeod_.pitch_std_dev) && - validValue(last_insnavgeod_.heading_std_dev)) - { - msg.orientation_covariance[0] = parsing_utilities::square( - deg2rad(last_insnavgeod_.roll_std_dev)); - msg.orientation_covariance[4] = parsing_utilities::square( - deg2rad(last_insnavgeod_.pitch_std_dev)); - msg.orientation_covariance[8] = parsing_utilities::square( - deg2rad(last_insnavgeod_.heading_std_dev)); - } else - { - valid_orientation = false; - } - } else - { - valid_orientation = false; - } - if ((last_insnavgeod_.sb_list & 64) != 0) - { - // Attitude cov - msg.orientation_covariance[1] = - deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.orientation_covariance[2] = - deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.orientation_covariance[3] = - deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.orientation_covariance[5] = - deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.orientation_covariance[6] = - deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.orientation_covariance[7] = - deg2radSq(last_insnavgeod_.heading_pitch_cov); - } - } - } else - { - valid_orientation = false; - } - } else - { - valid_orientation = false; - } - - if (!valid_orientation) - { - msg.orientation.w = std::numeric_limits::quiet_NaN(); - msg.orientation.x = std::numeric_limits::quiet_NaN(); - msg.orientation.y = std::numeric_limits::quiet_NaN(); - msg.orientation.z = std::numeric_limits::quiet_NaN(); - msg.orientation_covariance[0] = -1.0; - msg.orientation_covariance[4] = -1.0; - msg.orientation_covariance[8] = -1.0; - } - - return msg; -}; - -TwistWithCovarianceStampedMsg -io_comm_rx::RxMessage::TwistCallback(bool fromIns /* = false*/) -{ - TwistWithCovarianceStampedMsg msg; - - if (fromIns) - { - msg.header = last_insnavgeod_.header; - - if ((last_insnavgeod_.sb_list & 8) != 0) - { - // Linear velocity in navigation frame - double ve = 0.0; - if (validValue(last_insnavgeod_.ve)) - ve = last_insnavgeod_.ve; - double vn = 0.0; - if (validValue(last_insnavgeod_.vn)) - vn = last_insnavgeod_.vn; - double vu = 0.0; - if (validValue(last_insnavgeod_.vu)) - vu = last_insnavgeod_.vu; - Eigen::Vector3d vel; - if (settings_->use_ros_axis_orientation) - { - // (ENU) - vel << ve, vn, vu; - } else - { - // (NED) - vel << vn, ve, -vu; - } - // Linear velocity - msg.twist.twist.linear.x = vel(0); - msg.twist.twist.linear.y = vel(1); - msg.twist.twist.linear.z = vel(2); - } else - { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); - } - - if (((last_insnavgeod_.sb_list & 16) != 0) && - ((last_insnavgeod_.sb_list & 2) != 0) && - ((last_insnavgeod_.sb_list & 8) != 0)) - { - Eigen::Matrix3d Cov_vel_n = Eigen::Matrix3d::Zero(); - if ((last_insnavgeod_.sb_list & 128) != 0) - { - // Linear velocity covariance - if (validValue(last_insnavgeod_.ve_std_dev)) - if (settings_->use_ros_axis_orientation) - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); - else - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); - else - Cov_vel_n(0, 0) = -1.0; - if (validValue(last_insnavgeod_.vn_std_dev)) - if (settings_->use_ros_axis_orientation) - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); - else - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); - else - Cov_vel_n(1, 1) = -1.0; - if (validValue(last_insnavgeod_.vu_std_dev)) - Cov_vel_n(2, 2) = - parsing_utilities::square(last_insnavgeod_.vu_std_dev); - else - Cov_vel_n(2, 2) = -1.0; - - if (validValue(last_insnavgeod_.ve_vn_cov)) - Cov_vel_n(0, 1) = Cov_vel_n(1, 0) = last_insnavgeod_.ve_vn_cov; - if (settings_->use_ros_axis_orientation) - { - if (validValue(last_insnavgeod_.ve_vu_cov)) - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = - last_insnavgeod_.ve_vu_cov; - if (validValue(last_insnavgeod_.vn_vu_cov)) - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = - last_insnavgeod_.vn_vu_cov; - } else - { - if (validValue(last_insnavgeod_.vn_vu_cov)) - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = - -last_insnavgeod_.vn_vu_cov; - if (validValue(last_insnavgeod_.ve_vu_cov)) - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = - -last_insnavgeod_.ve_vu_cov; - } - } else - { - Cov_vel_n(0, 0) = -1.0; - Cov_vel_n(1, 1) = -1.0; - Cov_vel_n(2, 2) = -1.0; - } - - msg.twist.covariance[0] = Cov_vel_n(0, 0); - msg.twist.covariance[1] = Cov_vel_n(0, 1); - msg.twist.covariance[2] = Cov_vel_n(0, 2); - msg.twist.covariance[6] = Cov_vel_n(1, 0); - msg.twist.covariance[7] = Cov_vel_n(1, 1); - msg.twist.covariance[8] = Cov_vel_n(1, 2); - msg.twist.covariance[12] = Cov_vel_n(2, 0); - msg.twist.covariance[13] = Cov_vel_n(2, 1); - msg.twist.covariance[14] = Cov_vel_n(2, 2); - } else - { - msg.twist.covariance[0] = -1.0; - msg.twist.covariance[7] = -1.0; - msg.twist.covariance[14] = -1.0; - } - // Autocovariances of angular velocity - msg.twist.covariance[21] = -1.0; - msg.twist.covariance[28] = -1.0; - msg.twist.covariance[35] = -1.0; - - } else - { - msg.header = last_pvtgeodetic_.header; - - if (last_pvtgeodetic_.error == 0) - { - // Linear velocity in navigation frame - double ve = 0.0; - if (validValue(last_pvtgeodetic_.ve)) - ve = last_pvtgeodetic_.ve; - double vn = 0.0; - if (validValue(last_pvtgeodetic_.vn)) - vn = last_pvtgeodetic_.vn; - double vu = 0.0; - if (validValue(last_pvtgeodetic_.vu)) - vu = last_pvtgeodetic_.vu; - Eigen::Vector3d vel; - if (settings_->use_ros_axis_orientation) - { - // (ENU) - vel << ve, vn, vu; - } else - { - // (NED) - vel << vn, ve, -vu; - } - // Linear velocity - msg.twist.twist.linear.x = vel(0); - msg.twist.twist.linear.y = vel(1); - msg.twist.twist.linear.z = vel(2); - } else - { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); - } - - if (last_velcovgeodetic_.error == 0) - { - Eigen::Matrix3d Cov_vel_n = Eigen::Matrix3d::Zero(); - // Linear velocity covariance in navigation frame - if (validValue(last_velcovgeodetic_.cov_veve)) - if (settings_->use_ros_axis_orientation) - Cov_vel_n(0, 0) = last_velcovgeodetic_.cov_veve; - else - Cov_vel_n(1, 1) = last_velcovgeodetic_.cov_veve; - else - Cov_vel_n(0, 0) = -1.0; - if (validValue(last_velcovgeodetic_.cov_vnvn)) - if (settings_->use_ros_axis_orientation) - Cov_vel_n(1, 1) = last_velcovgeodetic_.cov_vnvn; - else - Cov_vel_n(0, 0) = last_velcovgeodetic_.cov_vnvn; - else - Cov_vel_n(1, 1) = -1.0; - if (validValue(last_velcovgeodetic_.cov_vuvu)) - Cov_vel_n(2, 2) = last_velcovgeodetic_.cov_vuvu; - else - Cov_vel_n(2, 2) = -1.0; - - Cov_vel_n(0, 1) = Cov_vel_n(1, 0) = last_velcovgeodetic_.cov_vnve; - if (settings_->use_ros_axis_orientation) - { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = last_velcovgeodetic_.cov_vevu; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = last_velcovgeodetic_.cov_vnvu; - } else - { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = -last_velcovgeodetic_.cov_vnvu; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = -last_velcovgeodetic_.cov_vevu; - } - - msg.twist.covariance[0] = Cov_vel_n(0, 0); - msg.twist.covariance[1] = Cov_vel_n(0, 1); - msg.twist.covariance[2] = Cov_vel_n(0, 2); - msg.twist.covariance[6] = Cov_vel_n(1, 0); - msg.twist.covariance[7] = Cov_vel_n(1, 1); - msg.twist.covariance[8] = Cov_vel_n(1, 2); - msg.twist.covariance[12] = Cov_vel_n(2, 0); - msg.twist.covariance[13] = Cov_vel_n(2, 1); - msg.twist.covariance[14] = Cov_vel_n(2, 2); - } else - { - msg.twist.covariance[0] = -1.0; - msg.twist.covariance[7] = -1.0; - msg.twist.covariance[14] = -1.0; - } - // Autocovariances of angular velocity - msg.twist.covariance[21] = -1.0; - msg.twist.covariance[28] = -1.0; - msg.twist.covariance[35] = -1.0; - } - - msg.header.frame_id = "navigation"; - - return msg; -}; - -/** - * Localization in UTM coordinates. Yaw angle is converted from true north to grid - * north. Linear velocity of twist in body frame as per msg definition. Angular - * velocity not available, thus according autocovariances are set to -1.0. - */ -LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() -{ - LocalizationUtmMsg msg; - - int zone; - std::string zonestring; - bool northernHemisphere; - double easting; - double northing; - double gamma = 0.0; - if (fixedUtmZone_) - { - double k; - GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, northernHemisphere); - GeographicLib::UTMUPS::Forward( - rad2deg(last_insnavgeod_.latitude), rad2deg(last_insnavgeod_.longitude), - zone, northernHemisphere, easting, northing, gamma, k, zone); - zonestring = *fixedUtmZone_; - } else - { - double k; - GeographicLib::UTMUPS::Forward( - rad2deg(last_insnavgeod_.latitude), rad2deg(last_insnavgeod_.longitude), - zone, northernHemisphere, easting, northing, gamma, k); - zonestring = GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); - } - if (settings_->lock_utm_zone && !fixedUtmZone_) - fixedUtmZone_ = std::make_shared(zonestring); - - // UTM position (ENU) - if (settings_->use_ros_axis_orientation) - { - msg.pose.pose.position.x = easting; - msg.pose.pose.position.y = northing; - msg.pose.pose.position.z = last_insnavgeod_.height; - } else // (NED) - { - msg.pose.pose.position.x = northing; - msg.pose.pose.position.y = easting; - msg.pose.pose.position.z = -last_insnavgeod_.height; - } - - msg.header.frame_id = "utm_" + zonestring; - if (settings_->ins_use_poi) - msg.child_frame_id = settings_->poi_frame_id; // TODO param - else - msg.child_frame_id = settings_->frame_id; - - if ((last_insnavgeod_.sb_list & 1) != 0) - { - // Position autocovariance - msg.pose.covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.pose.covariance[7] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.pose.covariance[14] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); - } else - { - msg.pose.covariance[0] = -1.0; - msg.pose.covariance[7] = -1.0; - msg.pose.covariance[14] = -1.0; - } - - // Euler angles - double roll = 0.0; - if (validValue(last_insnavgeod_.roll)) - roll = deg2rad(last_insnavgeod_.roll); - double pitch = 0.0; - if (validValue(last_insnavgeod_.pitch)) - pitch = deg2rad(last_insnavgeod_.pitch); - double yaw = 0.0; - if (validValue(last_insnavgeod_.heading)) - yaw = deg2rad(last_insnavgeod_.heading); - // gamma for conversion from true north to grid north - if (settings_->use_ros_axis_orientation) - yaw -= deg2rad(gamma); - else - yaw += deg2rad(gamma); - - Eigen::Matrix3d R_n_b = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); - if ((last_insnavgeod_.sb_list & 2) != 0) - { - // Attitude - msg.pose.pose.orientation = - parsing_utilities::convertEulerToQuaternion(yaw, pitch, roll); - } else - { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); - } - if ((last_insnavgeod_.sb_list & 4) != 0) - { - // Attitude autocovariance - if (validValue(last_insnavgeod_.roll_std_dev)) - msg.pose.covariance[21] = - parsing_utilities::square(deg2rad(last_insnavgeod_.roll_std_dev)); - else - msg.pose.covariance[21] = -1.0; - if (validValue(last_insnavgeod_.pitch_std_dev)) - msg.pose.covariance[28] = - parsing_utilities::square(deg2rad(last_insnavgeod_.pitch_std_dev)); - else - msg.pose.covariance[28] = -1.0; - if (validValue(last_insnavgeod_.heading_std_dev)) - msg.pose.covariance[35] = - parsing_utilities::square(deg2rad(last_insnavgeod_.heading_std_dev)); - else - msg.pose.covariance[35] = -1.0; - } else - { - msg.pose.covariance[21] = -1.0; - msg.pose.covariance[28] = -1.0; - msg.pose.covariance[35] = -1.0; - } - if ((last_insnavgeod_.sb_list & 8) != 0) - { - // Linear velocity (ENU) - double ve = 0.0; - if (validValue(last_insnavgeod_.ve)) - ve = last_insnavgeod_.ve; - double vn = 0.0; - if (validValue(last_insnavgeod_.vn)) - vn = last_insnavgeod_.vn; - double vu = 0.0; - if (validValue(last_insnavgeod_.vu)) - vu = last_insnavgeod_.vu; - Eigen::Vector3d vel_enu; - if (settings_->use_ros_axis_orientation) - { - // (ENU) - vel_enu << ve, vn, vu; - } else - { - // (NED) - vel_enu << vn, ve, -vu; - } - // Linear velocity, rotate to body coordinates - Eigen::Vector3d vel_body = R_n_b * vel_enu; - msg.twist.twist.linear.x = vel_body(0); - msg.twist.twist.linear.y = vel_body(1); - msg.twist.twist.linear.z = vel_body(2); - } else - { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); - } - Eigen::Matrix3d Cov_vel_n = Eigen::Matrix3d::Zero(); - if ((last_insnavgeod_.sb_list & 16) != 0) - { - // Linear velocity autocovariance - if (validValue(last_insnavgeod_.ve_std_dev)) - if (settings_->use_ros_axis_orientation) - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); - else - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); - else - Cov_vel_n(0, 0) = -1.0; - if (validValue(last_insnavgeod_.vn_std_dev)) - if (settings_->use_ros_axis_orientation) - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); - else - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); - else - Cov_vel_n(1, 1) = -1.0; - if (validValue(last_insnavgeod_.vu_std_dev)) - Cov_vel_n(2, 2) = parsing_utilities::square(last_insnavgeod_.vu_std_dev); - else - Cov_vel_n(2, 2) = -1.0; - } else - { - Cov_vel_n(0, 0) = -1.0; - Cov_vel_n(1, 1) = -1.0; - Cov_vel_n(2, 2) = -1.0; - } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - // Position covariance - msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; - - if (settings_->use_ros_axis_orientation) - { - // (ENU) - msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; - } else - { - // (NED) - msg.pose.covariance[2] = -last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[8] = -last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[12] = -last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[13] = -last_insnavgeod_.longitude_height_cov; - } - } - if ((last_insnavgeod_.sb_list & 64) != 0) - { - // Attitude covariancae - msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - - if (!settings_->use_ros_axis_orientation) - { - // (NED) - msg.pose.covariance[33] *= -1.0; - msg.pose.covariance[23] *= -1.0; - msg.pose.covariance[22] *= -1.0; - msg.pose.covariance[27] *= -1.0; - } - } - if ((last_insnavgeod_.sb_list & 128) != 0) - { - Cov_vel_n(0, 1) = Cov_vel_n(1, 0) = last_insnavgeod_.ve_vn_cov; - if (settings_->use_ros_axis_orientation) - { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = last_insnavgeod_.ve_vu_cov; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = last_insnavgeod_.vn_vu_cov; - } else - { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = -last_insnavgeod_.vn_vu_cov; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = -last_insnavgeod_.ve_vu_cov; - } - } - - if (((last_insnavgeod_.sb_list & 16) != 0) && - ((last_insnavgeod_.sb_list & 2) != 0) && - ((last_insnavgeod_.sb_list & 8) != 0) && - validValue(last_insnavgeod_.ve_std_dev) && - validValue(last_insnavgeod_.vn_std_dev) && - validValue(last_insnavgeod_.vu_std_dev)) - { - // Rotate covariance matrix to body coordinates - Eigen::Matrix3d Cov_vel_body = R_n_b * Cov_vel_n * R_n_b.transpose(); - - msg.twist.covariance[0] = Cov_vel_body(0, 0); - msg.twist.covariance[1] = Cov_vel_body(0, 1); - msg.twist.covariance[2] = Cov_vel_body(0, 2); - msg.twist.covariance[6] = Cov_vel_body(1, 0); - msg.twist.covariance[7] = Cov_vel_body(1, 1); - msg.twist.covariance[8] = Cov_vel_body(1, 2); - msg.twist.covariance[12] = Cov_vel_body(2, 0); - msg.twist.covariance[13] = Cov_vel_body(2, 1); - msg.twist.covariance[14] = Cov_vel_body(2, 2); - } else - { - msg.twist.covariance[0] = -1.0; - msg.twist.covariance[7] = -1.0; - msg.twist.covariance[14] = -1.0; - } - // Autocovariances of angular velocity - msg.twist.covariance[21] = -1.0; - msg.twist.covariance[28] = -1.0; - msg.twist.covariance[35] = -1.0; - return msg; -}; - -/** - * The position_covariance array is populated in row-major order, where the basis of - * the corresponding matrix is ENU (so Cov_lonlon is in location 11 of the matrix). - * The B2b signal type of BeiDou is not checked for usage, since the SignalInfo field - * of the PVTGeodetic block does not disclose it. For that, one would need to go to - * the ObsInfo field of the MeasEpochChannelType1 sub-block. - */ -NavSatFixMsg io_comm_rx::RxMessage::NavSatFixCallback() -{ - NavSatFixMsg msg; - uint16_t mask = 15; // We extract the first four bits using this mask. - if (settings_->septentrio_receiver_type == "gnss") - { - uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; - switch (type_of_pvt_map[type_of_pvt]) - { - case evNoPVT: - { - msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; - break; - } - case evStandAlone: - case evFixed: - { - msg.status.status = NavSatStatusMsg::STATUS_FIX; - break; - } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: - { - msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; - break; - } - case evSBAS: - { - msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "PVTGeodetic's Mode field contains an invalid type of PVT solution."); - break; - } - } - bool gps_in_pvt = false; - bool glo_in_pvt = false; - bool com_in_pvt = false; - bool gal_in_pvt = false; - uint32_t mask_2 = 1; - for (int bit = 0; bit != 31; ++bit) - { - bool in_use = last_pvtgeodetic_.signal_info & mask_2; - if (bit <= 5 && in_use) - { - gps_in_pvt = true; - } - if (8 <= bit && bit <= 12 && in_use) - glo_in_pvt = true; - if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) - com_in_pvt = true; - if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) - gal_in_pvt = true; - mask_2 *= 2; - } - // Below, booleans will be promoted to integers automatically. - uint16_t service = - gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; - msg.status.service = service; - msg.latitude = rad2deg(last_pvtgeodetic_.latitude); - msg.longitude = rad2deg(last_pvtgeodetic_.longitude); - msg.altitude = last_pvtgeodetic_.height; - msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; - msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; - return msg; - } - - if (settings_->septentrio_receiver_type == "ins") - { - NavSatFixMsg msg; - uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; - switch (type_of_pvt_map[type_of_pvt]) - { - case evNoPVT: - { - msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; - break; - } - case evStandAlone: - case evFixed: - { - msg.status.status = NavSatStatusMsg::STATUS_FIX; - break; - } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: - { - msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; - break; - } - case evSBAS: - { - msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "INSNavGeod's Mode field contains an invalid type of PVT solution."); - break; - } - } - bool gps_in_pvt = false; - bool glo_in_pvt = false; - bool com_in_pvt = false; - bool gal_in_pvt = false; - uint32_t mask_2 = 1; - for (int bit = 0; bit != 31; ++bit) - { - bool in_use = last_pvtgeodetic_.signal_info & mask_2; - if (bit <= 5 && in_use) - { - gps_in_pvt = true; - } - if (8 <= bit && bit <= 12 && in_use) - glo_in_pvt = true; - if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) - com_in_pvt = true; - if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) - gal_in_pvt = true; - mask_2 *= 2; - } - // Below, booleans will be promoted to integers automatically. - uint16_t service = - gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; - msg.status.service = service; - msg.latitude = rad2deg(last_insnavgeod_.latitude); - msg.longitude = rad2deg(last_insnavgeod_.longitude); - msg.altitude = last_insnavgeod_.height; - - if ((last_insnavgeod_.sb_list & 1) != 0) - { - msg.position_covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.position_covariance[4] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.position_covariance[8] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); - } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; - msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; - } - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; - } - return msg; -}; - -/** - * Note that the field "dip" denotes the local magnetic inclination in degrees - * (positive when the magnetic field points downwards (into the Earth)). - * This quantity cannot be calculated by most Septentrio - * receivers. We assume that for the ROS field "err_time", we are requested to - * provide the 2 sigma uncertainty on the clock bias estimate in square meters, not - * the clock drift estimate (latter would be - * "2*std::sqrt(last_velcovgeodetic_.Cov_DtDt)"). - * The "err_track" entry is calculated via the Gaussian error propagation formula - * from the eastward and the northward velocities. For the formula's usage we have to - * assume that the eastward and the northward velocities are independent variables. - * Note that elevations and azimuths of visible satellites are taken from the - * ChannelStatus block, which provides 1 degree precision, while the SatVisibility - * block could provide hundredths of degrees precision. Change if imperative for your - * application... Definition of "visible satellite" adopted here: We define a visible - * satellite as being !up to! "in sync" mode with the receiver, which corresponds to - * last_measepoch_.N (signal-to-noise ratios are thereby available for these), though - * not last_channelstatus_.N, which also includes those "in search". In case certain - * values appear unphysical, please consult the firmware, since those most likely - * refer to Do-Not-Use values. - */ -GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() -{ - GPSFixMsg msg; - msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); - - // MeasEpoch Processing - std::vector cno_tracked; - std::vector svid_in_sync; - { - cno_tracked.reserve(last_measepoch_.type1.size()); - svid_in_sync.reserve(last_measepoch_.type1.size()); - for (const auto& measepoch_channel_type1 : last_measepoch_.type1) - { - // Define MeasEpochChannelType1 struct for the corresponding sub-block - svid_in_sync.push_back( - static_cast(measepoch_channel_type1.sv_id)); - uint8_t type_mask = - 15; // We extract the first four bits using this mask. - if (((measepoch_channel_type1.type & type_mask) == - static_cast(1)) || - ((measepoch_channel_type1.type & type_mask) == - static_cast(2))) - { - cno_tracked.push_back( - static_cast(measepoch_channel_type1.cn0) / 4); - } else - { - cno_tracked.push_back( - static_cast(measepoch_channel_type1.cn0) / 4 + - static_cast(10)); - } - } - } - - // ChannelStatus Processing - std::vector svid_in_sync_2; - std::vector elevation_tracked; - std::vector azimuth_tracked; - std::vector svid_pvt; - svid_pvt.clear(); - std::vector ordering; - { - svid_in_sync_2.reserve(last_channelstatus_.satInfo.size()); - elevation_tracked.reserve(last_channelstatus_.satInfo.size()); - azimuth_tracked.reserve(last_channelstatus_.satInfo.size()); - for (const auto& channel_sat_info : last_channelstatus_.satInfo) - { - bool to_be_added = false; - for (int32_t j = 0; j < static_cast(svid_in_sync.size()); ++j) - { - if (svid_in_sync[j] == static_cast(channel_sat_info.sv_id)) - { - ordering.push_back(j); - to_be_added = true; - break; - } - } - if (to_be_added) - { - svid_in_sync_2.push_back( - static_cast(channel_sat_info.sv_id)); - elevation_tracked.push_back( - static_cast(channel_sat_info.elev)); - static uint16_t azimuth_mask = 511; - azimuth_tracked.push_back(static_cast( - (channel_sat_info.az_rise_set & azimuth_mask))); - } - svid_pvt.reserve(channel_sat_info.stateInfo.size()); - for (const auto& channel_state_info : channel_sat_info.stateInfo) - { - // Define ChannelStateInfo struct for the corresponding sub-block - bool pvt_status = false; - uint16_t pvt_status_mask = std::pow(2, 15) + std::pow(2, 14); - for (int k = 15; k != -1; k -= 2) - { - uint16_t pvt_status_value = - (channel_state_info.pvt_status & pvt_status_mask) >> k - 1; - if (pvt_status_value == 2) - { - pvt_status = true; - } - if (k > 1) - { - pvt_status_mask = pvt_status_mask - std::pow(2, k) - - std::pow(2, k - 1) + std::pow(2, k - 2) + - std::pow(2, k - 3); - } - } - if (pvt_status) - { - svid_pvt.push_back(static_cast(channel_sat_info.sv_id)); - } - } - } - } - msg.status.satellite_used_prn = - svid_pvt; // Entries such as int32[] in ROS messages are to be treated as - // std::vectors. - msg.status.satellites_visible = static_cast(svid_in_sync.size()); - msg.status.satellite_visible_prn = svid_in_sync_2; - msg.status.satellite_visible_z = elevation_tracked; - msg.status.satellite_visible_azimuth = azimuth_tracked; - - // Reordering CNO vector to that of all previous arrays - std::vector cno_tracked_reordered; - if (static_cast(last_channelstatus_.n) != 0) - { - for (int32_t k = 0; k < static_cast(ordering.size()); ++k) - { - cno_tracked_reordered.push_back(cno_tracked[ordering[k]]); - } - } - msg.status.satellite_visible_snr = cno_tracked_reordered; - msg.err_time = 2 * std::sqrt(last_poscovgeodetic_.cov_bb); - - if (settings_->septentrio_receiver_type == "gnss") - { - - // PVT Status Analysis - uint16_t status_mask = 15; // We extract the first four bits using this mask. - uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & status_mask; - switch (type_of_pvt_map[type_of_pvt]) - { - case evNoPVT: - { - msg.status.status = GPSStatusMsg::STATUS_NO_FIX; - break; - } - case evStandAlone: - case evFixed: - { - msg.status.status = GPSStatusMsg::STATUS_FIX; - break; - } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: - { - msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; - break; - } - case evSBAS: - { - uint16_t reference_id = last_pvtgeodetic_.reference_id; - // Here come the PRNs of the 4 WAAS satellites.. - if (reference_id == 131 || reference_id == 133 || reference_id == 135 || - reference_id == 135) - { - msg.status.status = GPSStatusMsg::STATUS_WAAS_FIX; - } else - { - msg.status.status = GPSStatusMsg::STATUS_SBAS_FIX; - } - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "PVTGeodetic's Mode field contains an invalid type of PVT solution."); - break; - } - } - // Doppler is not used when calculating the velocities of, say, mosaic-x5, - // hence: - msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; - // Doppler is not used when calculating the orientation of, say, mosaic-x5, - // hence: - msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; - msg.status.position_source = GPSStatusMsg::SOURCE_GPS; - msg.latitude = rad2deg(last_pvtgeodetic_.latitude); - msg.longitude = rad2deg(last_pvtgeodetic_.longitude); - msg.altitude = last_pvtgeodetic_.height; - // Note that cog is of type float32 while track is of type float64. - msg.track = last_pvtgeodetic_.cog; - msg.speed = std::sqrt(parsing_utilities::square(last_pvtgeodetic_.vn) + - parsing_utilities::square(last_pvtgeodetic_.ve)); - msg.climb = last_pvtgeodetic_.vu; - msg.pitch = last_atteuler_.pitch; - msg.roll = last_atteuler_.roll; - if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) - { - msg.gdop = -1.0; - } else - { - msg.gdop = std::sqrt(parsing_utilities::square(last_dop_.pdop) + - parsing_utilities::square(last_dop_.tdop)); - } - if (last_dop_.pdop == 0.0) - { - msg.pdop = -1.0; - } else - { - msg.pdop = last_dop_.pdop; - } - if (last_dop_.hdop == 0.0) - { - msg.hdop = -1.0; - } else - { - msg.hdop = last_dop_.hdop; - } - if (last_dop_.vdop == 0.0) - { - msg.vdop = -1.0; - } else - { - msg.vdop = last_dop_.vdop; - } - if (last_dop_.tdop == 0.0) - { - msg.tdop = -1.0; - } else - { - msg.tdop = last_dop_.tdop; - } - msg.time = static_cast(last_pvtgeodetic_.block_header.tow) / 1000 + - static_cast(last_pvtgeodetic_.block_header.wnc * 7 * 24 * - 60 * 60); - msg.err = - 2 * (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + - static_cast(last_poscovgeodetic_.cov_lonlon) + - static_cast(last_poscovgeodetic_.cov_hgthgt))); - msg.err_horz = - 2 * (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + - static_cast(last_poscovgeodetic_.cov_lonlon))); - msg.err_vert = - 2 * std::sqrt(static_cast(last_poscovgeodetic_.cov_hgthgt)); - msg.err_track = - 2 * - (std::sqrt(parsing_utilities::square( - 1.0 / (last_pvtgeodetic_.vn + - parsing_utilities::square(last_pvtgeodetic_.ve) / - last_pvtgeodetic_.vn)) * - last_poscovgeodetic_.cov_lonlon + - parsing_utilities::square( - (last_pvtgeodetic_.ve) / - (parsing_utilities::square(last_pvtgeodetic_.vn) + - parsing_utilities::square(last_pvtgeodetic_.ve))) * - last_poscovgeodetic_.cov_latlat)); - msg.err_speed = - 2 * (std::sqrt(static_cast(last_velcovgeodetic_.cov_vnvn) + - static_cast(last_velcovgeodetic_.cov_veve))); - msg.err_climb = - 2 * std::sqrt(static_cast(last_velcovgeodetic_.cov_vuvu)); - msg.err_pitch = - 2 * std::sqrt(static_cast(last_attcoveuler_.cov_pitchpitch)); - msg.err_roll = - 2 * std::sqrt(static_cast(last_attcoveuler_.cov_rollroll)); - msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; - msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; - } - - if (settings_->septentrio_receiver_type == "ins") - { - // PVT Status Analysis - uint16_t status_mask = 15; // We extract the first four bits using this mask. - uint16_t type_of_pvt = - ((uint16_t)(last_insnavgeod_.gnss_mode)) & status_mask; - switch (type_of_pvt_map[type_of_pvt]) - { - case evNoPVT: - { - msg.status.status = GPSStatusMsg::STATUS_NO_FIX; - break; - } - case evStandAlone: - case evFixed: - { - msg.status.status = GPSStatusMsg::STATUS_FIX; - break; - } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: - { - msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; - break; - } - case evSBAS: - default: - { - node_->log( - LogLevel::DEBUG, - "INSNavGeod's Mode field contains an invalid type of PVT solution."); - break; - } - } - // Doppler is not used when calculating the velocities of, say, mosaic-x5, - // hence: - msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; - // Doppler is not used when calculating the orientation of, say, mosaic-x5, - // hence: - msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; - msg.status.position_source = GPSStatusMsg::SOURCE_GPS; - msg.latitude = rad2deg(last_insnavgeod_.latitude); - msg.longitude = rad2deg(last_insnavgeod_.longitude); - msg.altitude = last_insnavgeod_.height; - // Note that cog is of type float32 while track is of type float64. - if ((last_insnavgeod_.sb_list & 2) != 0) - { - msg.track = last_insnavgeod_.heading; - msg.pitch = last_insnavgeod_.pitch; - msg.roll = last_insnavgeod_.roll; - } - if ((last_insnavgeod_.sb_list & 8) != 0) - { - msg.speed = std::sqrt(parsing_utilities::square(last_insnavgeod_.vn) + - parsing_utilities::square(last_insnavgeod_.ve)); - - msg.climb = last_insnavgeod_.vu; - } - if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) - { - msg.gdop = -1.0; - } else - { - msg.gdop = std::sqrt(parsing_utilities::square(last_dop_.pdop) + - parsing_utilities::square(last_dop_.tdop)); - } - if (last_dop_.pdop == 0.0) - { - msg.pdop = -1.0; - } else - { - msg.pdop = last_dop_.pdop; - } - if (last_dop_.hdop == 0.0) - { - msg.hdop = -1.0; - } else - { - msg.hdop = last_dop_.hdop; - } - if (last_dop_.vdop == 0.0) - { - msg.vdop = -1.0; - } else - { - msg.vdop = last_dop_.vdop; - } - if (last_dop_.tdop == 0.0) - { - msg.tdop = -1.0; - } else - { - msg.tdop = last_dop_.tdop; - } - msg.time = static_cast(last_insnavgeod_.block_header.tow) / 1000 + - static_cast(last_insnavgeod_.block_header.wnc * 7 * 24 * - 60 * 60); - if ((last_insnavgeod_.sb_list & 1) != 0) - { - msg.err = - 2 * - (std::sqrt( - parsing_utilities::square(last_insnavgeod_.latitude_std_dev) + - parsing_utilities::square(last_insnavgeod_.longitude_std_dev) + - parsing_utilities::square(last_insnavgeod_.height_std_dev))); - msg.err_horz = - 2 * - (std::sqrt( - parsing_utilities::square(last_insnavgeod_.latitude_std_dev) + - parsing_utilities::square(last_insnavgeod_.longitude_std_dev))); - msg.err_vert = 2 * (std::sqrt(parsing_utilities::square( - last_insnavgeod_.height_std_dev))); - } - if (((last_insnavgeod_.sb_list & 8) != 0) || - ((last_insnavgeod_.sb_list & 1) != 0)) - { - msg.err_track = - 2 * (std::sqrt( - parsing_utilities::square( - 1.0 / (last_insnavgeod_.vn + - parsing_utilities::square(last_insnavgeod_.ve) / - last_insnavgeod_.vn)) * - parsing_utilities::square( - last_insnavgeod_.longitude_std_dev) + - parsing_utilities::square( - (last_insnavgeod_.ve) / - (parsing_utilities::square(last_insnavgeod_.vn) + - parsing_utilities::square(last_insnavgeod_.ve))) * - parsing_utilities::square( - last_insnavgeod_.latitude_std_dev))); - } - if ((last_insnavgeod_.sb_list & 8) != 0) - { - msg.err_speed = - 2 * (std::sqrt(parsing_utilities::square(last_insnavgeod_.vn) + - parsing_utilities::square(last_insnavgeod_.ve))); - msg.err_climb = - 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.vn)); - } - if ((last_insnavgeod_.sb_list & 2) != 0) - { - msg.err_pitch = - 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.pitch)); - } - if ((last_insnavgeod_.sb_list & 2) != 0) - { - msg.err_pitch = - 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.roll)); - } - if ((last_insnavgeod_.sb_list & 1) != 0) - { - msg.position_covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.position_covariance[4] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.position_covariance[8] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); - } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; - msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; - } - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; - } - return msg; -}; - -Timestamp io_comm_rx::RxMessage::timestampSBF(const uint8_t* data, - bool use_gnss_time) -{ - uint32_t tow = parsing_utilities::getTow(data); - uint16_t wnc = parsing_utilities::getWnc(data); - - return timestampSBF(tow, wnc, use_gnss_time); -} - -/// If the current time shall be employed, it is calculated via the time(NULL) -/// function found in the \ library At the time of writing the code (2020), -/// the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the -/// settings_->leap_seconds ROSaic parameter accordingly as soon as the next leap -/// second is inserted into the UTC time. -Timestamp io_comm_rx::RxMessage::timestampSBF(uint32_t tow, uint16_t wnc, - bool use_gnss_time) -{ - Timestamp time_obj; - if (use_gnss_time) - { - // conversion from GPS time of week and week number to UTC taking leap - // seconds into account - static uint64_t secToNSec = 1000000000; - static uint64_t mSec2NSec = 1000000; - static uint64_t nsOfGpsStart = - 315964800 * - secToNSec; // GPS week counter starts at 1980-01-06 which is 315964800 - // seconds since Unix epoch (1970-01-01 UTC) - static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec; - - time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek; - - if (current_leap_seconds_ != -128) - time_obj -= current_leap_seconds_ * secToNSec; - } else - { - time_obj = recvTimestamp_; - } - return time_obj; -} - -bool io_comm_rx::RxMessage::found() -{ - if (found_) - return true; - - // Verify header bytes - if (!this->isSBF() && !this->isNMEA() && !this->isResponse() && - !(g_read_cd && this->isConnectionDescriptor())) - { - return false; - } - - found_ = true; - return true; -} - -const uint8_t* io_comm_rx::RxMessage::search() -{ - if (found_) - { - next(); - } - // Search for message or a response header - for (; count_ > 0; --count_, ++data_) - { - if (this->isSBF() || this->isNMEA() || this->isResponse() || - (g_read_cd && this->isConnectionDescriptor())) - { - break; - } - } - found_ = true; - return data_; -} - -std::size_t io_comm_rx::RxMessage::messageSize() -{ - uint16_t pos = 0; - message_size_ = 0; - std::size_t count_copy = count_; - if (this->isResponse()) - { - do - { - ++message_size_; - ++pos; - --count_copy; - if (count_copy == 0) - break; - } while (!((data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED)) || - (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED && - data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 && - data_[pos + 4] == 0x4E) || - (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED && - data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 && - data_[pos + 4] == 0x53) || - (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED && - data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 && - data_[pos + 4] == 0x52)); - } else - { - do - { - ++message_size_; - ++pos; - --count_copy; - if (count_copy == 0) - break; - } while (!((data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED) || - data_[pos] == CARRIAGE_RETURN || data_[pos] == LINE_FEED)); - } - return message_size_; -} - -bool io_comm_rx::RxMessage::isMessage(const uint16_t id) -{ - if (this->isSBF()) - { - return (parsing_utilities::getId(data_) == static_cast(id)); - } else - { - return false; - } -} - -bool io_comm_rx::RxMessage::isMessage(std::string id) -{ - if (this->isNMEA()) - { - boost::char_separator sep(","); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - if (*tokens.begin() == id) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io_comm_rx::RxMessage::isSBF() -{ - if (count_ >= 2) - { - if (data_[0] == SBF_SYNC_BYTE_1 && data_[1] == SBF_SYNC_BYTE_2) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io_comm_rx::RxMessage::isNMEA() -{ - if (count_ >= 2) - { - if ((data_[0] == NMEA_SYNC_BYTE_1 && data_[1] == NMEA_SYNC_BYTE_2_1) || - (data_[0] == NMEA_SYNC_BYTE_1 && data_[1] == NMEA_SYNC_BYTE_2_2)) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io_comm_rx::RxMessage::isResponse() -{ - if (count_ >= 2) - { - if (data_[0] == RESPONSE_SYNC_BYTE_1 && data_[1] == RESPONSE_SYNC_BYTE_2) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io_comm_rx::RxMessage::isConnectionDescriptor() -{ - if (count_ >= 2) - { - if (data_[0] == CONNECTION_DESCRIPTOR_BYTE_1 && - data_[1] == CONNECTION_DESCRIPTOR_BYTE_2) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io_comm_rx::RxMessage::isErrorMessage() -{ - if (count_ >= 3) - { - if (data_[0] == RESPONSE_SYNC_BYTE_1 && data_[1] == RESPONSE_SYNC_BYTE_2 && - data_[2] == RESPONSE_SYNC_BYTE_3) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -std::string io_comm_rx::RxMessage::messageID() -{ - if (this->isSBF()) - { - std::stringstream ss; - ss << parsing_utilities::getId(data_); - return ss.str(); - } - if (this->isNMEA()) - { - boost::char_separator sep(","); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - return *tokens.begin(); - } - return std::string(); // less CPU work than return ""; -} - -const uint8_t* io_comm_rx::RxMessage::getPosBuffer() { return data_; } - -const uint8_t* io_comm_rx::RxMessage::getEndBuffer() { return data_ + count_; } - -uint16_t io_comm_rx::RxMessage::getBlockLength() -{ - if (this->isSBF()) - { - uint16_t block_length; - // Note that static_cast(data_[6]) would just take the one byte - // "data_[6]" and cast it as requested, !neglecting! the byte "data_[7]". - block_length = parsing_utilities::parseUInt16(data_ + 6); - return block_length; - } else - { - return 0; - } -} - -/** - * This method won't make data_ jump to the next message if the current one is an - * NMEA message or a command reply. In that case, search() will check the bytes one - * by one for the new message's sync bytes ($P, $G or $R). - */ -void io_comm_rx::RxMessage::next() -{ - std::size_t jump_size; - if (found()) - { - if (this->isNMEA() || this->isResponse() || - (g_read_cd && this->isConnectionDescriptor())) - { - if (g_read_cd && this->isConnectionDescriptor() && g_cd_count == 2) - { - g_read_cd = false; - } - jump_size = static_cast(1); - } - if (this->isSBF()) - { - if (crc_check_) - { - jump_size = static_cast(this->getBlockLength()); - // Some corrupted messages that survive the CRC check (this happened) - // could tell ROSaic their size is 0, which would lead to an endless - // while loop in the ReadCallback() method of the CallbackHandlers - // class. - if (jump_size == 0) - jump_size = static_cast(1); - } else - { - jump_size = static_cast(1); - } - } - } - found_ = false; - data_ += jump_size; - count_ -= jump_size; - // node_->log(LogLevel::DEBUG, "Jump about to happen with jump size %li and count - // after jump being %li.", jump_size, count_); - return; // For readability -} - -/** - * If GNSS time is used, Publishing is only done with valid leap seconds - */ -template -void io_comm_rx::RxMessage::publish(const std::string& topic, const M& msg) -{ - // TODO: maybe publish only if wnc and tow is valid? - if (!settings_->use_gnss_time || - (settings_->use_gnss_time && (current_leap_seconds_ != -128))) - { - node_->publishMessage(topic, msg); - } else - { - node_->log( - LogLevel::DEBUG, - "Not publishing message with GNSS time because no leap seconds are available yet."); - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - node_->log( - LogLevel::WARN, - "No leap seconds were set and none were received from log yet."); - } -} - -/** - * If GNSS time is used, Publishing is only done with valid leap seconds - */ -void io_comm_rx::RxMessage::publishTf(const LocalizationUtmMsg& msg) -{ - // TODO: maybe publish only if wnc and tow is valid? - if (!settings_->use_gnss_time || - (settings_->use_gnss_time && (current_leap_seconds_ != -128) && - (current_leap_seconds_ != 0))) - { - node_->publishTf(msg); - } else - { - node_->log( - LogLevel::DEBUG, - "Not publishing tf with GNSS time because no leap seconds are available yet."); - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - node_->log( - LogLevel::WARN, - "No leap seconds were set and none were received from log yet."); - } -} - -/** - * Note that putting the default in the definition's argument list instead of the - * declaration's is an added extra that is not available for function templates, - * hence no search = false here. Also note that the SBF block header part of the - * SBF-echoing ROS messages have ID fields that only show the block number as found - * in the firmware (e.g. 4007 for PVTGeodetic), without the revision number. NMEA - * 0183 messages are at most 82 characters long in principle, but most Septentrio Rxs - * by default increase precision on lat/lon s.t. the maximum allowed e.g. for GGA - * seems to be 89 on a mosaic-x5. Luckily, when parsing we do not care since we just - * search for \\. - */ -bool io_comm_rx::RxMessage::read(std::string message_key, bool search) -{ - if (search) - this->search(); - if (!found()) - return false; - if (this->isSBF()) - { - // If the CRC check is unsuccessful, return false - crc_check_ = isValid(data_); - if (!crc_check_) - { - node_->log( - LogLevel::DEBUG, - "CRC Check returned False. Not a valid data block. Retrieving full SBF block."); - return false; - } - } - switch (rx_id_map[message_key]) - { - case evPVTCartesian: // Position and velocity in XYZ - { // The curly bracket here is crucial: Declarations inside a block remain - // inside, and will die at - // the end of the block. Otherwise variable overloading etc. - PVTCartesianMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PVTCartesianParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PVTCartesian"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pvtcartesian", msg); - break; - } - case evPVTGeodetic: // Position and velocity in geodetic coordinate frame (ENU - // frame) - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PVTGeodeticParser(node_, dvec.begin(), dvec.end(), last_pvtgeodetic_)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PVTGeodetic"); - break; - } - last_pvtgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_gpsfix_ = true; - pvtgeodetic_has_arrived_navsatfix_ = true; - pvtgeodetic_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_pvtgeodetic) - publish("/pvtgeodetic", last_pvtgeodetic_); - break; - } - case evBaseVectorCart: - { - BaseVectorCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!BaseVectorCartParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in BaseVectorCart"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/basevectorcart", msg); - break; - } - case evBaseVectorGeod: - { - BaseVectorGeodMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!BaseVectorGeodParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in BaseVectorGeod"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/basevectorgeod", msg); - break; - } - case evPosCovCartesian: - { - PosCovCartesianMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PosCovCartesianParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PosCovCartesian"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/poscovcartesian", msg); - break; - } - case evPosCovGeodetic: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PosCovGeodeticParser(node_, dvec.begin(), dvec.end(), - last_poscovgeodetic_)) - { - poscovgeodetic_has_arrived_gpsfix_ = false; - poscovgeodetic_has_arrived_navsatfix_ = false; - poscovgeodetic_has_arrived_pose_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PosCovGeodetic"); - break; - } - last_poscovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); - poscovgeodetic_has_arrived_gpsfix_ = true; - poscovgeodetic_has_arrived_navsatfix_ = true; - poscovgeodetic_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_poscovgeodetic) - publish("/poscovgeodetic", last_poscovgeodetic_); - break; - } - case evAttEuler: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!AttEulerParser(node_, dvec.begin(), dvec.end(), last_atteuler_, - settings_->use_ros_axis_orientation)) - { - atteuler_has_arrived_gpsfix_ = false; - atteuler_has_arrived_pose_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in AttEuler"); - break; - } - last_atteuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_atteuler_.header.stamp = timestampToRos(time_obj); - atteuler_has_arrived_gpsfix_ = true; - atteuler_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_atteuler) - publish("/atteuler", last_atteuler_); - break; - } - case evAttCovEuler: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!AttCovEulerParser(node_, dvec.begin(), dvec.end(), last_attcoveuler_, - settings_->use_ros_axis_orientation)) - { - attcoveuler_has_arrived_gpsfix_ = false; - attcoveuler_has_arrived_pose_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in AttCovEuler"); - break; - } - last_attcoveuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_attcoveuler_.header.stamp = timestampToRos(time_obj); - attcoveuler_has_arrived_gpsfix_ = true; - attcoveuler_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_attcoveuler) - publish("/attcoveuler", last_attcoveuler_); - break; - } - case evINSNavCart: // Position, velocity and orientation in cartesian coordinate - // frame (ENU frame) - { - INSNavCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in INSNavCart"); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/insnavcart", msg); - break; - } - case evINSNavGeod: // Position, velocity and orientation in geodetic coordinate - // frame (ENU frame) - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), last_insnavgeod_, - settings_->use_ros_axis_orientation)) - { - insnavgeod_has_arrived_gpsfix_ = false; - insnavgeod_has_arrived_navsatfix_ = false; - insnavgeod_has_arrived_pose_ = false; - insnavgeod_has_arrived_localization_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in INSNavGeod"); - break; - } - if (settings_->ins_use_poi) - { - last_insnavgeod_.header.frame_id = settings_->poi_frame_id; - } else - { - last_insnavgeod_.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_insnavgeod_.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_gpsfix_ = true; - insnavgeod_has_arrived_navsatfix_ = true; - insnavgeod_has_arrived_pose_ = true; - insnavgeod_has_arrived_localization_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_insnavgeod) - publish("/insnavgeod", last_insnavgeod_); - if (settings_->publish_twist) - { - TwistWithCovarianceStampedMsg twist = TwistCallback(true); - publish("/twist_ins", twist); - } - break; - } - - case evIMUSetup: // IMU orientation and lever arm - { - IMUSetupMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!IMUSetupParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in IMUSetup"); - break; - } - msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/imusetup", msg); - break; - } - - case evVelSensorSetup: // Velocity sensor lever arm - { - VelSensorSetupMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!VelSensorSetupParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in VelSensorSetup"); - break; - } - msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/velsensorsetup", msg); - break; - } - - case evExtEventINSNavCart: // Position, velocity and orientation in cartesian - // coordinate frame (ENU frame) - { - INSNavCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtEventINSNavCart"); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/exteventinsnavcart", msg); - break; - } - - case evExtEventINSNavGeod: - { - INSNavGeodMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtEventINSNavGeod"); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/exteventinsnavgeod", msg); - break; - } - - case evExtSensorMeas: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - bool hasImuMeas = false; - if (!ExtSensorMeasParser(node_, dvec.begin(), dvec.end(), last_extsensmeas_, - settings_->use_ros_axis_orientation, hasImuMeas)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtSensorMeas"); - break; - } - last_extsensmeas_.header.frame_id = settings_->imu_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_extsensmeas_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_extsensormeas) - publish("/extsensormeas", last_extsensmeas_); - if (settings_->publish_imu && hasImuMeas) - { - ImuMsg msg; - try - { - msg = ImuCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "ImuMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->imu_frame_id; - msg.header.stamp = last_extsensmeas_.header.stamp; - publish("/imu", msg); - } - break; - } - - case evGPST: - { - TimeReferenceMsg msg; - Timestamp time_obj = - timestampSBF(data_, true); // We need the GPS time, hence true - msg.time_ref = timestampToRos(time_obj); - msg.source = "GPST"; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpst", msg); - break; - } - case evGPGGA: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - // No kept delimiters, hence "". Also, we specify that empty tokens should - // show up in the output when two delimiters are next to each other. Hence we - // also append the checksum part of the GGA message to "body" below, though - // it is not parsed. - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpggaParser::parseASCII - NMEASentence gga_message(id, body); - GpggaMsg msg; - Timestamp time_obj = node_->getTime(); - GpggaParser parser_obj; - try - { - msg = parser_obj.parseASCII(gga_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what())); - break; - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgga", msg); - break; - } - case evGPRMC: - { - Timestamp time_obj = node_->getTime(); - - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GprmcParser::parseASCII - NMEASentence rmc_message(id, body); - GprmcMsg msg; - GprmcParser parser_obj; - try - { - msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what())); - break; - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gprmc", msg); - break; - } - case evGPGSA: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsaParser::parseASCII - NMEASentence gsa_message(id, body); - GpgsaMsg msg; - GpgsaParser parser_obj; - try - { - msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsa", msg); - break; - } - case evGPGSV: - case evGLGSV: - case evGAGSV: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsvParser::parseASCII - NMEASentence gsv_message(id, body); - GpgsvMsg msg; - GpgsvParser parser_obj; - try - { - msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsv", msg); - break; - } - - if (settings_->septentrio_receiver_type == "gnss") - { - case evNavSatFix: - { - NavSatFixMsg msg; - try - { - msg = NavSatFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "NavSatFixMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_navsatfix_ = false; - poscovgeodetic_has_arrived_navsatfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/navsatfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSNavSatFix: - { - NavSatFixMsg msg; - try - { - msg = NavSatFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "NavSatFixMsg: " + std::string(e.what())); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_navsatfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/navsatfix", msg); - break; - } - } - - if (settings_->septentrio_receiver_type == "gnss") - { - case evGPSFix: - { - GPSFixMsg msg; - try - { - msg = GPSFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "GPSFixMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->frame_id; - msg.status.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - msg.status.header.stamp = timestampToRos(time_obj); - ++count_gpsfix_; - channelstatus_has_arrived_gpsfix_ = false; - measepoch_has_arrived_gpsfix_ = false; - dop_has_arrived_gpsfix_ = false; - pvtgeodetic_has_arrived_gpsfix_ = false; - poscovgeodetic_has_arrived_gpsfix_ = false; - velcovgeodetic_has_arrived_gpsfix_ = false; - atteuler_has_arrived_gpsfix_ = false; - attcoveuler_has_arrived_gpsfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpsfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSGPSFix: - { - GPSFixMsg msg; - try - { - msg = GPSFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "GPSFixMsg: " + std::string(e.what())); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - msg.status.header.frame_id = msg.header.frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - msg.status.header.stamp = timestampToRos(time_obj); - ++count_gpsfix_; - channelstatus_has_arrived_gpsfix_ = false; - measepoch_has_arrived_gpsfix_ = false; - dop_has_arrived_gpsfix_ = false; - insnavgeod_has_arrived_gpsfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpsfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - case evPoseWithCovarianceStamped: - { - PoseWithCovarianceStampedMsg msg; - try - { - msg = PoseWithCovarianceStampedCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "PoseWithCovarianceStampedMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_pose_ = false; - poscovgeodetic_has_arrived_pose_ = false; - atteuler_has_arrived_pose_ = false; - attcoveuler_has_arrived_pose_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pose", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSPoseWithCovarianceStamped: - { - PoseWithCovarianceStampedMsg msg; - try - { - msg = PoseWithCovarianceStampedCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "PoseWithCovarianceStampedMsg: " + std::string(e.what())); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_pose_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pose", msg); - break; - } - } - case evChannelStatus: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ChannelStatusParser(node_, dvec.begin(), dvec.end(), - last_channelstatus_)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ChannelStatus"); - break; - } - channelstatus_has_arrived_gpsfix_ = true; - break; - } - case evMeasEpoch: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!MeasEpochParser(node_, dvec.begin(), dvec.end(), last_measepoch_)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in MeasEpoch"); - break; - } - last_measepoch_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_measepoch_.header.stamp = timestampToRos(time_obj); - measepoch_has_arrived_gpsfix_ = true; - if (settings_->publish_measepoch) - publish("/measepoch", last_measepoch_); - break; - } - case evDOP: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!DOPParser(node_, dvec.begin(), dvec.end(), last_dop_)) - { - dop_has_arrived_gpsfix_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in DOP"); - break; - } - dop_has_arrived_gpsfix_ = true; - break; - } - case evVelCovGeodetic: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!VelCovGeodeticParser(node_, dvec.begin(), dvec.end(), - last_velcovgeodetic_)) - { - velcovgeodetic_has_arrived_gpsfix_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in VelCovGeodetic"); - break; - } - last_velcovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); - velcovgeodetic_has_arrived_gpsfix_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_velcovgeodetic) - publish("/velcovgeodetic", last_velcovgeodetic_); - if (settings_->publish_twist) - { - TwistWithCovarianceStampedMsg twist = TwistCallback(); - publish("/twist", twist); - } - break; - } - case evDiagnosticArray: - { - DiagnosticArrayMsg msg; - try - { - msg = DiagnosticArrayCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "DiagnosticArrayMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - msg.header.frame_id = settings_->frame_id; - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - receiverstatus_has_arrived_diagnostics_ = false; - qualityind_has_arrived_diagnostics_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/diagnostics", msg); - break; - } - case evLocalization: - { - LocalizationUtmMsg msg; - try - { - msg = LocalizationUtmCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); - break; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_localization) - publish("/localization", msg); - if (settings_->publish_tf) - publishTf(msg); - break; - } - case evReceiverStatus: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverStatusParser(node_, dvec.begin(), dvec.end(), - last_receiverstatus_)) - { - receiverstatus_has_arrived_diagnostics_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverStatus"); - break; - } - receiverstatus_has_arrived_diagnostics_ = true; - break; - } - case evQualityInd: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!QualityIndParser(node_, dvec.begin(), dvec.end(), last_qualityind_)) - { - qualityind_has_arrived_diagnostics_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in QualityInd"); - break; - } - qualityind_has_arrived_diagnostics_ = true; - break; - } - case evReceiverSetup: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverSetupParser(node_, dvec.begin(), dvec.end(), - last_receiversetup_)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverSetup"); - break; - } - static int32_t ins_major = 1; - static int32_t ins_minor = 3; - static int32_t ins_patch = 2; - static int32_t gnss_major = 4; - static int32_t gnss_minor = 10; - static int32_t gnss_patch = 0; - boost::tokenizer<> tok(last_receiversetup_.rx_version); - boost::tokenizer<>::iterator it = tok.begin(); - std::vector major_minor_patch; - major_minor_patch.reserve(3); - for (boost::tokenizer<>::iterator it = tok.begin(); it != tok.end(); ++it) - { - int32_t v = std::atoi(it->c_str()); - major_minor_patch.push_back(v); - } - if (major_minor_patch.size() < 3) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error of firmware version."); - } else - { - if ((settings_->septentrio_receiver_type == "ins") || - settings_->ins_in_gnss_mode) - { - if ((major_minor_patch[0] < ins_major) || - ((major_minor_patch[0] == ins_major) && - (major_minor_patch[1] < ins_minor)) || - ((major_minor_patch[0] == ins_major) && - (major_minor_patch[1] == ins_minor) && - (major_minor_patch[2] < ins_patch))) - { - node_->log( - LogLevel::WARN, - "INS receiver has firmware version: " + - last_receiversetup_.rx_version + - ", which does not support all features. Please update to at least " + - std::to_string(ins_major) + "." + - std::to_string(ins_minor) + "." + - std::to_string(ins_patch) + " or consult README."); - } - } else if (settings_->septentrio_receiver_type == "gnss") - { - if (major_minor_patch[0] < 3) - { - node_->log( - LogLevel::WARN, - "INS receiver seems to be used as GNSS. Some settings may trigger warnings or errors. Consider using 'ins_in_gnss_mode' as receiver type."); - } else if ((major_minor_patch[0] < gnss_major) || - ((major_minor_patch[0] == gnss_major) && - (major_minor_patch[1] < gnss_minor)) || - ((major_minor_patch[0] == gnss_major) && - (major_minor_patch[1] == gnss_minor) && - (major_minor_patch[2] < gnss_patch))) - { - node_->log( - LogLevel::WARN, - "GNSS receiver has firmware version: " + - last_receiversetup_.rx_version + - ", which may not support all features. Please update to at least " + - std::to_string(gnss_major) + "." + - std::to_string(gnss_minor) + "." + - std::to_string(gnss_patch) + " or consult README."); - } else - node_->log(LogLevel::ERROR, "gnss"); - } - } - - break; - } - case evReceiverTime: - { - ReceiverTimeMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverTimeParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverTime"); - break; - } - current_leap_seconds_ = msg.delta_ls; - break; - } - - // Many more to be implemented... - } - return true; -} - -void io_comm_rx::RxMessage::wait(Timestamp time_obj) -{ - Timestamp unix_old = unix_time_; - unix_time_ = time_obj; - if ((unix_old != 0) && (unix_time_ != unix_old)) - { - if (unix_time_ > unix_old) - { - auto sleep_nsec = unix_time_ - unix_old; - - std::stringstream ss; - ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; - node_->log(LogLevel::DEBUG, ss.str()); - - std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); - } - } - - // set leap seconds to paramter only if it was not set otherwise (by - // ReceiverTime) - if (current_leap_seconds_ == -128) - current_leap_seconds_ = settings_->leap_seconds; -} - -bool io_comm_rx::RxMessage::gnss_gpsfix_complete(uint32_t id) -{ - std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, - measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, - pvtgeodetic_has_arrived_gpsfix_, - poscovgeodetic_has_arrived_gpsfix_, - velcovgeodetic_has_arrived_gpsfix_, - atteuler_has_arrived_gpsfix_, - attcoveuler_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); -} - -bool io_comm_rx::RxMessage::ins_gpsfix_complete(uint32_t id) -{ - std::vector gpsfix_vec = { - channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, insnavgeod_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); -} - -bool io_comm_rx::RxMessage::gnss_navsatfix_complete(uint32_t id) -{ - std::vector navsatfix_vec = {pvtgeodetic_has_arrived_navsatfix_, - poscovgeodetic_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); -} - -bool io_comm_rx::RxMessage::ins_navsatfix_complete(uint32_t id) -{ - std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); -} - -bool io_comm_rx::RxMessage::gnss_pose_complete(uint32_t id) -{ - std::vector pose_vec = { - pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, - atteuler_has_arrived_pose_, attcoveuler_has_arrived_pose_}; - return allTrue(pose_vec, id); -} - -bool io_comm_rx::RxMessage::ins_pose_complete(uint32_t id) -{ - std::vector pose_vec = {insnavgeod_has_arrived_pose_}; - return allTrue(pose_vec, id); -} - -bool io_comm_rx::RxMessage::diagnostics_complete(uint32_t id) -{ - std::vector diagnostics_vec = {receiverstatus_has_arrived_diagnostics_, - qualityind_has_arrived_diagnostics_}; - return allTrue(diagnostics_vec, id); -} - -bool io_comm_rx::RxMessage::ins_localization_complete(uint32_t id) -{ - std::vector loc_vec = {insnavgeod_has_arrived_localization_}; - return allTrue(loc_vec, id); -} - -bool io_comm_rx::RxMessage::allTrue(std::vector& vec, uint32_t id) -{ - vec.erase(vec.begin() + id); - // Checks whether all entries in vec are true - return (std::all_of(vec.begin(), vec.end(), [](bool v) { return v; }) == true); -} diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp new file mode 100644 index 00000000..d7c8170e --- /dev/null +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -0,0 +1,159 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +/** + * @file telegram_handler.cpp + * @date 22/08/20 + * @brief Handles callbacks when reading NMEA/SBF messages + */ + +namespace io { + + void TelegramHandler::handleTelegram(const std::shared_ptr& telegram) + { + switch (telegram->type) + { + case telegram_type::SBF: + { + handleSbf(telegram); + break; + } + case telegram_type::NMEA: + { + handleNmea(telegram); + break; + } + case telegram_type::NMEA_INS: + { + handleNmea(telegram); + break; + } + case telegram_type::RESPONSE: + { + handleResponse(telegram); + break; + } + case telegram_type::ERROR_RESPONSE: + { + handleResponse(telegram); + break; + } + case telegram_type::CONNECTION_DESCRIPTOR: + { + handleCd(telegram); + break; + } + case telegram_type::UNKNOWN: + { + std::string block_in_string(telegram->message.begin(), + telegram->message.end()); + + node_->log(log_level::DEBUG, "A message received: " + block_in_string); + if (block_in_string.find("ReceiverCapabilities") != std::string::npos) + { + if (block_in_string.find("INS") != std::string::npos) + { + node_->setIsIns(); + } + + if (block_in_string.find("Heading") != std::string::npos) + { + node_->setHasHeading(); + } + capabilitiesSemaphore_.notify(); + } + break; + } + default: + { + node_->log(log_level::DEBUG, + "TelegramHandler received an invalid message to handle"); + break; + } + } + } + + void TelegramHandler::handleSbf(const std::shared_ptr& telegram) + { + messageHandler_.parseSbf(telegram); + } + + void TelegramHandler::handleNmea(const std::shared_ptr& telegram) + { + messageHandler_.parseNmea(telegram); + } + + void TelegramHandler::handleResponse(const std::shared_ptr& telegram) + { + std::string block_in_string(telegram->message.begin(), + telegram->message.end()); + + if (telegram->type == telegram_type::ERROR_RESPONSE) + { + node_->log( + log_level::ERROR, + "Invalid command just sent to the Rx! The Rx's response contains " + + std::to_string(block_in_string.size()) + " bytes and reads:\n " + + block_in_string); + + if (block_in_string == + std::string( + "$R? setGNSSAttitude: Argument 'Source' is invalid!\r\n")) + { + node_->log( + log_level::WARN, + "Rx does not support dual antenna mode, set parameter multi_antenna to false and/or disable publishing of atteuler."); + } + } else + { + node_->log(log_level::DEBUG, "The Rx's response contains " + + std::to_string(block_in_string.size()) + + " bytes and reads:\n " + + block_in_string); + } + responseSemaphore_.notify(); + } + + void TelegramHandler::handleCd(const std::shared_ptr& telegram) + { + node_->log(log_level::DEBUG, + "handleCd: " + std::string(telegram->message.begin(), + telegram->message.end())); + if (telegram->message.back() == CONNECTION_DESCRIPTOR_FOOTER) + { + mainConnectionDescriptor_ = + std::string(telegram->message.begin(), telegram->message.end() - 1); + + cdSemaphore_.notify(); + } + } +} // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/crc/crc.cpp b/src/septentrio_gnss_driver/crc/crc.cpp index 00e655ef..e3085a17 100644 --- a/src/septentrio_gnss_driver/crc/crc.cpp +++ b/src/septentrio_gnss_driver/crc/crc.cpp @@ -17,60 +17,69 @@ // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// POSSIBILITY OF SUCH DAMAGE. // // ***************************************************************************** -#include +#include #include -/** - * @file crc.cpp - * @brief Defines the CRC table and the functions to compute and validate the CRC of an SBF block - * @date 17/08/20 - */ +namespace crc { + /** + * @file crc.cpp + * @brief Defines the CRC table and the functions to compute and validate the CRC + * of an SBF block + * @date 17/08/20 + */ -uint16_t compute16CCITT (const uint8_t *buf, size_t buf_length) // The CRC we choose is 2 bytes, remember, hence uint16_t.. -{ - uint16_t crc = 0; // Seed is 0, as suggested by the firmware, will compute CRC in the forward direction.. - - for (size_t i = 0; i < buf_length; i++) - { - crc = (crc << 8) ^ CRC_LOOK_UP[uint8_t( (crc >> 8) ^ buf[i])]; - // The ^ (bitwise XOR) in C or C++ takes two numbers as operands and does XOR on every bit of two numbers. - // The result of XOR is 1 if the two bits are different. - // The << (left shift) in C or C++ takes two numbers, left shifts the bits of the first operand, - // the second operand decides the number of places to shift. - // The >> (right shift) in C or C++ takes two numbers, right shifts the bits of the first operand, - // the second operand decides the number of places to shift; you can just loose the smallest values if big-endian. - // The left shift and right shift operators should not be used for negative numbers. - // The left-shift and right-shift operators are equivalent to multiplication and division by 2 respectively, - // hence only rightshift is non-exact (remainder is not retained). - // CRC_LOOK_UP is constructed from truncated polynomial (divisor). - // The above implements a kind of CRC 32 algorithm: efficient, fast. - } + uint16_t compute16CCITT(const uint8_t* buf, + size_t buf_length) // The CRC we choose is 2 bytes, + // remember, hence uint16_t.. + { + uint16_t crc = 0; // Seed is 0, as suggested by the firmware, will compute + // CRC in the forward direction.. - return crc; -} + for (size_t i = 0; i < buf_length; i++) + { + crc = (crc << 8) ^ CRC_LOOK_UP[uint8_t((crc >> 8) ^ buf[i])]; + // The ^ (bitwise XOR) in C or C++ takes two numbers as operands and does + // XOR on every bit of two numbers. The result of XOR is 1 if the two + // bits are different. The << (left shift) in C or C++ takes two numbers, + // left shifts the bits of the first operand, the second operand decides + // the number of places to shift. The >> (right shift) in C or C++ takes + // two numbers, right shifts the bits of the first operand, the second + // operand decides the number of places to shift; you can just loose the + // smallest values if big-endian. The left shift and right shift + // operators should not be used for negative numbers. The left-shift and + // right-shift operators are equivalent to multiplication and division by + // 2 respectively, hence only rightshift is non-exact (remainder is not + // retained). CRC_LOOK_UP is constructed from truncated polynomial + // (divisor). The above implements a kind of CRC 32 algorithm: efficient, + // fast. + } -bool isValid(const uint8_t *block) -{ - // We need all of the message except for the first 4 bytes (Sync and CRC), i.e. we start at the address of ID. - uint16_t length = parsing_utilities::getLength(block); - if (length > 4) - { - uint16_t crc = compute16CCITT(block + 4, length - 4); - return (crc == parsing_utilities::getCrc(block)); - } - else - { - return false; - } -} + return crc; + } + + bool isValid(const std::vector& message) + { + // We need all of the message except for the first 4 bytes (Sync and CRC), + // i.e. we start at the address of ID. + uint16_t length = parsing_utilities::getLength(message); + if (length > 4) + { + uint16_t crc = compute16CCITT(message.data() + 4, length - 4); + return (crc == parsing_utilities::getCrc(message)); + } else + { + return false; + } + } +} // namespace crc diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 2f080c56..6d5e6ecd 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -38,7 +38,7 @@ * @brief The heart of the ROSaic driver: The ROS node that represents it */ -rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_) +rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) { param("activate_debug_log", settings_.activate_debug_log, false); if (settings_.activate_debug_log) @@ -50,7 +50,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_) ros::console::notifyLoggerLevelsChanged(); } - this->log(LogLevel::DEBUG, "Called ROSaicNode() constructor.."); + this->log(log_level::DEBUG, "Called ROSaicNode() constructor.."); tfListener_.reset(new tf2_ros::TransformListener(tfBuffer_)); @@ -59,60 +59,67 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_) return; // Initializes Connection - IO_.initializeIO(); + IO_.connect(); - // Subscribes to all requested Rx messages by adding entries to the C++ multimap - // storing the callback handlers and publishes ROS messages - IO_.defineMessages(); - - // Sends commands to the Rx regarding which SBF/NMEA messages it should output - // and sets all its necessary corrections-related parameters - if (!settings_.read_from_sbf_log && !settings_.read_from_pcap) - { - IO_.configureRx(); - } - - this->log(LogLevel::DEBUG, "Leaving ROSaicNode() constructor.."); + this->log(log_level::DEBUG, "Leaving ROSaicNode() constructor.."); } -bool rosaic_node::ROSaicNode::getROSParams() +[[nodiscard]] bool rosaic_node::ROSaicNode::getROSParams() { param("use_gnss_time", settings_.use_gnss_time, true); - param("frame_id", settings_.frame_id, (std::string) "gnss"); - param("imu_frame_id", settings_.imu_frame_id, (std::string) "imu"); - param("poi_frame_id", settings_.poi_frame_id, (std::string) "base_link"); - param("vsm_frame_id", settings_.vsm_frame_id, (std::string) "vsm"); - param("aux1_frame_id", settings_.aux1_frame_id, (std::string) "aux1"); + param("latency_compensation", settings_.latency_compensation, false); + param("frame_id", settings_.frame_id, static_cast("gnss")); + param("imu_frame_id", settings_.imu_frame_id, static_cast("imu")); + param("poi_frame_id", settings_.poi_frame_id, + static_cast("base_link")); + param("vsm_frame_id", settings_.vsm_frame_id, static_cast("vsm")); + param("aux1_frame_id", settings_.aux1_frame_id, + static_cast("aux1")); param("vehicle_frame_id", settings_.vehicle_frame_id, settings_.poi_frame_id); - param("local_frame_id", settings_.local_frame_id, (std::string) "odom"); + param("local_frame_id", settings_.local_frame_id, + static_cast("odom")); param("insert_local_frame", settings_.insert_local_frame, false); param("lock_utm_zone", settings_.lock_utm_zone, true); param("leap_seconds", settings_.leap_seconds, -128); + param("configure_rx", settings_.configure_rx, true); + // Communication parameters - param("device", settings_.device, std::string("/dev/ttyACM0")); + param("device", settings_.device, static_cast("/dev/ttyACM0")); getUint32Param("serial/baudrate", settings_.baudrate, static_cast(921600)); - param("serial/hw_flow_control", settings_.hw_flow_control, std::string("off")); - param("serial/rx_serial_port", settings_.rx_serial_port, std::string("USB1")); - param("login/user", settings_.login_user, std::string("")); - param("login/password", settings_.login_password, std::string("")); + param("serial/hw_flow_control", settings_.hw_flow_control, + static_cast("off")); + getUint32Param("stream_device/tcp/port", settings_.tcp_port, + static_cast(0)); + param("stream_device/tcp/ip_server", settings_.tcp_ip_server, + static_cast("")); + getUint32Param("stream_device/udp/port", settings_.udp_port, + static_cast(0)); + param("stream_device/udp/unicast_ip", settings_.udp_unicast_ip, + static_cast("")); + param("stream_device/udp/ip_server", settings_.udp_ip_server, + static_cast("")); + param("login/user", settings_.login_user, static_cast("")); + param("login/password", settings_.login_password, static_cast("")); settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list. - param("receiver_type", settings_.septentrio_receiver_type, std::string("gnss")); + param("receiver_type", settings_.septentrio_receiver_type, + static_cast("gnss")); if (!((settings_.septentrio_receiver_type == "gnss") || - (settings_.septentrio_receiver_type == "ins") || - (settings_.septentrio_receiver_type == "ins_in_gnss_mode"))) + (settings_.septentrio_receiver_type == "ins"))) { - this->log(LogLevel::FATAL, "Unkown septentrio_receiver_type " + - settings_.septentrio_receiver_type + - " use either gnss or ins."); + this->log(log_level::FATAL, "Unkown septentrio_receiver_type " + + settings_.septentrio_receiver_type + + " use either gnss or ins."); return false; } - if (settings_.septentrio_receiver_type == "ins_in_gnss_mode") + if (settings_.configure_rx && !settings_.tcp_ip_server.empty() && + !settings_.udp_ip_server.empty()) { - settings_.septentrio_receiver_type = "gnss"; - settings_.ins_in_gnss_mode = true; + this->log( + log_level::WARN, + "Both UDP and TCP have been defined to receive data, only TCP will be used. If UDP is intended, leave tcp/ip_server empty."); } // Polling period parameters @@ -122,7 +129,7 @@ bool rosaic_node::ROSaicNode::getROSParams() settings_.septentrio_receiver_type == "ins"))) { this->log( - LogLevel::FATAL, + log_level::FATAL, "Please specify a valid polling period for PVT-related SBF blocks and NMEA messages."); return false; } @@ -132,11 +139,16 @@ bool rosaic_node::ROSaicNode::getROSParams() settings_.septentrio_receiver_type == "ins"))) { this->log( - LogLevel::FATAL, + log_level::FATAL, "Please specify a valid polling period for PVT-unrelated SBF blocks and NMEA messages."); return false; } + // OSNMA parameters + param("osnma/mode", settings_.osnma.mode, std::string("off")); + param("osnma/ntp_server", settings_.osnma.ntp_server, std::string("")); + param("osnma/keep_open", settings_.osnma.keep_open, true); + // multi_antenna param param("multi_antenna", settings_.multi_antenna, false); @@ -146,30 +158,25 @@ bool rosaic_node::ROSaicNode::getROSParams() param("publish/gpsfix", settings_.publish_gpsfix, false); param("publish/pose", settings_.publish_pose, false); param("publish/diagnostics", settings_.publish_diagnostics, false); + param("publish/aimplusstatus", settings_.publish_aimplusstatus, false); + param("publish/galauthstatus", settings_.publish_galauthstatus, false); param("publish/gpgga", settings_.publish_gpgga, false); param("publish/gprmc", settings_.publish_gprmc, false); param("publish/gpgsa", settings_.publish_gpgsa, false); param("publish/gpgsv", settings_.publish_gpgsv, false); param("publish/measepoch", settings_.publish_measepoch, false); param("publish/pvtcartesian", settings_.publish_pvtcartesian, false); - param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, - (settings_.septentrio_receiver_type == "gnss")); + param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, false); param("publish/basevectorcart", settings_.publish_basevectorcart, false); param("publish/basevectorgeod", settings_.publish_basevectorgeod, false); param("publish/poscovcartesian", settings_.publish_poscovcartesian, false); - param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, - (settings_.septentrio_receiver_type == "gnss")); - param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, - (settings_.septentrio_receiver_type == "gnss")); - param( - "publish/atteuler", settings_.publish_atteuler, - ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)); - param( - "publish/attcoveuler", settings_.publish_attcoveuler, - ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)); + param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, false); + param("publish/velcovcartesian", settings_.publish_velcovcartesian, false); + param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, false); + param("publish/atteuler", settings_.publish_atteuler, false); + param("publish/attcoveuler", settings_.publish_attcoveuler, false); param("publish/insnavcart", settings_.publish_insnavcart, false); - param("publish/insnavgeod", settings_.publish_insnavgeod, - (settings_.septentrio_receiver_type == "ins")); + param("publish/insnavgeod", settings_.publish_insnavgeod, false); param("publish/imusetup", settings_.publish_imusetup, false); param("publish/velsensorsetup", settings_.publish_velsensorsetup, false); param("publish/exteventinsnavgeod", settings_.publish_exteventinsnavgeod, false); @@ -177,11 +184,24 @@ bool rosaic_node::ROSaicNode::getROSParams() param("publish/extsensormeas", settings_.publish_extsensormeas, false); param("publish/imu", settings_.publish_imu, false); param("publish/localization", settings_.publish_localization, false); + param("publish/localization_ecef", settings_.publish_localization_ecef, false); param("publish/twist", settings_.publish_twist, false); param("publish/tf", settings_.publish_tf, false); + param("publish/tf_ecef", settings_.publish_tf_ecef, false); + + if (settings_.publish_tf && settings_.publish_tf_ecef) + { + this->log( + log_level::WARN, + "Only one of the tfs may be published at once, just activating tf in ECEF "); + settings_.publish_tf = false; + } // Datum and marker-to-ARP offset param("datum", settings_.datum, std::string("Default")); + // WGS84 is equivalent to Default and kept for backwards compatibility + if (settings_.datum == "Default") + settings_.datum = "WGS84"; param("ant_type", settings_.ant_type, std::string("Unknown")); param("ant_aux1_type", settings_.ant_aux1_type, std::string("Unknown")); param("ant_serial_nr", settings_.ant_serial_nr, std::string()); @@ -331,28 +351,28 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.publish_atteuler) { this->log( - LogLevel::WARN, + log_level::WARN, "Pitch angle output by topic /atteuler is a tilt angle rotated by " + std::to_string(settings_.heading_offset) + "."); } if (settings_.publish_pose && (settings_.septentrio_receiver_type == "gnss")) { this->log( - LogLevel::WARN, + log_level::WARN, "Pitch angle output by topic /pose is a tilt angle rotated by " + std::to_string(settings_.heading_offset) + "."); } } - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "IMU roll offset: " + std::to_string(settings_.theta_x)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "IMU pitch offset: " + std::to_string(settings_.theta_y)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "IMU yaw offset: " + std::to_string(settings_.theta_z)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "Ant heading offset: " + std::to_string(settings_.heading_offset)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "Ant pitch offset: " + std::to_string(settings_.pitch_offset)); // ins_initial_heading param @@ -368,7 +388,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.publish_tf && !settings_.ins_use_poi) { this->log( - LogLevel::ERROR, + log_level::ERROR, "If tf shall be published, ins_use_poi has to be set to true! It is set automatically to true."); settings_.ins_use_poi = true; } @@ -414,7 +434,7 @@ bool rosaic_node::ROSaicNode::getROSParams() ntripSettings.send_gga = "off"; param("rtk_settings/" + ntrip + "/keep_open", ntripSettings.keep_open, true); - settings_.rtk_settings.ntrip.push_back(ntripSettings); + settings_.rtk.ntrip.push_back(ntripSettings); } // IP server for (uint8_t i = 1; i < 6; ++i) @@ -436,7 +456,7 @@ bool rosaic_node::ROSaicNode::getROSParams() ipSettings.send_gga = "off"; param("rtk_settings/" + ips + "/keep_open", ipSettings.keep_open, true); - settings_.rtk_settings.ip_server.push_back(ipSettings); + settings_.rtk.ip_server.push_back(ipSettings); } // Serial for (uint8_t i = 1; i < 6; ++i) @@ -460,7 +480,7 @@ bool rosaic_node::ROSaicNode::getROSParams() param("rtk_settings/" + serial + "/keep_open", serialSettings.keep_open, true); - settings_.rtk_settings.serial.push_back(serialSettings); + settings_.rtk.serial.push_back(serialSettings); } { @@ -471,28 +491,28 @@ bool rosaic_node::ROSaicNode::getROSParams() param("ntrip_settings/mode", tempString, std::string("")); if (tempString != "") this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/mode has been removed, see README under section rtk_settings."); param("ntrip_settings/caster", tempString, std::string("")); if (tempString != "") this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/caster has been removed, see README under section rtk_settings."); param("ntrip_settings/rx_has_internet", tempBool, false); if (tempBool) this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/rx_has_internet has been removed, see README under section rtk_settings."); param("ntrip_settings/rx_input_corrections_tcp", tempInt, 0); if (tempInt != 0) this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/rx_input_corrections_tcp has been removed, see README under section rtk_settings."); param("ntrip_settings/rx_input_corrections_serial", tempString, std::string("")); if (tempString != "") this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/rx_input_corrections_serial has been removed, see README under section rtk_settings."); } @@ -501,7 +521,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (!settings_.multi_antenna) { this->log( - LogLevel::WARN, + log_level::WARN, "AttEuler needs multi-antenna receiver. Multi-antenna setting automatically activated. Deactivate publishing of AttEuler if multi-antenna operation is not available."); settings_.multi_antenna = true; } @@ -516,9 +536,9 @@ bool rosaic_node::ROSaicNode::getROSParams() ins_use_vsm = ((settings_.ins_vsm_ros_source == "odometry") || (settings_.ins_vsm_ros_source == "twist")); if (!settings_.ins_vsm_ros_source.empty() && !ins_use_vsm) - this->log(LogLevel::ERROR, "unknown ins_vsm/ros/source " + - settings_.ins_vsm_ros_source + - " -> VSM input will not be used!"); + this->log(log_level::ERROR, "unknown ins_vsm/ros/source " + + settings_.ins_vsm_ros_source + + " -> VSM input will not be used!"); param("ins_vsm/ip_server/id", settings_.ins_vsm_ip_server_id, std::string("")); if (!settings_.ins_vsm_ip_server_id.empty()) @@ -529,7 +549,7 @@ bool rosaic_node::ROSaicNode::getROSParams() param("ins_vsm/ip_server/keep_open", settings_.ins_vsm_ip_server_keep_open, true); this->log( - LogLevel::INFO, + log_level::INFO, "external velocity sensor measurements via ip_server are used."); } @@ -541,7 +561,7 @@ bool rosaic_node::ROSaicNode::getROSParams() static_cast(115200)); param("ins_vsm/serial/keep_open", settings_.ins_vsm_serial_keep_open, true); - this->log(LogLevel::INFO, + this->log(log_level::INFO, "external velocity sensor measurements via serial are used."); } @@ -555,7 +575,7 @@ bool rosaic_node::ROSaicNode::getROSParams() { ins_use_vsm = false; this->log( - LogLevel::ERROR, + log_level::ERROR, "all elements of ins_vsm/ros/config have been set to false -> VSM input will not be used!"); } else { @@ -568,7 +588,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.ins_vsm_ros_variances.size() != 3) { this->log( - LogLevel::ERROR, + log_level::ERROR, "ins_vsm/ros/variances has to be of size 3 for var_x, var_y, and var_z -> VSM input will not be used!"); ins_use_vsm = false; settings_.ins_vsm_ros_source = ""; @@ -581,7 +601,7 @@ bool rosaic_node::ROSaicNode::getROSParams() (settings_.ins_vsm_ros_variances[i] <= 0.0)) { this->log( - LogLevel::ERROR, + log_level::ERROR, "ins_vsm/ros/config of element " + std::to_string(i) + " has been set to be used but its variance is not > 0.0 -> its VSM input will not be used!"); @@ -596,7 +616,7 @@ bool rosaic_node::ROSaicNode::getROSParams() ins_use_vsm = false; settings_.ins_vsm_ros_source = ""; this->log( - LogLevel::ERROR, + log_level::ERROR, "all elements of ins_vsm/ros/config have been set to false due to invalid covariances -> VSM input will not be used!"); } } @@ -605,24 +625,120 @@ bool rosaic_node::ROSaicNode::getROSParams() { settings_.ins_vsm_ros_source = ""; this->log( - LogLevel::ERROR, + log_level::ERROR, "ins_vsm/ros/config has to be of size 3 to signal wether to use v_x, v_y, and v_z -> VSM input will not be used!"); } if (ins_use_vsm) { - this->log(LogLevel::INFO, "ins_vsm/ros/source " + - settings_.ins_vsm_ros_source + - " will be used."); + this->log(log_level::INFO, "ins_vsm/ros/source " + + settings_.ins_vsm_ros_source + + " will be used."); registerSubscriber(); } + + if (!settings_.tcp_ip_server.empty()) + { + if (settings_.tcp_ip_server == settings_.udp_ip_server) + this->log( + log_level::ERROR, + "tcp/ip_server and udp/ip_server cannot use the same IP server"); + if (settings_.tcp_ip_server == settings_.ins_vsm_ip_server_id) + this->log( + log_level::ERROR, + "tcp/ip_server and ins_vsm/ip_server/id cannot use the same IP server"); + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.tcp_ip_server == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "tcp/ip_server and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } + } + if (!settings_.udp_ip_server.empty()) + { + if (settings_.udp_ip_server == settings_.ins_vsm_ip_server_id) + this->log( + log_level::ERROR, + "udp/ip_server and ins_vsm/ip_server/id cannot use the same IP server"); + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.udp_ip_server == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "udp/ip_server and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } + } + if (!settings_.ins_vsm_ip_server_id.empty()) + { + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.ins_vsm_ip_server_id == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "ins_vsm/ip_server/id and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } + } + if (settings_.rtk.ip_server.size() == 2) + { + if (!settings_.rtk.ip_server[0].id.empty() && + (settings_.rtk.ip_server[0].id == settings_.rtk.ip_server[1].id)) + this->log( + log_level::ERROR, + "rtk_settings/ip_server_1/id and rtk_settings/ip_server_2/id cannot use the same IP server"); + } + } + + boost::smatch match; + if (boost::regex_match(settings_.device, match, + boost::regex("(tcp)://(.+):(\\d+)"))) + { + settings_.device_tcp_ip = match[2]; + settings_.device_tcp_port = match[3]; + settings_.device_type = device_type::TCP; + } else if (boost::regex_match(settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) + { + settings_.read_from_sbf_log = true; + settings_.use_gnss_time = true; + settings_.device = match[2]; + settings_.device_type = device_type::SBF_FILE; + } else if (boost::regex_match( + settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) + { + settings_.read_from_pcap = true; + settings_.use_gnss_time = true; + settings_.device = match[2]; + settings_.device_type = device_type::PCAP_FILE; + } else if (boost::regex_match(settings_.device, match, + boost::regex("(serial):(.+)"))) + { + std::string proto(match[2]); + settings_.device_type = device_type::SERIAL; + settings_.device = proto; + } else + { + if (settings_.udp_ip_server.empty() || settings_.configure_rx || + (settings_.ins_vsm_ros_source == "odometry") || + (settings_.ins_vsm_ros_source == "twist")) + { + std::stringstream ss; + ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; + this->log(log_level::ERROR, ss.str()); + return false; + } } // To be implemented: RTCM, raw data settings, PPP, SBAS ... - this->log(LogLevel::DEBUG, "Finished getROSParams() method"); + this->log(log_level::DEBUG, "Finished getROSParams() method"); return true; } -bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns) +[[nodiscard]] bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, + bool isIns) const { return ((period == 0) || ((period == 5 && isIns)) || (period == 10) || (period == 20) || (period == 40) || (period == 50) || (period == 100) || @@ -635,7 +751,7 @@ bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns) void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, const std::string& sourceFrame, - TransformStampedMsg& T_s_t) + TransformStampedMsg& T_s_t) const { bool found = false; while (!found) @@ -648,16 +764,16 @@ void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, found = true; } catch (const tf2::TransformException& ex) { - this->log(LogLevel::WARN, "Waiting for transform from " + sourceFrame + - " to " + targetFrame + ": " + ex.what() + - "."); + this->log(log_level::WARN, "Waiting for transform from " + sourceFrame + + " to " + targetFrame + ": " + ex.what() + + "."); found = false; } } } void rosaic_node::ROSaicNode::getRPY(const QuaternionMsg& qm, double& roll, - double& pitch, double& yaw) + double& pitch, double& yaw) const { Eigen::Quaterniond q(qm.w, qm.x, qm.y, qm.z); Eigen::Quaterniond::RotationMatrixType C = q.matrix(); diff --git a/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp b/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp index 0e548757..e8396f5f 100644 --- a/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp +++ b/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp @@ -69,7 +69,12 @@ GpgsaMsg GpgsaParser::parseASCII(const NMEASentence& sentence, msg.header.frame_id = frame_id; msg.message_id = sentence.get_body()[0]; msg.auto_manual_mode = sentence.get_body()[1]; - parsing_utilities::parseUInt8(sentence.get_body()[2], msg.fix_mode); + if (!parsing_utilities::parseUInt8(sentence.get_body()[2], msg.fix_mode)) + { + std::stringstream error; + error << "GPGSA fix_mode parsing error."; + throw ParseException(error.str()); + } // Words 3-14 of the sentence are SV PRNs. Copying only the non-null strings.. // 0 is the character needed to fill the new character space, in case 12 (first // argument) is larger than sv_ids. @@ -81,14 +86,34 @@ GpgsaMsg GpgsaParser::parseASCII(const NMEASentence& sentence, { if (!id->empty()) { - parsing_utilities::parseUInt8(*id, msg.sv_ids[n_svs]); + if (!parsing_utilities::parseUInt8(*id, msg.sv_ids[n_svs])) + { + std::stringstream error; + error << "GPGSA sv_ids parsing error."; + throw ParseException(error.str()); + } ++n_svs; } } msg.sv_ids.resize(n_svs); - parsing_utilities::parseFloat(sentence.get_body()[15], msg.pdop); - parsing_utilities::parseFloat(sentence.get_body()[16], msg.hdop); - parsing_utilities::parseFloat(sentence.get_body()[17], msg.vdop); + if (!parsing_utilities::parseFloat(sentence.get_body()[15], msg.pdop)) + { + std::stringstream error; + error << "GPGSA pdop parsing error."; + throw ParseException(error.str()); + } + if (!parsing_utilities::parseFloat(sentence.get_body()[16], msg.hdop)) + { + std::stringstream error; + error << "GPGSA hdop parsing error."; + throw ParseException(error.str()); + } + if (!parsing_utilities::parseFloat(sentence.get_body()[17], msg.vdop)) + { + std::stringstream error; + error << "GPGSA vdop parsing error."; + throw ParseException(error.str()); + } return msg; } \ No newline at end of file diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index c7e3502e..6879b7ed 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -30,7 +30,7 @@ // ROSaic includes #include -#include +#include // C++ library includes #include // Boost @@ -44,22 +44,16 @@ namespace parsing_utilities { + const double pihalf = boost::math::constants::pi() / 2.0; + namespace qi = boost::spirit::qi; - double wrapAngle180to180(double angle) + [[nodiscard]] double wrapAngle180to180(double angle) { - while (angle > 180.0) - { - angle -= 360.0; - } - while (angle < -180.0) - { - angle += 360.0; - } - return angle; + return std::remainder(angle, 360.0); } - double parseDouble(const uint8_t* buffer) + [[nodiscard]] double parseDouble(const uint8_t* buffer) { double val; qi::parse(buffer, buffer + 8, qi::little_bin_double, val); @@ -71,12 +65,12 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseDouble(const std::string& string, double& value) + [[nodiscard]] bool parseDouble(const std::string& string, double& value) { return string_utilities::toDouble(string, value) || string.empty(); } - float parseFloat(const uint8_t* buffer) + [[nodiscard]] float parseFloat(const uint8_t* buffer) { float val; qi::parse(buffer, buffer + 4, qi::little_bin_float, val); @@ -88,7 +82,7 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseFloat(const std::string& string, float& value) + [[nodiscard]] bool parseFloat(const std::string& string, float& value) { return string_utilities::toFloat(string, value) || string.empty(); } @@ -100,7 +94,7 @@ namespace parsing_utilities { * reinterpret_cast(&x). Recall: data_type *var_name = reinterpret_cast * (pointer_variable) converts the pointer type, no return type */ - int16_t parseInt16(const uint8_t* buffer) + [[nodiscard]] int16_t parseInt16(const uint8_t* buffer) { int16_t val; qi::parse(buffer, buffer + 2, qi::little_word, val); @@ -112,7 +106,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseInt16(const std::string& string, int16_t& value, int32_t base) + [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, + int32_t base) { value = 0; if (string.empty()) @@ -132,7 +127,7 @@ namespace parsing_utilities { return false; } - int32_t parseInt32(const uint8_t* buffer) + [[nodiscard]] int32_t parseInt32(const uint8_t* buffer) { int32_t val; qi::parse(buffer, buffer + 4, qi::little_dword, val); @@ -144,7 +139,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseInt32(const std::string& string, int32_t& value, int32_t base) + [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, + int32_t base) { return string_utilities::toInt32(string, value, base) || string.empty(); } @@ -154,7 +150,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseUInt8(const std::string& string, uint8_t& value, int32_t base) + [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, + int32_t base) { value = 0; if (string.empty()) @@ -173,7 +170,7 @@ namespace parsing_utilities { return false; } - uint16_t parseUInt16(const uint8_t* buffer) + [[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer) { uint16_t val; qi::parse(buffer, buffer + 2, qi::little_word, val); @@ -185,7 +182,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseUInt16(const std::string& string, uint16_t& value, int32_t base) + [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, + int32_t base) { value = 0; if (string.empty()) @@ -204,7 +202,7 @@ namespace parsing_utilities { return false; } - uint32_t parseUInt32(const uint8_t* buffer) + [[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer) { uint32_t val; qi::parse(buffer, buffer + 4, qi::little_dword, val); @@ -216,7 +214,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseUInt32(const std::string& string, uint32_t& value, int32_t base) + [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, + int32_t base) { return string_utilities::toUInt32(string, value, base) || string.empty(); } @@ -226,14 +225,13 @@ namespace parsing_utilities { * in both the without-colon-delimiter and the number-of-seconds-since-midnight * formats. */ - double convertUTCDoubleToSeconds(double utc_double) + [[nodiscard]] double convertUTCDoubleToSeconds(double utc_double) { uint32_t hours = static_cast(utc_double) / 10000; uint32_t minutes = (static_cast(utc_double) - hours * 10000) / 100; - double seconds = - utc_double - static_cast(hours * 10000 + minutes * 100); - seconds += static_cast(hours * 3600 + minutes * 60); - return seconds; + + return utc_double - static_cast(hours * 10000 + minutes * 100) + + static_cast(hours * 3600 + minutes * 60); } /** @@ -241,12 +239,11 @@ namespace parsing_utilities { * into 60 seconds (of arc). Use of the degrees-minutes-seconds system is also * called DMS notation. */ - double convertDMSToDegrees(double dms) + [[nodiscard]] double convertDMSToDegrees(double dms) { uint32_t whole_degrees = static_cast(dms) / 100; double minutes = dms - static_cast(whole_degrees * 100); - double degrees = static_cast(whole_degrees) + minutes / 60.0; - return degrees; + return static_cast(whole_degrees) + minutes / 60.0; } /** @@ -265,7 +262,7 @@ namespace parsing_utilities { * "header.stamp.sec" field of ROS messages, while "header.stamp.nsec" is taken * care of separately. */ - time_t convertUTCtoUnix(double utc_double) + [[nodiscard]] time_t convertUTCtoUnix(double utc_double) { time_t time_now = time(0); struct tm* timeinfo; @@ -295,20 +292,16 @@ namespace parsing_utilities { */ // Inverse of gmtime, the latter converts time_t (Unix time) to tm (UTC time) - time_t date = timegm(timeinfo); - - // ROS_DEBUG("Since 1970/01/01 %jd seconds have passed.\n", (intmax_t) date); - return date; + return timegm(timeinfo); } //! The rotational sequence convention we adopt here (and Septentrio receivers' //! pitch, roll, yaw definition too) is the yaw-pitch-roll sequence, i.e. the - //! 3-2-1 sequence: The body first does yaw around the Z=Down-axis, then pitches - //! around the new Y=East=right-axis and finally rolls around the new - //! X=North=forward-axis. - QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll) + //! 3-2-1 sequence: The body first does yaw around the z-axis, then pitches + //! around the new y-axis and finally rolls around the new x-axis. + [[nodiscard]] Eigen::Quaterniond + convertEulerToQuaternion(double roll, double pitch, double yaw) { - // Abbreviations for the angular functions double cy = std::cos(yaw * 0.5); double sy = std::sin(yaw * 0.5); double cp = std::cos(pitch * 0.5); @@ -316,16 +309,95 @@ namespace parsing_utilities { double cr = std::cos(roll * 0.5); double sr = std::sin(roll * 0.5); - QuaternionMsg q; - q.w = cr * cp * cy + sr * sp * sy; - q.x = sr * cp * cy - cr * sp * sy; - q.y = cr * sp * cy + sr * cp * sy; - q.z = cr * cp * sy - sr * sp * cy; + return Eigen::Quaterniond( + cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy); + } + + [[nodiscard]] QuaternionMsg + quaternionToQuaternionMsg(const Eigen::Quaterniond& q) + { + QuaternionMsg qm; - return q; + qm.w = q.w(); + qm.x = q.x(); + qm.y = q.y(); + qm.z = q.z(); + + return qm; } - std::string convertUserPeriodToRxCommand(uint32_t period_user) + [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, + double pitch, double yaw) + { + return quaternionToQuaternionMsg(convertEulerToQuaternion(roll, pitch, yaw)); + } + + [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon) + { + double sr = sin((pihalf - lat) / 2.0); + double cr = cos((pihalf - lat) / 2.0); + double sy = sin((lon + pihalf) / 2.0); + double cy = cos((lon + pihalf) / 2.0); + + return Eigen::Quaterniond(cr * cy, sr * cy, sr * sy, cr * sy); + } + + [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon) + { + double sp = sin((-lat - pihalf) / 2.0); + double cp = cos((-lat - pihalf) / 2.0); + double sy = sin(lon / 2.0); + double cy = cos(lon / 2.0); + + return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy); + } + + [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon) + { + Eigen::Matrix3d R; + + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double sin_lon = sin(lon); + double cos_lon = cos(lon); + + R(0, 0) = -sin_lon; + R(0, 1) = -cos_lon * sin_lat; + R(0, 2) = cos_lon * cos_lat; + R(1, 0) = cos_lon; + R(1, 1) = -sin_lon * sin_lat; + R(1, 2) = sin_lon * cos_lat; + R(2, 0) = 0.0; + R(2, 1) = cos_lat; + R(2, 2) = sin_lat; + + return R; + } + + [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon) + { + Eigen::Matrix3d R; + + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double sin_lon = sin(lon); + double cos_lon = cos(lon); + + R(0, 0) = -cos_lon * sin_lat; + R(0, 1) = -sin_lon; + R(0, 2) = -cos_lon * cos_lat; + R(1, 0) = -sin_lon * sin_lat; + R(1, 1) = cos_lon; + R(1, 2) = -sin_lon * cos_lat; + R(2, 0) = cos_lat; + R(2, 1) = 0.0; + R(2, 2) = -sin_lat; + + return R; + } + + [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user) { std::string cmd; @@ -339,21 +411,34 @@ namespace parsing_utilities { return "min" + std::to_string(period_user / 60000); } - uint16_t getCrc(const uint8_t* buffer) { return parseUInt16(buffer + 2); } + [[nodiscard]] uint16_t getCrc(const std::vector& message) + { + return parseUInt16(message.data() + 2); + } - uint16_t getId(const uint8_t* buffer) + [[nodiscard]] uint16_t getId(const std::vector& message) { // Defines bit mask.. // Highest three bits are for revision and rest for block number static uint16_t mask = 8191; // Bitwise AND gives us all but highest 3 bits set to zero, rest unchanged - return parseUInt16(buffer + 4) & mask; + return parseUInt16(message.data() + 4) & mask; } - uint16_t getLength(const uint8_t* buffer) { return parseUInt16(buffer + 6); } + [[nodiscard]] uint16_t getLength(const std::vector& message) + { + return parseUInt16(message.data() + 6); + } - uint32_t getTow(const uint8_t* buffer) { return parseUInt32(buffer + 8); } + [[nodiscard]] uint32_t getTow(const std::vector& message) + { + return parseUInt32(message.data() + 8); + } + + [[nodiscard]] uint16_t getWnc(const std::vector& message) + { + return parseUInt16(message.data() + 12); + } - uint16_t getWnc(const uint8_t* buffer) { return parseUInt16(buffer + 12); } } // namespace parsing_utilities diff --git a/src/septentrio_gnss_driver/parsers/string_utilities.cpp b/src/septentrio_gnss_driver/parsers/string_utilities.cpp index 040b539c..e38b55b9 100644 --- a/src/septentrio_gnss_driver/parsers/string_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/string_utilities.cpp @@ -29,7 +29,7 @@ // ***************************************************************************** // ROSaic includes -#include +#include // C++ library includes #include #include @@ -50,7 +50,7 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toDouble(const std::string& string, double& value) + [[nodiscard]] bool toDouble(const std::string& string, double& value) { if (string.empty()) { @@ -76,7 +76,7 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toFloat(const std::string& string, float& value) + [[nodiscard]] bool toFloat(const std::string& string, float& value) { if (string.empty()) { @@ -101,7 +101,8 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toInt32(const std::string& string, int32_t& value, int32_t base) + [[nodiscard]] bool toInt32(const std::string& string, int32_t& value, + int32_t base) { if (string.empty()) { @@ -132,7 +133,8 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toUInt32(const std::string& string, uint32_t& value, int32_t base) + [[nodiscard]] bool toUInt32(const std::string& string, uint32_t& value, + int32_t base) { if (string.empty()) { @@ -160,7 +162,8 @@ namespace string_utilities { /** * Not used as of now.. */ - int8_t toInt8(const std::string& string, int8_t& value, int32_t base) + [[nodiscard]] int8_t toInt8(const std::string& string, int8_t& value, + int32_t base) { char* end; errno = 0; @@ -173,7 +176,8 @@ namespace string_utilities { /** * Not used as of now.. */ - uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base) + [[nodiscard]] uint8_t toUInt8(const std::string& string, uint8_t& value, + int32_t base) { char* end; errno = 0; @@ -183,7 +187,7 @@ namespace string_utilities { return true; } - std::string trimDecimalPlaces(double num) + [[nodiscard]] std::string trimDecimalPlaces(double num) { num = std::round(num * 1000); num = num / 1000; @@ -194,7 +198,7 @@ namespace string_utilities { return ss.str(); } - bool containsSpace(const std::string str) + [[nodiscard]] bool containsSpace(const std::string str) { for (size_t i = 0; i < str.size(); ++i) {