diff --git a/README.md b/README.md index ebb31e79..15abbe93 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # ublox -The `ublox` package provides support for [u-blox](http://www.u-blox.com) GPS receivers. Only the _serial_ configuration of the driver is documented here, but TCP communication is also supported by the driver (untested). +The `ublox` package provides support for [u-blox](http://www.u-blox.com) GPS receivers. Only the _serial_ configuration of the driver is documented here, but TCP/UDP communication is also supported by the driver (untested). The driver was originally written by Johannes Meyer. Changes made later are detailed in the version history below. diff --git a/ublox/CHANGELOG.rst b/ublox/CHANGELOG.rst index e5c7e907..c6570749 100644 --- a/ublox/CHANGELOG.rst +++ b/ublox/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package ublox ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.0 (2022-04-13) +------------------ + +2.0.0 (2020-10-13) +------------------ +* Initial ROS 2 port to Dashing +* Port the ublox package to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Contributors: Chao Qu, Chris Lalancette + 1.2.0 (2019-11-19) ------------------ diff --git a/ublox/CMakeLists.txt b/ublox/CMakeLists.txt index c3a0bbb1..a8d8404b 100644 --- a/ublox/CMakeLists.txt +++ b/ublox/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/ublox/package.xml b/ublox/package.xml index d9ae9cd4..b34a1235 100644 --- a/ublox/package.xml +++ b/ublox/package.xml @@ -1,19 +1,21 @@ - + ublox - 1.2.0 + 2.3.0 Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. Johannes Meyer Veronica Lane BSD - catkin http://wiki.ros.org/ublox + ament_cmake + ublox_serialization + ublox_msgs + ublox_gps + - - - + ament_cmake diff --git a/ublox_gps/CHANGELOG.rst b/ublox_gps/CHANGELOG.rst index 7c0390d3..e5f4cc88 100644 --- a/ublox_gps/CHANGELOG.rst +++ b/ublox_gps/CHANGELOG.rst @@ -2,6 +2,130 @@ Changelog for package ublox_gps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.0 (2022-04-13) +------------------ +* Revamp the building of the driver for modern ROS 2 practices. +* Fix parameter declaration types. (`#146 `_) +* Add the types to declared parameters. (`#141 `_) +* Add UDP support (`#140 `_) +* add Ublox ZED_F9P config (`#131 `_) +* Fix warnings in launch. +* [FEAT]: add launch and config directories to 'intall' package to avoid wrong launch location (`#125 `_) +* Fix wrong variable name in launch (`#120 `_) +* Contributors: CHAIWIT PHONKHEN, Chao Qu, Chris Lalancette, Daisuke Nishimatsu, Davidson Daniel Rojas Cediel, Kevin Hallenbeck + +2.0.0 (2020-10-13) +------------------ +* Initial ROS 2 port to Dashing +* Create publishers based on ROS parameters (`#1 `_) +* Make sure to reset for survey-in mode. +* Make sure to depend on tf2 properly. +* Make sure to have package.xml depend on ament_cmake_ros +* Fix getting RTCM ids and rates. +* Increase the timeout for NavPVT messages during Survey-In config. +* Configure the base more accurately. +* Update rover configuration. +* Create ROS 2 launch files. +* Finish porting configs to ROS 2. +* Fix heading output to comply with REP-103 +* Make ublox_gps composable. +* Fixes pointed out by clang-tidy. +* Rename rawDataStreamPa member variable to raw_data_stream_pa +* Fix a bug for reads of zero size. +* Re-enable logging in the AsyncWorker. +* Switch a couple more catch blocks to const. +* Remove -ggdb3 flag from CMakeLists.txt. +* Fix a few bugs pointed out by valgrind. +* Pass the logger down to the Gps level. +* Minor cleanup in the gps code. +* More fixes for declaring parameters. +* Declare more parameters. +* Declare more parameters correctly. +* More fixes so that we get the correct rates. +* More fixes around the codebase. +* Mark constants as such. +* Remove declareRosBoolean. +* Update configuration files for ROS 2. +* Port ublox_gps to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Split the main out into its own file. +* Move spinning out of the constructor. +* Cleanup includes in node.{hpp,cpp}. +* Move HpPosRecProduct class to its own files. +* Move HpgRefProduct class to its own files. +* Move UbloxFirmware9 class to its own files. +* Move UbloxFirmware8 class to its own files. +* Move UbloxFirmware7 to its own files. +* Move UbloxFirmware7Plus class to its own header file. +* Move UbloxFirmware6 class to its own files. +* Move UbloxFirmware class to its own files. +* Move RawDataProduct class into its own files. +* Move HpgRovProduct class to its own files. +* Move AdrUdrProduct class into its own files. +* Move TimProduct class into its own file. +* Make the node handle a member variable of UbloxNode. +* Remove the last uses of the global variable. +* Pass the nodehandle into more methods. +* Start passing the node into functions. +* Declare a few more booleans. +* Declare a lot more parameters. +* Finish removing the "enable" map. +* Move some more boolean parameters to declarations. +* Declare more parameters. +* Convert a few more parameters over to being declared. +* Declare the config_on_startup flag. +* Make dat/set a declared parameter. +* Make sure to declare the sbas parameter. +* Add in ROS2-like declare and get parameters. +* Replace templated publish call with lambdas. +* Make fix_status_service a member variable of UbloxFirmware. +* Move fixFromString and modelFromString into node.cpp +* Make gps a member variable of UbloxNode. +* Add namespaces to component_interface and fts_product.hpp +* Minor code improvements. +* Move FixDiagnostic class to its own file. +* Move UbloxTopicDiagnostic to its own file. +* Make gnss a member variable of UbloxNode. +* Move kNavSvInfoSubscribeRate into the base class that uses it. +* Remove kSubscribeRate. +* Move the kDefaultMeasRate to the class that needs it. +* Remove kROSQueueSize constant. +* Make rtcms a member variable of UbloxNode. +* Switch to a structure for RTCMs. +* Make freq_diag a member variable of UbloxNode. +* Make updater a member variable of UbloxNode. +* Make frame_id a member variable. +* Make config_on_startup_flag a member variable. +* Make meas_rate a member variable. +* Make nav_rate a class variable. +* Lots of small code updates throughout the GPS module. +* Get rid of global 'debug' variable. +* Move serialization into the ublox_serialization module. +* More rearrangement of header files to make a more sane structure. +* Move FTSProduct class into its own file. +* UbloxNode is not a component. +* Move ComponentInterface to its own file. +* Remove the one static ros::Subscriber. +* Remove last static advertiser. +* Switch to const references for std::string where possible. +* Switch to more idiomatic publisher initialization. +* Switch the one use of tf to tf2. +* Remove 'using namespace' uses. +* Switch to non-boost asio. +* Remove uses of 'new' throughout the codebase. +* Remove the last of boost from node.cpp +* Remove most of boost from node.cpp/.hpp. +* Remove a bunch of boost from node.cpp +* Remove more boost. +* Remove boost from worker.hpp +* Remove some uses of boost from async_worker.hpp +* Fully de-boostify callback.hpp +* Rearrange messages. +* Switch out boost time and mutex for std +* Remove trailing whitespace in all files. +* Rename header files to have .hpp extension. +* Contributors: Chao Qu, Chris Lalancette, Mabel Zhang + 1.2.0 (2019-11-19) ------------------ * Add support for ZED-F9P new RELPOSNED message and provide heading output diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index f3dc3fc1..9472ea3a 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -1,78 +1,113 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox_gps) -find_package(catkin REQUIRED COMPONENTS - tf - roscpp - roscpp_serialization - ublox_msgs - ublox_serialization - diagnostic_updater -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS tf roscpp ublox_msgs ublox_serialization) - -# include boost -find_package(Boost REQUIRED COMPONENTS system regex thread) -link_directories(${Boost_LIBRARY_DIR}) -include_directories(${Boost_INCLUDE_DIR}) - -# include other ublox packages -include_directories(${PROJECT_SOURCE_DIR}/include) -include_directories(${catkin_INCLUDE_DIRS}) -# link pthread -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread") +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") + +find_package(ament_cmake_ros REQUIRED) +find_package(asio REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nmea_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rtcm_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(ublox_msgs REQUIRED) +find_package(ublox_serialization REQUIRED) -# build library -add_library(ublox_gps src/gps.cpp) - -# fix msg compile order bug -add_dependencies(ublox_gps ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(ublox_gps - boost_system - boost_regex - boost_thread +# build node +add_library(ublox_gps + src/adr_udr_product.cpp + src/gnss.cpp + src/gps.cpp + src/hp_pos_rec_product.cpp + src/hpg_ref_product.cpp + src/hpg_rov_product.cpp + src/mkgmtime.c + src/node.cpp + src/raw_data_pa.cpp + src/raw_data_product.cpp + src/tim_product.cpp + src/ublox_firmware.cpp + src/ublox_firmware6.cpp + src/ublox_firmware7.cpp + src/ublox_firmware8.cpp + src/ublox_firmware9.cpp) +target_include_directories(ublox_gps PUBLIC + "$" + "$" + ${diagnostic_updater_INCLUDE_DIRS} ) - -target_link_libraries(ublox_gps - ${catkin_LIBRARIES} +target_link_libraries(ublox_gps PUBLIC + ${asio_LIBRARIES} + ${diagnostic_updater_LIBRARIES} + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_components::component + ${nmea_msgs_TARGETS} + ${rtcm_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + ${std_srvs_TARGETS} + tf2::tf2 + ${ublox_msgs_TARGETS} + ublox_serialization::ublox_serialization ) -# build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp) -set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) +install(TARGETS ublox_gps EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -target_link_libraries(ublox_gps_node boost_system boost_regex boost_thread) -target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) -target_link_libraries(ublox_gps_node ublox_gps) +add_executable(ublox_gps_node src/node_main.cpp) +target_link_libraries(ublox_gps_node PRIVATE + rclcpp::rclcpp + ublox_gps +) # build logger node -add_executable(ublox_logger_node src/logger_node_pa.cpp src/raw_data_pa.cpp) -set_target_properties(ublox_logger_node PROPERTIES OUTPUT_NAME ublox_logger) - -target_link_libraries(ublox_logger_node ${catkin_LIBRARIES}) - -install(TARGETS ublox_gps ublox_gps_node ublox_logger_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +add_executable(ublox_logger src/logger_node_pa.cpp src/raw_data_pa.cpp) +target_link_libraries(ublox_logger PRIVATE + rclcpp::rclcpp + ${std_msgs_TARGETS} + ublox_gps ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE +install(TARGETS + ublox_gps_node ublox_logger + DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config - PATTERN ".svn" EXCLUDE +rclcpp_components_register_nodes(ublox_gps + "ublox_node::UbloxNode") + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} ) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME}) + +ament_package() diff --git a/ublox_gps/cmake/Findasio.cmake b/ublox_gps/cmake/Findasio.cmake new file mode 100644 index 00000000..002b599a --- /dev/null +++ b/ublox_gps/cmake/Findasio.cmake @@ -0,0 +1,5 @@ +include(FindPackageHandleStandardArgs) + +find_path(ASIO_INCLUDE_DIR NAMES asio.hpp) +find_package_handle_standard_args(asio DEFAULT_MSG ASIO_INCLUDE_DIR) +mark_as_advanced(ASIO_INCLUDE_DIR) diff --git a/ublox_gps/config/c94_m8p_base.yaml b/ublox_gps/config/c94_m8p_base.yaml index dac60a47..a845728f 100644 --- a/ublox_gps/config/c94_m8p_base.yaml +++ b/ublox_gps/config/c94_m8p_base.yaml @@ -1,72 +1,73 @@ # Configuration Settings for C94-M8P device +ublox_gps_node: + ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) -debug: 1 # Range 0-4 (0 means no debug statements will print) + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM + device: /dev/ttyACM0 + frame_id: gps_base + dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics + # assumed. + fix_mode: auto + dr_limit: 0 + enable_ppp: false # Not supported by C94-M8P -device: /dev/ttyACM0 -frame_id: gps_base -dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics - # assumed. -fix_mode: auto -dr_limit: 0 -enable_ppp: false # Not supported by C94-M8P + rate: 1.0 # Measurement rate in Hz + nav_rate: 1 # in number of measurement cycles -rate: 4 # Measurement rate in Hz -nav_rate: 4 # in number of measurement cycles + uart1: + baudrate: 19200 # C94-M8P specific + in: 0 # No UART in for base + out: 32 # RTCM 3 -uart1: - baudrate: 19200 # C94-M8P specific - in: 0 # No UART in for base - out: 32 # RTCM 3 + # TMODE3 Config + tmode3: 1 # Survey-In Mode + sv_in: + reset: true # True: disables and re-enables survey-in (resets) + # False: Disables survey-in only if TMODE3 is + # disabled + min_dur: 300 # Survey-In Minimum Duration [s] + acc_lim: 3.0 # Survey-In Accuracy Limit [m] -# TMODE3 Config -tmode3: 1 # Survey-In Mode -sv_in: - reset: false # True: disables and re-enables survey-in (resets) - # False: Disables survey-in only if TMODE3 is - # disabled - min_dur: 300 # Survey-In Minimum Duration [s] - acc_lim: 3.0 # Survey-In Accuracy Limit [m] + # RTCM out config + rtcm: + ids: [5, 87, 77, 230] # RTCM Messages to configure for Base station + # Enabled: GPS MSM7, GLONASS MSM7, + # GLONASS CP bias, Stationary RTK ref + # 0xF5 0x05 Stationary RTK reference station + # ARP + # 0xF5 0x4A GPS MSM4 + # 0xF5 0x4D GPS MSM7 + # 0xF5 0x54 GLONASS MSM4 + # 0xF5 0x57 GLONASS MSM7 + # 0xF5 0x7C BeiDou MSM4 + # 0xF5 0x7F BeiDou MSM7 + # 0xF5 0xE6 GLONASS code-phase biases + # 0xF5 0xFE Reference station PVT + #(u-blox proprietary RTCM Message) + rates: [1, 1, 1, 10] # in number of navigation solutions, must match + # nav_rate, except for 0xE6 -# RTCM out config -rtcm: - ids: [5, 87, 77, 230] # RTCM Messages to configure for Base station - # Enabled: GPS MSM7, GLONASS MSM7, - # GLONASS CP bias, Stationary RTK ref - # 0xF5 0x05 Stationary RTK reference station - # ARP - # 0xF5 0x4A GPS MSM4 - # 0xF5 0x4D GPS MSM7 - # 0xF5 0x54 GLONASS MSM4 - # 0xF5 0x57 GLONASS MSM7 - # 0xF5 0x7C BeiDou MSM4 - # 0xF5 0x7F BeiDou MSM7 - # 0xF5 0xE6 GLONASS code-phase biases - # 0xF5 0xFE Reference station PVT - #(u-blox proprietary RTCM Message) - rates: [1, 1, 1, 10] # in number of navigation solutions, must match - # nav_rate, except for 0xE6 + dat: + set: false -dat: - set: false + # GNSS Config + gnss: + glonass: true # Supported by C94-M8P + beidou: false # Supported by C94-M8P + qzss: true # Supported by C94-M8P -# GNSS Config -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P + inf: + all: true # Whether to display all INF messages in console -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - aid: - hui: false - nav: - posecef: false + # Enable u-blox message publishers + publish: + all: true + aid: + hui: false + nav: + posecef: false diff --git a/ublox_gps/config/c94_m8p_rover.yaml b/ublox_gps/config/c94_m8p_rover.yaml index d16b5747..cd6f16cd 100644 --- a/ublox_gps/config/c94_m8p_rover.yaml +++ b/ublox_gps/config/c94_m8p_rover.yaml @@ -1,46 +1,56 @@ # Configuration Settings for C94-M8P device - -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM - -device: /dev/ttyACM1 -frame_id: gps -rate: 4 # in Hz -nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may - # be either 5 Hz (Dual constellation) or - # 8 Hz (GPS only) -dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), - # Max Alt: 50km - # Max Horizontal Velocity: 250 m/s, - # Max Vertical Velocity: 100 m/s -fix_mode: auto -enable_ppp: false # Not supported by C94-M8P -dr_limit: 0 - -uart1: - baudrate: 19200 # C94-M8P specific - in: 32 # RTCM 3 - out: 0 # No UART out for rover - -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P - -dgnss_mode: 3 # Fixed mode - -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - aid: - hui: false - - nav: - posecef: false +ublox_gps_node: + ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) + + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM + + device: /dev/ttyACM0 + frame_id: gps + rate: 1.0 # in Hz + nav_rate: 1 # [# of measurement cycles], recommended 1 Hz, may + # be either 5 Hz (Dual constellation) or + # 8 Hz (GPS only) + dynamic_model: portable # Airborne < 2G, 2D fix not supported (3D only), + # Max Alt: 50km + # Max Horizontal Velocity: 250 m/s, + # Max Vertical Velocity: 100 m/s + fix_mode: auto + enable_ppp: false # Not supported by C94-M8P + dr_limit: 0 + + # TMODE3 Config + tmode3: 0 # Survey-In Mode + sv_in: + reset: false # True: disables and re-enables survey-in (resets) + # False: Disables survey-in only if TMODE3 is + # disabled + min_dur: 300 # Survey-In Minimum Duration [s] + acc_lim: 3.0 # Survey-In Accuracy Limit [m] + + uart1: + baudrate: 19200 # C94-M8P specific + in: 32 # RTCM 3 + out: 0 # No UART out for rover + + gnss: + glonass: true # Supported by C94-M8P + beidou: false # Supported by C94-M8P + qzss: true # Supported by C94-M8P + + dgnss_mode: 3 # Fixed mode + + inf: + all: true # Whether to display all INF messages in console + + # Enable u-blox message publishers + publish: + all: true + aid: + hui: false + + nav: + posecef: false diff --git a/ublox_gps/config/c94_m8t_base.yaml b/ublox_gps/config/c94_m8t_base.yaml index d7c9dfe4..42c9a93f 100644 --- a/ublox_gps/config/c94_m8t_base.yaml +++ b/ublox_gps/config/c94_m8t_base.yaml @@ -1,25 +1,26 @@ -# Configuration Settings for 4-M8P device +# Configuration Settings for C94-M8T device +ublox_gps_node: + ros__parameters: + debug: 2 # Range 0-4 (0 means no debug statements will print) -debug: 2 # Range 0-4 (0 means no debug statements will print) + device: /dev/ttyACM0 + frame_id: m8t_base + dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics + # assumed. + fix_mode: auto -device: /dev/ttyACM0 -frame_id: m8t_base -dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics - # assumed. -fix_mode: auto + rate: 1 # Measurement rate in Hz + nav_rate: 1 # in number of measurement cycles -rate: 1 # Measurement rate in Hz -nav_rate: 1 # in number of measurement cycles + uart1: + baudrate: 9600 # C94-M8P specific + in: 0 # No UART in for base + out: 0 # RTCM 3 -uart1: - baudrate: 9600 # C94-M8P specific - in: 0 # No UART in for base - out: 0 # RTCM 3 + inf: + all: true # Whether to display all INF messages in console -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - tim: - tm2: true + # Enable u-blox message publishers + publish: + tim: + tm2: true diff --git a/ublox_gps/config/c94_m8t_rover.yaml b/ublox_gps/config/c94_m8t_rover.yaml index f154b45d..741fa28e 100644 --- a/ublox_gps/config/c94_m8t_rover.yaml +++ b/ublox_gps/config/c94_m8t_rover.yaml @@ -1,46 +1,48 @@ -# Configuration Settings for C94-M8P device - -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM - -device: /dev/ttyACM1 -frame_id: gps -rate: 4 # in Hz -nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may - # be either 5 Hz (Dual constellation) or - # 8 Hz (GPS only) -dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), - # Max Alt: 50km - # Max Horizontal Velocity: 250 m/s, - # Max Vertical Velocity: 100 m/s -fix_mode: auto -enable_ppp: false # Not supported by C94-M8P -dr_limit: 0 - -uart1: - baudrate: 19200 # C94-M8P specific - in: 32 # RTCM 3 - out: 0 # No UART out for rover - -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P - -dgnss_mode: 3 # Fixed mode - -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - aid: - hui: false - - nav: - posecef: false \ No newline at end of file +# Configuration Settings for C94-M8T device +ublox_gps_node: + ros__parameters: + + debug: 1 # Range 0-4 (0 means no debug statements will print) + + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM + + device: /dev/ttyACM1 + frame_id: gps + rate: 4 # in Hz + nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may + # be either 5 Hz (Dual constellation) or + # 8 Hz (GPS only) + dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), + # Max Alt: 50km + # Max Horizontal Velocity: 250 m/s, + # Max Vertical Velocity: 100 m/s + fix_mode: auto + enable_ppp: false # Not supported by C94-M8T + dr_limit: 0 + + uart1: + baudrate: 19200 # C94-M8P specific + in: 32 # RTCM 3 + out: 0 # No UART out for rover + + gnss: + glonass: true # Supported by C94-M8P + beidou: false # Supported by C94-M8P + qzss: false # Supported by C94-M8P + + dgnss_mode: 3 # Fixed mode + + inf: + all: true # Whether to display all INF messages in console + + # Enable u-blox message publishers + publish: + all: true + aid: + hui: false + + nav: + posecef: false diff --git a/ublox_gps/config/m8u_rover.yaml b/ublox_gps/config/m8u_rover.yaml deleted file mode 100644 index 5f57df95..00000000 --- a/ublox_gps/config/m8u_rover.yaml +++ /dev/null @@ -1,46 +0,0 @@ -# Configuration Settings for C94-M8P device - -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM - -device: /dev/ttyACM0 -frame_id: m8u -rate: 4 # in Hz -nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may - # be either 5 Hz (Dual constellation) or - # 8 Hz (GPS only) -dynamic_model: 0 # Airborne < 2G, 2D fix not supported (3D only), - # Max Alt: 50km - # Max Horizontal Velocity: 250 m/s, - # Max Vertical Velocity: 100 m/s -fix_mode: 3 -enable_ppp: true # Not supported by C94-M8P -dr_limit: 1 - -uart1: - baudrate: 115200 # C94-M8P specific - in: 16 # RTCM 3 - out: 16 # No UART out for rover - -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P - -dgnss_mode: 3 # Fixed mode - -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - esf: true - aid: - hui: false - nav: - posecef: false diff --git a/ublox_gps/config/neo_m8u_rover.yaml b/ublox_gps/config/neo_m8u_rover.yaml new file mode 100644 index 00000000..351b5d68 --- /dev/null +++ b/ublox_gps/config/neo_m8u_rover.yaml @@ -0,0 +1,48 @@ +# Configuration Settings for NEO-M8U device + +ublox_gps_node: + ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) + + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM + + device: /dev/ttyACM0 + frame_id: m8u + rate: 4.0 # in Hz + nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may + # be either 5 Hz (Dual constellation) or + # 8 Hz (GPS only) + dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), + # Max Alt: 50km + # Max Horizontal Velocity: 250 m/s, + # Max Vertical Velocity: 100 m/s + fix_mode: 3d + enable_ppp: true + dr_limit: 1 + + uart1: + baudrate: 115200 # NEO-M8U specific + in: 16 # RTCM 3 + out: 16 # No UART out for rover + + gnss: + glonass: true # Supported by NEO-M8U + beidou: false # Supported by NEO-M8U + qzss: false # Supported by NEO-M8U + + dgnss_mode: 3 # Fixed mode + + inf: + all: true # Whether to display all INF messages in console + + # Enable u-blox message publishers + publish: + all: true + esf: true + aid: + hui: false + nav: + posecef: false diff --git a/ublox_gps/config/zed_f9p.yaml b/ublox_gps/config/zed_f9p.yaml new file mode 100644 index 00000000..9fd45c79 --- /dev/null +++ b/ublox_gps/config/zed_f9p.yaml @@ -0,0 +1,26 @@ +# Configuration Settings for C94-M8P device +ublox_gps_node: + ros__parameters: + debug: 0 # Range 0-4 (0 means no debug statements will print) + device: /dev/ttyACM0 + frame_id: gps + uart1: + baudrate: 9600 + # TMODE3 Config + tmode3: 1 # Survey-In Mode + sv_in: + reset: True # True: disables and re-enables survey-in (resets) + # False: Disables survey-in only if TMODE3 is + # disabled + min_dur: 300 # Survey-In Minimum Duration [s] + acc_lim: 3.0 # Survey-In Accuracy Limit [m] + + inf: + all: true # Whether to display all INF messages in console + + publish: + all: true + aid: + hui: false + nav: + posecef: false diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp new file mode 100644 index 00000000..b00022df --- /dev/null +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -0,0 +1,93 @@ +#ifndef UBLOX_GPS_ADR_UDR_PRODUCT_HPP +#define UBLOX_GPS_ADR_UDR_PRODUCT_HPP + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for Automotive Dead Reckoning (ADR) and + * Untethered Dead Reckoning (UDR) Devices. + */ +class AdrUdrProduct final : public virtual ComponentInterface { + public: + explicit AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); + + /** + * @brief Get the ADR/UDR parameters. + * + * @details Get the use_adr parameter and check that the nav_rate is 1 Hz. + */ + void getRosParams() override; + + /** + * @brief Configure ADR/UDR settings. + * @details Configure the use_adr setting. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Initialize the ROS diagnostics for the ADR/UDR device. + * @todo unimplemented + */ + void initializeRosDiagnostics() override { + // RCLCPP_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", + // "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); + } + + /** + * @brief Subscribe to ADR/UDR messages. + * + * @details Subscribe to NavATT, ESF and HNR messages based on user + * parameters. + */ + void subscribe(std::shared_ptr gps) override; + + private: + //! Whether or not to enable dead reckoning + bool use_adr_; + float protocol_version_; + + sensor_msgs::msg::Imu imu_; + sensor_msgs::msg::TimeReference t_ref_; + + void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); + + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr time_ref_pub_; + rclcpp::Publisher::SharedPtr nav_att_pub_; + rclcpp::Publisher::SharedPtr esf_ins_pub_; + rclcpp::Publisher::SharedPtr esf_meas_pub_; + rclcpp::Publisher::SharedPtr esf_raw_pub_; + rclcpp::Publisher::SharedPtr esf_status_pub_; + rclcpp::Publisher::SharedPtr hnr_pvt_pub_; + + uint16_t nav_rate_; + uint16_t meas_rate_; + + std::string frame_id_; + std::shared_ptr updater_; + rclcpp::Node* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP \ No newline at end of file diff --git a/ublox_gps/include/ublox_gps/async_worker.h b/ublox_gps/include/ublox_gps/async_worker.h deleted file mode 100644 index c41e74d1..00000000 --- a/ublox_gps/include/ublox_gps/async_worker.h +++ /dev/null @@ -1,271 +0,0 @@ -//============================================================================== -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * 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. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, 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 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 UBLOX_GPS_ASYNC_WORKER_H -#define UBLOX_GPS_ASYNC_WORKER_H - -#include - -#include -#include -#include -#include -#include - - -#include "worker.h" - -namespace ublox_gps { - -int debug; //!< Used to determine which debug messages to display - -/** - * @brief Handles Asynchronous I/O reading and writing. - */ -template -class AsyncWorker : public Worker { - public: - typedef boost::mutex Mutex; - typedef boost::mutex::scoped_lock ScopedLock; - - /** - * @brief Construct an Asynchronous I/O worker. - * @param stream the stream for th I/O service - * @param io_service the I/O service - * @param buffer_size the size of the input and output buffers - */ - AsyncWorker(boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size = 8192); - virtual ~AsyncWorker(); - - /** - * @brief Set the callback function which handles input messages. - * @param callback the read callback which handles received messages - */ - void setCallback(const Callback& callback) { read_callback_ = callback; } - - /** - * @brief Set the callback function which handles raw data. - * @param callback the write callback which handles raw data - */ - void setRawDataCallback(const Callback& callback) { write_callback_ = callback; } - - /** - * @brief Send the data bytes via the I/O stream. - * @param data the buffer of data bytes to send - * @param size the size of the buffer - */ - bool send(const unsigned char* data, const unsigned int size); - /** - * @brief Wait for incoming messages. - * @param timeout the maximum time to wait - */ - void wait(const boost::posix_time::time_duration& timeout); - - bool isOpen() const { return stream_->is_open(); } - - protected: - /** - * @brief Read the input stream. - */ - void doRead(); - - /** - * @brief Process messages read from the input stream. - * @param error_code an error code for read failures - * @param the number of bytes received - */ - void readEnd(const boost::system::error_code&, std::size_t); - - /** - * @brief Send all the data in the output buffer. - */ - void doWrite(); - - /** - * @brief Close the I/O stream. - */ - void doClose(); - - boost::shared_ptr stream_; //!< The I/O stream - boost::shared_ptr io_service_; //!< The I/O service - - Mutex read_mutex_; //!< Lock for the input buffer - boost::condition read_condition_; - std::vector in_; //!< The input buffer - std::size_t in_buffer_size_; //!< number of bytes currently in the input - //!< buffer - - Mutex write_mutex_; //!< Lock for the output buffer - boost::condition write_condition_; - std::vector out_; //!< The output buffer - - boost::shared_ptr background_thread_; //!< thread for the I/O - //!< service - Callback read_callback_; //!< Callback function to handle received messages - Callback write_callback_; //!< Callback function to handle raw data - - bool stopping_; //!< Whether or not the I/O service is closed -}; - -template -AsyncWorker::AsyncWorker(boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size) - : stopping_(false) { - stream_ = stream; - io_service_ = io_service; - in_.resize(buffer_size); - in_buffer_size_ = 0; - - out_.reserve(buffer_size); - - io_service_->post(boost::bind(&AsyncWorker::doRead, this)); - background_thread_.reset(new boost::thread( - boost::bind(&boost::asio::io_service::run, io_service_))); -} - -template -AsyncWorker::~AsyncWorker() { - io_service_->post(boost::bind(&AsyncWorker::doClose, this)); - background_thread_->join(); - //io_service_->reset(); -} - -template -bool AsyncWorker::send(const unsigned char* data, - const unsigned int size) { - ScopedLock lock(write_mutex_); - if(size == 0) { - ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); - return true; - } - - if (out_.capacity() - out_.size() < size) { - ROS_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message"); - return false; - } - out_.insert(out_.end(), data, data + size); - - io_service_->post(boost::bind(&AsyncWorker::doWrite, this)); - return true; -} - -template -void AsyncWorker::doWrite() { - ScopedLock lock(write_mutex_); - // Do nothing if out buffer is empty - if (out_.size() == 0) { - return; - } - // Write all the data in the out buffer - boost::asio::write(*stream_, boost::asio::buffer(out_.data(), out_.size())); - - if (debug >= 2) { - // Print the data that was sent - std::ostringstream oss; - for (std::vector::iterator it = out_.begin(); - it != out_.end(); ++it) - oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); - } - // Clear the buffer & unlock - out_.clear(); - write_condition_.notify_all(); -} - -template -void AsyncWorker::doRead() { - ScopedLock lock(read_mutex_); - stream_->async_read_some( - boost::asio::buffer(in_.data() + in_buffer_size_, - in_.size() - in_buffer_size_), - boost::bind(&AsyncWorker::readEnd, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); -} - -template -void AsyncWorker::readEnd(const boost::system::error_code& error, - std::size_t bytes_transfered) { - ScopedLock lock(read_mutex_); - if (error) { - ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", - error.message().c_str(), - bytes_transfered); - } else if (bytes_transfered > 0) { - in_buffer_size_ += bytes_transfered; - - unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); - std::size_t raw_data_stream_size = bytes_transfered; - - if (write_callback_) - write_callback_(pRawDataStart, raw_data_stream_size); - - if (debug >= 4) { - std::ostringstream oss; - for (std::vector::iterator it = - in_.begin() + in_buffer_size_ - bytes_transfered; - it != in_.begin() + in_buffer_size_; ++it) - oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, - oss.str().c_str()); - } - - if (read_callback_) - read_callback_(in_.data(), in_buffer_size_); - - read_condition_.notify_all(); - } - - if (!stopping_) - io_service_->post(boost::bind(&AsyncWorker::doRead, this)); -} - -template -void AsyncWorker::doClose() { - ScopedLock lock(read_mutex_); - stopping_ = true; - boost::system::error_code error; - stream_->close(error); - if(error) - ROS_ERROR_STREAM( - "Error while closing the AsyncWorker stream: " << error.message()); -} - -template -void AsyncWorker::wait( - const boost::posix_time::time_duration& timeout) { - ScopedLock lock(read_mutex_); - read_condition_.timed_wait(lock, timeout); -} - -} // namespace ublox_gps - -#endif // UBLOX_GPS_ASYNC_WORKER_H diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp new file mode 100644 index 00000000..2315163e --- /dev/null +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -0,0 +1,342 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 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 UBLOX_GPS_ASYNC_WORKER_HPP +#define UBLOX_GPS_ASYNC_WORKER_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "worker.hpp" + +namespace ublox_gps { + +/** + * @brief Handles Asynchronous I/O reading and writing. + */ +template +class AsyncWorker final : public Worker { + public: + /** + * @brief Construct an Asynchronous I/O worker. + * @param stream the stream for th I/O service + * @param io_service the I/O service + * @param buffer_size the size of the input and output buffers + */ + explicit AsyncWorker(std::shared_ptr stream, + std::shared_ptr io_service, + std::size_t buffer_size, + int debug, + const rclcpp::Logger& logger); + ~AsyncWorker() override; + + AsyncWorker(AsyncWorker &&c) = delete; + AsyncWorker &operator=(AsyncWorker &&c) = delete; + AsyncWorker(const AsyncWorker &c) = delete; + AsyncWorker &operator=(const AsyncWorker &c) = delete; + + /** + * @brief Set the callback function which handles input messages. + * @param callback the read callback which handles received messages + */ + void setCallback(const WorkerCallback& callback) override { read_callback_ = callback; } + + /** + * @brief Set the callback function which handles raw data. + * @param callback the write callback which handles raw data + */ + void setRawDataCallback(const WorkerRawCallback& callback) override { raw_callback_ = callback; } + + /** + * @brief Send the data bytes via the I/O stream. + * @param data the buffer of data bytes to send + * @param size the size of the buffer + */ + bool send(const unsigned char* data, const unsigned int size) override; + /** + * @brief Wait for incoming messages. + * @param timeout the maximum time to wait + */ + void wait(const std::chrono::milliseconds& timeout) override; + + bool isOpen() const override { return stream_->is_open(); } + + private: + /** + * @brief Read the input stream. + */ + void doRead(); + + /** + * @brief Process messages read from the input stream. + * @param error_code an error code for read failures + * @param bytes_received the number of bytes received + */ + void readEnd(const asio::error_code& error, std::size_t bytes_transferred); + + /** + * @brief Send all the data in the output buffer. + */ + void doWrite(); + + /** + * @brief Close the I/O stream. + */ + void doClose(); + + std::shared_ptr stream_; //!< The I/O stream + std::shared_ptr io_service_; //!< The I/O service + + std::mutex read_mutex_; //!< Lock for the input buffer + std::condition_variable read_condition_; + std::vector in_; //!< The input buffer + std::size_t in_buffer_size_; //!< number of bytes currently in the input + //!< buffer + + std::mutex write_mutex_; //!< Lock for the output buffer + std::condition_variable write_condition_; + std::vector out_; //!< The output buffer + + std::shared_ptr background_thread_; //!< thread for the I/O + //!< service + WorkerCallback read_callback_; //!< Callback function to handle received messages + WorkerRawCallback raw_callback_; //!< Callback function to handle raw data + + bool stopping_; //!< Whether or not the I/O service is closed + + int debug_; //!< Used to determine which debug messages to display + + rclcpp::Logger logger_; +}; + +template +AsyncWorker::AsyncWorker(std::shared_ptr stream, + std::shared_ptr io_service, + std::size_t buffer_size, + int debug, + const rclcpp::Logger& logger) + : stream_(stream), io_service_(io_service), in_buffer_size_(0), stopping_(false), debug_(debug), logger_(logger) { + in_.resize(buffer_size); + + out_.reserve(buffer_size); + + io_service_->post(std::bind(&AsyncWorker::doRead, this)); + background_thread_ = std::make_shared([this]{ io_service_->run(); }); +} + +template +AsyncWorker::~AsyncWorker() { + io_service_->post(std::bind(&AsyncWorker::doClose, this)); + background_thread_->join(); + //io_service_->reset(); +} + +template +bool AsyncWorker::send(const unsigned char* data, + const unsigned int size) { + std::lock_guard lock(write_mutex_); + if (size == 0) { + RCLCPP_ERROR(logger_, "Ublox AsyncWorker::send: Size of message to send is 0"); + return true; + } + + if (out_.capacity() - out_.size() < size) { + RCLCPP_ERROR(logger_, "Ublox AsyncWorker::send: Output buffer too full to send message"); + return false; + } + out_.insert(out_.end(), data, data + size); + + io_service_->post(std::bind(&AsyncWorker::doWrite, this)); + return true; +} + +template +void AsyncWorker::doWrite() { + std::lock_guard lock(write_mutex_); + // Do nothing if out buffer is empty + if (out_.size() == 0) { + return; + } + // Write all the data in the out buffer + asio::write(*stream_, asio::buffer(out_.data(), out_.size())); + + if (debug_ >= 2) { + // Print the data that was sent + std::ostringstream oss; + for (std::vector::iterator it = out_.begin(); + it != out_.end(); ++it) { + oss << std::hex << static_cast(*it) << " "; + } + RCLCPP_DEBUG(logger_, "U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); + } + // Clear the buffer & unlock + out_.clear(); + write_condition_.notify_all(); +} +template <> +inline void AsyncWorker::doWrite() { + std::lock_guard lock(write_mutex_); + // Do nothing if out buffer is empty + if (out_.size() == 0) { + return; + } + // Write all the data in the out buffer + stream_->send(asio::buffer(out_.data(), out_.size())); + + if (debug_ >= 2) { + // Print the data that was sent + std::ostringstream oss; + for (std::vector::iterator it = out_.begin(); + it != out_.end(); ++it) { + oss << std::hex << static_cast(*it) << " "; + } + RCLCPP_DEBUG(logger_, "U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); + } + // Clear the buffer & unlock + out_.clear(); + write_condition_.notify_all(); +} + +template +void AsyncWorker::doRead() { + std::lock_guard lock(read_mutex_); + if (in_.size() - in_buffer_size_ == 0) { + // In some circumstances, it is possible that there is no room left in the + // buffer. This can happen, for instance, if one of the UBlox messages + // has a value in the Length field that is much larger than this buffer + // can accomodate. We definitely don't want to ask for a 0-byte read (as + // we will get into an endless loop of asking for, and then receiving, + // 0 bytes), so we just throw away all of the data in the buffer. + in_buffer_size_ = 0; + } + + stream_->async_read_some( + asio::buffer(in_.data() + in_buffer_size_, + in_.size() - in_buffer_size_), + std::bind(&AsyncWorker::readEnd, this, + std::placeholders::_1, std::placeholders::_2)); +} +template <> +inline void AsyncWorker::doRead() { + std::lock_guard lock(read_mutex_); + if (in_.size() - in_buffer_size_ == 0) { + // In some circumstances, it is possible that there is no room left in the + // buffer. This can happen, for instance, if one of the UBlox messages + // has a value in the Length field that is much larger than this buffer + // can accomodate. We definitely don't want to ask for a 0-byte read (as + // we will get into an endless loop of asking for, and then receiving, + // 0 bytes), so we just throw away all of the data in the buffer. + in_buffer_size_ = 0; + } + + stream_->async_receive( + asio::buffer(in_.data() + in_buffer_size_, + in_.size() - in_buffer_size_), + std::bind(&AsyncWorker::readEnd, this, + std::placeholders::_1, std::placeholders::_2)); +} + +template +void AsyncWorker::readEnd(const asio::error_code& error, + std::size_t bytes_transferred) { + std::lock_guard lock(read_mutex_); + if (error) { + RCLCPP_ERROR(logger_, "U-Blox ASIO input buffer read error: %s, %li", + error.message().c_str(), + bytes_transferred); + } else if (bytes_transferred > 0) { + in_buffer_size_ += bytes_transferred; + + unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transferred))); + std::size_t raw_data_stream_size = bytes_transferred; + + if (raw_callback_) { + raw_callback_(pRawDataStart, raw_data_stream_size); + } + + if (debug_ >= 4) { + std::ostringstream oss; + for (std::vector::iterator it = + in_.begin() + in_buffer_size_ - bytes_transferred; + it != in_.begin() + in_buffer_size_; ++it) { + oss << std::hex << static_cast(*it) << " "; + } + RCLCPP_DEBUG(logger_, "U-Blox received %li bytes \n%s", bytes_transferred, + oss.str().c_str()); + } + + if (read_callback_) { + in_buffer_size_ -= read_callback_(in_.data(), in_buffer_size_); + } + + read_condition_.notify_all(); + } else { + RCLCPP_ERROR(logger_, "U-Blox ASIO transferred zero bytes"); + } + + if (!stopping_) { + io_service_->post(std::bind(&AsyncWorker::doRead, this)); + } +} + +template +void AsyncWorker::doClose() { + std::lock_guard lock(read_mutex_); + stopping_ = true; + asio::error_code error; + stream_->close(error); + if (error) { + RCLCPP_ERROR(logger_, "Error while closing the AsyncWorker stream: %s", + error.message().c_str()); + } +} + +template +void AsyncWorker::wait( + const std::chrono::milliseconds& timeout) { + std::unique_lock lock(read_mutex_); + read_condition_.wait_for(lock, timeout); +} + +} // namespace ublox_gps + +#endif // UBLOX_GPS_ASYNC_WORKER_HPP diff --git a/ublox_gps/include/ublox_gps/callback.h b/ublox_gps/include/ublox_gps/callback.hpp similarity index 57% rename from ublox_gps/include/ublox_gps/callback.h rename to ublox_gps/include/ublox_gps/callback.hpp index bbf51f2d..819a8ea9 100644 --- a/ublox_gps/include/ublox_gps/callback.h +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -26,14 +26,22 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_CALLBACK_H -#define UBLOX_GPS_CALLBACK_H +#ifndef UBLOX_GPS_CALLBACK_HPP +#define UBLOX_GPS_CALLBACK_HPP -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include namespace ublox_gps { @@ -50,14 +58,14 @@ class CallbackHandler { /** * @brief Wait for on the condition. */ - bool wait(const boost::posix_time::time_duration& timeout) { - boost::mutex::scoped_lock lock(mutex_); - return condition_.timed_wait(lock, timeout); + bool wait(const std::chrono::milliseconds& timeout) { + std::unique_lock lock(mutex_); + return condition_.wait_for(lock, timeout) == std::cv_status::no_timeout; } protected: - boost::mutex mutex_; //!< Lock for the handler - boost::condition_variable condition_; //!< Condition for the handler lock + std::mutex mutex_; //!< Lock for the handler + std::condition_variable condition_; //!< Condition for the handler lock }; /** @@ -65,16 +73,16 @@ class CallbackHandler { * @typedef T the message type */ template -class CallbackHandler_ : public CallbackHandler { +class CallbackHandler_ final : public CallbackHandler { public: - typedef boost::function Callback; //!< A callback function + using Callback = std::function; - /** + /** * @brief Initialize the Callback Handler with a callback function * @param func a callback function for the message, defaults to none */ - CallbackHandler_(const Callback& func = Callback()) : func_(func) {} - + explicit CallbackHandler_(const Callback& func = Callback(), int debug = 1) : func_(func), debug_(debug) {} + /** * @brief Get the last received message. */ @@ -84,42 +92,47 @@ class CallbackHandler_ : public CallbackHandler { * @brief Decode the U-Blox message & call the callback function if it exists. * @param reader a reader to decode the message buffer */ - void handle(ublox::Reader& reader) { - boost::mutex::scoped_lock lock(mutex_); + void handle(ublox::Reader& reader) override { + std::lock_guard lock(mutex_); try { if (!reader.read(message_)) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", - static_cast(reader.classId()), - static_cast(reader.messageId()), - reader.length()); + // RCLCPP_DEBUG_COND(debug_ >= 2, + // "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + // static_cast(reader.classId()), + // static_cast(reader.messageId()), + // reader.length()); condition_.notify_all(); return; } - } catch (std::runtime_error& e) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", - static_cast(reader.classId()), - static_cast(reader.messageId()), - reader.length()); + } catch (const std::runtime_error& e) { + // RCLCPP_DEBUG_COND(debug_ >= 2, + // "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + // static_cast(reader.classId()), + // static_cast(reader.messageId()), + // reader.length()); condition_.notify_all(); return; } - if (func_) func_(message_); + if (func_) { + func_(message_); + } condition_.notify_all(); } - + private: Callback func_; //!< the callback function to handle the message T message_; //!< The last received message + int debug_; }; /** * @brief Callback handlers for incoming u-blox messages. */ -class CallbackHandlers { +class CallbackHandlers final { public: + explicit CallbackHandlers(int debug) : debug_(debug) {} + /** * @brief Add a callback handler for the given message type. * @param callback the callback handler for the message @@ -127,15 +140,14 @@ class CallbackHandlers { */ template void insert(typename CallbackHandler_::Callback callback) { - boost::mutex::scoped_lock lock(callback_mutex_); - CallbackHandler_* handler = new CallbackHandler_(callback); + std::lock_guard lock(callback_mutex_); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler))); + std::make_shared>(callback, debug_))); } /** - * @brief Add a callback handler for the given message type and ID. This is + * @brief Add a callback handler for the given message type and ID. This is * used for messages in which have the same structure (and therefore msg file) * and same class ID but different message IDs. (e.g. INF, ACK) * @param callback the callback handler for the message @@ -144,13 +156,21 @@ class CallbackHandlers { */ template void insert( - typename CallbackHandler_::Callback callback, + typename CallbackHandler_::Callback callback, unsigned int message_id) { - boost::mutex::scoped_lock lock(callback_mutex_); - CallbackHandler_* handler = new CallbackHandler_(callback); + std::lock_guard lock(callback_mutex_); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, message_id), - boost::shared_ptr(handler))); + std::make_shared>(callback, debug_))); + } + + /** + * @brief Add a callback handler for nmea messages + * @param callback the callback handler for the message + */ + void set_nmea_callback(std::function callback) { + std::lock_guard lock(callback_mutex_); + callback_nmea_ = callback; } /** @@ -159,12 +179,35 @@ class CallbackHandlers { */ void handle(ublox::Reader& reader) { // Find the callback handlers for the message & decode it - boost::mutex::scoped_lock lock(callback_mutex_); + std::lock_guard lock(callback_mutex_); Callbacks::key_type key = std::make_pair(reader.classId(), reader.messageId()); for (Callbacks::iterator callback = callbacks_.lower_bound(key); - callback != callbacks_.upper_bound(key); ++callback) + callback != callbacks_.upper_bound(key); ++callback) { callback->second->handle(reader); + } + } + + /** + * @brief Calls the callback handler for the nmea messages in the reader. + * @param reader a reader containing an nmea message + */ + void handle_nmea(ublox::Reader& reader) { + std::lock_guard lock(callback_mutex_); + if (callback_nmea_ == nullptr) { + return; + } + + const std::string buffer = reader.getExtraData(); + size_t nmea_start = buffer.find('$', 0); + size_t nmea_end = buffer.find('\n', nmea_start); + while(nmea_start != std::string::npos && nmea_end != std::string::npos) { + std::string sentence = buffer.substr(nmea_start, nmea_end - nmea_start + 1); + callback_nmea_(sentence); + + nmea_start = buffer.find('$', nmea_end + 1); + nmea_end = buffer.find('\n', nmea_start); + } } /** @@ -173,14 +216,14 @@ class CallbackHandlers { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, const boost::posix_time::time_duration& timeout) { + bool read(T& message, const std::chrono::milliseconds& timeout) { bool result = false; // Create a callback handler for this message callback_mutex_.lock(); - CallbackHandler_* handler = new CallbackHandler_(); + auto handler = std::make_shared>(typename CallbackHandler_::Callback(), debug_); Callbacks::iterator callback = callbacks_.insert( (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler)))); + handler))); callback_mutex_.unlock(); // Wait for the message @@ -188,7 +231,7 @@ class CallbackHandlers { message = handler->get(); result = true; } - + // Remove the callback handler callback_mutex_.lock(); callbacks_.erase(callback); @@ -201,38 +244,46 @@ class CallbackHandlers { * messages from the buffer. * @param data the buffer of u-blox messages to process * @param size the size of the buffer + * @return the number of bytes consumed */ - void readCallback(unsigned char* data, std::size_t& size) { + size_t readCallback(unsigned char* data, std::size_t size) { ublox::Reader reader(data, size); // Read all U-Blox messages in buffer while (reader.search() != reader.end() && reader.found()) { - if (debug >= 3) { + if (debug_ >= 3) { // Print the received bytes std::ostringstream oss; for (ublox::Reader::iterator it = reader.pos(); - it != reader.pos() + reader.length() + 8; ++it) - oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, - oss.str().c_str()); + it != reader.pos() + reader.length() + 8; ++it) { + oss << std::hex << static_cast(*it) << " "; + } + // RCLCPP_DEBUG(logger_, "U-blox: reading %d bytes\n%s", reader.length() + 8, + // oss.str().c_str()); } handle(reader); } + handle_nmea(reader); // delete read bytes from ASIO input buffer std::copy(reader.pos(), reader.end(), data); - size -= reader.pos() - data; + + return reader.pos() - data; } private: - typedef std::multimap, - boost::shared_ptr > Callbacks; + using Callbacks = std::multimap, + std::shared_ptr>; // Call back handlers for u-blox messages Callbacks callbacks_; - boost::mutex callback_mutex_; + std::mutex callback_mutex_; + int debug_; + + //! Callback handler for nmea messages + std::function callback_nmea_{nullptr}; }; } // namespace ublox_gps -#endif // UBLOX_GPS_CALLBACK_H +#endif // UBLOX_GPS_CALLBACK_HPP diff --git a/ublox_gps/include/ublox_gps/component_interface.hpp b/ublox_gps/include/ublox_gps/component_interface.hpp new file mode 100644 index 00000000..951f4e2e --- /dev/null +++ b/ublox_gps/include/ublox_gps/component_interface.hpp @@ -0,0 +1,80 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 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 UBLOX_GPS_COMPONENT_INTERFACE_HPP +#define UBLOX_GPS_COMPONENT_INTERFACE_HPP + +#include + +#include + +// This file declares the ComponentInterface which acts as a high level +// interface for u-blox firmware, product categories, etc. It contains methods +// to configure the u-blox and subscribe to u-blox messages. +// + +namespace ublox_node { + +/** + * @brief This interface is used to add functionality to the main node. + * + * @details This interface is generic and can be implemented for other features + * besides the main node, hardware versions, and firmware versions. + */ +class ComponentInterface { + public: + /** + * @brief Get the ROS parameters. + * @throws std::runtime_error if a parameter is invalid or required + * parameters are not set. + */ + virtual void getRosParams() = 0; + + /** + * @brief Configure the U-Blox settings. + * @return true if configured correctly, false otherwise + */ + virtual bool configureUblox(std::shared_ptr gps) = 0; + + /** + * @brief Initialize the diagnostics. + * + * @details Function may be empty. + */ + virtual void initializeRosDiagnostics() = 0; + + /** + * @brief Subscribe to u-blox messages and publish to ROS topics. + */ + virtual void subscribe(std::shared_ptr gps) = 0; +}; + +} // namespace ublox_node + +#endif diff --git a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp new file mode 100644 index 00000000..556b1ba7 --- /dev/null +++ b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp @@ -0,0 +1,60 @@ +#ifndef UBLOX_GPS_FIX_DIAGNOSTIC_HPP +#define UBLOX_GPS_FIX_DIAGNOSTIC_HPP + +#include +#include + +#include +#include + +namespace ublox_node { + +//! Topic diagnostics for fix / fix_velocity messages +class FixDiagnostic { +public: + /** + * @brief Add a topic diagnostic to the diagnostic updater for fix topics. + * + * @details The minimum and maximum frequency are equal to the nav rate in Hz. + * @param name the ROS topic + * @param freq_tol the tolerance [%] for the topic frequency + * @param freq_window the number of messages to use for diagnostic statistics + * @param stamp_min the minimum allowed time delay + */ + explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, + double stamp_min, uint16_t nav_rate, uint16_t meas_rate, + std::shared_ptr updater) { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + min_freq = target_freq; + max_freq = target_freq; + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, + freq_tol, freq_window); + double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); + diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); + diagnostic = std::make_shared(name, + *updater, + freq_param, + time_param); + } + + // Must not copy this struct (would confuse FrequencyStatusParam pointers) + FixDiagnostic(FixDiagnostic &&c) = delete; + FixDiagnostic &operator=(FixDiagnostic &&c) = delete; + FixDiagnostic(const FixDiagnostic &c) = delete; + FixDiagnostic &operator=(const FixDiagnostic &c) = delete; + + ~FixDiagnostic() = default; + + //! Topic frequency diagnostic updater + std::shared_ptr diagnostic; + +private: + //! Minimum allow frequency of topic + double min_freq; + //! Maximum allow frequency of topic + double max_freq; +}; + +} // namespace ublox_node + +#endif diff --git a/ublox_serialization/include/ublox/serialization_ros.h b/ublox_gps/include/ublox_gps/fts_product.hpp similarity index 52% rename from ublox_serialization/include/ublox/serialization_ros.h rename to ublox_gps/include/ublox_gps/fts_product.hpp index 9236ff46..fed2c843 100644 --- a/ublox_serialization/include/ublox/serialization_ros.h +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -14,10 +14,11 @@ // 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 BE LIABLE FOR ANY +// 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 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 @@ -26,36 +27,54 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_SERIALIZATION_ROS_H -#define UBLOX_SERIALIZATION_ROS_H +#ifndef UBLOX_GPS_FTS_PRODUCT_HPP +#define UBLOX_GPS_FTS_PRODUCT_HPP -#include "serialization.h" -#include "checksum.h" +#include -#include +#include +#include -namespace ublox { +namespace ublox_node { -template -void Serializer::read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message) { - ros::serialization::IStream stream(const_cast(data), count); - ros::serialization::Serializer::read(stream, message); -} +/** + * @brief Implements functions for FTS products. Currently unimplemented. + * @todo Unimplemented. + */ +class FtsProduct final : public virtual ComponentInterface { + /** + * @brief Get the FTS parameters. + * @todo Currently unimplemented. + */ + void getRosParams() override { + // RCLCPP_WARN("Functionality specific to u-blox FTS devices is %s", + // "unimplemented. See FtsProduct class in node.hpp & node.cpp."); + } -template -uint32_t Serializer::serializedLength( - typename boost::call_traits::param_type message) { - return ros::serialization::Serializer::serializedLength(message); -} + /** + * @brief Configure FTS settings. + * @todo Currently unimplemented. + */ + bool configureUblox(std::shared_ptr gps) override { + (void)gps; + return false; + } -template -void Serializer::write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message) { - ros::serialization::OStream stream(data, size); - ros::serialization::Serializer::write(stream, message); -} + /** + * @brief Adds diagnostic updaters for FTS status. + * @todo Currently unimplemented. + */ + void initializeRosDiagnostics() override {} -} // namespace ublox + /** + * @brief Subscribe to FTS messages. + * @todo Currently unimplemented. + */ + void subscribe(std::shared_ptr gps) override { + (void)gps; + } +}; -#endif // UBLOX_SERIALIZATION_ROS_H +} // namespace ublox_node + +#endif diff --git a/ublox_gps/include/ublox_gps/gnss.hpp b/ublox_gps/include/ublox_gps/gnss.hpp new file mode 100644 index 00000000..799a1227 --- /dev/null +++ b/ublox_gps/include/ublox_gps/gnss.hpp @@ -0,0 +1,28 @@ +#ifndef UBLOX_GPS_GNSS_HPP +#define UBLOX_GPS_GNSS_HPP + +#include +#include + +namespace ublox_node { + +class Gnss final { +public: + Gnss() = default; + ~Gnss() = default; + + Gnss(Gnss &&c) = delete; + Gnss &operator=(Gnss &&c) = delete; + Gnss(const Gnss &c) = delete; + Gnss &operator=(const Gnss &c) = delete; + + void add(const std::string & gnss); + bool isSupported(const std::string & gnss); + +private: + std::set supported_; +}; + +} // namespace ublox_node + +#endif diff --git a/ublox_gps/include/ublox_gps/gps.h b/ublox_gps/include/ublox_gps/gps.hpp similarity index 81% rename from ublox_gps/include/ublox_gps/gps.h rename to ublox_gps/include/ublox_gps/gps.hpp index 468aae28..70f2b906 100644 --- a/ublox_gps/include/ublox_gps/gps.h +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -27,25 +27,25 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_H -#define UBLOX_GPS_H +#ifndef UBLOX_GPS_GPS_HPP +#define UBLOX_GPS_GPS_HPP // STL -#include -#include +#include +#include #include +#include #include -// Boost -#include -#include -#include -#include -// ROS -#include +#include +#include + +#include + // Other u-blox packages -#include +#include // u-blox gps -#include -#include +#include +#include +#include /** * @namespace ublox_gps @@ -54,28 +54,33 @@ */ namespace ublox_gps { //! Possible baudrates for u-blox devices -constexpr static unsigned int kBaudrates[] = { 4800, - 9600, - 19200, - 38400, - 57600, - 115200, - 230400, - 460800 }; +const std::vector kBaudrates{ 4800, + 9600, + 19200, + 38400, + 57600, + 115200, + 230400, + 460800 }; /** * @brief Handles communication with and configuration of the u-blox device */ -class Gps { +class Gps final { public: //! Sleep time [ms] after setting the baudrate - constexpr static int kSetBaudrateSleepMs = 500; + const int kSetBaudrateSleepMs = 500; //! Default timeout for ACK messages in seconds constexpr static double kDefaultAckTimeout = 1.0; //! Size of write buffer for output messages constexpr static int kWriterSize = 2056; - Gps(); - virtual ~Gps(); + explicit Gps(int debug, const rclcpp::Logger & logger); + ~Gps(); + + Gps(Gps &&c) = delete; + Gps &operator=(Gps &&c) = delete; + Gps(const Gps &c) = delete; + Gps &operator=(const Gps &c) = delete; /** * @brief If called, when the node shuts down, it will send a command to @@ -89,14 +94,21 @@ class Gps { * @brief Set the internal flag for enabling or disabling the initial configurations. * @param config_on_startup boolean flag */ - void setConfigOnStartup(const bool config_on_startup) { config_on_startup_flag_ = config_on_startup; } + void setConfigOnStartup(bool config_on_startup) { config_on_startup_flag_ = config_on_startup; } /** * @brief Initialize TCP I/O. * @param host the TCP host * @param port the TCP port */ - void initializeTcp(std::string host, std::string port); + void initializeTcp(const std::string & host, const std::string & port); + + /** + * @brief Initialize UDP I/O. + * @param host the UDP host + * @param port the UDP port + */ + void initializeUdp(const std::string & host, const std::string & port); /** * @brief Initialize the Serial I/O port. @@ -105,7 +117,7 @@ class Gps { * @param uart_in the UART In protocol, see CfgPRT for options * @param uart_out the UART Out protocol, see CfgPRT for options */ - void initializeSerial(std::string port, unsigned int baudrate, + void initializeSerial(const std::string & port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out); /** @@ -115,7 +127,13 @@ class Gps { * @param uart_in the UART In protocol, see CfgPRT for options * @param uart_out the UART Out protocol, see CfgPRT for options */ - void resetSerial(std::string port); + void resetSerial(const std::string & port); + + /** + * @brief Send rtcm correction messages to the connected device. + * @param message the RTCM correction data as a vector + */ + bool sendRtcm(const std::vector &message); /** * @brief Closes the I/O port, and initiates save on shutdown procedure @@ -127,7 +145,7 @@ class Gps { * @brief Reset I/O communications. * @param wait Time to wait before restarting communications */ - void reset(const boost::posix_time::time_duration& wait); + void reset(const std::chrono::milliseconds& wait); /** * @brief Send a reset message to the u-blox device. @@ -144,8 +162,8 @@ class Gps { * @return true if the GNSS was configured, the device was reset, and the * I/O reset successfully */ - bool configGnss(ublox_msgs::CfgGNSS gnss, - const boost::posix_time::time_duration& wait); + bool configGnss(ublox_msgs::msg::CfgGNSS gnss, + const std::chrono::milliseconds& wait); /** * @brief Send a message to the receiver to delete the BBR data stored in @@ -171,7 +189,7 @@ class Gps { * configuration parameters * @return true on ACK, false on other conditions. */ - bool disableUart1(ublox_msgs::CfgPRT& prev_cfg); + bool disableUart1(ublox_msgs::msg::CfgPRT& prev_config); /** * @brief Configure the USB Port. @@ -198,7 +216,7 @@ class Gps { * @param rates the send rates for each RTCM message ID, valid range: [0, 255] * @return true on ACK, false on other conditions. */ - bool configRtcm(std::vector ids, std::vector rates); + bool configRtcm(const std::vector & rtcms); /** * @brief Configure the SBAS settings. @@ -225,7 +243,7 @@ class Gps { * @return true on ACK, false if settings are incorrect or on other conditions */ bool configTmode3Fixed(bool lla_flag, - std::vector arp_position, + std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc); @@ -282,7 +300,7 @@ class Gps { * @note This is part of the expert settings. It is recommended you check * the ublox manual first. */ - bool setPpp(bool enable); + bool setPpp(bool enable, float protocol_version); /** * @brief Set the DGNSS mode (see CfgDGNSS message for details). @@ -296,10 +314,10 @@ class Gps { * @param enable If true, enable ADR. * @return true on ACK, false on other conditions. */ - bool setUseAdr(bool enable); + bool setUseAdr(bool enable, float protocol_version); /** - * @brief Configure the U-Blox to UTC time + * @brief Configure the U-Blox to UTC time * @return true on ACK, false on other conditions. * * @note This is part of the expert settings. It is recommended you check @@ -316,7 +334,7 @@ class Gps { * the ublox manual first. */ bool setTimtm2(uint8_t rate); - + /** * @brief Configure the U-Blox send rate of the message & subscribe to the * given message @@ -333,6 +351,12 @@ class Gps { template void subscribe(typename CallbackHandler_::Callback callback); + /** + * @brief Subscribe to the given Ublox message. + * @param callback the callback handler for the message + */ + void subscribe_nmea(std::function callback); + /** * @brief Subscribe to the message with the given ID. This is used for * messages which have the same format but different message IDs, @@ -351,9 +375,9 @@ class Gps { */ template bool read(T& message, - const boost::posix_time::time_duration& timeout = default_timeout_); + const std::chrono::milliseconds& timeout = default_timeout_); - bool isInitialized() const { return worker_ != 0; } + bool isInitialized() const { return worker_ != nullptr; } bool isConfigured() const { return isInitialized() && configured_; } bool isOpen() const { return worker_->isOpen(); } @@ -367,7 +391,7 @@ class Gps { template bool poll(ConfigT& message, const std::vector& payload = std::vector(), - const boost::posix_time::time_duration& timeout = default_timeout_); + const std::chrono::milliseconds& timeout = default_timeout_); /** * Poll a u-blox message. * @param class_id the u-blox message class id @@ -396,14 +420,16 @@ class Gps { * @param msg_id the expected message ID of the ACK * @return true if expected ACK received, false otherwise */ - bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, + bool waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id); /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - void setRawDataCallback(const Worker::Callback& callback); + void setRawDataCallback(const Worker::WorkerRawCallback& callback); + + bool hotrestart(); private: //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK @@ -424,7 +450,7 @@ class Gps { * @brief Set the I/O worker * @param an I/O handler */ - void setWorker(const boost::shared_ptr& worker); + void setWorker(const std::shared_ptr& worker); /** * @brief Subscribe to ACK/NACK messages and UPD-SOS-ACK messages. @@ -435,19 +461,19 @@ class Gps { * @brief Callback handler for UBX-ACK message. * @param m the message to process */ - void processAck(const ublox_msgs::Ack &m); + void processAck(const ublox_msgs::msg::Ack &m); /** * @brief Callback handler for UBX-NACK message. * @param m the message to process */ - void processNack(const ublox_msgs::Ack &m); + void processNack(const ublox_msgs::msg::Ack &m); /** * @brief Callback handler for UBX-UPD-SOS-ACK message. * @param m the message to process */ - void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m); + void processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m); /** * @brief Execute save on shutdown procedure. @@ -462,7 +488,7 @@ class Gps { bool saveOnShutdown(); //! Processes I/O stream data - boost::shared_ptr worker_; + std::shared_ptr worker_; //! Whether or not the I/O port has been configured bool configured_; //! Whether or not to save Flash BBR on shutdown @@ -472,20 +498,26 @@ class Gps { //! The default timeout for ACK messages - static const boost::posix_time::time_duration default_timeout_; + static const std::chrono::milliseconds default_timeout_; //! Stores last received ACK accessed by multiple threads - mutable boost::atomic ack_; + std::atomic ack_{}; + + int debug_; //! Callback handlers for u-blox messages CallbackHandlers callbacks_; std::string host_, port_; + + rclcpp::Logger logger_; }; template void Gps::subscribe( typename CallbackHandler_::Callback callback, unsigned int rate) { - if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return; + if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) { + return; + } subscribe(callback); } @@ -503,38 +535,48 @@ void Gps::subscribeId(typename CallbackHandler_::Callback callback, template bool Gps::poll(ConfigT& message, const std::vector& payload, - const boost::posix_time::time_duration& timeout) { - if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false; + const std::chrono::milliseconds& timeout) { + if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) { + return false; + } return read(message, timeout); } template -bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) { - if (!worker_) return false; +bool Gps::read(T& message, const std::chrono::milliseconds& timeout) { + if (!worker_) { + return false; + } return callbacks_.read(message, timeout); } template bool Gps::configure(const ConfigT& message, bool wait) { - if (!worker_) return false; + if (!worker_) { + return false; + } // Reset ack - Ack ack; + Ack ack{}; ack.type = WAIT; - ack_.store(ack, boost::memory_order_seq_cst); + ack.class_id = 0; + ack.msg_id = 0; + ack_.store(ack, std::memory_order_seq_cst); // Encode the message std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); if (!writer.write(message)) { - ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x", - message.CLASS_ID, message.MESSAGE_ID); + RCLCPP_ERROR(logger_, "Failed to encode config message 0x%02x / 0x%02x", + message.CLASS_ID, message.MESSAGE_ID); return false; } // Send the message to the device worker_->send(out.data(), writer.end() - out.data()); - if (!wait) return true; + if (!wait) { + return true; + } // Wait for an acknowledgment and return whether or not it was received return waitForAcknowledge(default_timeout_, @@ -544,4 +586,4 @@ bool Gps::configure(const ConfigT& message, bool wait) { } // namespace ublox_gps -#endif // UBLOX_GPS_H +#endif // UBLOX_GPS_GPS_HPP diff --git a/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp b/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp new file mode 100644 index 00000000..94464cb4 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp @@ -0,0 +1,51 @@ +#ifndef UBLOX_GPS_HP_POS_REC_PRODUCT_HPP +#define UBLOX_GPS_HP_POS_REC_PRODUCT_HPP + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +class HpPosRecProduct final : public virtual HpgRefProduct { + public: + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node); + + /** + * @brief Subscribe to Rover messages, such as NavRELPOSNED. + */ + void subscribe(std::shared_ptr gps) override; + + private: + + /** + * @brief Set the last received message and call rover diagnostic updater + * + * @details Publish received NavRELPOSNED messages if enabled + */ + void callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 &m); + + sensor_msgs::msg::Imu imu_; + + //! Last relative position (used for diagnostic updater) + ublox_msgs::msg::NavRELPOSNED9 last_rel_pos_; + + rclcpp::Publisher::SharedPtr nav_relposned_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + + std::string frame_id_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HP_POS_REC_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/hpg_ref_product.hpp b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp new file mode 100644 index 00000000..ff386ef3 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp @@ -0,0 +1,145 @@ +#ifndef UBLOX_GPS_HPG_REF_PRODUCT_HPP +#define UBLOX_GPS_HPG_REF_PRODUCT_HPP + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for High Precision GNSS Reference station + * devices. + */ +class HpgRefProduct: public virtual ComponentInterface { + public: + //! Default measurement period for HPG devices + constexpr static uint16_t kDefaultMeasPeriod = 250; + + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node); + + /** + * @brief Get the ROS parameters specific to the Reference Station + * configuration. + * + * @details Get the TMODE3 settings, the parameters it gets depends on the + * tmode3 parameter. For example, it will get survey-in parameters if the + * tmode3 parameter is set to survey in or it will get the fixed parameters if + * it is set to fixed. + */ + void getRosParams() override; + + /** + * @brief Configure the u-blox Reference Station settings. + * + * @details Configure the TMODE3 settings and sets the internal state based + * on the TMODE3 status. If the TMODE3 is set to fixed, it will configure + * the RTCM messages. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Add diagnostic updaters for the TMODE3 status. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to u-blox Reference Station messages. + * + * @details Subscribe to NavSVIN messages based on user parameters. + */ + void subscribe(std::shared_ptr gps) override; + + /** + * @brief Update the last received NavSVIN message and call diagnostic updater + * + * @details When the survey in finishes, it changes the measurement & + * navigation rate to the user configured values and enables the user + * configured RTCM messages. Publish received Nav SVIN messages if enabled. + * @param m the message to process + */ + void callbackNavSvIn(const ublox_msgs::msg::NavSVIN& m); + + protected: + /** + * @brief Update the TMODE3 diagnostics. + * + * @details Updates the status of the survey-in if in survey-in mode or the + * RTCM messages if in time mode. + */ + void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); + + /** + * @brief Set the device mode to time mode (internal state variable). + * + * @details Configure the RTCM messages and measurement and navigation rate. + */ + bool setTimeMode(std::shared_ptr gps); + + //! The last received Nav SVIN message + ublox_msgs::msg::NavSVIN last_nav_svin_; + + //! TMODE3 to set, such as disabled, survey-in, fixed + uint8_t tmode3_; + + // TMODE3 = Fixed mode settings + //! True if coordinates are in LLA, false if ECEF + /*! Used only for fixed mode */ + bool lla_flag_; + //! Antenna Reference Point Position [m] or [deg] + /*! Used only for fixed mode */ + std::vector arp_position_; + //! Antenna Reference Point Position High Precision [0.1 mm] or [deg * 1e-9] + /*! Used only for fixed mode */ + std::vector arp_position_hp_; + //! Fixed Position Accuracy [m] + /*! Used only for fixed mode */ + float fixed_pos_acc_; + + // Settings for TMODE3 = Survey-in + //! Whether to always reset the survey-in during configuration. + /*! + * If false, it only resets survey-in if there's no fix and TMODE3 is + * disabled before configuration. + * This variable is used only if TMODE3 is set to survey-in. + */ + bool svin_reset_; + //! Measurement period used during Survey-In [s] + /*! This variable is used only if TMODE3 is set to survey-in. */ + uint32_t sv_in_min_dur_; + //! Survey in accuracy limit [m] + /*! This variable is used only if TMODE3 is set to survey-in. */ + float sv_in_acc_lim_; + + //! Status of device time mode + enum { + INIT, //!< Initialization mode (before configuration) + FIXED, //!< Fixed mode (should switch to time mode almost immediately) + DISABLED, //!< Time mode disabled + SURVEY_IN, //!< Survey-In mode + TIME //!< Time mode, after survey-in or after configuring fixed mode + } mode_{INIT}; + + rclcpp::Publisher::SharedPtr navsvin_pub_; + + uint16_t nav_rate_; + uint16_t meas_rate_; + std::shared_ptr updater_; + + std::vector rtcms_; + std::shared_ptr gps_; + rclcpp::Node* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HPG_REF_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/hpg_rov_product.hpp b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp new file mode 100644 index 00000000..2474e1c9 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp @@ -0,0 +1,95 @@ +#ifndef UBLOX_GPS_HPG_ROV_PRODUCT_HPP +#define UBLOX_GPS_HPG_ROV_PRODUCT_HPP + +#include + +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for High Precision GNSS Rover devices. + */ +class HpgRovProduct final : public virtual ComponentInterface { + public: + // Constants for diagnostic updater + //! Diagnostic updater: RTCM topic frequency min [Hz] + const double kRtcmFreqMin = 1; + //! Diagnostic updater: RTCM topic frequency max [Hz] + const double kRtcmFreqMax = 10; + //! Diagnostic updater: RTCM topic frequency tolerance [%] + const double kRtcmFreqTol = 0.1; + //! Diagnostic updater: RTCM topic frequency window [num messages] + const int kRtcmFreqWindow = 25; + + explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node); + + /** + * @brief Get the ROS parameters specific to the Rover configuration. + * + * @details Get the DGNSS mode. + */ + void getRosParams() override; + + /** + * @brief Configure rover settings. + * + * @details Configure the DGNSS mode. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Add diagnostic updaters for rover GNSS status, including + * status of RTCM messages. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to Rover messages, such as NavRELPOSNED. + */ + void subscribe(std::shared_ptr gps) override; + + private: + /** + * @brief Update the rover diagnostics, including the carrier phase solution + * status (float or fixed). + */ + void carrierPhaseDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper& stat); + + /** + * @brief Set the last received message and call rover diagnostic updater + * + * @details Publish received NavRELPOSNED messages if enabled + */ + void callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED &m); + + + //! Last relative position (used for diagnostic updater) + ublox_msgs::msg::NavRELPOSNED last_rel_pos_; + + //! The DGNSS mode + /*! see CfgDGNSS message for possible values */ + uint8_t dgnss_mode_; + + //! The RTCM topic frequency diagnostic updater + std::unique_ptr freq_rtcm_; + + rclcpp::Publisher::SharedPtr nav_rel_pos_ned_pub_; + + uint16_t nav_rate_; + std::shared_ptr updater_; + rclcpp::Node* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HPG_ROV_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/mkgmtime.h b/ublox_gps/include/ublox_gps/mkgmtime.h index 8537379c..900c1ca2 100644 --- a/ublox_gps/include/ublox_gps/mkgmtime.h +++ b/ublox_gps/include/ublox_gps/mkgmtime.h @@ -1,6 +1,6 @@ /* mkgmtime.h -- make a time_t from a gmtime struct tm $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $ - + * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -8,7 +8,7 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * 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 @@ -18,7 +18,7 @@ * 3. The name "Carnegie Mellon University" must not be used to * endorse or promote products derived from this software without * prior written permission. For permission or any other legal - * details, please contact + * details, please contact * Office of Technology Transfer * Carnegie Mellon University * 5000 Forbes Avenue @@ -41,15 +41,23 @@ * */ -#ifndef INCLUDED_MKGMTIME_H -#define INCLUDED_MKGMTIME_H +#ifndef UBLOX_GPS_MKGMTIME_H +#define UBLOX_GPS_MKGMTIME_H #include +#ifdef __cplusplus +extern "C" { +#endif + /** * @brief Get the UTC time in seconds and nano-seconds from a time struct in * GM time. */ -extern time_t mkgmtime(struct tm * const tmp); +time_t mkgmtime(struct tm * const tmp); + +#ifdef __cplusplus +} +#endif -#endif /* INCLUDED_MKGMTIME_H */ \ No newline at end of file +#endif // UBLOX_GPS_MKGMTIME_H diff --git a/ublox_gps/include/ublox_gps/node.h b/ublox_gps/include/ublox_gps/node.h deleted file mode 100644 index 0f33aa41..00000000 --- a/ublox_gps/include/ublox_gps/node.h +++ /dev/null @@ -1,1379 +0,0 @@ -//============================================================================== -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * 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. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, 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 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 UBLOX_GPS_NODE_H -#define UBLOX_GPS_NODE_H - -// STL -#include -#include -// Boost -#include -#include -#include -// ROS includes -#include -#include -#include -#include -#include -#include -// ROS messages -#include -#include -#include -#include -#include -// Other U-Blox package includes -#include -// Ublox GPS includes -#include -#include -#include - -// This file declares the ComponentInterface which acts as a high level -// interface for u-blox firmware, product categories, etc. It contains methods -// to configure the u-blox and subscribe to u-blox messages. -// -// This file also declares UbloxNode which implements ComponentInterface and is -// the main class and ros node. it implements functionality which applies to -// any u-blox device, regardless of the firmware version or product type. -// The class is designed in compositional style; it contains ComponentInterfaces -// which implement features specific to the device based on its firmware version -// and product category. UbloxNode calls the public methods of each component. -// -// This file declares UbloxFirmware is an abstract class which implements -// ComponentInterface and functions generic to all firmware (such as the -// initializing the fix diagnostics). Subclasses of UbloxFirmware for firmware -// versions 6-8 are also declared in this file. -// -// Lastly, this file declares classes for each product category which also -// implement u-blox interface, currently only the class for High Precision -// GNSS devices has been fully implemented and tested. - -/** - * @namespace ublox_node - * This namespace is for the ROS u-blox node and handles anything regarding - * ROS parameters, message passing, diagnostics, etc. - */ -namespace ublox_node { - -//! Queue size for ROS publishers -constexpr static uint32_t kROSQueueSize = 1; -//! Default measurement period for HPG devices -constexpr static uint16_t kDefaultMeasPeriod = 250; -//! Default subscribe Rate to u-blox messages [Hz] -constexpr static uint32_t kSubscribeRate = 1; -//! Subscribe Rate for u-blox SV Info messages -constexpr static uint32_t kNavSvInfoSubscribeRate = 20; - -// ROS objects -//! ROS diagnostic updater -boost::shared_ptr updater; -//! Node Handle for GPS node -boost::shared_ptr nh; - -//! Handles communication with the U-Blox Device -ublox_gps::Gps gps; -//! Which GNSS are supported by the device -std::set supported; -//! Whether or not to publish the given ublox message -/*! - * key is the message name (all lowercase) without firmware version numbers - * (e.g. NavPVT instead of NavPVT7). Value indicates whether or not to enable - * the message. */ -std::map enabled; -//! The ROS frame ID of this device -std::string frame_id; -//! The fix status service type, set in the Firmware Component -//! based on the enabled GNSS -int fix_status_service; -//! The measurement [ms], see CfgRate.msg -uint16_t meas_rate; -//! Navigation rate in measurement cycles, see CfgRate.msg -uint16_t nav_rate; -//! IDs of RTCM out messages to configure. -std::vector rtcm_ids; -//! Rates of RTCM out messages. Size must be the same as rtcm_ids -std::vector rtcm_rates; -//! Flag for enabling configuration on startup -bool config_on_startup_flag_; - - -//! Topic diagnostics for u-blox messages -struct UbloxTopicDiagnostic { - UbloxTopicDiagnostic() {} - - // Must not copy this struct (would confuse FrequencyStatusParam pointers) - UbloxTopicDiagnostic(const UbloxTopicDiagnostic&) = delete; - - /** - * @brief Add a topic diagnostic to the diagnostic updater for - * - * @details The minimum and maximum frequency are equal to the nav rate in Hz. - * @param name the ROS topic - * @param freq_tol the tolerance [%] for the topic frequency - * @param freq_window the number of messages to use for diagnostic statistics - */ - UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz - min_freq = target_freq; - max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); - } - - /** - * @brief Add a topic diagnostic to the diagnostic updater for - * - * @details The minimum and maximum frequency are equal to the nav rate in Hz. - * @param name the ROS topic - * @param freq_min the minimum acceptable frequency for the topic - * @param freq_max the maximum acceptable frequency for the topic - * @param freq_tol the tolerance [%] for the topic frequency - * @param freq_window the number of messages to use for diagnostic statistics - */ - UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max, - double freq_tol, int freq_window) { - min_freq = freq_min; - max_freq = freq_max; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); - } - - //! Topic frequency diagnostic updater - diagnostic_updater::HeaderlessTopicDiagnostic *diagnostic; - //! Minimum allow frequency of topic - double min_freq; - //! Maximum allow frequency of topic - double max_freq; -}; - -//! Topic diagnostics for fix / fix_velocity messages -struct FixDiagnostic { - FixDiagnostic() {} - - // Must not copy this struct (would confuse FrequencyStatusParam pointers) - FixDiagnostic(const FixDiagnostic&) = delete; - - /** - * @brief Add a topic diagnostic to the diagnostic updater for fix topics. - * - * @details The minimum and maximum frequency are equal to the nav rate in Hz. - * @param name the ROS topic - * @param freq_tol the tolerance [%] for the topic frequency - * @param freq_window the number of messages to use for diagnostic statistics - * @param stamp_min the minimum allowed time delay - */ - FixDiagnostic (std::string name, double freq_tol, int freq_window, - double stamp_min) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz - min_freq = target_freq; - max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); - diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); - diagnostic = new diagnostic_updater::TopicDiagnostic(name, - *updater, - freq_param, - time_param); - } - - //! Topic frequency diagnostic updater - diagnostic_updater::TopicDiagnostic *diagnostic; - //! Minimum allow frequency of topic - double min_freq; - //! Maximum allow frequency of topic - double max_freq; -}; - -//! fix frequency diagnostic updater -boost::shared_ptr freq_diag; - -/** - * @brief Determine dynamic model from human-readable string. - * @param model One of the following (case-insensitive): - * - portable - * - stationary - * - pedestrian - * - automotive - * - sea - * - airborne1 - * - airborne2 - * - airborne4 - * - wristwatch - * @return DynamicModel - * @throws std::runtime_error on invalid argument. - */ -uint8_t modelFromString(const std::string& model); - -/** - * @brief Determine fix mode from human-readable string. - * @param mode One of the following (case-insensitive): - * - 2d - * - 3d - * - auto - * @return FixMode - * @throws std::runtime_error on invalid argument. - */ -uint8_t fixModeFromString(const std::string& mode); - -/** - * @brief Check that the parameter is above the minimum. - * @param val the value to check - * @param min the minimum for this value - * @param name the name of the parameter - * @throws std::runtime_error if it is below the minimum - */ -template -void checkMin(V val, T min, std::string name) { - if(val < min) { - std::stringstream oss; - oss << "Invalid settings: " << name << " must be > " << min; - throw std::runtime_error(oss.str()); - } -} - -/** - * @brief Check that the parameter is in the range. - * @param val the value to check - * @param min the minimum for this value - * @param max the maximum for this value - * @param name the name of the parameter - * @throws std::runtime_error if it is out of bounds - */ -template -void checkRange(V val, T min, T max, std::string name) { - if(val < min || val > max) { - std::stringstream oss; - oss << "Invalid settings: " << name << " must be in range [" << min << - ", " << max << "]."; - throw std::runtime_error(oss.str()); - } -} - -/** - * @brief Check that the elements of the vector are in the range. - * @param val the vector to check - * @param min the minimum for this value - * @param max the maximum for this value - * @param name the name of the parameter - * @throws std::runtime_error value it is out of bounds - */ -template -void checkRange(std::vector val, T min, T max, std::string name) { - for(size_t i = 0; i < val.size(); i++) { - std::stringstream oss; - oss << name << "[" << i << "]"; - checkRange(val[i], min, max, oss.str()); - } -} - -/** - * @brief Get a unsigned integer value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -bool getRosUint(const std::string& key, U &u) { - int param; - if (!nh->getParam(key, param)) return false; - // Check the bounds - U min = std::numeric_limits::lowest(); - U max = std::numeric_limits::max(); - checkRange(param, min, max, key); - // set the output - u = (U) param; - return true; -} - -/** - * @brief Get a unsigned integer value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @param val value to use if the server doesn't contain this parameter. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -void getRosUint(const std::string& key, U &u, V default_val) { - if(!getRosUint(key, u)) - u = default_val; -} - -/** - * @brief Get a unsigned integer vector from the parameter server. - * @throws std::runtime_error if the parameter is out of bounds. - * @return true if found, false if not found. - */ -template -bool getRosUint(const std::string& key, std::vector &u) { - std::vector param; - if (!nh->getParam(key, param)) return false; - - // Check the bounds - U min = std::numeric_limits::lowest(); - U max = std::numeric_limits::max(); - checkRange(param, min, max, key); - - // set the output - u.insert(u.begin(), param.begin(), param.end()); - return true; -} - -/** - * @brief Get a integer (size 8 or 16) value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -bool getRosInt(const std::string& key, I &u) { - int param; - if (!nh->getParam(key, param)) return false; - // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); - checkRange(param, min, max, key); - // set the output - u = (I) param; - return true; -} - -/** - * @brief Get an integer value (size 8 or 16) from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @param val value to use if the server doesn't contain this parameter. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -void getRosInt(const std::string& key, U &u, V default_val) { - if(!getRosInt(key, u)) - u = default_val; -} - -/** - * @brief Get a int (size 8 or 16) vector from the parameter server. - * @throws std::runtime_error if the parameter is out of bounds. - * @return true if found, false if not found. - */ -template -bool getRosInt(const std::string& key, std::vector &i) { - std::vector param; - if (!nh->getParam(key, param)) return false; - - // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); - checkRange(param, min, max, key); - - // set the output - i.insert(i.begin(), param.begin(), param.end()); - return true; -} - -/** - * @brief Publish a ROS message of type MessageT. - * - * @details This function should be used to publish all messages which are - * simply read from u-blox and published. - * @param m the message to publish - * @param topic the topic to publish the message on - */ -template -void publish(const MessageT& m, const std::string& topic) { - static ros::Publisher publisher = nh->advertise(topic, - kROSQueueSize); - publisher.publish(m); -} - -/** - * @param gnss The string representing the GNSS. Refer MonVER message protocol. - * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES - * @return true if the device supports the given GNSS - */ -bool supportsGnss(std::string gnss) { - return supported.count(gnss) > 0; -} - -/** - * @brief This interface is used to add functionality to the main node. - * - * @details This interface is generic and can be implemented for other features - * besides the main node, hardware versions, and firmware versions. - */ -class ComponentInterface { - public: - /** - * @brief Get the ROS parameters. - * @throws std::runtime_error if a parameter is invalid or required - * parameters are not set. - */ - virtual void getRosParams() = 0; - - /** - * @brief Configure the U-Blox settings. - * @return true if configured correctly, false otherwise - */ - virtual bool configureUblox() = 0; - - /** - * @brief Initialize the diagnostics. - * - * @details Function may be empty. - */ - virtual void initializeRosDiagnostics() = 0; - - /** - * @brief Subscribe to u-blox messages and publish to ROS topics. - */ - virtual void subscribe() = 0; -}; - -typedef boost::shared_ptr ComponentPtr; - -/** - * @brief This class represents u-blox ROS node for *all* firmware and product - * versions. - * - * @details It loads the user parameters, configures the u-blox - * device, subscribes to u-blox messages, and configures the device hardware. - * Functionality specific to a given product or firmware version, etc. should - * NOT be implemented in this class. Instead, the user should add the - * functionality to the appropriate implementation of ComponentInterface. - * If necessary, the user should create a class which implements u-blox - * interface, then add a pointer to an instance of the class to the - * components vector. - * The UbloxNode calls the public methods of ComponentInterface for each - * element in the components vector. - */ -class UbloxNode : public virtual ComponentInterface { - public: - //! How long to wait during I/O reset [s] - constexpr static int kResetWait = 10; - //! how often (in seconds) to call poll messages - constexpr static double kPollDuration = 1.0; - // Constants used for diagnostic frequency updater - //! [s] 5Hz diagnostic period - constexpr static float kDiagnosticPeriod = 0.2; - //! Tolerance for Fix topic frequency as percentage of target frequency - constexpr static double kFixFreqTol = 0.15; - //! Window [num messages] for Fix Frequency Diagnostic - constexpr static double kFixFreqWindow = 10; - //! Minimum Time Stamp Status for fix frequency diagnostic - constexpr static double kTimeStampStatusMin = 0; - - /** - * @brief Initialize and run the u-blox node. - */ - UbloxNode(); - - /** - * @brief Get the node parameters from the ROS Parameter Server. - */ - void getRosParams(); - - /** - * @brief Configure the device based on ROS parameters. - * @return true if configured successfully - */ - bool configureUblox(); - - /** - * @brief Subscribe to all requested u-blox messages. - */ - void subscribe(); - - /** - * @brief Initialize the diagnostic updater and add the fix diagnostic. - */ - void initializeRosDiagnostics(); - - /** - * @brief Print an INF message to the ROS console. - */ - void printInf(const ublox_msgs::Inf &m, uint8_t id); - - private: - - /** - * @brief Initialize the I/O handling. - */ - void initializeIo(); - - /** - * @brief Initialize the U-Blox node. Configure the U-Blox and subscribe to - * messages. - */ - void initialize(); - - /** - * @brief Shutdown the node. Closes the serial port. - */ - void shutdown(); - - /** - * @brief Send a reset message the u-blox device & re-initialize the I/O. - * @return true if reset was successful, false otherwise. - */ - bool resetDevice(); - - /** - * @brief Process the MonVer message and add firmware and product components. - * - * @details Determines the protocol version, product type and supported GNSS. - */ - void processMonVer(); - - /** - * @brief Add the interface for firmware specific configuration, subscribers, - * & diagnostics. This assumes the protocol_version_ has been set. - */ - void addFirmwareInterface(); - - /** - * @brief Add the interface which is used for product category - * configuration, subscribers, & diagnostics. - * @param the product category, i.e. SPG, HPG, ADR, UDR, TIM, or FTS. - * @param for HPG/TIM products, this value is either REF or ROV, for other - * products this string is empty - */ - void addProductInterface(std::string product_category, - std::string ref_rov = ""); - - /** - * @brief Poll messages from the U-Blox device. - * @param event a timer indicating how often to poll the messages - */ - void pollMessages(const ros::TimerEvent& event); - - /** - * @brief Configure INF messages, call after subscribe. - */ - void configureInf(); - - //! The u-blox node components - /*! - * The node will call the functions in these interfaces for each object - * in the vector. - */ - std::vector > components_; - - //! Determined From Mon VER - float protocol_version_ = 0; - // Variables set from parameter server - //! Device port - std::string device_; - //! dynamic model type - std::string dynamic_model_; - //! Fix mode type - std::string fix_mode_; - //! Set from dynamic model string - uint8_t dmodel_; - //! Set from fix mode string - uint8_t fmode_; - //! UART1 baudrate - uint32_t baudrate_; - //! UART in protocol (see CfgPRT message for constants) - uint16_t uart_in_; - //! UART out protocol (see CfgPRT message for constants) - uint16_t uart_out_; - //! USB TX Ready Pin configuration (see CfgPRT message for constants) - uint16_t usb_tx_; - //! Whether to configure the USB port - /*! Set to true if usb_in & usb_out parameters are set */ - bool set_usb_; - //! USB in protocol (see CfgPRT message for constants) - uint16_t usb_in_; - //! USB out protocol (see CfgPRT message for constants) - uint16_t usb_out_ ; - //! The measurement rate in Hz - double rate_; - //! If true, set configure the User-Defined Datum - bool set_dat_; - //! User-defined Datum - ublox_msgs::CfgDAT cfg_dat_; - //! Whether or not to enable SBAS - bool enable_sbas_; - //! Whether or not to enable PPP (advanced setting) - bool enable_ppp_; - //! SBAS Usage parameter (see CfgSBAS message) - uint8_t sbas_usage_; - //! Max SBAS parameter (see CfgSBAS message) - uint8_t max_sbas_; - //! Dead reckoning limit parameter - uint8_t dr_limit_; - //! Parameters to load from non-volatile memory during configuration - ublox_msgs::CfgCFG load_; - //! Parameters to save to non-volatile memory after configuration - ublox_msgs::CfgCFG save_; - //! rate for TIM-TM2 - uint8_t tim_rate_; - - //! raw data stream logging - RawDataStreamPa rawDataStreamPa_; -}; - -/** - * @brief This abstract class represents a firmware component. - * - * @details The Firmware components update the fix diagnostics. - */ -class UbloxFirmware : public virtual ComponentInterface { - public: - /** - * @brief Add the fix diagnostics to the updater. - */ - void initializeRosDiagnostics(); - - protected: - /** - * @brief Handle to send fix status to ROS diagnostics. - */ - virtual void fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; -}; - -/** - * @brief Implements functions for firmware version 6. - */ -class UbloxFirmware6 : public UbloxFirmware { - public: - UbloxFirmware6(); - - /** - * @brief Sets the fix status service type to GPS. - */ - void getRosParams(); - - /** - * @brief Prints a warning, GNSS configuration not available in this version. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(); - - /** - * @brief Subscribe to NavPVT, RxmRAW, and RxmSFRB messages. - */ - void subscribe(); - - protected: - /** - * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL - * messages. - */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - - private: - /** - * @brief Publish the fix and call the fix diagnostic updater. - * - * @details Also updates the last known position and publishes the NavPosLLH - * message if publishing is enabled. - * @param m the message to process - */ - void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m); - - /** - * @brief Update the last known velocity. - * - * @details Publish the message if publishing is enabled. - * @param m the message to process - */ - void callbackNavVelNed(const ublox_msgs::NavVELNED& m); - - /** - * @brief Update the number of SVs used for the fix. - * - * @details Publish the message if publishing is enabled. - * @param m the message to process - */ - void callbackNavSol(const ublox_msgs::NavSOL& m); - - //! The last received navigation position - ublox_msgs::NavPOSLLH last_nav_pos_; - //! The last received navigation velocity - ublox_msgs::NavVELNED last_nav_vel_; - //! The last received num SVs used - ublox_msgs::NavSOL last_nav_sol_; - //! The last NavSatFix based on last_nav_pos_ - sensor_msgs::NavSatFix fix_; - //! The last Twist based on last_nav_vel_ - geometry_msgs::TwistWithCovarianceStamped velocity_; - - //! Used to configure NMEA (if set_nmea_) filled with ROS parameters - ublox_msgs::CfgNMEA6 cfg_nmea_; - //! Whether or not to configure the NMEA settings - bool set_nmea_; -}; - -/** - * @brief Abstract class for Firmware versions >= 7. - * - * @details This class keeps track of the last NavPVT message uses it to - * update the fix diagnostics. It is a template class because the NavPVT message - * is a different length for firmware versions 7 and 8. - * - * @typedef NavPVT the NavPVT message type for the given firmware version - */ -template -class UbloxFirmware7Plus : public UbloxFirmware { - public: - /** - * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. - * - * @details If a fixed carrier phase solution is available, the NavSatFix - * status is set to GBAS fixed. If NavPVT publishing is enabled, the message - * is published. This function also calls the ROS diagnostics updater. - * @param m the message to publish - */ - void callbackNavPvt(const NavPVT& m) { - if(enabled["nav_pvt"]) { - // NavPVT publisher - static ros::Publisher publisher = nh->advertise("navpvt", - kROSQueueSize); - publisher.publish(m); - } - - // - // NavSatFix message - // - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); - - sensor_msgs::NavSatFix fix; - fix.header.frame_id = frame_id; - // set the timestamp - uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; - if (((m.valid & valid_time) == valid_time) && - (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) { - // Use NavPVT timestamp since it is valid - // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9 - // The ros time uses only unsigned values, so a negative nano seconds must be - // converted to a positive value - if (m.nano < 0) { - fix.header.stamp.sec = toUtcSeconds(m) - 1; - fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9); - } - else { - fix.header.stamp.sec = toUtcSeconds(m); - fix.header.stamp.nsec = (uint32_t)(m.nano); - } - } else { - // Use ROS time since NavPVT timestamp is not valid - fix.header.stamp = ros::Time::now(); - } - // Set the LLA - fix.latitude = m.lat * 1e-7; // to deg - fix.longitude = m.lon * 1e-7; // to deg - fix.altitude = m.height * 1e-3; // to [m] - // Set the Fix status - bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; - if (fixOk && m.fixType >= m.FIX_TYPE_2D) { - fix.status.status = fix.status.STATUS_FIX; - if(m.flags & m.CARRIER_PHASE_FIXED) - fix.status.status = fix.status.STATUS_GBAS_FIX; - } - else { - fix.status.status = fix.status.STATUS_NO_FIX; - } - // Set the service based on GNSS configuration - fix.status.service = fix_status_service; - - // Set the position covariance - const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2] - const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2] - fix.position_covariance[0] = varH; - fix.position_covariance[4] = varH; - fix.position_covariance[8] = varV; - fix.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - - fixPublisher.publish(fix); - - // - // Twist message - // - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); - geometry_msgs::TwistWithCovarianceStamped velocity; - velocity.header.stamp = fix.header.stamp; - velocity.header.frame_id = frame_id; - - // convert to XYZ linear velocity [m/s] in ENU - velocity.twist.twist.linear.x = m.velE * 1e-3; - velocity.twist.twist.linear.y = m.velN * 1e-3; - velocity.twist.twist.linear.z = -m.velD * 1e-3; - // Set the covariance - const double covSpeed = pow(m.sAcc * 1e-3, 2); - const int cols = 6; - velocity.twist.covariance[cols * 0 + 0] = covSpeed; - velocity.twist.covariance[cols * 1 + 1] = covSpeed; - velocity.twist.covariance[cols * 2 + 2] = covSpeed; - velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - - velocityPublisher.publish(velocity); - - // - // Update diagnostics - // - last_nav_pvt_ = m; - freq_diag->diagnostic->tick(fix.header.stamp); - updater->update(); - } - - protected: - - /** - * @brief Update the fix diagnostics from Nav PVT message. - */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { - // check the last message, convert to diagnostic - if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Dead reckoning only"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "2D fix"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "3D fix"; - } else if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Time only fix"; - } - - // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message += ", fix not ok"; - } - // Raise diagnostic level to error if no fix - if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "No fix"; - } - - // append last fix position - stat.add("iTOW [ms]", last_nav_pvt_.iTOW); - stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7); - stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7); - stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3); - stat.add("Height above MSL [m]", last_nav_pvt_.hMSL * 1e-3); - stat.add("Horizontal Accuracy [m]", last_nav_pvt_.hAcc * 1e-3); - stat.add("Vertical Accuracy [m]", last_nav_pvt_.vAcc * 1e-3); - stat.add("# SVs used", (int)last_nav_pvt_.numSV); - } - - //! The last received NavPVT message - NavPVT last_nav_pvt_; - // Whether or not to enable the given GNSS - //! Whether or not to enable GPS - bool enable_gps_; - //! Whether or not to enable GLONASS - bool enable_glonass_; - //! Whether or not to enable QZSS - bool enable_qzss_; - //! Whether or not to enable SBAS - bool enable_sbas_; - //! The QZSS Signal configuration, see CfgGNSS message - uint32_t qzss_sig_cfg_; -}; - -/** - * @brief Implements functions for firmware version 7. - */ -class UbloxFirmware7 : public UbloxFirmware7Plus { - public: - UbloxFirmware7(); - - /** - * @brief Get the parameters specific to firmware version 7. - * - * @details Get the GNSS and NMEA settings. - */ - void getRosParams(); - - /** - * @brief Configure GNSS individually. Only configures GLONASS. - */ - bool configureUblox(); - - /** - * @brief Subscribe to messages which are not generic to all firmware. - * - * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. - */ - void subscribe(); - - private: - //! Used to configure NMEA (if set_nmea_) - /*! - * Filled from ROS parameters - */ - ublox_msgs::CfgNMEA7 cfg_nmea_; - //! Whether or not to Configure the NMEA settings - bool set_nmea_; -}; - -/** - * @brief Implements functions for firmware version 8. - */ -class UbloxFirmware8 : public UbloxFirmware7Plus { - public: - UbloxFirmware8(); - - /** - * @brief Get the ROS parameters specific to firmware version 8. - * - * @details Get the GNSS, NMEA, and UPD settings. - */ - void getRosParams(); - - /** - * @brief Configure settings specific to firmware 8 based on ROS parameters. - * - * @details Configure GNSS, if it is different from current settings. - * Configure the NMEA if desired by the user. It also may clear the - * flash memory based on the ROS parameters. - */ - bool configureUblox(); - - /** - * @brief Subscribe to u-blox messages which are not generic to all firmware - * versions. - * - * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based - * on user settings. - */ - void subscribe(); - - private: - // Set from ROS parameters - //! Whether or not to enable the Galileo GNSS - bool enable_galileo_; - //! Whether or not to enable the BeiDuo GNSS - bool enable_beidou_; - //! Whether or not to enable the IMES GNSS - bool enable_imes_; - //! Whether or not to configure the NMEA settings - bool set_nmea_; - //! Desired NMEA configuration. - ublox_msgs::CfgNMEA cfg_nmea_; - //! Whether to clear the flash memory during configuration - bool clear_bbr_; -}; - -/** - * @brief Implements functions for firmware version 9. - * For now it simply re-uses the firmware version 8 class - * but allows for future expansion of functionality - */ -class UbloxFirmware9 : public UbloxFirmware8 { -}; - -/** - * @brief Implements functions for Raw Data products. - */ -class RawDataProduct: public virtual ComponentInterface { - public: - static constexpr double kRtcmFreqTol = 0.15; - static constexpr int kRtcmFreqWindow = 25; - - /** - * @brief Does nothing since there are no Raw Data product specific settings. - */ - void getRosParams() {} - - /** - * @brief Does nothing since there are no Raw Data product specific settings. - * @return always returns true - */ - bool configureUblox() { return true; } - - /** - * @brief Subscribe to Raw Data Product messages and set up ROS publishers. - * - * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. - */ - void subscribe(); - - /** - * @brief Adds frequency diagnostics for RTCM topics. - */ - void initializeRosDiagnostics(); - - private: - //! Topic diagnostic updaters - std::vector > freq_diagnostics_; -}; - -/** - * @brief Implements functions for Automotive Dead Reckoning (ADR) and - * Untethered Dead Reckoning (UDR) Devices. - */ -class AdrUdrProduct: public virtual ComponentInterface { - public: - /** - * @brief Get the ADR/UDR parameters. - * - * @details Get the use_adr parameter and check that the nav_rate is 1 Hz. - */ - void getRosParams(); - - /** - * @brief Configure ADR/UDR settings. - * @details Configure the use_adr setting. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(); - - /** - * @brief Subscribe to ADR/UDR messages. - * - * @details Subscribe to NavATT, ESF and HNR messages based on user - * parameters. - */ - void subscribe(); - - /** - * @brief Initialize the ROS diagnostics for the ADR/UDR device. - * @todo unimplemented - */ - void initializeRosDiagnostics() { - ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", - "unimplemented. See AdrUdrProduct class in node.h & node.cpp."); - } - - protected: - //! Whether or not to enable dead reckoning - bool use_adr_; - - - sensor_msgs::Imu imu_; - sensor_msgs::TimeReference t_ref_; - ublox_msgs::TimTM2 timtm2; - - void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); -}; - -/** - * @brief Implements functions for FTS products. Currently unimplemented. - * @todo Unimplemented. - */ -class FtsProduct: public virtual ComponentInterface { - /** - * @brief Get the FTS parameters. - * @todo Currently unimplemented. - */ - void getRosParams() { - ROS_WARN("Functionality specific to u-blox FTS devices is %s", - "unimplemented. See FtsProduct class in node.h & node.cpp."); - } - - /** - * @brief Configure FTS settings. - * @todo Currently unimplemented. - */ - bool configureUblox() { return false; } - - /** - * @brief Subscribe to FTS messages. - * @todo Currently unimplemented. - */ - void subscribe() {} - - /** - * @brief Adds diagnostic updaters for FTS status. - * @todo Currently unimplemented. - */ - void initializeRosDiagnostics() {} -}; - -/** - * @brief Implements functions for High Precision GNSS Reference station - * devices. - */ -class HpgRefProduct: public virtual ComponentInterface { - public: - /** - * @brief Get the ROS parameters specific to the Reference Station - * configuration. - * - * @details Get the TMODE3 settings, the parameters it gets depends on the - * tmode3 parameter. For example, it will get survey-in parameters if the - * tmode3 parameter is set to survey in or it will get the fixed parameters if - * it is set to fixed. - */ - void getRosParams(); - - /** - * @brief Configure the u-blox Reference Station settings. - * - * @details Configure the TMODE3 settings and sets the internal state based - * on the TMODE3 status. If the TMODE3 is set to fixed, it will configure - * the RTCM messages. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(); - - /** - * @brief Subscribe to u-blox Reference Station messages. - * - * @details Subscribe to NavSVIN messages based on user parameters. - */ - void subscribe(); - - /** - * @brief Add diagnostic updaters for the TMODE3 status. - */ - void initializeRosDiagnostics(); - - /** - * @brief Update the last received NavSVIN message and call diagnostic updater - * - * @details When the survey in finishes, it changes the measurement & - * navigation rate to the user configured values and enables the user - * configured RTCM messages. Publish received Nav SVIN messages if enabled. - * @param m the message to process - */ - void callbackNavSvIn(ublox_msgs::NavSVIN m); - - protected: - /** - * @brief Update the TMODE3 diagnostics. - * - * @details Updates the status of the survey-in if in survey-in mode or the - * RTCM messages if in time mode. - */ - void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - - /** - * @brief Set the device mode to time mode (internal state variable). - * - * @details Configure the RTCM messages and measurement and navigation rate. - */ - bool setTimeMode(); - - //! The last received Nav SVIN message - ublox_msgs::NavSVIN last_nav_svin_; - - //! TMODE3 to set, such as disabled, survey-in, fixed - uint8_t tmode3_; - - // TMODE3 = Fixed mode settings - //! True if coordinates are in LLA, false if ECEF - /*! Used only for fixed mode */ - bool lla_flag_; - //! Antenna Reference Point Position [m] or [deg] - /*! Used only for fixed mode */ - std::vector arp_position_; - //! Antenna Reference Point Position High Precision [0.1 mm] or [deg * 1e-9] - /*! Used only for fixed mode */ - std::vector arp_position_hp_; - //! Fixed Position Accuracy [m] - /*! Used only for fixed mode */ - float fixed_pos_acc_; - - // Settings for TMODE3 = Survey-in - //! Whether to always reset the survey-in during configuration. - /*! - * If false, it only resets survey-in if there's no fix and TMODE3 is - * disabled before configuration. - * This variable is used only if TMODE3 is set to survey-in. - */ - bool svin_reset_; - //! Measurement period used during Survey-In [s] - /*! This variable is used only if TMODE3 is set to survey-in. */ - uint32_t sv_in_min_dur_; - //! Survey in accuracy limit [m] - /*! This variable is used only if TMODE3 is set to survey-in. */ - float sv_in_acc_lim_; - - //! Status of device time mode - enum { - INIT, //!< Initialization mode (before configuration) - FIXED, //!< Fixed mode (should switch to time mode almost immediately) - DISABLED, //!< Time mode disabled - SURVEY_IN, //!< Survey-In mode - TIME //!< Time mode, after survey-in or after configuring fixed mode - } mode_; -}; - -/** - * @brief Implements functions for High Precision GNSS Rover devices. - */ -class HpgRovProduct: public virtual ComponentInterface { - public: - // Constants for diagnostic updater - //! Diagnostic updater: RTCM topic frequency min [Hz] - constexpr static double kRtcmFreqMin = 1; - //! Diagnostic updater: RTCM topic frequency max [Hz] - constexpr static double kRtcmFreqMax = 10; - //! Diagnostic updater: RTCM topic frequency tolerance [%] - constexpr static double kRtcmFreqTol = 0.1; - //! Diagnostic updater: RTCM topic frequency window [num messages] - constexpr static int kRtcmFreqWindow = 25; - /** - * @brief Get the ROS parameters specific to the Rover configuration. - * - * @details Get the DGNSS mode. - */ - void getRosParams(); - - /** - * @brief Configure rover settings. - * - * @details Configure the DGNSS mode. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(); - - /** - * @brief Subscribe to Rover messages, such as NavRELPOSNED. - */ - void subscribe(); - - /** - * @brief Add diagnostic updaters for rover GNSS status, including - * status of RTCM messages. - */ - void initializeRosDiagnostics(); - - protected: - /** - * @brief Update the rover diagnostics, including the carrier phase solution - * status (float or fixed). - */ - void carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat); - - /** - * @brief Set the last received message and call rover diagnostic updater - * - * @details Publish received NavRELPOSNED messages if enabled - */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); - - - //! Last relative position (used for diagnostic updater) - ublox_msgs::NavRELPOSNED last_rel_pos_; - - //! The DGNSS mode - /*! see CfgDGNSS message for possible values */ - uint8_t dgnss_mode_; - - //! The RTCM topic frequency diagnostic updater - UbloxTopicDiagnostic freq_rtcm_; -}; - -class HpPosRecProduct: public virtual HpgRefProduct { - public: - /** - * @brief Subscribe to Rover messages, such as NavRELPOSNED. - */ - void subscribe(); - - protected: - - /** - * @brief Set the last received message and call rover diagnostic updater - * - * @details Publish received NavRELPOSNED messages if enabled - */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m); - - sensor_msgs::Imu imu_; - - //! Last relative position (used for diagnostic updater) - ublox_msgs::NavRELPOSNED9 last_rel_pos_; -}; - -/** - * @brief Implements functions for Time Sync products. - * @todo partially implemented - */ -class TimProduct: public virtual ComponentInterface { - /** - * @brief Get the Time Sync parameters. - * @todo Currently unimplemented. - */ - void getRosParams(); - - /** - * @brief Configure Time Sync settings. - * @todo Currently unimplemented. - */ - bool configureUblox(); - - /** - * @brief Subscribe to Time Sync messages. - * - * @details Subscribes to RxmRAWX & RxmSFRBX messages. - */ - void subscribe(); - - /** - * @brief Adds diagnostic updaters for Time Sync status. - * @todo Currently unimplemented. - */ - void initializeRosDiagnostics(); - - protected: - /** - * @brief - * @details Publish recieved TimTM2 messages if enabled - */ - void callbackTimTM2(const ublox_msgs::TimTM2 &m); - - sensor_msgs::TimeReference t_ref_; -}; - -} - -#endif diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp new file mode 100644 index 00000000..4e54352c --- /dev/null +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -0,0 +1,312 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 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 UBLOX_GPS_NODE_HPP +#define UBLOX_GPS_NODE_HPP + +// STL +#include +#include +#include +// ROS includes +#include +#include +#include +// U-Blox msgs nicludes +#include +#include +#include +#include +#include +// Ublox GPS includes +#include +#include +#include +#include +#include +#include + +// This file also declares UbloxNode which is the main class and ros node. It +// implements functionality which applies to any u-blox device, regardless of +// the firmware version or product type. The class is designed in compositional +// style; it contains ComponentInterfaces which implement features specific to +// the device based on its firmware version and product category. UbloxNode +// calls the public methods of each component. + +/** + * @namespace ublox_node + * This namespace is for the ROS u-blox node and handles anything regarding + * ROS parameters, message passing, diagnostics, etc. + */ +namespace ublox_node { + +/** + * @brief This class represents u-blox ROS node for *all* firmware and product + * versions. + * + * @details It loads the user parameters, configures the u-blox + * device, subscribes to u-blox messages, and configures the device hardware. + * Functionality specific to a given product or firmware version, etc. should + * NOT be implemented in this class. Instead, the user should add the + * functionality to the appropriate implementation of ComponentInterface. + * If necessary, the user should create a class which implements u-blox + * interface, then add a pointer to an instance of the class to the + * components vector. + * The UbloxNode calls the public methods of ComponentInterface for each + * element in the components vector. + */ +class UbloxNode final : public rclcpp::Node { + public: + //! How long to wait during I/O reset [s] + constexpr static int kResetWait = 10; + //! How often (in seconds) to send keep-alive message + constexpr static double kKeepAlivePeriod = 10.0; + //! How often (in seconds) to call poll messages + constexpr static double kPollDuration = 1.0; + // Constants used for diagnostic frequency updater + //! [s] 5Hz diagnostic period + const float kDiagnosticPeriod = 0.5; + //! Tolerance for Fix topic frequency as percentage of target frequency + const double kFixFreqTol = 0.3; + //! Window [num messages] for Fix Frequency Diagnostic + const double kFixFreqWindow = 25; + //! Minimum Time Stamp Status for fix frequency diagnostic + const double kTimeStampStatusMin = 0; + + /** + * @brief Initialize and run the u-blox node. + */ + explicit UbloxNode(const rclcpp::NodeOptions & options); + + ~UbloxNode() override; + + UbloxNode(UbloxNode &&c) = delete; + UbloxNode &operator=(UbloxNode &&c) = delete; + UbloxNode(const UbloxNode &c) = delete; + UbloxNode &operator=(const UbloxNode &c) = delete; + + /** + * @brief Get the node parameters from the ROS Parameter Server. + */ + void getRosParams(); + + /** + * @brief Configure the device based on ROS parameters. + * @return true if configured successfully + */ + bool configureUblox(); + + /** + * @brief Subscribe to all requested u-blox messages. + */ + void subscribe(); + + /** + * @brief Initialize the diagnostic updater and add the fix diagnostic. + */ + void initializeRosDiagnostics(); + + /** + * @brief Print an INF message to the ROS console. + */ + void printInf(const ublox_msgs::msg::Inf &m, uint8_t id); + + private: + + /** + * @brief Callback for '/ntrip_client/rtcm' subscription to handle RTCM correction data + */ + void rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg); + + /** + * @brief Subscription handler for RTCM data + */ + rclcpp::Subscription::SharedPtr subscription_; + + /** + * @brief Initialize the I/O handling. + */ + void initializeIo(); + + /** + * @brief Initialize the U-Blox node. Configure the U-Blox and subscribe to + * messages. + */ + void initialize(); + + /** + * @brief Shutdown the node. Closes the serial port. + */ + void shutdown(); + + /** + * @brief Send a reset message the u-blox device & re-initialize the I/O. + * @return true if reset was successful, false otherwise. + */ + bool resetDevice(); + + /** + * @brief Process the MonVer message and add firmware and product components. + * + * @details Determines the protocol version, product type and supported GNSS. + */ + void processMonVer(); + + /** + * @brief Add the interface for firmware specific configuration, subscribers, + * & diagnostics. This assumes the protocol_version_ has been set. + */ + void addFirmwareInterface(); + + /** + * @brief Add the interface which is used for product category + * configuration, subscribers, & diagnostics. + * @param the product category, i.e. SPG, HPG, ADR, UDR, TIM, or FTS. + * @param for HPG/TIM products, this value is either REF or ROV, for other + * products this string is empty + */ + void addProductInterface(const std::string & product_category, + const std::string & ref_rov = ""); + + /** + * @brief Poll version message from the U-Blox device to keep socket active. + */ + void keepAlive(); + + /** + * @brief Poll messages from the U-Blox device. + */ + void pollMessages(); + + /** + * @brief Configure INF messages, call after subscribe. + */ + void configureInf(); + + //! The u-blox node components + /*! + * The node will call the functions in these interfaces for each object + * in the vector. + */ + std::vector > components_; + + //! Determined From Mon VER + float protocol_version_ = 0.0; + // Variables set from parameter server + //! Device port + std::string device_; + //! dynamic model type + std::string dynamic_model_; + //! Fix mode type + std::string fix_mode_; + //! Set from dynamic model string + uint8_t dmodel_{0}; + //! Set from fix mode string + uint8_t fmode_{0}; + //! UART1 baudrate + uint32_t baudrate_{0}; + //! UART in protocol (see CfgPRT message for constants) + uint16_t uart_in_{0}; + //! UART out protocol (see CfgPRT message for constants) + uint16_t uart_out_{0}; + //! USB TX Ready Pin configuration (see CfgPRT message for constants) + uint16_t usb_tx_{0}; + //! Whether to configure the USB port + /*! Set to true if usb_in & usb_out parameters are set */ + bool set_usb_{false}; + //! USB in protocol (see CfgPRT message for constants) + uint16_t usb_in_{0}; + //! USB out protocol (see CfgPRT message for constants) + uint16_t usb_out_{0}; + //! The measurement rate in Hz + double rate_{0.0}; + //! User-defined Datum + ublox_msgs::msg::CfgDAT cfg_dat_; + //! SBAS Usage parameter (see CfgSBAS message) + uint8_t sbas_usage_{0}; + //! Max SBAS parameter (see CfgSBAS message) + uint8_t max_sbas_{0}; + //! Dead reckoning limit parameter + uint8_t dr_limit_{0}; + //! Parameters to load from non-volatile memory during configuration + ublox_msgs::msg::CfgCFG load_; + //! Parameters to save to non-volatile memory after configuration + ublox_msgs::msg::CfgCFG save_; + //! rate for TIM-TM2 + uint8_t tim_rate_{0}; + + //! raw data stream logging + std::shared_ptr raw_data_stream_pa_; + + rclcpp::Publisher::SharedPtr nav_status_pub_; + rclcpp::Publisher::SharedPtr nav_posecef_pub_; + rclcpp::Publisher::SharedPtr nav_clock_pub_; + rclcpp::Publisher::SharedPtr nav_cov_pub_; + rclcpp::Publisher::SharedPtr aid_alm_pub_; + rclcpp::Publisher::SharedPtr aid_eph_pub_; + rclcpp::Publisher::SharedPtr aid_hui_pub_; + rclcpp::Publisher::SharedPtr nmea_pub_; + + void publish_nmea(const std::string & sentence, const std::string & topic); + + //! Navigation rate in measurement cycles, see CfgRate.msg + uint16_t nav_rate_{0}; + + //! The measurement [ms], see CfgRate.msg + uint16_t meas_rate_{0}; + + //! The ROS frame ID of this device + std::string frame_id_; + + //! ROS diagnostic updater + std::shared_ptr updater_; + + //! fix frequency diagnostic updater + std::shared_ptr freq_diag_; + + std::vector rtcms_; + + //! Which GNSS are supported by the device + std::shared_ptr gnss_; + + //! Handles communication with the U-Blox Device + std::shared_ptr gps_; + + rclcpp::TimerBase::SharedPtr keep_alive_; + rclcpp::TimerBase::SharedPtr poller_; + + rclcpp::Service::SharedPtr shutdown_srv_; + std::atomic_bool shutdown_requested_{false}; + rclcpp::TimerBase::SharedPtr shutdown_timer_; + +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_NODE_HPP diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.h b/ublox_gps/include/ublox_gps/raw_data_pa.hpp similarity index 50% rename from ublox_gps/include/ublox_gps/raw_data_pa.h rename to ublox_gps/include/ublox_gps/raw_data_pa.hpp index 95d71ac9..24d13d92 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.h +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -33,19 +33,18 @@ // measured data with the rtklib. -#ifndef UBLOX_RAW_DATA_PA_H -#define UBLOX_RAW_DATA_PA_H +#ifndef UBLOX_GPS_RAW_DATA_PA_HPP +#define UBLOX_GPS_RAW_DATA_PA_HPP // STL -#include -#include #include +#include // ROS includes -#include +#include // ROS messages -#include +#include /** * @namespace ublox_node @@ -57,88 +56,85 @@ namespace ublox_node { /** * @brief Implements functions for raw data stream. */ -class RawDataStreamPa { - public: - - - /** - * @brief Constructor. - * Initialises variables and the nodehandle. - */ - RawDataStreamPa(bool is_ros_subscriber = false); - - /** - * @brief Get the raw data stream parameters. - */ - void getRosParams(void); - - /** - * @brief Returns the if raw data streaming is enabled. - */ - bool isEnabled(void); - - /** - * @brief Initializes raw data streams - * If storing to file is enabled, the filename is created and the - * corresponding filedescriptor will be opened. - * If publishing ros messages is enabled, an empty msg will be published. - * (This will implicitly create the publisher) - */ - void initialize(void); - - /** - * @brief Callback function which handles raw data. - * @param data the buffer of u-blox messages to process - * @param size the size of the buffer - */ - void ubloxCallback(const unsigned char* data, - const std::size_t size); - - /** - * @brief Callback function which handles raw data. - * @param msg ros message - */ - void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); - - private: - /** - * @brief Converts a string into an uint8 multibyte array - */ - std_msgs::UInt8MultiArray str2uint8(const std::string str); - - /** - * @brief Publishes data stream as ros message - * @param str raw data stream as string - */ - void publishMsg(const std::string str); - - /** - * @brief Stores data to given file - * @param str raw data stream as string - */ - void saveToFile(const std::string str); - - //! Directoy name for storing raw data - std::string file_dir_; - //! Filename for storing raw data - std::string file_name_; - //! Handle for file access - std::ofstream file_handle_; - - //! Flag for publishing raw data - bool flag_publish_; - - //! Internal flag - //! true : subscribing to ros messages and storing those to file - //! false: publishing ros messages and/or storing to file - bool is_ros_subscriber_; - - //! ROS private node handle (for params and publisher) - ros::NodeHandle pnh_; - //! ROS node handle (only for subscriber) - ros::NodeHandle nh_; +class RawDataStreamPa final : public rclcpp::Node { + public: + + /** + * @brief Constructor. + * Initialises variables and the nodehandle. + */ + explicit RawDataStreamPa(bool is_ros_subscriber = false); + + /** + * @brief Get the raw data stream parameters. + */ + void getRosParams(); + + /** + * @brief Returns the if raw data streaming is enabled. + */ + bool isEnabled(); + + /** + * @brief Initializes raw data streams + * If storing to file is enabled, the filename is created and the + * corresponding filedescriptor will be opened. + * If publishing ros messages is enabled, an empty msg will be published. + * (This will implicitly create the publisher) + */ + void initialize(); + + /** + * @brief Callback function which handles raw data. + * @param data the buffer of u-blox messages to process + * @param size the size of the buffer + */ + void ubloxCallback(const unsigned char* data, + std::size_t size); + + private: + /** + * @brief Callback function which handles raw data. + * @param msg ros message + */ + void msgCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + + /** + * @brief Converts a string into an uint8 multibyte array + */ + std_msgs::msg::UInt8MultiArray str2uint8(const std::string & str); + + /** + * @brief Publishes data stream as ros message + * @param str raw data stream as string + */ + void publishMsg(const std::string & str); + + /** + * @brief Stores data to given file + * @param str raw data stream as string + */ + void saveToFile(const std::string & str); + + //! Directory name for storing raw data + std::string file_dir_; + //! Filename for storing raw data + std::string file_name_; + //! Handle for file access + std::ofstream file_handle_; + + //! Flag for publishing raw data + bool flag_publish_; + + //! Internal flag + //! true : subscribing to ros messages and storing those to file + //! false: publishing ros messages and/or storing to file + bool is_ros_subscriber_; + + rclcpp::Publisher::SharedPtr raw_pub_; + rclcpp::Subscription::SharedPtr raw_data_stream_sub_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/raw_data_product.hpp b/ublox_gps/include/ublox_gps/raw_data_product.hpp new file mode 100644 index 00000000..46afef81 --- /dev/null +++ b/ublox_gps/include/ublox_gps/raw_data_product.hpp @@ -0,0 +1,74 @@ +#ifndef UBLOX_GPS_RAW_DATA_PRODUCT_HPP +#define UBLOX_GPS_RAW_DATA_PRODUCT_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for Raw Data products. + */ +class RawDataProduct final : public virtual ComponentInterface { + public: + const double kRtcmFreqTol = 0.15; + const int kRtcmFreqWindow = 25; + + explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node); + + /** + * @brief Does nothing since there are no Raw Data product specific settings. + */ + void getRosParams() override {} + + /** + * @brief Does nothing since there are no Raw Data product specific settings. + * @return always returns true + */ + bool configureUblox(std::shared_ptr gps) override { + (void)gps; + return true; + } + + /** + * @brief Adds frequency diagnostics for RTCM topics. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to Raw Data Product messages and set up ROS publishers. + * + * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. + */ + void subscribe(std::shared_ptr gps) override; + + private: + //! Topic diagnostic updaters + std::vector > freq_diagnostics_; + + rclcpp::Publisher::SharedPtr rxm_raw_pub_; + rclcpp::Publisher::SharedPtr rxm_sfrb_pub_; + rclcpp::Publisher::SharedPtr rxm_eph_pub_; + rclcpp::Publisher::SharedPtr rxm_alm_pub_; + + uint16_t nav_rate_; + uint16_t meas_rate_; + std::shared_ptr updater_; + rclcpp::Node* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_RAW_DATA_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/rtcm.hpp b/ublox_gps/include/ublox_gps/rtcm.hpp new file mode 100644 index 00000000..689faa20 --- /dev/null +++ b/ublox_gps/include/ublox_gps/rtcm.hpp @@ -0,0 +1,17 @@ +#ifndef UBLOX_GPS_RTCM_HPP +#define UBLOX_GPS_RTCM_HPP + +#include + +namespace ublox_gps { + +struct Rtcm { + //! ID of RTCM out message to configure. + uint8_t id; + //! Rate of RTCM out message. + uint8_t rate; +}; + +} // namespace ublox_gps + +#endif diff --git a/ublox_gps/include/ublox_gps/tim_product.hpp b/ublox_gps/include/ublox_gps/tim_product.hpp new file mode 100644 index 00000000..57719593 --- /dev/null +++ b/ublox_gps/include/ublox_gps/tim_product.hpp @@ -0,0 +1,75 @@ +#ifndef UBLOX_GPS_TIM_PRODUCT_HPP +#define UBLOX_GPS_TIM_PRODUCT_HPP + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for Time Sync products. + * @todo partially implemented + */ +class TimProduct final : public virtual ComponentInterface { + public: + explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); + + /** + * @brief Get the Time Sync parameters. + * @todo Currently unimplemented. + */ + void getRosParams() override; + + /** + * @brief Configure Time Sync settings. + * @todo Currently unimplemented. + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Adds diagnostic updaters for Time Sync status. + * @todo Currently unimplemented. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to Time Sync messages. + * + * @details Subscribes to RxmRAWX & RxmSFRBX messages. + */ + void subscribe(std::shared_ptr gps) override; + + private: + /** + * @brief + * @details Publish recieved TimTM2 messages if enabled + */ + void callbackTimTM2(const ublox_msgs::msg::TimTM2 &m); + + sensor_msgs::msg::TimeReference t_ref_; + + rclcpp::Publisher::SharedPtr timtm2_pub_; + rclcpp::Publisher::SharedPtr interrupt_time_pub_; + rclcpp::Publisher::SharedPtr rxm_sfrb_pub_; + rclcpp::Publisher::SharedPtr rxm_raw_pub_; + + std::string frame_id_; + std::shared_ptr updater_; + + rclcpp::Node* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_TIM_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_firmware.hpp b/ublox_gps/include/ublox_gps/ublox_firmware.hpp new file mode 100644 index 00000000..f9f7f804 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware.hpp @@ -0,0 +1,52 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE_HPP + +#include + +#include +#include + +#include +#include + +// This file declares UbloxFirmware is an abstract class which implements +// ComponentInterface and functions generic to all firmware (such as the +// initializing the fix diagnostics). + +namespace ublox_node { + +/** + * @brief This abstract class represents a firmware component. + * + * @details The Firmware components update the fix diagnostics. + */ +class UbloxFirmware : public virtual ComponentInterface { + public: + //! Subscribe Rate for u-blox SV Info messages + constexpr static uint32_t kNavSvInfoSubscribeRate = 20; + + explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, rclcpp::Node* node); + + /** + * @brief Add the fix diagnostics to the updater. + */ + void initializeRosDiagnostics() override; + + protected: + /** + * @brief Handle to send fix status to ROS diagnostics. + */ + virtual void fixDiagnostic( + diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; + + std::shared_ptr updater_; + std::shared_ptr gnss_; + //! The fix status service type, set in the Firmware Component + //! based on the enabled GNSS + int fix_status_service_{0}; + rclcpp::Node* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_firmware6.hpp b/ublox_gps/include/ublox_gps/ublox_firmware6.hpp new file mode 100644 index 00000000..0d7a0af4 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware6.hpp @@ -0,0 +1,107 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE6_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE6_HPP + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 6. + */ +class UbloxFirmware6 final : public UbloxFirmware { + public: + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node); + + /** + * @brief Sets the fix status service type to GPS. + */ + void getRosParams() override; + + /** + * @brief Prints a warning, GNSS configuration not available in this version. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Subscribe to NavPVT, RxmRAW, and RxmSFRB messages. + */ + void subscribe(std::shared_ptr gps) override; + + protected: + /** + * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL + * messages. + */ + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override; + + private: + /** + * @brief Publish the fix and call the fix diagnostic updater. + * + * @details Also updates the last known position and publishes the NavPosLLH + * message if publishing is enabled. + * @param m the message to process + */ + void callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m); + + /** + * @brief Update the last known velocity. + * + * @details Publish the message if publishing is enabled. + * @param m the message to process + */ + void callbackNavVelNed(const ublox_msgs::msg::NavVELNED& m); + + /** + * @brief Update the number of SVs used for the fix. + * + * @details Publish the message if publishing is enabled. + * @param m the message to process + */ + void callbackNavSol(const ublox_msgs::msg::NavSOL& m); + + //! The last received navigation position + ublox_msgs::msg::NavPOSLLH last_nav_pos_; + //! The last received navigation velocity + ublox_msgs::msg::NavVELNED last_nav_vel_; + //! The last received num SVs used + ublox_msgs::msg::NavSOL last_nav_sol_; + //! The last NavSatFix based on last_nav_pos_ + sensor_msgs::msg::NavSatFix fix_; + //! The last Twist based on last_nav_vel_ + geometry_msgs::msg::TwistWithCovarianceStamped velocity_; + + //! Used to configure NMEA (if set_nmea_) filled with ROS parameters + ublox_msgs::msg::CfgNMEA6 cfg_nmea_; + + rclcpp::Publisher::SharedPtr nav_pos_llh_pub_; + rclcpp::Publisher::SharedPtr fix_pub_; + rclcpp::Publisher::SharedPtr nav_vel_ned_pub_; + rclcpp::Publisher::SharedPtr vel_pub_; + rclcpp::Publisher::SharedPtr nav_sol_pub_; + rclcpp::Publisher::SharedPtr nav_svinfo_pub_; + rclcpp::Publisher::SharedPtr mon_hw_pub_; + + std::string frame_id_; + std::shared_ptr freq_diag_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE6_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp new file mode 100644 index 00000000..6f6f95d4 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp @@ -0,0 +1,68 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE7_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE7_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 7. + */ +class UbloxFirmware7 final : public UbloxFirmware7Plus { + public: + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + if (getRosBoolean(node_, "publish.nav.svinfo")) { + nav_svinfo_pub_ = node->create_publisher("navsvinfo", 1); + } + if (getRosBoolean(node_, "publish.mon.hw")) { + mon_hw_pub_ = node->create_publisher("monhw", 1); + } + } + + /** + * @brief Get the parameters specific to firmware version 7. + * + * @details Get the GNSS and NMEA settings. + */ + void getRosParams() override; + + /** + * @brief Configure GNSS individually. Only configures GLONASS. + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Subscribe to messages which are not generic to all firmware. + * + * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. + */ + void subscribe(std::shared_ptr gps) override; + + private: + //! Used to configure NMEA (if set_nmea_) + /*! + * Filled from ROS parameters + */ + ublox_msgs::msg::CfgNMEA7 cfg_nmea_; + + rclcpp::Publisher::SharedPtr nav_svinfo_pub_; + rclcpp::Publisher::SharedPtr mon_hw_pub_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE7_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp new file mode 100644 index 00000000..db4a2ca5 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -0,0 +1,214 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE7PLUS_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE7PLUS_HPP + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Abstract class for Firmware versions >= 7. + * + * @details This class keeps track of the last NavPVT message uses it to + * update the fix diagnostics. It is a template class because the NavPVT message + * is a different length for firmware versions 7 and 8. + * + * @typedef NavPVT the NavPVT message type for the given firmware version + */ +template +class UbloxFirmware7Plus : public UbloxFirmware { + public: + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { + // NavPVT publisher + if (getRosBoolean(node_, "publish.nav.pvt")) { + nav_pvt_pub_ = node_->create_publisher("~/navpvt", 1); + } + + fix_pub_ = + node_->create_publisher("~/fix", 1); + vel_pub_ = + node_->create_publisher("~/fix_velocity", + 1); + } + + /** + * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. + * + * @details If a fixed carrier phase solution is available, the NavSatFix + * status is set to GBAS fixed. If NavPVT publishing is enabled, the message + * is published. This function also calls the ROS diagnostics updater. + * @param m the message to publish + */ + void callbackNavPvt(const NavPVT& m) { + if (getRosBoolean(node_, "publish.nav.pvt")) { + // NavPVT publisher + nav_pvt_pub_->publish(m); + } + + // + // NavSatFix message + // + sensor_msgs::msg::NavSatFix fix; + fix.header.frame_id = frame_id_; + // set the timestamp + uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; + if (((m.valid & valid_time) == valid_time) && + (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) { + // Use NavPVT timestamp since it is valid + // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9 + // The ros time uses only unsigned values, so a negative nano seconds must be + // converted to a positive value + if (m.nano < 0) { + fix.header.stamp.sec = toUtcSeconds(m) - 1; + fix.header.stamp.nanosec = static_cast(m.nano + 1e9); + } + else { + fix.header.stamp.sec = toUtcSeconds(m); + fix.header.stamp.nanosec = static_cast(m.nano); + } + } else { + // Use ROS time since NavPVT timestamp is not valid + fix.header.stamp = node_->now(); + } + // Set the LLA + fix.latitude = m.lat * 1e-7; // to deg + fix.longitude = m.lon * 1e-7; // to deg + fix.altitude = m.height * 1e-3; // to [m] + // Set the Fix status + bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; + if (fixOk && m.fix_type >= m.FIX_TYPE_2D) { + fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + if (m.flags & m.CARRIER_PHASE_FIXED) { + fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; + } + } else { + fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } + // Set the service based on GNSS configuration + fix.status.service = fix_status_service_; + + // Set the position covariance + const double var_h = pow(m.h_acc / 1000.0, 2); // to [m^2] + const double var_v = pow(m.v_acc / 1000.0, 2); // to [m^2] + fix.position_covariance[0] = var_h; + fix.position_covariance[4] = var_h; + fix.position_covariance[8] = var_v; + fix.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + fix_pub_->publish(fix); + + // + // Twist message + // + geometry_msgs::msg::TwistWithCovarianceStamped velocity; + velocity.header.stamp = fix.header.stamp; + velocity.header.frame_id = frame_id_; + + // convert to XYZ linear velocity [m/s] in ENU + velocity.twist.twist.linear.x = m.vel_e * 1e-3; + velocity.twist.twist.linear.y = m.vel_n * 1e-3; + velocity.twist.twist.linear.z = -m.vel_d * 1e-3; + // Set the covariance + const double cov_speed = pow(m.s_acc * 1e-3, 2); + const int cols = 6; + velocity.twist.covariance[cols * 0 + 0] = cov_speed; + velocity.twist.covariance[cols * 1 + 1] = cov_speed; + velocity.twist.covariance[cols * 2 + 2] = cov_speed; + velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported + + vel_pub_->publish(velocity); + + // + // Update diagnostics + // + last_nav_pvt_ = m; + freq_diag_->diagnostic->tick(fix.header.stamp); + } + + protected: + + /** + * @brief Update the fix diagnostics from Nav PVT message. + */ + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override { + // check the last message, convert to diagnostic + if (last_nav_pvt_.fix_type == + ublox_msgs::msg::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Dead reckoning only"; + } else if (last_nav_pvt_.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "2D fix"; + } else if (last_nav_pvt_.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_3D) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "3D fix"; + } else if (last_nav_pvt_.fix_type == + ublox_msgs::msg::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "GPS and dead reckoning combined"; + } else if (last_nav_pvt_.fix_type == + ublox_msgs::msg::NavPVT::FIX_TYPE_TIME_ONLY) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Time only fix"; + } + + // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level + if (!(last_nav_pvt_.flags & ublox_msgs::msg::NavPVT::FLAGS_GNSS_FIX_OK)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message += ", fix not ok"; + } + // Raise diagnostic level to error if no fix + if (last_nav_pvt_.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_NO_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "No fix"; + } + + // append last fix position + stat.add("iTOW [ms]", last_nav_pvt_.i_tow); + stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7); + stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7); + stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3); + stat.add("Height above MSL [m]", last_nav_pvt_.h_msl * 1e-3); + stat.add("Horizontal Accuracy [m]", last_nav_pvt_.h_acc * 1e-3); + stat.add("Vertical Accuracy [m]", last_nav_pvt_.v_acc * 1e-3); + stat.add("# SVs used", static_cast(last_nav_pvt_.num_sv)); + } + + //! The last received NavPVT message + NavPVT last_nav_pvt_; + // Whether or not to enable the given GNSS + //! Whether or not to enable GPS + bool enable_gps_{false}; + //! Whether or not to enable GLONASS + bool enable_glonass_{false}; + //! Whether or not to enable QZSS + bool enable_qzss_{false}; + //! The QZSS Signal configuration, see CfgGNSS message + uint32_t qzss_sig_cfg_{0}; + + typename rclcpp::Publisher::SharedPtr nav_pvt_pub_; + rclcpp::Publisher::SharedPtr fix_pub_; + rclcpp::Publisher::SharedPtr vel_pub_; + + std::string frame_id_; + std::shared_ptr freq_diag_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE7PLUS_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp new file mode 100644 index 00000000..e5bbc895 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -0,0 +1,86 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE8_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE8_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 8. + */ +class UbloxFirmware8 : public UbloxFirmware7Plus { + public: + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + if (getRosBoolean(node_, "publish.nav.sat")) { + nav_sat_pub_ = node->create_publisher("navstate", 1); + } + if (getRosBoolean(node_, "publish.mon.hw")) { + mon_hw_pub_ = node->create_publisher("monhw", 1); + } + if (getRosBoolean(node_, "publish.rxm.rtcm")) { + rxm_rtcm_pub_ = node->create_publisher("rxmrtcm", 1); + } + } + + /** + * @brief Get the ROS parameters specific to firmware version 8. + * + * @details Get the GNSS, NMEA, and UPD settings. + */ + void getRosParams() override; + + /** + * @brief Configure settings specific to firmware 8 based on ROS parameters. + * + * @details Configure GNSS, if it is different from current settings. + * Configure the NMEA if desired by the user. It also may clear the + * flash memory based on the ROS parameters. + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Subscribe to u-blox messages which are not generic to all firmware + * versions. + * + * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based + * on user settings. + */ + void subscribe(std::shared_ptr gps) override; + +protected: + // Set from ROS parameters + //! Whether or not to enable the Galileo GNSS + bool enable_galileo_{false}; + //! Whether or not to enable the BeiDuo GNSS + bool enable_beidou_{false}; + //! Whether or not to enable the IMES GNSS + bool enable_imes_{false}; + //! Desired NMEA configuration. + ublox_msgs::msg::CfgNMEA cfg_nmea_; + //! Whether to clear the flash memory during configuration + bool clear_bbr_{false}; + bool save_on_shutdown_{false}; + +private: + rclcpp::Publisher::SharedPtr nav_sat_pub_; + rclcpp::Publisher::SharedPtr mon_hw_pub_; + rclcpp::Publisher::SharedPtr rxm_rtcm_pub_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE8_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp new file mode 100644 index 00000000..9e0a4d2b --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -0,0 +1,48 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE9_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE9_HPP + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 9. + */ +class UbloxFirmware9 final : public UbloxFirmware8 { +public: + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node); + + /** + * @brief Configure settings specific to firmware 9 based on ROS parameters. + * + * @details Configure GNSS. The hardware has internal logic for + * detecting differences between the new and active GNSS + * configuration and will internally trigger a reset if necessary. + * Configure the NMEA if desired by the user. It also may clear the + * flash memory based on the ROS parameters. + */ + bool configureUblox(std::shared_ptr gps) override; + +private: + /** + * @brief Populate the CfgVALSETCfgData data type + * + * @details A helper function used to generate a configuration for a single signal. + */ + ublox_msgs::msg::CfgVALSETCfgdata generateSignalConfig(uint32_t signalID, bool enable); +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE9_HPP diff --git a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp new file mode 100644 index 00000000..ce181b8a --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp @@ -0,0 +1,76 @@ +#ifndef UBLOX_GPS_UBLOX_TOPIC_DIAGNOSTIC_HPP +#define UBLOX_GPS_UBLOX_TOPIC_DIAGNOSTIC_HPP + +#include +#include + +#include +#include +#include + +namespace ublox_node { + +//! Topic diagnostics for u-blox messages +struct UbloxTopicDiagnostic { + UbloxTopicDiagnostic() = default; + + // Must not copy this struct (would confuse FrequencyStatusParam pointers) + UbloxTopicDiagnostic(UbloxTopicDiagnostic &&c) = delete; + UbloxTopicDiagnostic &operator=(UbloxTopicDiagnostic &&c) = delete; + UbloxTopicDiagnostic(const UbloxTopicDiagnostic &c) = delete; + UbloxTopicDiagnostic &operator=(const UbloxTopicDiagnostic &c) = delete; + + ~UbloxTopicDiagnostic() = default; + + /** + * @brief Add a topic diagnostic to the diagnostic updater for + * + * @details The minimum and maximum frequency are equal to the nav rate in Hz. + * @param name the ROS topic + * @param freq_tol the tolerance [%] for the topic frequency + * @param freq_window the number of messages to use for diagnostic statistics + */ + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, + uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + min_freq = target_freq; + max_freq = target_freq; + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, + freq_tol, freq_window); + diagnostic = std::make_shared(topic, + *updater, + freq_param); + } + + /** + * @brief Add a topic diagnostic to the diagnostic updater for + * + * @details The minimum and maximum frequency are equal to the nav rate in Hz. + * @param name the ROS topic + * @param freq_min the minimum acceptable frequency for the topic + * @param freq_max the maximum acceptable frequency for the topic + * @param freq_tol the tolerance [%] for the topic frequency + * @param freq_window the number of messages to use for diagnostic statistics + */ + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, + double freq_tol, int freq_window, std::shared_ptr updater) { + min_freq = freq_min; + max_freq = freq_max; + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, + freq_tol, freq_window); + diagnostic = std::make_shared(topic, + *updater, + freq_param); + } + + //! Topic frequency diagnostic updater + std::shared_ptr diagnostic; + //! Minimum allow frequency of topic + double min_freq{0.0}; + //! Maximum allow frequency of topic + double max_freq{0.0}; +}; + +} // namespace ublox_node + +#endif diff --git a/ublox_gps/include/ublox_gps/utils.h b/ublox_gps/include/ublox_gps/utils.h deleted file mode 100644 index ee649eae..00000000 --- a/ublox_gps/include/ublox_gps/utils.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef UBLOX_GPS_UTILS_H -#define UBLOX_GPS_UTILS_H - -#include -#include -#include "ublox_msgs/NavPVT.h" - -extern "C" { - #include "ublox_gps/mkgmtime.h" -} - -/** - * @brief Convert date/time to UTC time in seconds. - */ -template -long toUtcSeconds(const NavPVT& msg) { - // Create TM struct for mkgmtime - struct tm time = {0}; - time.tm_year = msg.year - 1900; - time.tm_mon = msg.month - 1; - time.tm_mday = msg.day; - time.tm_hour = msg.hour; - time.tm_min = msg.min; - time.tm_sec = msg.sec; - // C++ STL doesn't have a mkgmtime (though other libraries do) - // STL mktime converts date/time to seconds in local time - // A modified version of code external library is used for mkgmtime - return mkgmtime(&time); -} - -#endif diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp new file mode 100644 index 00000000..0e535b0d --- /dev/null +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -0,0 +1,182 @@ +#ifndef UBLOX_GPS_UTILS_HPP +#define UBLOX_GPS_UTILS_HPP + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace ublox_node { + +/** + * @brief Convert date/time to UTC time in seconds. + */ +template +time_t toUtcSeconds(const NavPVT& msg) { + // Create TM struct for mkgmtime + struct tm time{}; + time.tm_year = msg.year - 1900; + time.tm_mon = msg.month - 1; + time.tm_mday = msg.day; + time.tm_hour = msg.hour; + time.tm_min = msg.min; + time.tm_sec = msg.sec; + // C++ STL doesn't have a mkgmtime (though other libraries do) + // STL mktime converts date/time to seconds in local time + // A modified version of code external library is used for mkgmtime + return mkgmtime(&time); +} + +/** + * @brief Check that the parameter is above the minimum. + * @param val the value to check + * @param min the minimum for this value + * @param name the name of the parameter + * @throws std::runtime_error if it is below the minimum + */ +template +void checkMin(V val, T min, const std::string & name) { + if (val < min) { + std::stringstream oss; + oss << "Invalid settings: " << name << " must be > " << min; + throw std::runtime_error(oss.str()); + } +} + +/** + * @brief Check that the parameter is in the range. + * @param val the value to check + * @param min the minimum for this value + * @param max the maximum for this value + * @param name the name of the parameter + * @throws std::runtime_error if it is out of bounds + */ +template +void checkRange(V val, T min, T max, const std::string & name) { + if (val < min || val > max) { + std::stringstream oss; + oss << "Invalid settings: " << name << " must be in range [" << min << + ", " << max << "]."; + throw std::runtime_error(oss.str()); + } +} + +/** + * @brief Check that the elements of the vector are in the range. + * @param val the vector to check + * @param min the minimum for this value + * @param max the maximum for this value + * @param name the name of the parameter + * @throws std::runtime_error value it is out of bounds + */ +template +void checkRange(std::vector val, T min, T max, const std::string & name) { + for (size_t i = 0; i < val.size(); i++) { + std::stringstream oss; + oss << name << "[" << i << "]"; + checkRange(val[i], min, max, oss.str()); + } +} + +/** + * @brief Get a unsigned integer value from the parameter server. + * @param key the key to be used in the parameter server's dictionary + * @param u storage for the retrieved value. + * @throws std::runtime_error if the parameter is out of bounds + * @return true if found, false if not found. + */ +template +bool getRosUint(rclcpp::Node* node, const std::string& key, U &u) { + rclcpp::Parameter parameter; + if (!node->get_parameter(key, parameter)) { + return false; + } + U param = parameter.get_value(); + // Check the bounds + U min = std::numeric_limits::lowest(); + U max = std::numeric_limits::max(); + checkRange(param, min, max, key); + // set the output + u = static_cast(param); + return true; +} + +/** + * @brief Get a unsigned integer value from the parameter server. + * @param key the key to be used in the parameter server's dictionary + * @param u storage for the retrieved value. + * @param val value to use if the server doesn't contain this parameter. + * @throws std::runtime_error if the parameter is out of bounds + * @return true if found, false if not found. + */ +template +void getRosUint(rclcpp::Node* node, const std::string& key, U &u, V default_val) { + if (!getRosUint(node, key, u)) { + u = default_val; + } +} + +/** + * @brief Get a unsigned integer vector from the parameter server. + * @throws std::runtime_error if the parameter is out of bounds. + * @return true if found, false if not found. + */ +template +bool getRosUint(rclcpp::Node* node, const std::string& key, std::vector &u) { + std::vector param; + if (!node->get_parameter(key, param)) { + return false; + } + + // Check the bounds + U min = std::numeric_limits::lowest(); + U max = std::numeric_limits::max(); + checkRange(param, min, max, key); + + // set the output + u.insert(u.begin(), param.begin(), param.end()); + return true; +} + +static inline bool getRosBoolean(rclcpp::Node* node, const std::string &name) +{ + bool ret; + if (!node->get_parameter(name, ret)) { + // Note that if this is used after declare_parameter, this should never happen. + throw std::runtime_error("Required parameter '" + name + "' has not been declared"); + } + + return ret; +} + +template +T declareRosIntParameter(rclcpp::Node* node, const std::string& name, int64_t default_value) +{ + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.name = name; + param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + param_desc.description = name; + rcl_interfaces::msg::IntegerRange range; + range.from_value = std::numeric_limits::lowest(); + range.to_value = std::numeric_limits::max(); + param_desc.integer_range.push_back(range); + return node->declare_parameter(name, default_value, param_desc); +} + +static inline bool isRosParameterSet(rclcpp::Node* node, const std::string& name) +{ + rclcpp::Parameter param; + node->get_parameter(name, param); + return param.get_type() != rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; +} + +} // namespace ublox_node + +#endif // UBLOX_GPS_UTILS_HPP diff --git a/ublox_gps/include/ublox_gps/worker.h b/ublox_gps/include/ublox_gps/worker.hpp similarity index 70% rename from ublox_gps/include/ublox_gps/worker.h rename to ublox_gps/include/ublox_gps/worker.hpp index 6a9bef52..93c5323c 100644 --- a/ublox_gps/include/ublox_gps/worker.h +++ b/ublox_gps/include/ublox_gps/worker.hpp @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -26,33 +26,41 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_WORKER_H -#define UBLOX_GPS_WORKER_H +#ifndef UBLOX_GPS_WORKER_HPP +#define UBLOX_GPS_WORKER_HPP -#include -#include +#include +#include namespace ublox_gps { /** * @brief Handles I/O reading and writing. */ -class Worker { +// clang-tidy insists that we follow rule-of-5 for this abstract base class. +// Generally we'd define the copy and move constructors as delete, but once +// we do that we also have to explicitly define a default constructor (otherwise +// it fails to compile). That all works, but harms the readability of this +// completely abstract base class (basically an interface), so we just disable +// the checks for this class. +class Worker { // NOLINT(hicpp-special-member-functions, cppcoreguidelines-special-member-functions) public: - typedef boost::function Callback; - virtual ~Worker() {} + using WorkerCallback = std::function; + using WorkerRawCallback = std::function; + + virtual ~Worker() = default; /** * @brief Set the callback function for received messages. * @param callback the callback function which process messages in the buffer */ - virtual void setCallback(const Callback& callback) = 0; + virtual void setCallback(const WorkerCallback& callback) = 0; /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - virtual void setRawDataCallback(const Callback& callback) = 0; + virtual void setRawDataCallback(const WorkerRawCallback& callback) = 0; /** * @brief Send the data in the buffer. @@ -60,12 +68,12 @@ class Worker { * @param size the size of the buffer */ virtual bool send(const unsigned char* data, const unsigned int size) = 0; - + /** * @brief Wait for an incoming message. * @param timeout the maximum time to wait. */ - virtual void wait(const boost::posix_time::time_duration& timeout) = 0; + virtual void wait(const std::chrono::milliseconds& timeout) = 0; /** * @brief Whether or not the I/O stream is open. @@ -75,4 +83,4 @@ class Worker { } // namespace ublox_gps -#endif // UBLOX_GPS_WORKER_H +#endif // UBLOX_GPS_WORKER_HPP diff --git a/ublox_gps/launch/ublox_device.launch b/ublox_gps/launch/ublox_device.launch deleted file mode 100644 index 9bd8ce10..00000000 --- a/ublox_gps/launch/ublox_device.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - diff --git a/ublox_gps/launch/ublox_gps_node-composed-launch.py b/ublox_gps/launch/ublox_gps_node-composed-launch.py new file mode 100644 index 00000000..76c7f5aa --- /dev/null +++ b/ublox_gps/launch/ublox_gps_node-composed-launch.py @@ -0,0 +1,68 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of {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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the ublox gps node in a composable container with c94-m8p configuration.""" + +import os + +import ament_index_python.packages + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import yaml + + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ublox_gps'), + 'config') + param_config = os.path.join(config_directory, 'c94_m8p_rover.yaml') + with open(param_config, 'r') as f: + params = yaml.safe_load(f)['ublox_gps_node']['ros__parameters'] + container = ComposableNodeContainer( + name='ublox_gps_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='ublox_gps', + plugin='ublox_node::UbloxNode', + name='ublox_gps_node', + parameters=[params]), + ], + output='both', + ) + + return LaunchDescription([container]) diff --git a/ublox_gps/launch/ublox_gps_node-launch.py b/ublox_gps/launch/ublox_gps_node-launch.py new file mode 100644 index 00000000..9a8d9b2a --- /dev/null +++ b/ublox_gps/launch/ublox_gps_node-launch.py @@ -0,0 +1,60 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of {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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the ublox gps node with c94-m8p configuration.""" + +import os + +import ament_index_python.packages +import launch +import launch_ros.actions + + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ublox_gps'), + 'config') + params = os.path.join(config_directory, 'c94_m8p_rover.yaml') + ublox_gps_node = launch_ros.actions.Node(package='ublox_gps', + executable='ublox_gps_node', + output='both', + parameters=[params]) + + return launch.LaunchDescription([ublox_gps_node, + + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=ublox_gps_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )), + ]) diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 59e28bc8..a09c76b9 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -1,7 +1,7 @@ ublox_gps - 1.2.0 + 2.3.0 Driver for u-blox GPS devices. @@ -12,13 +12,25 @@ BSD http://ros.org/wiki/ublox - catkin + ament_cmake_ros - ublox_serialization - ublox_msgs - roscpp - roscpp_serialization - tf + asio + diagnostic_msgs diagnostic_updater + geometry_msgs + nmea_msgs + rcl_interfaces + rclcpp + rclcpp_components + rtcm_msgs + sensor_msgs + std_msgs + std_srvs + tf2 + ublox_msgs + ublox_serialization + + ament_cmake + diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp new file mode 100644 index 00000000..7713ba7c --- /dev/null +++ b/ublox_gps/src/adr_udr_product.cpp @@ -0,0 +1,207 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +// +// u-blox ADR devices, partially implemented +// +AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) + : protocol_version_(protocol_version) ,use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) +{ + if (getRosBoolean(node_, "publish.esf.meas")) { + imu_pub_ = + node_->create_publisher("imu_meas", 1); + time_ref_pub_ = + node_->create_publisher("interrupt_time", 1); + + esf_meas_pub_ = node_->create_publisher("esfmeas", 1); + } + if (getRosBoolean(node_, "publish.nav.att")) { + nav_att_pub_ = node_->create_publisher("navatt", 1); + } + if (getRosBoolean(node_, "publish.esf.ins")) { + esf_ins_pub_ = node_->create_publisher("esfins", 1); + } + if (getRosBoolean(node_, "publish.esf.raw")) { + esf_raw_pub_ = node_->create_publisher("esfraw", 1); + } + if (getRosBoolean(node_, "publish.esf.status")) { + esf_status_pub_ = node_->create_publisher("esfstatus", 1); + } + if (getRosBoolean(node_, "publish.hnr.pvt")) { + hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); + } +} + +void AdrUdrProduct::getRosParams() { + use_adr_ = getRosBoolean(node_, "use_adr"); + // Check the nav rate + float nav_rate_hz = 1000.0 / (meas_rate_ * nav_rate_); + if (nav_rate_hz != 1) { + RCLCPP_WARN(node_->get_logger(), "Nav Rate recommended to be 1 Hz"); + } +} + +bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { + if (!gps->setUseAdr(use_adr_, protocol_version_)) { + throw std::runtime_error(std::string("Failed to ") + + (use_adr_ ? "enable" : "disable") + "use_adr"); + } + return true; +} + +void AdrUdrProduct::subscribe(std::shared_ptr gps) { + // Subscribe to NAV ATT messages + if (getRosBoolean(node_, "publish.nav.att")) { + gps->subscribe([this](const ublox_msgs::msg::NavATT &m) { nav_att_pub_->publish(m); }, + 1); + } + + // Subscribe to ESF INS messages + if (getRosBoolean(node_, "publish.esf.ins")) { + gps->subscribe([this](const ublox_msgs::msg::EsfINS &m) { esf_ins_pub_->publish(m); }, + 1); + } + + // Subscribe to ESF Meas messages + if (getRosBoolean(node_, "publish.esf.meas")) { + gps->subscribe([this](const ublox_msgs::msg::EsfMEAS &m) { esf_meas_pub_->publish(m); }, + 1); + + // also publish sensor_msgs::Imu + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); + } + + // Subscribe to ESF Raw messages + if (getRosBoolean(node_, "publish.esf.raw")) { + gps->subscribe([this](const ublox_msgs::msg::EsfRAW &m) { esf_raw_pub_->publish(m); }, + 1); + } + + // Subscribe to ESF Status messages + if (getRosBoolean(node_, "publish.esf.status")) { + gps->subscribe([this](const ublox_msgs::msg::EsfSTATUS &m) { esf_status_pub_->publish(m); }, + 1); + } + + // Subscribe to HNR PVT messages + if (getRosBoolean(node_, "publish.hnr.pvt")) { + gps->subscribe([this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, + 1); + } +} + +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { + if (getRosBoolean(node_, "publish.esf.meas")) { + imu_.header.stamp = node_->now(); + imu_.header.frame_id = frame_id_; + + float rad_per_sec = ::pow(2, -12) * M_PI / 180.0F; + float m_per_sec_sq = ::pow(2, -10); + + std::vector imu_data = m.data; + for (unsigned int datapoint : imu_data) { + unsigned int data_type = datapoint >> 24; //grab the last six bits of data + double data_sign = (datapoint & (1 << 23)); //grab the sign (+/-) of the rest of the data + unsigned int data_value = datapoint & 0x7FFFFF; //grab the rest of the data...should be 23 bits + + if (data_sign == 0) { + data_sign = -1; + } else { + data_sign = 1; + } + + // RCLCPP_INFO(node_->get_logger(), "data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray + + imu_.orientation_covariance[0] = -1; + imu_.linear_acceleration_covariance[0] = -1; + imu_.angular_velocity_covariance[0] = -1; + + if (data_type == 14) { + if (data_sign == 1) { + imu_.angular_velocity.x = 2048 - data_value * rad_per_sec; + } else { + imu_.angular_velocity.x = data_sign * data_value * rad_per_sec; + } + } else if (data_type == 16) { + //RCLCPP_INFO(node_->get_logger(), "data_sign: %f", data_sign); + //RCLCPP_INFO(node_->get_logger(), "data_value: %u", data_value * m); + if (data_sign == 1) { + imu_.linear_acceleration.x = 8191 - data_value * m_per_sec_sq; + } else { + imu_.linear_acceleration.x = data_sign * data_value * m_per_sec_sq; + } + } else if (data_type == 13) { + if (data_sign == 1) { + imu_.angular_velocity.y = 2048 - data_value * rad_per_sec; + } else { + imu_.angular_velocity.y = data_sign * data_value * rad_per_sec; + } + } else if (data_type == 17) { + if (data_sign == 1) { + imu_.linear_acceleration.y = 8191 - data_value * m_per_sec_sq; + } else { + imu_.linear_acceleration.y = data_sign * data_value * m_per_sec_sq; + } + } else if (data_type == 5) { + if (data_sign == 1) { + imu_.angular_velocity.z = 2048 - data_value * rad_per_sec; + } else { + imu_.angular_velocity.z = data_sign * data_value * rad_per_sec; + } + } else if (data_type == 18) { + if (data_sign == 1) { + imu_.linear_acceleration.z = 8191 - data_value * m_per_sec_sq; + } else { + imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq; + } + } else if (data_type == 12) { + // RCLCPP_INFO("Temperature in celsius: %f", data_value * deg_c); + } else { + RCLCPP_INFO(node_->get_logger(), "data_type: %u", data_type); + RCLCPP_INFO(node_->get_logger(), "data_value: %u", data_value); + } + + // create time ref message and put in the data + //t_ref_.header.seq = m.risingEdgeCount; + //t_ref_.header.stamp = node_->now(); + //t_ref_.header.frame_id = frame_id_; + + //t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + + //std::ostringstream src; + //src << "TIM" << int(m.ch); + //t_ref_.source = src.str(); + + t_ref_.header.stamp = node_->now(); // create a new timestamp + t_ref_.header.frame_id = frame_id_; + + time_ref_pub_->publish(t_ref_); + imu_pub_->publish(imu_); + } + } + +} + +} // namespace ublox_node diff --git a/ublox_gps/src/gnss.cpp b/ublox_gps/src/gnss.cpp new file mode 100644 index 00000000..c2155011 --- /dev/null +++ b/ublox_gps/src/gnss.cpp @@ -0,0 +1,17 @@ +#include + +#include + +namespace ublox_node { + +void Gnss::add(const std::string & gnss) +{ + supported_.insert(gnss); +} + +bool Gnss::isSupported(const std::string & gnss) +{ + return supported_.count(gnss) > 0; +} + +} // namespace ublox_node diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 8073d957..d438d25d 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -27,138 +27,156 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include -#include +#include +#include +#include +#include +#include -namespace ublox_gps { +#include +#include +#include +#include +#include + +#include -using namespace ublox_msgs; +#include +#include -const boost::posix_time::time_duration Gps::default_timeout_ = - boost::posix_time::milliseconds( +namespace ublox_gps { + +const std::chrono::milliseconds Gps::default_timeout_ = + std::chrono::milliseconds( static_cast(Gps::kDefaultAckTimeout * 1000)); -Gps::Gps() : configured_(false), config_on_startup_flag_(true) { - subscribeAcks(); +Gps::Gps(int debug, const rclcpp::Logger& logger) : configured_(false), save_on_shutdown_(false), config_on_startup_flag_(true), debug_(debug), callbacks_(debug), logger_(logger) { + subscribeAcks(); } -Gps::~Gps() { close(); } +Gps::~Gps() { + close(); +} -void Gps::setWorker(const boost::shared_ptr& worker) { - if (worker_) return; +void Gps::setWorker(const std::shared_ptr& worker) { + if (worker_) { + return; + } worker_ = worker; - worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, - &callbacks_, _1, _2)); + worker_->setCallback(std::bind(&CallbackHandlers::readCallback, + &callbacks_, std::placeholders::_1, + std::placeholders::_2)); configured_ = static_cast(worker); } +void Gps::subscribe_nmea(std::function callback) { + callbacks_.set_nmea_callback(callback); +} + void Gps::subscribeAcks() { // Set NACK handler - subscribeId(boost::bind(&Gps::processNack, this, _1), - ublox_msgs::Message::ACK::NACK); + subscribeId(std::bind(&Gps::processNack, this, + std::placeholders::_1), + ublox_msgs::Message::ACK::NACK); // Set ACK handler - subscribeId(boost::bind(&Gps::processAck, this, _1), - ublox_msgs::Message::ACK::ACK); + subscribeId(std::bind(&Gps::processAck, this, + std::placeholders::_1), + ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler - subscribe( - boost::bind(&Gps::processUpdSosAck, this, _1)); + subscribe(std::bind(&Gps::processUpdSosAck, this, + std::placeholders::_1)); } -void Gps::processAck(const ublox_msgs::Ack &m) { +void Gps::processAck(const ublox_msgs::msg::Ack &m) { // Process ACK/NACK messages - Ack ack; + Ack ack{}; ack.type = ACK; - ack.class_id = m.clsID; - ack.msg_id = m.msgID; + ack.class_id = m.cls_id; + ack.msg_id = m.msg_id; // store the ack atomically - ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - m.clsID, m.msgID); + ack_.store(ack, std::memory_order_seq_cst); + RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", + m.cls_id, m.msg_id); } -void Gps::processNack(const ublox_msgs::Ack &m) { +void Gps::processNack(const ublox_msgs::msg::Ack &m) { // Process ACK/NACK messages - Ack ack; + Ack ack{}; ack.type = NACK; - ack.class_id = m.clsID; - ack.msg_id = m.msgID; + ack.class_id = m.cls_id; + ack.msg_id = m.msg_id; // store the ack atomically - ack_.store(ack, boost::memory_order_seq_cst); - ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID); + ack_.store(ack, std::memory_order_seq_cst); + RCLCPP_ERROR(logger_, "U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } -void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { - if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) { - Ack ack; - ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; - ack.class_id = m.CLASS_ID; - ack.msg_id = m.MESSAGE_ID; +void Gps::processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m) { + if (m.cmd == ublox_msgs::msg::UpdSOSAck::CMD_BACKUP_CREATE_ACK) { + Ack ack{}; + ack.type = (m.response == ublox_msgs::msg::UpdSOSAck::BACKUP_CREATE_ACK) ? ACK : NACK; + ack.class_id = ublox_msgs::msg::UpdSOSAck::CLASS_ID; + ack.msg_id = ublox_msgs::msg::UpdSOSAck::MESSAGE_ID; // store the ack atomically - ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(ack.type == ACK && debug >= 2, - "U-blox: received UPD SOS Backup ACK"); - if(ack.type == NACK) - ROS_ERROR("U-blox: received UPD SOS Backup NACK"); + ack_.store(ack, std::memory_order_seq_cst); + RCLCPP_DEBUG_EXPRESSION(logger_, ack.type == ACK && debug_ >= 2, + "U-blox: received UPD SOS Backup ACK"); + if (ack.type == NACK) { + RCLCPP_ERROR(logger_, "U-blox: received UPD SOS Backup NACK"); + } } } -void Gps::initializeSerial(std::string port, unsigned int baudrate, +void Gps::initializeSerial(const std::string & port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) { port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); + auto io_service = std::make_shared(); + auto serial = std::make_shared(*io_service); // open serial port try { serial->open(port); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } - ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - - if(BOOST_VERSION < 106600) - { - // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but - // until v1.66.0 there was a bug which didn't enable the relevant code, - // fixed by commit: https://github.com/boostorg/asio/commit/619cea4356 - int fd = serial->native_handle(); - termios tio; - tcgetattr(fd, &tio); - cfmakeraw(&tio); - tcsetattr(fd, TCSANOW, &tio); - } + RCLCPP_INFO(logger_, "U-Blox: Opened serial port %s", port.c_str()); + + int fd = serial->native_handle(); + termios tio{}; + tcgetattr(fd, &tio); + cfmakeraw(&tio); + tcsetattr(fd, TCSANOW, &tio); // Set the I/O worker - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(serial, io_service))); + if (worker_) { + return; + } + setWorker(std::make_shared>(serial, io_service, 8192, debug_, logger_)); configured_ = false; // Set the baudrate - boost::asio::serial_port_base::baud_rate current_baudrate; + asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { - if (current_baudrate.value() == baudrate) + for (unsigned int fixed_baudrate : kBaudrates) { + if (current_baudrate.value() == baudrate) { break; + } // Don't step down, unless the desired baudrate is lower - if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) + if (current_baudrate.value() > fixed_baudrate && baudrate > fixed_baudrate) { continue; - serial->set_option( - boost::asio::serial_port_base::baud_rate(kBaudrates[i])); - boost::this_thread::sleep( - boost::posix_time::milliseconds(kSetBaudrateSleepMs)); + } + serial->set_option(asio::serial_port_base::baud_rate(fixed_baudrate)); + std::this_thread::sleep_for( + std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); - ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); + RCLCPP_DEBUG(logger_, "U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } if (config_on_startup_flag_) { configured_ = configUart1(baudrate, uart_in, uart_out); - if(!configured_ || current_baudrate.value() != baudrate) { + if (!configured_ || current_baudrate.value() != baudrate) { throw std::runtime_error("Could not configure serial baud rate"); } } else { @@ -166,130 +184,164 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } } -void Gps::resetSerial(std::string port) { - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); +void Gps::resetSerial(const std::string & port) { + auto io_service = std::make_shared(); + auto serial = std::make_shared(*io_service); // open serial port try { serial->open(port); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } - ROS_INFO("U-Blox: Reset serial port %s", port.c_str()); + RCLCPP_INFO(logger_, "U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(serial, io_service))); + if (worker_) { + return; + } + setWorker(std::make_shared>(serial, io_service, 8192, debug_, logger_)); configured_ = false; // Poll UART PRT Config std::vector payload; - payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { - ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); + payload.push_back(ublox_msgs::msg::CfgPRT::PORT_ID_UART1); + if (!poll(ublox_msgs::msg::CfgPRT::CLASS_ID, ublox_msgs::msg::CfgPRT::MESSAGE_ID, payload)) { + RCLCPP_ERROR(logger_, "Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } - CfgPRT prt; - if(!read(prt, default_timeout_)) { - ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", - "message"); + ublox_msgs::msg::CfgPRT prt; + if (!read(prt, default_timeout_)) { + RCLCPP_ERROR(logger_, "Resetting Serial Port: Could not read polled UART1 CfgPRT %s", + "message"); return; } // Set the baudrate - serial->set_option(boost::asio::serial_port_base::baud_rate(prt.baudRate)); + serial->set_option(asio::serial_port_base::baud_rate(prt.baud_rate)); configured_ = true; } -void Gps::initializeTcp(std::string host, std::string port) { +void Gps::initializeTcp(const std::string & host, const std::string & port) { host_ = host; port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::asio::ip::tcp::resolver::iterator endpoint; + auto io_service = std::make_shared(); + asio::ip::tcp::resolver::iterator endpoint; try { - boost::asio::ip::tcp::resolver resolver(*io_service); + asio::ip::tcp::resolver resolver(*io_service); endpoint = - resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); - } catch (std::runtime_error& e) { + resolver.resolve(asio::ip::tcp::resolver::query(host, port)); + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } - boost::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); + auto socket = std::make_shared(*io_service); try { socket->connect(*endpoint); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + ": " + e.what()); } - ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - endpoint->service_name().c_str()); + RCLCPP_INFO(logger_, "U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), + endpoint->service_name().c_str()); - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(socket, - io_service))); + if (worker_) { + return; + } + setWorker(std::make_shared>(socket, io_service, 8192, debug_, logger_)); +} + +void Gps::initializeUdp(const std::string & host, const std::string & port) { + host_ = host; + port_ = port; + auto io_service = std::make_shared(); + asio::ip::udp::resolver::iterator endpoint; + + try { + asio::ip::udp::resolver resolver(*io_service); + endpoint = + resolver.resolve(asio::ip::udp::resolver::query(host, port)); + } catch (const std::runtime_error& e) { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + + port + " " + e.what()); + } + + auto socket = std::make_shared(*io_service); + + try { + socket->connect(*endpoint); + } catch (const std::runtime_error& e) { + throw std::runtime_error("U-Blox: Could not connect to " + + endpoint->host_name() + ":" + + endpoint->service_name() + ": " + e.what()); + } + + RCLCPP_INFO(logger_, "U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), + endpoint->service_name().c_str()); + + if (worker_) { + return; + } + setWorker(std::make_shared>(socket, io_service, 8192, debug_, logger_)); } void Gps::close() { - if(save_on_shutdown_) { - if(saveOnShutdown()) - ROS_INFO("U-Blox Flash BBR saved"); - else - ROS_INFO("U-Blox Flash BBR failed to save"); + if (save_on_shutdown_) { + if (saveOnShutdown()) { + RCLCPP_INFO(logger_, "U-Blox Flash BBR saved"); + } else { + RCLCPP_INFO(logger_, "U-Blox Flash BBR failed to save"); + } } + hotrestart(); worker_.reset(); configured_ = false; } -void Gps::reset(const boost::posix_time::time_duration& wait) { +void Gps::reset(const std::chrono::milliseconds& wait) { worker_.reset(); configured_ = false; // sleep because of undefined behavior after I/O reset - boost::this_thread::sleep(wait); - if (host_ == "") + std::this_thread::sleep_for(wait); + if (host_ == "") { resetSerial(port_); - else + } else { initializeTcp(host_, port_); + } } bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { - ROS_WARN("Resetting u-blox. If device address changes, %s", - "node must be relaunched."); + RCLCPP_WARN(logger_, "Resetting u-blox. If device address changes, %s", + "node must be relaunched."); - CfgRST rst; - rst.navBbrMask = nav_bbr_mask; - rst.resetMode = reset_mode; + ublox_msgs::msg::CfgRST rst; + rst.nav_bbr_mask = nav_bbr_mask; + rst.reset_mode = reset_mode; // Don't wait for ACK, return if it fails - if (!configure(rst, false)) - return false; - return true; + return configure(rst, false); } -bool Gps::configGnss(CfgGNSS gnss, - const boost::posix_time::time_duration& wait) { +bool Gps::configGnss(ublox_msgs::msg::CfgGNSS gnss, + const std::chrono::milliseconds& wait) { // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox - ROS_DEBUG("Re-configuring GNSS."); - if (!configure(gnss)) + RCLCPP_DEBUG(logger_, "Re-configuring GNSS."); + if (!configure(gnss)) { return false; + } // Cold reset the GNSS - ROS_WARN("GNSS re-configured, cold resetting device."); - if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS)) + RCLCPP_WARN(logger_, "GNSS re-configured, cold resetting device."); + if (!configReset(ublox_msgs::msg::CfgRST::NAV_BBR_COLD_START, ublox_msgs::msg::CfgRST::RESET_MODE_GNSS)) { return false; - ros::Duration(1.0).sleep(); + } + std::this_thread::sleep_for(std::chrono::seconds(1)); // Reset the I/O reset(wait); return isConfigured(); @@ -297,64 +349,78 @@ bool Gps::configGnss(CfgGNSS gnss, bool Gps::saveOnShutdown() { // Command the receiver to stop - CfgRST rst; - rst.navBbrMask = rst.NAV_BBR_HOT_START; - rst.resetMode = rst.RESET_MODE_GNSS_STOP; - if(!configure(rst)) + ublox_msgs::msg::CfgRST rst; + rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_HOT_START; + rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_GNSS_STOP; + + if (!configure(rst)) { return false; + } // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK - UpdSOS backup; + ublox_msgs::msg::UpdSOS backup; return configure(backup); } +bool Gps::hotrestart() { + ublox_msgs::msg::CfgRST rst; + // rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_COLD_START; + // rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_HW_IMMEDIATE; + rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_HOT_START; + rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_GNSS; + + return configure(rst); +} + bool Gps::clearBbr() { // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK - UpdSOS sos; - sos.cmd = sos.CMD_FLASH_BACKUP_CLEAR; + ublox_msgs::msg::UpdSOS sos; + sos.cmd = ublox_msgs::msg::UpdSOS::CMD_FLASH_BACKUP_CLEAR; return configure(sos); } bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask) { - if (!worker_) return true; - - ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", - baudrate, in_proto_mask, out_proto_mask); - - CfgPRT port; - port.portID = CfgPRT::PORT_ID_UART1; - port.baudRate = baudrate; - port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | - CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; - port.inProtoMask = in_proto_mask; - port.outProtoMask = out_proto_mask; + if (!worker_) { + return true; + } + + RCLCPP_DEBUG(logger_, "Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", + baudrate, in_proto_mask, out_proto_mask); + + ublox_msgs::msg::CfgPRT port; + port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_UART1; + port.baud_rate = baudrate; + port.mode = ublox_msgs::msg::CfgPRT::MODE_RESERVED1 | ublox_msgs::msg::CfgPRT::MODE_CHAR_LEN_8BIT | + ublox_msgs::msg::CfgPRT::MODE_PARITY_NO | ublox_msgs::msg::CfgPRT::MODE_STOP_BITS_1; + port.in_proto_mask = in_proto_mask; + port.out_proto_mask = out_proto_mask; return configure(port); } -bool Gps::disableUart1(CfgPRT& prev_config) { - ROS_DEBUG("Disabling UART1"); +bool Gps::disableUart1(ublox_msgs::msg::CfgPRT& prev_config) { + RCLCPP_DEBUG(logger_, "Disabling UART1"); // Poll UART PRT Config std::vector payload; - payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { - ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); + payload.push_back(ublox_msgs::msg::CfgPRT::PORT_ID_UART1); + if (!poll(ublox_msgs::msg::CfgPRT::CLASS_ID, ublox_msgs::msg::CfgPRT::MESSAGE_ID, payload)) { + RCLCPP_ERROR(logger_, "disableUart: Could not poll UART1 CfgPRT"); return false; } - if(!read(prev_config, default_timeout_)) { - ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); + if (!read(prev_config, default_timeout_)) { + RCLCPP_ERROR(logger_, "disableUart: Could not read polled UART1 CfgPRT message"); return false; } // Keep original settings, but disable in/out - CfgPRT port; - port.portID = CfgPRT::PORT_ID_UART1; + ublox_msgs::msg::CfgPRT port; + port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_UART1; port.mode = prev_config.mode; - port.baudRate = prev_config.baudRate; - port.inProtoMask = 0; - port.outProtoMask = 0; - port.txReady = prev_config.txReady; + port.baud_rate = prev_config.baud_rate; + port.in_proto_mask = 0; + port.out_proto_mask = 0; + port.tx_ready = prev_config.tx_ready; port.flags = prev_config.flags; return configure(port); } @@ -362,35 +428,37 @@ bool Gps::disableUart1(CfgPRT& prev_config) { bool Gps::configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask) { - if (!worker_) return true; + if (!worker_) { + return true; + } - ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", - tx_ready, in_proto_mask, out_proto_mask); + RCLCPP_DEBUG(logger_, "Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", + tx_ready, in_proto_mask, out_proto_mask); - CfgPRT port; - port.portID = CfgPRT::PORT_ID_USB; - port.txReady = tx_ready; - port.inProtoMask = in_proto_mask; - port.outProtoMask = out_proto_mask; + ublox_msgs::msg::CfgPRT port; + port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_USB; + port.tx_ready = tx_ready; + port.in_proto_mask = in_proto_mask; + port.out_proto_mask = out_proto_mask; return configure(port); } bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { - ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", - meas_rate, nav_rate); + RCLCPP_DEBUG(logger_, "Configuring measurement rate to %u ms and nav rate to %u cycles", + meas_rate, nav_rate); - CfgRATE rate; - rate.measRate = meas_rate; - rate.navRate = nav_rate; // must be fixed at 1 for ublox 5 and 6 - rate.timeRef = CfgRATE::TIME_REF_GPS; + ublox_msgs::msg::CfgRATE rate; + rate.meas_rate = meas_rate; + rate.nav_rate = nav_rate; // must be fixed at 1 for ublox 5 and 6 + rate.time_ref = ublox_msgs::msg::CfgRATE::TIME_REF_GPS; return configure(rate); } -bool Gps::configRtcm(std::vector ids, std::vector rates) { - for(size_t i = 0; i < ids.size(); ++i) { - ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]); - if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { - ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]); +bool Gps::configRtcm(const std::vector & rtcms) { + for (const Rtcm & rtcm : rtcms) { + RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcm.id, rtcm.rate); + if (!setRate(ublox_msgs::Class::RTCM, rtcm.id, rtcm.rate)) { + RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcm.id, rtcm.rate); return false; } } @@ -398,159 +466,173 @@ bool Gps::configRtcm(std::vector ids, std::vector rates) { } bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { - ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); + RCLCPP_DEBUG(logger_, "Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); - ublox_msgs::CfgSBAS msg; - msg.mode = (enable ? CfgSBAS::MODE_ENABLED : 0); + ublox_msgs::msg::CfgSBAS msg; + msg.mode = (enable ? ublox_msgs::msg::CfgSBAS::MODE_ENABLED : 0); msg.usage = usage; - msg.maxSBAS = max_sbas; + msg.max_sbas = max_sbas; return configure(msg); } bool Gps::configTmode3Fixed(bool lla_flag, - std::vector arp_position, + std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc) { - if(arp_position.size() != 3 || arp_position_hp.size() != 3) { - ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", - "& arp_position_hp args must be 3"); + if (arp_position.size() != 3 || arp_position_hp.size() != 3) { + RCLCPP_ERROR(logger_, "Configuring TMODE3 to Fixed: size of position %s", + "& arp_position_hp args must be 3"); return false; } - ROS_DEBUG("Configuring TMODE3 to Fixed"); + RCLCPP_DEBUG(logger_, "Configuring TMODE3 to Fixed"); - CfgTMODE3 tmode3; - tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK; - tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; + ublox_msgs::msg::CfgTMODE3 tmode3; + tmode3.flags = ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED & ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_MASK; + tmode3.flags |= lla_flag ? ublox_msgs::msg::CfgTMODE3::FLAGS_LLA : 0; // Set position - if(lla_flag) { + if (lla_flag) { // Convert from [deg] to [deg * 1e-7] - tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7); - tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7); - tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e7); + tmode3.ecef_x_or_lat = static_cast(round(arp_position[0] * 1e7)); + tmode3.ecef_y_or_lon = static_cast(round(arp_position[1] * 1e7)); + tmode3.ecef_z_or_alt = static_cast(round(arp_position[2] * 1e7)); } else { // Convert from m to cm - tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e2); - tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e2); - tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e2); + tmode3.ecef_x_or_lat = static_cast(round(arp_position[0] * 1e2)); + tmode3.ecef_y_or_lon = static_cast(round(arp_position[1] * 1e2)); + tmode3.ecef_z_or_alt = static_cast(round(arp_position[2] * 1e2)); } - tmode3.ecefXOrLatHP = arp_position_hp[0]; - tmode3.ecefYOrLonHP = arp_position_hp[1]; - tmode3.ecefZOrAltHP = arp_position_hp[2]; + tmode3.ecef_x_or_lat_hp = arp_position_hp[0]; + tmode3.ecef_y_or_lon_hp = arp_position_hp[1]; + tmode3.ecef_z_or_alt_hp = arp_position_hp[2]; // Convert from m to [0.1 mm] - tmode3.fixedPosAcc = (uint32_t)round(fixed_pos_acc * 1e4); + tmode3.fixed_pos_acc = static_cast(round(fixed_pos_acc * 1e4)); return configure(tmode3); } bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) { - CfgTMODE3 tmode3; - ROS_DEBUG("Setting TMODE3 to Survey In"); - tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; - tmode3.svinMinDur = svin_min_dur; + ublox_msgs::msg::CfgTMODE3 tmode3; + RCLCPP_DEBUG(logger_, "Setting TMODE3 to Survey In"); + tmode3.flags = ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_SURVEY_IN & ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_MASK; + tmode3.svin_min_dur = svin_min_dur; // Convert from m to [0.1 mm] - tmode3.svinAccLimit = (int)round(svin_acc_limit * 1e4); + tmode3.svin_acc_limit = static_cast(round(svin_acc_limit * 1e4)); return configure(tmode3); } bool Gps::disableTmode3() { - ROS_DEBUG("Disabling TMODE3"); + RCLCPP_DEBUG(logger_, "Disabling TMODE3"); - CfgTMODE3 tmode3; - tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK; + ublox_msgs::msg::CfgTMODE3 tmode3; + tmode3.flags = ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED & ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_MASK; return configure(tmode3); } bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { - ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, - message_id, rate); - ublox_msgs::CfgMSG msg; - msg.msgClass = class_id; - msg.msgID = message_id; + RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, + message_id, rate); + ublox_msgs::msg::CfgMSG msg; + msg.msg_class = class_id; + msg.msg_id = message_id; msg.rate = rate; return configure(msg); } bool Gps::setDynamicModel(uint8_t model) { - ROS_DEBUG("Setting dynamic model to %u", model); + RCLCPP_DEBUG(logger_, "Setting dynamic model to %u", model); - ublox_msgs::CfgNAV5 msg; - msg.dynModel = model; - msg.mask = ublox_msgs::CfgNAV5::MASK_DYN; + ublox_msgs::msg::CfgNAV5 msg; + msg.dyn_model = model; + msg.mask = ublox_msgs::msg::CfgNAV5::MASK_DYN; return configure(msg); } bool Gps::setFixMode(uint8_t mode) { - ROS_DEBUG("Setting fix mode to %u", mode); + RCLCPP_DEBUG(logger_, "Setting fix mode to %u", mode); - ublox_msgs::CfgNAV5 msg; - msg.fixMode = mode; - msg.mask = ublox_msgs::CfgNAV5::MASK_FIX_MODE; + ublox_msgs::msg::CfgNAV5 msg; + msg.fix_mode = mode; + msg.mask = ublox_msgs::msg::CfgNAV5::MASK_FIX_MODE; return configure(msg); } bool Gps::setDeadReckonLimit(uint8_t limit) { - ROS_DEBUG("Setting DR Limit to %u", limit); + RCLCPP_DEBUG(logger_, "Setting DR Limit to %u", limit); - ublox_msgs::CfgNAV5 msg; - msg.drLimit = limit; - msg.mask = ublox_msgs::CfgNAV5::MASK_DR_LIM; + ublox_msgs::msg::CfgNAV5 msg; + msg.dr_limit = limit; + msg.mask = ublox_msgs::msg::CfgNAV5::MASK_DR_LIM; return configure(msg); } -bool Gps::setPpp(bool enable) { - ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); +bool Gps::setPpp(bool enable, float protocol_version) { + RCLCPP_DEBUG(logger_,"%s PPP", (enable ? "Enabling" : "Disabling")); - ublox_msgs::CfgNAVX5 msg; - msg.usePPP = enable; - msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP; + ublox_msgs::msg::CfgNAVX5 msg; + msg.use_ppp = enable; + if(protocol_version >= 18){ + msg.version = 2; + } + msg.mask1 = ublox_msgs::msg::CfgNAVX5::MASK1_PPP; return configure(msg); } bool Gps::setDgnss(uint8_t mode) { - CfgDGNSS cfg; - ROS_DEBUG("Setting DGNSS mode to %u", mode); - cfg.dgnssMode = mode; + ublox_msgs::msg::CfgDGNSS cfg; + RCLCPP_DEBUG(logger_, "Setting DGNSS mode to %u", mode); + cfg.dgnss_mode = mode; return configure(cfg); } -bool Gps::setUseAdr(bool enable) { - ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); +bool Gps::setUseAdr(bool enable, float protocol_version) { + RCLCPP_DEBUG(logger_, "%s ADR/UDR", (enable ? "Enabling" : "Disabling")); - ublox_msgs::CfgNAVX5 msg; - msg.useAdr = enable; - msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR; + ublox_msgs::msg::CfgNAVX5 msg; + msg.use_adr = enable; + if(protocol_version >= 18){ + msg.version = 2; + } + msg.mask2 = ublox_msgs::msg::CfgNAVX5::MASK2_ADR; return configure(msg); } +bool Gps::sendRtcm(const std::vector& rtcm) { + worker_->send(rtcm.data(), rtcm.size()); + return true; +} + bool Gps::poll(uint8_t class_id, uint8_t message_id, const std::vector& payload) { - if (!worker_) return false; + if (!worker_) { + return false; + } std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); - if (!writer.write(payload.data(), payload.size(), class_id, message_id)) + if (!writer.write(payload.data(), payload.size(), class_id, message_id)) { return false; + } worker_->send(out.data(), writer.end() - out.data()); return true; } -bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, +bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id) { - ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", - class_id, msg_id); - boost::posix_time::ptime wait_until( - boost::posix_time::second_clock::local_time() + timeout); + RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "Waiting for ACK 0x%02x / 0x%02x", + class_id, msg_id); + std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); + std::chrono::system_clock::time_point wait_until = now + timeout; - Ack ack = ack_.load(boost::memory_order_seq_cst); - while (boost::posix_time::second_clock::local_time() < wait_until + Ack ack = ack_.load(std::memory_order_seq_cst); + while (std::chrono::system_clock::now() < wait_until && (ack.class_id != class_id || ack.msg_id != msg_id || ack.type == WAIT)) { worker_->wait(timeout); - ack = ack_.load(boost::memory_order_seq_cst); + ack = ack_.load(std::memory_order_seq_cst); } bool result = ack.type == ACK && ack.class_id == class_id @@ -558,25 +640,28 @@ bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, return result; } -void Gps::setRawDataCallback(const Worker::Callback& callback) { - if (! worker_) return; +void Gps::setRawDataCallback(const Worker::WorkerRawCallback& callback) { + if (!worker_) { + return; + } worker_->setRawDataCallback(callback); } bool Gps::setUTCtime() { - ROS_DEBUG("Setting time to UTC time"); + RCLCPP_DEBUG(logger_, "Setting time to UTC time"); - ublox_msgs::CfgNAV5 msg; - msg.utcStandard = 3; + ublox_msgs::msg::CfgNAV5 msg; + msg.utc_standard = 3; return configure(msg); } bool Gps::setTimtm2(uint8_t rate) { - ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate ); - ublox_msgs::CfgMSG msg; - msg.msgClass = ublox_msgs::TimTM2::CLASS_ID; - msg.msgID = ublox_msgs::TimTM2::MESSAGE_ID; - msg.rate = rate; + RCLCPP_DEBUG(logger_, "TIM-TM2 send rate on current port set to %u", rate); + ublox_msgs::msg::CfgMSG msg; + msg.msg_class = ublox_msgs::msg::TimTM2::CLASS_ID; + msg.msg_id = ublox_msgs::msg::TimTM2::MESSAGE_ID; + msg.rate = rate; return configure(msg); } + } // namespace ublox_gps diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp new file mode 100644 index 00000000..51947d77 --- /dev/null +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -0,0 +1,78 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox High Precision Positioning Receiver +// +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) + : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) +{ + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_relposned_pub_ = + node_->create_publisher("navrelposned", 1); + } + + if (getRosBoolean(node_, "publish.nav.heading")) { + imu_pub_ = + node_->create_publisher("navheading", 1); + } +} + +void HpPosRecProduct::subscribe(std::shared_ptr gps) { + // Whether to publish Nav Relative Position NED + // Subscribe to Nav Relative Position NED messages (also updates diagnostics) + gps->subscribe(std::bind( + &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); +} + +void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 &m) { + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_relposned_pub_->publish(m); + } + + if (getRosBoolean(node_, "publish.nav.heading")) { + imu_.header.stamp = node_->now(); + imu_.header.frame_id = frame_id_; + + imu_.linear_acceleration_covariance[0] = -1; + imu_.angular_velocity_covariance[0] = -1; + + // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103). + double heading = M_PI_2 - (static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI); + tf2::Quaternion orientation; + orientation.setRPY(0, 0, heading); + imu_.orientation.x = orientation[0]; + imu_.orientation.y = orientation[1]; + imu_.orientation.z = orientation[2]; + imu_.orientation.w = orientation[3]; + imu_.orientation_covariance[0] = 1000.0; + imu_.orientation_covariance[4] = 1000.0; + imu_.orientation_covariance[8] = 1000.0; + // When heading is reported to be valid, use accuracy reported in 1e-5 deg units + if (m.flags & ublox_msgs::msg::NavRELPOSNED9::FLAGS_REL_POS_HEAD_VALID) { + imu_.orientation_covariance[8] = ::pow(m.acc_heading * 1e-5 / 180.0 * M_PI, 2); + } + + imu_pub_->publish(imu_); + } + + last_rel_pos_ = m; +} + +} // namespace ublox_node diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp new file mode 100644 index 00000000..5fef3573 --- /dev/null +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace ublox_node { + +// +// u-blox High Precision GNSS Reference Station +// + +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) + : tmode3_(0), lla_flag_(false), fixed_pos_acc_(0.0), svin_reset_(false), sv_in_min_dur_(0), sv_in_acc_lim_(0.0), nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) +{ + if (getRosBoolean(node_, "publish.nav.svin")) { + navsvin_pub_ = + node_->create_publisher("navsvin", 1); + } +} + +/** + * @brief Get a int (size 8) vector from the parameter server. + * @throws std::runtime_error if the parameter is out of bounds. + * @return true if found, false if not found. + */ +bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { + std::vector param; + if (!node->get_parameter(key, param)) { + return false; + } + + // Check the bounds + int8_t min = std::numeric_limits::lowest(); + int8_t max = std::numeric_limits::max(); + checkRange(param, min, max, key); + + // set the output + i.insert(i.begin(), param.begin(), param.end()); + return true; +} + +void HpgRefProduct::getRosParams() { + if (getRosBoolean(node_, "config_on_startup")) { + if (nav_rate_ * meas_rate_ != 1000) { + RCLCPP_WARN(node_->get_logger(), "For HPG Ref devices, nav_rate should be exactly 1 Hz."); + } + + if (!getRosUint(node_, "tmode3", tmode3_)) { + throw std::runtime_error("Invalid settings: TMODE3 must be set"); + } + + if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!node_->get_parameter("arp.position", arp_position_)) { + throw std::runtime_error(std::string("Invalid settings: arp.position ") + + "must be set if TMODE3 is fixed"); + } + if (!getRosInt(node_, "arp.position_hp", arp_position_hp_)) { + throw std::runtime_error(std::string("Invalid settings: arp.position_hp ") + + "must be set if TMODE3 is fixed"); + } + if (!node_->get_parameter("arp.acc", fixed_pos_acc_)) { + throw std::runtime_error(std::string("Invalid settings: arp.acc ") + + "must be set if TMODE3 is fixed"); + } + if (!node_->get_parameter("arp.lla_flag", lla_flag_)) { + RCLCPP_WARN(node_->get_logger(), "arp/lla_flag param not set, assuming ARP coordinates are %s", + "in ECEF"); + lla_flag_ = false; + } + } else if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + svin_reset_ = getRosBoolean(node_, "sv_in.reset"); + if (!getRosUint(node_, "sv_in.min_dur", sv_in_min_dur_)) { + throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + + "must be set if TMODE3 is survey-in"); + } + if (!node_->get_parameter("sv_in.acc_lim", sv_in_acc_lim_)) { + throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + + "must be set if TMODE3 is survey-in"); + } + } else if (tmode3_ != ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED) { + throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + + " flag constants for possible values."); + } + } +} + +bool HpgRefProduct::configureUblox(std::shared_ptr gps) { + // Configure TMODE3 + if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED) { + if (!gps->disableTmode3()) { + throw std::runtime_error("Failed to disable TMODE3."); + } + mode_ = DISABLED; + } else if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!gps->configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, + fixed_pos_acc_)) { + throw std::runtime_error("Failed to set TMODE3 to fixed."); + } + if (!gps->configRtcm(rtcms_)) { + throw std::runtime_error("Failed to set RTCM rates"); + } + mode_ = FIXED; + } else if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + if (!svin_reset_) { + ublox_msgs::msg::NavSVIN nav_svin; + if (!gps->poll(nav_svin)) { + throw std::runtime_error(std::string("Failed to poll NavSVIN while") + + " configuring survey-in"); + } + // Don't reset survey-in if it's already active + if (nav_svin.active) { + mode_ = SURVEY_IN; + return true; + } + // Don't reset survey-in if it already has a valid value + if (nav_svin.valid) { + setTimeMode(gps); + return true; + } + ublox_msgs::msg::NavPVT nav_pvt; + if (!gps->poll(nav_pvt, std::vector(), std::chrono::milliseconds(15000))) { + throw std::runtime_error(std::string("Failed to poll NavPVT while") + + " configuring survey-in"); + } + // Don't reset survey in if in time mode with a good fix + if (nav_pvt.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_TIME_ONLY + && nav_pvt.flags & ublox_msgs::msg::NavPVT::FLAGS_GNSS_FIX_OK) { + setTimeMode(gps); + return true; + } + } + // Reset the Survey In + // For Survey in, meas rate must be at least 1 Hz + uint16_t meas_rate_temp = meas_rate_ < 1000 ? meas_rate_ : 1000; // [ms] + // If measurement period isn't a factor of 1000, set to default + if (1000 % meas_rate_temp != 0) { + meas_rate_temp = kDefaultMeasPeriod; + } + // Set nav rate to 1 Hz during survey in + if (!gps->configRate(meas_rate_temp, 1000 / meas_rate_temp)) { + throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + + "before setting TMODE3 to survey-in."); + } + // As recommended in the documentation, first disable, then set to survey in + if (!gps->disableTmode3()) { + RCLCPP_ERROR(node_->get_logger(), "Failed to disable TMODE3 before setting to survey-in."); + } else { + mode_ = DISABLED; + } + // Set to Survey in mode + if (!gps->configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) { + throw std::runtime_error("Failed to set TMODE3 to survey-in."); + } + mode_ = SURVEY_IN; + } + return true; +} + +void HpgRefProduct::subscribe(std::shared_ptr gps) { + // Subscribe to Nav Survey-In + // Save off the gps pointer so we can use it in the callback later. + gps_ = gps; + gps->subscribe(std::bind( + &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); +} + +void HpgRefProduct::callbackNavSvIn(const ublox_msgs::msg::NavSVIN& m) { + if (getRosBoolean(node_, "publish.nav.svin")) { + navsvin_pub_->publish(m); + } + + last_nav_svin_ = m; + + if (!m.active && m.valid && mode_ == SURVEY_IN) { + setTimeMode(gps_); + } + +} + +bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { + RCLCPP_INFO(node_->get_logger(), "Setting mode (internal state) to Time Mode"); + mode_ = TIME; + + // Set the Measurement & nav rate to user config + // (survey-in sets nav_rate to 1 Hz regardless of user setting) + if (!gps->configRate(meas_rate_, nav_rate_)) { + RCLCPP_ERROR(node_->get_logger(), "Failed to set measurement rate to %d ms navigation rate to %d cycles", + meas_rate_, nav_rate_); + } + // Enable the RTCM out messages + if (!gps->configRtcm(rtcms_)) { + RCLCPP_ERROR(node_->get_logger(), "Failed to configure RTCM IDs"); + return false; + } + return true; +} + +void HpgRefProduct::initializeRosDiagnostics() { + updater_->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); + updater_->force_update(); +} + +void HpgRefProduct::tmode3Diagnostics( + diagnostic_updater::DiagnosticStatusWrapper& stat) { + if (mode_ == INIT) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Not configured"; + } else if (mode_ == DISABLED){ + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Disabled"; + } else if (mode_ == SURVEY_IN) { + if (!last_nav_svin_.active && !last_nav_svin_.valid) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "Survey-In inactive and invalid"; + } else if (last_nav_svin_.active && !last_nav_svin_.valid) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Survey-In active but invalid"; + } else if (!last_nav_svin_.active && last_nav_svin_.valid) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Survey-In complete"; + } else if (last_nav_svin_.active && last_nav_svin_.valid) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Survey-In active and valid"; + } + + stat.add("iTOW [ms]", last_nav_svin_.i_tow); + stat.add("Duration [s]", last_nav_svin_.dur); + stat.add("# observations", last_nav_svin_.obs); + stat.add("Mean X [m]", last_nav_svin_.mean_x * 1e-2); + stat.add("Mean Y [m]", last_nav_svin_.mean_y * 1e-2); + stat.add("Mean Z [m]", last_nav_svin_.mean_z * 1e-2); + stat.add("Mean X HP [m]", last_nav_svin_.mean_xhp * 1e-4); + stat.add("Mean Y HP [m]", last_nav_svin_.mean_yhp * 1e-4); + stat.add("Mean Z HP [m]", last_nav_svin_.mean_zhp * 1e-4); + stat.add("Mean Accuracy [m]", last_nav_svin_.mean_acc * 1e-4); + } else if (mode_ == FIXED) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Fixed Position"; + } else if (mode_ == TIME) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Time"; + } +} + +} // namespace ublox_node diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp new file mode 100644 index 00000000..76baef27 --- /dev/null +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox High Precision GNSS Rover +// +HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node) + : dgnss_mode_(ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FIXED), nav_rate_(nav_rate), updater_(updater), node_(node) +{ + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_rel_pos_ned_pub_ = + node_->create_publisher("navrelposned", 1); + } +} + +void HpgRovProduct::getRosParams() { + // default to fixed, see CfgDGNSS message for details + getRosUint(node_, "dgnss_mode", dgnss_mode_, + ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FIXED); +} + +bool HpgRovProduct::configureUblox(std::shared_ptr gps) { + // Configure the DGNSS + if (!gps->setDgnss(dgnss_mode_)) { + throw std::runtime_error(std::string("Failed to Configure DGNSS")); + } + return true; +} + +void HpgRovProduct::subscribe(std::shared_ptr gps) { + // Subscribe to Nav Relative Position NED messages (also updates diagnostics) + gps->subscribe(std::bind( + &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); +} + +void HpgRovProduct::initializeRosDiagnostics() { + freq_rtcm_ = std::make_unique(std::string("rxmrtcm"), + kRtcmFreqMin, kRtcmFreqMax, + kRtcmFreqTol, kRtcmFreqWindow, updater_); + updater_->add("Carrier Phase Solution", this, + &HpgRovProduct::carrierPhaseDiagnostics); + updater_->force_update(); +} + +void HpgRovProduct::carrierPhaseDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper& stat) { + uint32_t carr_soln = last_rel_pos_.flags & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_MASK; + stat.add("iTow", last_rel_pos_.i_tow); + if (carr_soln & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_NONE || + !(last_rel_pos_.flags & ublox_msgs::msg::NavRELPOSNED::FLAGS_DIFF_SOLN && + last_rel_pos_.flags & ublox_msgs::msg::NavRELPOSNED::FLAGS_REL_POS_VALID)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "None"; + } else { + if (carr_soln & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_FLOAT) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Float"; + } else if (carr_soln & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_FIXED) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Fixed"; + } + stat.add("Ref Station ID", last_rel_pos_.ref_station_id); + + double rel_pos_n = (last_rel_pos_.rel_pos_n + + (last_rel_pos_.rel_pos_hpn * 1e-2)) * 1e-2; + double rel_pos_e = (last_rel_pos_.rel_pos_e + + (last_rel_pos_.rel_pos_hpe * 1e-2)) * 1e-2; + double rel_pos_d = (last_rel_pos_.rel_pos_d + + (last_rel_pos_.rel_pos_hpd * 1e-2)) * 1e-2; + stat.add("Relative Position N [m]", rel_pos_n); + stat.add("Relative Accuracy N [m]", last_rel_pos_.acc_n * 1e-4); + stat.add("Relative Position E [m]", rel_pos_e); + stat.add("Relative Accuracy E [m]", last_rel_pos_.acc_e * 1e-4); + stat.add("Relative Position D [m]", rel_pos_d); + stat.add("Relative Accuracy D [m]", last_rel_pos_.acc_d * 1e-4); + } +} + +void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED &m) { + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_rel_pos_ned_pub_->publish(m); + } + + last_rel_pos_ = m; +} + +} // namespace ublox_node diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index 5cb4fb91..4ec94fb5 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -33,11 +33,9 @@ // measured data with the rtklib. // ROS includes -#include +#include // Ublox GPS includes -#include - -using namespace ublox_node; +#include // // Raw Data Stream (feature from TUC-ProAut) @@ -45,13 +43,15 @@ using namespace ublox_node; int main(int argc, char** argv) { - ros::init(argc, argv, "ublox_logger"); - - RawDataStreamPa node(true); - node.getRosParams(); - node.initialize(); + rclcpp::init(argc, argv); + + auto node = std::make_shared(true); + node->getRosParams(); + node->initialize(); + + rclcpp::spin(node); - ros::spin(); + rclcpp::shutdown(); - return 0; + return 0; } diff --git a/ublox_gps/src/mkgmtime.c b/ublox_gps/src/mkgmtime.c index a2feca1a..47e61f8f 100644 --- a/ublox_gps/src/mkgmtime.c +++ b/ublox_gps/src/mkgmtime.c @@ -1,6 +1,6 @@ /* mkgmtime.c - make time corresponding to a GMT timeval struct $Id: mkgmtime.c,v 1.10 2003/10/22 18:50:12 rjs3 Exp $ - + * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -8,7 +8,7 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * 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 @@ -18,7 +18,7 @@ * 3. The name "Carnegie Mellon University" must not be used to * endorse or promote products derived from this software without * prior written permission. For permission or any other legal - * details, please contact + * details, please contact * Office of Technology Transfer * Carnegie Mellon University * 5000 Forbes Avenue @@ -87,70 +87,80 @@ */ #include "ublox_gps/mkgmtime.h" -#ifndef WRONG -#define WRONG (-1) -#endif /* !defined WRONG */ -static int tmcomp(register const struct tm * const atmp, - register const struct tm * const btmp) +static int tmcomp(const struct tm * const atmp, + const struct tm * const btmp) { - register int result; + int result; - if ((result = (atmp->tm_year - btmp->tm_year)) == 0 && - (result = (atmp->tm_mon - btmp->tm_mon)) == 0 && - (result = (atmp->tm_mday - btmp->tm_mday)) == 0 && - (result = (atmp->tm_hour - btmp->tm_hour)) == 0 && - (result = (atmp->tm_min - btmp->tm_min)) == 0) - result = atmp->tm_sec - btmp->tm_sec; - return result; + if ((result = (atmp->tm_year - btmp->tm_year)) == 0 && + (result = (atmp->tm_mon - btmp->tm_mon)) == 0 && + (result = (atmp->tm_mday - btmp->tm_mday)) == 0 && + (result = (atmp->tm_hour - btmp->tm_hour)) == 0 && + (result = (atmp->tm_min - btmp->tm_min)) == 0) { + result = atmp->tm_sec - btmp->tm_sec; + } + return result; } -time_t mkgmtime(struct tm * const tmp) { - register int dir; - register int bits; - register int saved_seconds; - time_t t; - struct tm yourtm, *mytm; +time_t mkgmtime(struct tm * const tmp) +{ + int dir; + int bits; + int saved_seconds; + time_t t; + struct tm yourtm, *mytm; - yourtm = *tmp; - saved_seconds = yourtm.tm_sec; - yourtm.tm_sec = 0; - /* - ** Calculate the number of magnitude bits in a time_t - ** (this works regardless of whether time_t is - ** signed or unsigned, though lint complains if unsigned). - */ - for (bits = 0, t = 1; t > 0; ++bits, t <<= 1) - ; - /* - ** If time_t is signed, then 0 is the median value, - ** if time_t is unsigned, then 1 << bits is median. - */ - t = (t < 0) ? 0 : ((time_t) 1 << bits); + yourtm = *tmp; + saved_seconds = yourtm.tm_sec; + yourtm.tm_sec = 0; + /* + * Calculate the number of magnitude bits in a time_t + * (this works regardless of whether time_t is + * signed or unsigned, though lint complains if unsigned). + */ + for (bits = 0, t = 1; t > 0; ++bits, t <<= 1) + { + } - /* Some gmtime() implementations are broken and will return - * NULL for time_ts larger than 40 bits even on 64-bit platforms - * so we'll just cap it at 40 bits */ - if(bits > 40) bits = 40; + /* + * If time_t is signed, then 0 is the median value, + * if time_t is unsigned, then 1 << bits is median. + */ + t = (t < 0) ? 0 : ((time_t) 1 << bits); - for ( ; ; ) { - mytm = gmtime(&t); + /* Some gmtime() implementations are broken and will return + * NULL for time_ts larger than 40 bits even on 64-bit platforms + * so we'll just cap it at 40 bits */ + if (bits > 40) { + bits = 40; + } - if(!mytm) return WRONG; + for ( ; ; ) { + mytm = gmtime(&t); + + if (!mytm) { + return -1; + } - dir = tmcomp(mytm, &yourtm); - if (dir != 0) { - if (bits-- < 0) - return WRONG; - if (bits < 0) - --t; - else if (dir > 0) - t -= (time_t) 1 << bits; - else t += (time_t) 1 << bits; - continue; - } - break; + dir = tmcomp(mytm, &yourtm); + if (dir != 0) { + if (bits-- < 0) { + return -1; + } + if (bits < 0) { + --t; + } + else if (dir > 0) { + t -= (time_t) 1 << bits; + } + else { + t += (time_t) 1 << bits; + } + continue; } - t += saved_seconds; - return t; + break; + } + t += saved_seconds; + return t; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index d7210c4b..97df1606 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -27,211 +27,505 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include "ublox_gps/node.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include -#include +#include +#include +#include +#include #include - -using namespace ublox_node; - -// -// ublox_node namespace -// -uint8_t ublox_node::modelFromString(const std::string& model) { +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Determine dynamic model from human-readable string. + * @param model One of the following (case-insensitive): + * - portable + * - stationary + * - pedestrian + * - automotive + * - sea + * - airborne1 + * - airborne2 + * - airborne4 + * - wristwatch + * @return DynamicModel + * @throws std::runtime_error on invalid argument. + */ +uint8_t modelFromString(const std::string &model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if(lower == "portable") { - return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE; - } else if(lower == "stationary") { - return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY; - } else if(lower == "pedestrian") { - return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN; - } else if(lower == "automotive") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE; - } else if(lower == "sea") { - return ublox_msgs::CfgNAV5::DYN_MODEL_SEA; - } else if(lower == "airborne1") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G; - } else if(lower == "airborne2") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G; - } else if(lower == "airborne4") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G; - } else if(lower == "wristwatch") { - return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH; - } - - throw std::runtime_error("Invalid settings: " + lower + - " is not a valid dynamic model."); + if (lower == "portable") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_PORTABLE; + } + if (lower == "stationary") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_STATIONARY; + } + if (lower == "pedestrian") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_PEDESTRIAN; + } + if (lower == "automotive") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AUTOMOTIVE; + } + if (lower == "sea") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_SEA; + } + if (lower == "airborne1") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_1G; + } + if (lower == "airborne2") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_2G; + } + if (lower == "airborne4") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_4G; + } + if (lower == "wristwatch") { + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_WRIST_WATCH; + } + + throw std::runtime_error("Invalid settings: " + lower + " is not a valid dynamic model."); } -uint8_t ublox_node::fixModeFromString(const std::string& mode) { +/** + * @brief Determine fix mode from human-readable string. + * @param mode One of the following (case-insensitive): + * - 2d + * - 3d + * - auto + * @return FixMode + * @throws std::runtime_error on invalid argument. + */ +uint8_t fixModeFromString(const std::string &mode) { std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "2d") { - return ublox_msgs::CfgNAV5::FIX_MODE_2D_ONLY; - } else if (lower == "3d") { - return ublox_msgs::CfgNAV5::FIX_MODE_3D_ONLY; - } else if (lower == "auto") { - return ublox_msgs::CfgNAV5::FIX_MODE_AUTO; + return ublox_msgs::msg::CfgNAV5::FIX_MODE_2D_ONLY; + } + if (lower == "3d") { + return ublox_msgs::msg::CfgNAV5::FIX_MODE_3D_ONLY; + } + if (lower == "auto") { + return ublox_msgs::msg::CfgNAV5::FIX_MODE_AUTO; + } + + throw std::runtime_error("Invalid settings: " + mode + " is not a valid fix mode."); +} + +std::vector stringSplit(const std::string &str, const std::string &splitter) { + std::vector ret; + size_t next = 0; + size_t current = next; + + if (splitter.empty()) { + // If the splitter is blank, just return the original + ret.push_back(str); + return ret; + } + + while (next != std::string::npos) { + next = str.find(splitter, current); + ret.push_back(str.substr(current, next - current)); + current = next + splitter.length(); } - throw std::runtime_error("Invalid settings: " + mode + - " is not a valid fix mode."); + return ret; } // // u-blox ROS Node // -UbloxNode::UbloxNode() { +UbloxNode::UbloxNode(const rclcpp::NodeOptions &options) : rclcpp::Node("ublox_gps_node", options) { + int debug = this->declare_parameter("debug", 1); + if (debug) { + if (rcutils_logging_set_logger_level("ublox_gps_node", RCUTILS_LOG_SEVERITY_DEBUG) != RCUTILS_RET_OK) { + RCLCPP_WARN(this->get_logger(), "Failed to set the debugging level"); + } + } + + gps_ = std::make_shared(debug, this->get_logger()); + + gnss_ = std::make_shared(); + + updater_ = std::make_shared(this); + updater_->setHardwareID("ublox"); + initialize(); } +void UbloxNode::rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg) { + gps_->sendRtcm(msg->message); +} + void UbloxNode::addFirmwareInterface() { int ublox_version; - if (protocol_version_ < 14) { - components_.push_back(ComponentPtr(new UbloxFirmware6)); + if (protocol_version_ < 14.0) { + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 6; - } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(ComponentPtr(new UbloxFirmware7)); + } + else if (protocol_version_ >= 14.0 && protocol_version_ <= 15.0) { + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 7; - } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(ComponentPtr(new UbloxFirmware8)); + } + else if (protocol_version_ > 15.0 && protocol_version_ <= 23.0) { + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 8; - } else { - components_.push_back(ComponentPtr(new UbloxFirmware9)); + } + else { + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 9; } - ROS_INFO("U-Blox Firmware Version: %d", ublox_version); + RCLCPP_INFO(this->get_logger(), "U-Blox Firmware Version: %d", ublox_version); } - -void UbloxNode::addProductInterface(std::string product_category, - std::string ref_rov) { - if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) - components_.push_back(ComponentPtr(new HpgRefProduct)); - else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) - components_.push_back(ComponentPtr(new HpgRovProduct)); - else if (product_category.compare("HPG") == 0) - components_.push_back(ComponentPtr(new HpPosRecProduct)); - else if (product_category.compare("TIM") == 0) - components_.push_back(ComponentPtr(new TimProduct)); - else if (product_category.compare("ADR") == 0 || - product_category.compare("UDR") == 0) - components_.push_back(ComponentPtr(new AdrUdrProduct)); - else if (product_category.compare("FTS") == 0) - components_.push_back(ComponentPtr(new FtsProduct)); - else if(product_category.compare("SPG") != 0) - ROS_WARN("Product category %s %s from MonVER message not recognized %s", - product_category.c_str(), ref_rov.c_str(), - "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); +void UbloxNode::addProductInterface(const std::string &product_category, const std::string &ref_rov) { + if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "REF") { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, this)); + } + else if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "ROV") { + components_.push_back(std::make_shared(nav_rate_, updater_, this)); + } + else if (product_category == "HPG" || product_category == "HPS") { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); + } + else if (product_category == "TIM") { + components_.push_back(std::make_shared(frame_id_, updater_, this)); + } + else if (product_category == "ADR" || product_category == "UDR") { + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); + } + else if (product_category == "FTS") { + components_.push_back(std::make_shared()); + } + else if (product_category == "HPS") { + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); + components_.push_back(std::make_shared(nav_rate_, updater_, this)); + } + else { + RCLCPP_WARN( + this->get_logger(), "Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), + "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, HPS"); + } } void UbloxNode::getRosParams() { - nh->param("device", device_, std::string("/dev/ttyACM0")); - nh->param("frame_id", frame_id, std::string("gps")); + device_ = this->declare_parameter("device", std::string("/dev/ttyACM0")); + frame_id_ = this->declare_parameter("frame_id", std::string("gps")); // Save configuration parameters - getRosUint("load/mask", load_.loadMask, 0); - getRosUint("load/device", load_.deviceMask, 0); - getRosUint("save/mask", save_.saveMask, 0); - getRosUint("save/device", save_.deviceMask, 0); + load_.load_mask = declareRosIntParameter(this, "load.mask", 0); + load_.device_mask = declareRosIntParameter(this, "load.device", 0); + save_.save_mask = declareRosIntParameter(this, "save.mask", 0); + save_.device_mask = declareRosIntParameter(this, "save.device", 0); // UART 1 params - getRosUint("uart1/baudrate", baudrate_, 9600); - getRosUint("uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX - | ublox_msgs::CfgPRT::PROTO_NMEA - | ublox_msgs::CfgPRT::PROTO_RTCM); - getRosUint("uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); + baudrate_ = declareRosIntParameter(this, "uart1.baudrate", 9600); + uart_in_ = declareRosIntParameter( + this, "uart1.in", ublox_msgs::msg::CfgPRT::PROTO_UBX | ublox_msgs::msg::CfgPRT::PROTO_NMEA | ublox_msgs::msg::CfgPRT::PROTO_RTCM); + uart_out_ = declareRosIntParameter(this, "uart1.out", ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { + this->declare_parameter("usb.in", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("usb.out", rclcpp::PARAMETER_INTEGER); + usb_tx_ = declareRosIntParameter(this, "usb.tx_ready", 0); + if (isRosParameterSet(this, "usb.in") || isRosParameterSet(this, "usb.out")) { set_usb_ = true; - if(!getRosUint("usb/in", usb_in_)) { - throw std::runtime_error(std::string("usb/out is set, therefore ") + - "usb/in must be set"); + if (!getRosUint(this, "usb.in", usb_in_)) { + throw std::runtime_error(std::string("usb.out is set, therefore ") + "usb.in must be set"); } - if(!getRosUint("usb/out", usb_out_)) { - throw std::runtime_error(std::string("usb/in is set, therefore ") + - "usb/out must be set"); + if (!getRosUint(this, "usb.out", usb_out_)) { + throw std::runtime_error(std::string("usb.in is set, therefore ") + "usb.out must be set"); } - getRosUint("usb/tx_ready", usb_tx_, 0); } // Measurement rate params - nh->param("rate", rate_, 4.0); // in Hz - getRosUint("nav_rate", nav_rate, 1); // # of measurement rate cycles + rate_ = this->declare_parameter("rate", 4.0); // in Hz + checkMin(rate_, 0.0, "rate"); + + nav_rate_ = declareRosIntParameter(this, "nav_rate", 1); // # of measurement rate cycles + // RTCM params - getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs - getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates - // PPP: Advanced Setting - nh->param("enable_ppp", enable_ppp_, false); - // SBAS params, only for some devices - nh->param("sbas", enable_sbas_, false); - getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels - getRosUint("sbas/usage", sbas_usage_, 0); - nh->param("dynamic_model", dynamic_model_, std::string("portable")); - nh->param("fix_mode", fix_mode_, std::string("auto")); - getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit + this->declare_parameter("rtcm.ids", rclcpp::PARAMETER_INTEGER_ARRAY); + this->declare_parameter("rtcm.rates", rclcpp::PARAMETER_INTEGER_ARRAY); + std::vector rtcm_ids; + std::vector rtcm_rates; + this->get_parameter("rtcm.ids", rtcm_ids); + this->get_parameter("rtcm.rates", rtcm_rates); - if (enable_ppp_) - ROS_WARN("Warning: PPP is enabled - this is an expert setting."); + if (rtcm_ids.size() != rtcm_rates.size()) { + throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); + } - checkMin(rate_, 0, "rate"); + rtcms_.resize(rtcm_ids.size()); + for (size_t i = 0; i < rtcm_ids.size(); ++i) { + if (rtcm_ids[i] < 0 || rtcm_ids[i] > 255) { + throw std::runtime_error("RTCM IDs must be between 0 and 255"); + } + if (rtcm_rates[i] < 0 || rtcm_rates[i] > 255) { + throw std::runtime_error("RTCM rates must be between 0 and 255"); + } + rtcms_[i].id = rtcm_ids[i]; + rtcms_[i].rate = rtcm_rates[i]; + } - if(rtcm_ids.size() != rtcm_rates.size()) - throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + - " must match size of rtcm_rates"); + // PPP: Advanced Setting + this->declare_parameter("enable_ppp", false); + if (getRosBoolean(this, "enable_ppp")) { + RCLCPP_WARN(this->get_logger(), "Warning: PPP is enabled - this is an expert setting."); + } + // SBAS params, only for some devices + this->declare_parameter("gnss.sbas", false); + this->declare_parameter("gnss.gps", true); + this->declare_parameter("gnss.glonass", false); + this->declare_parameter("gnss.qzss", false); + this->declare_parameter("gnss.galileo", false); + this->declare_parameter("gnss.beidou", false); + this->declare_parameter("gnss.imes", false); + max_sbas_ = declareRosIntParameter(this, "sbas.max", 0); // Maximum number of SBAS channels + sbas_usage_ = declareRosIntParameter(this, "sbas.usage", 0); + dynamic_model_ = this->declare_parameter("dynamic_model", std::string("portable")); dmodel_ = modelFromString(dynamic_model_); + fix_mode_ = this->declare_parameter("fix_mode", std::string("auto")); fmode_ = fixModeFromString(fix_mode_); + dr_limit_ = declareRosIntParameter(this, "dr_limit", 0); // Dead reckoning limit + + this->declare_parameter("dat.set", false); + this->declare_parameter("dat.majA", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("dat.flat", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("dat.shift", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("dat.rot", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("dat.scale", rclcpp::PARAMETER_DOUBLE); + if (getRosBoolean(this, "dat.set")) { + std::vector shift, rot; + if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) || !this->get_parameter("dat.flat", cfg_dat_.flat) || + !this->get_parameter("dat.shift", shift) || !this->get_parameter("dat.rot", rot) || !this->get_parameter("dat.scale", cfg_dat_.scale)) { + throw std::runtime_error(std::string("dat.set is true, therefore ") + "dat.majA, dat.flat, dat.shift, dat.rot, & dat.scale must be set"); + } + if (shift.size() != 3 || rot.size() != 3) { + throw std::runtime_error(std::string("size of dat.shift & dat.rot ") + "must be 3"); + } + checkRange(cfg_dat_.maj_a, 6300000.0, 6500000.0, "dat.majA"); + checkRange(cfg_dat_.flat, 0.0, 500.0, "dat.flat"); + + checkRange(shift, 0.0, 500.0, "dat.shift"); + cfg_dat_.d_x = shift[0]; + cfg_dat_.d_y = shift[1]; + cfg_dat_.d_z = shift[2]; - nh->param("dat/set", set_dat_, false); - if(set_dat_) { - std::vector shift, rot; - if (!nh->getParam("dat/majA", cfg_dat_.majA) - || nh->getParam("dat/flat", cfg_dat_.flat) - || nh->getParam("dat/shift", shift) - || nh->getParam("dat/rot", rot) - || nh->getParam("dat/scale", cfg_dat_.scale)) - throw std::runtime_error(std::string("dat/set is true, therefore ") + - "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); - if(shift.size() != 3 || rot.size() != 3) - throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + - "must be 3"); - checkRange(cfg_dat_.majA, 6300000.0, 6500000.0, "dat/majA"); - checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat"); - - checkRange(shift, 0.0, 500.0, "dat/shift"); - cfg_dat_.dX = shift[0]; - cfg_dat_.dY = shift[1]; - cfg_dat_.dZ = shift[2]; - - checkRange(rot, -5000.0, 5000.0, "dat/rot"); - cfg_dat_.rotX = rot[0]; - cfg_dat_.rotY = rot[1]; - cfg_dat_.rotZ = rot[2]; + checkRange(rot, -5000.0, 5000.0, "dat.rot"); + cfg_dat_.rot_x = rot[0]; + cfg_dat_.rot_y = rot[1]; + cfg_dat_.rot_z = rot[2]; checkRange(cfg_dat_.scale, 0.0, 50.0, "scale"); } // measurement period [ms] - meas_rate = 1000 / rate_; + meas_rate_ = 1000 / rate_; // activate/deactivate any config - nh->param("config_on_startup", config_on_startup_flag_, true); + this->declare_parameter("config_on_startup", true); + this->declare_parameter("raw_data", false); + this->declare_parameter("clear_bbr", false); + this->declare_parameter("save_on_shutdown", false); + this->declare_parameter("use_adr", true); + + this->declare_parameter("sv_in.reset", true); + this->declare_parameter("sv_in.min_dur", 0); + this->declare_parameter("sv_in.acc_lim", 0.0); - // raw data stream logging - rawDataStreamPa_.getRosParams(); + this->declare_parameter("dgnss_mode", rclcpp::PARAMETER_INTEGER); + + // raw data stream logging + this->declare_parameter("raw_data_stream.enable", false); + if (getRosBoolean(this, "raw_data_stream.enable")) { + raw_data_stream_pa_ = std::make_shared(getRosBoolean(this, "raw_data_stream.enable")); + raw_data_stream_pa_->getRosParams(); + } + + // NMEA parameters + this->declare_parameter("nmea.set", false); + this->declare_parameter("nmea.compat", false); + this->declare_parameter("nmea.consider", false); + this->declare_parameter("nmea.limit82", false); + this->declare_parameter("nmea.high_prec", false); + this->declare_parameter("nmea.filter.pos", false); + this->declare_parameter("nmea.filter.msk_pos", false); + this->declare_parameter("nmea.filter.time", false); + this->declare_parameter("nmea.filter.date", false); + this->declare_parameter("nmea.filter.sbas", false); + this->declare_parameter("nmea.filter.track", false); + this->declare_parameter("nmea.filter.gps_only", false); + this->declare_parameter("nmea.gnssToFilter.gps", false); + this->declare_parameter("nmea.gnssToFilter.sbas", false); + this->declare_parameter("nmea.gnssToFilter.qzss", false); + this->declare_parameter("nmea.gnssToFilter.glonass", false); + this->declare_parameter("nmea.gnssToFilter.beidou", false); + + // Publish parameters + this->declare_parameter("publish.all", false); + + this->declare_parameter("publish.nav.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.nav.att", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.clock", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.cov", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.heading", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.posecef", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.posllh", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.pvt", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.relposned", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.sat", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.sol", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.svin", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.svinfo", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.status", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.velned", getRosBoolean(this, "publish.nav.all")); + + this->declare_parameter("publish.rxm.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.rxm.almRaw", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.eph", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.rtcm", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.raw", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.sfrb", getRosBoolean(this, "publish.rxm.all")); + + this->declare_parameter("publish.aid.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.aid.alm", getRosBoolean(this, "publish.aid.all")); + this->declare_parameter("publish.aid.eph", getRosBoolean(this, "publish.aid.all")); + this->declare_parameter("publish.aid.hui", getRosBoolean(this, "publish.aid.all")); + + this->declare_parameter("publish.mon.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.mon.hw", getRosBoolean(this, "publish.mon.all")); + + this->declare_parameter("publish.tim.tm2", false); + + this->declare_parameter("publish.nmea", true); + + // INF parameters + this->declare_parameter("inf.all", true); + this->declare_parameter("inf.debug", false); + this->declare_parameter("inf.error", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.notice", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.test", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.warning", getRosBoolean(this, "inf.all")); + + // ESF parameters + this->declare_parameter("publish.esf.all", true); + this->declare_parameter("publish.esf.ins", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.meas", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.raw", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.status", getRosBoolean(this, "publish.esf.all")); + + // HNR parameters + this->declare_parameter("publish.hnr.pvt", true); + + this->declare_parameter("tmode3", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("arp.position", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("arp.position_hp", rclcpp::PARAMETER_INTEGER_ARRAY); + this->declare_parameter("arp.acc", 0.0); + this->declare_parameter("arp.lla_flag", false); + + this->declare_parameter("diagnostic_period", kDiagnosticPeriod); + + // Create publishers based on parameters + if (getRosBoolean(this, "publish.nav.status")) { + nav_status_pub_ = this->create_publisher("navstatus", 1); + } + if (getRosBoolean(this, "publish.nav.posecef")) { + nav_posecef_pub_ = this->create_publisher("navposecef", 1); + } + if (getRosBoolean(this, "publish.nav.cov")) { + nav_cov_pub_ = this->create_publisher("navcov", 1); + } + if (getRosBoolean(this, "publish.nav.clock")) { + nav_clock_pub_ = this->create_publisher("navclock", 1); + } + if (getRosBoolean(this, "publish.aid.alm")) { + aid_alm_pub_ = this->create_publisher("aidalm", 1); + } + if (getRosBoolean(this, "publish.aid.eph")) { + aid_eph_pub_ = this->create_publisher("aideph", 1); + } + if (getRosBoolean(this, "publish.aid.hui")) { + aid_hui_pub_ = this->create_publisher("aidhui", 1); + } + if (getRosBoolean(this, "publish.nmea")) { + // Larger queue depth to handle all NMEA strings being published consecutively + nmea_pub_ = this->create_publisher("nmea", 20); + } + + // Create subscriber for RTCM correction data to enable RTK + this->subscription_ = + this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); + + shutdown_timer_ = this->create_wall_timer(std::chrono::milliseconds(0), [this]() { + if (!shutdown_requested_.exchange(false)) + return; + rclcpp::shutdown(); + }); + shutdown_timer_->cancel(); // 普段は止めておく + + shutdown_srv_ = this->create_service( + "shutdown", [this](const std::shared_ptr, + std::shared_ptr res) { + res->success = true; + res->message = "Shutdown requested. Exiting..."; + shutdown_requested_ = true; + shutdown_timer_->reset(); // タイマースタート + }); +} + +void UbloxNode::keepAlive() { + // Poll version message to keep UDP socket active + gps_->poll(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER); } -void UbloxNode::pollMessages(const ros::TimerEvent& event) { +void UbloxNode::pollMessages() { static std::vector payload(1, 1); - if (enabled["aid_alm"]) - gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); - if (enabled["aid_eph"]) - gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); - if (enabled["aid_hui"]) - gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); + if (getRosBoolean(this, "publish.aid.alm")) { + gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); + } + if (getRosBoolean(this, "publish.aid.eph")) { + gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); + } + if (getRosBoolean(this, "publish.aid.hui")) { + gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); + } payload[0]++; if (payload[0] > 32) { @@ -239,174 +533,217 @@ void UbloxNode::pollMessages(const ros::TimerEvent& event) { } } -void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { - if (id == ublox_msgs::Message::INF::ERROR) - ROS_ERROR_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); - else if (id == ublox_msgs::Message::INF::WARNING) - ROS_WARN_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); - else if (id == ublox_msgs::Message::INF::DEBUG) - ROS_DEBUG_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); - else - ROS_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); +void UbloxNode::printInf(const ublox_msgs::msg::Inf &m, uint8_t id) { + if (id == ublox_msgs::Message::INF::ERROR) { + RCLCPP_ERROR(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); + } + else if (id == ublox_msgs::Message::INF::WARNING) { + RCLCPP_WARN(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); + } + else if (id == ublox_msgs::Message::INF::DEBUG) { + RCLCPP_DEBUG(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); + } + else { + RCLCPP_INFO(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); + } } void UbloxNode::subscribe() { - ROS_DEBUG("Subscribing to U-Blox messages"); + RCLCPP_DEBUG(this->get_logger(), "Subscribing to U-Blox messages"); // subscribe messages - nh->param("publish/all", enabled["all"], false); - nh->param("inf/all", enabled["inf"], true); - nh->param("publish/nav/all", enabled["nav"], enabled["all"]); - nh->param("publish/rxm/all", enabled["rxm"], enabled["all"]); - nh->param("publish/aid/all", enabled["aid"], enabled["all"]); - nh->param("publish/mon/all", enabled["mon"], enabled["all"]); // Nav Messages - nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); - if (enabled["nav_status"]) - gps.subscribe(boost::bind( - publish, _1, "navstatus"), kSubscribeRate); + if (getRosBoolean(this, "publish.nav.status")) { + gps_->subscribe( + [this](const ublox_msgs::msg::NavSTATUS &m) { + nav_status_pub_->publish(m); + }, + 1); + } - nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); - if (enabled["nav_posecef"]) - gps.subscribe(boost::bind( - publish, _1, "navposecef"), kSubscribeRate); + if (getRosBoolean(this, "publish.nav.posecef")) { + gps_->subscribe( + [this](const ublox_msgs::msg::NavPOSECEF &m) { + nav_posecef_pub_->publish(m); + }, + 1); + } + + if (getRosBoolean(this, "publish.nav.clock")) { + gps_->subscribe( + [this](const ublox_msgs::msg::NavCLOCK &m) { + nav_clock_pub_->publish(m); + }, + 1); + } - nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); - if (enabled["nav_clock"]) - gps.subscribe(boost::bind( - publish, _1, "navclock"), kSubscribeRate); + if (getRosBoolean(this, "publish.nav.cov")) { + gps_->subscribe( + [this](const ublox_msgs::msg::NavCOV &m) { + nav_cov_pub_->publish(m); + }, + 1); + } // INF messages - nh->param("inf/debug", enabled["inf_debug"], false); - if (enabled["inf_debug"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::DEBUG), - ublox_msgs::Message::INF::DEBUG); - - nh->param("inf/error", enabled["inf_error"], enabled["inf"]); - if (enabled["inf_error"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::ERROR), - ublox_msgs::Message::INF::ERROR); - - nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); - if (enabled["inf_notice"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::NOTICE), - ublox_msgs::Message::INF::NOTICE); - - nh->param("inf/test", enabled["inf_test"], enabled["inf"]); - if (enabled["inf_test"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::TEST), - ublox_msgs::Message::INF::TEST); - - nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); - if (enabled["inf_warning"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::WARNING), - ublox_msgs::Message::INF::WARNING); + if (getRosBoolean(this, "inf.debug")) { + gps_->subscribeId( + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); + } + + if (getRosBoolean(this, "inf.error")) { + gps_->subscribeId( + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); + } + + if (getRosBoolean(this, "inf.notice")) { + gps_->subscribeId( + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); + } + + if (getRosBoolean(this, "inf.test")) { + gps_->subscribeId( + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); + } + + if (getRosBoolean(this, "inf.warning")) { + gps_->subscribeId( + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); + } // AID messages - nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); - if (enabled["aid_alm"]) - gps.subscribe(boost::bind( - publish, _1, "aidalm"), kSubscribeRate); - - nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); - if (enabled["aid_eph"]) - gps.subscribe(boost::bind( - publish, _1, "aideph"), kSubscribeRate); - - nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); - if (enabled["aid_hui"]) - gps.subscribe(boost::bind( - publish, _1, "aidhui"), kSubscribeRate); - - for(int i = 0; i < components_.size(); i++) - components_[i]->subscribe(); -} + if (getRosBoolean(this, "publish.aid.alm")) { + gps_->subscribe( + [this](const ublox_msgs::msg::AidALM &m) { + aid_alm_pub_->publish(m); + }, + 1); + } -void UbloxNode::initializeRosDiagnostics() { - if (!nh->hasParam("diagnostic_period")) - nh->setParam("diagnostic_period", kDiagnosticPeriod); + if (getRosBoolean(this, "publish.aid.eph")) { + gps_->subscribe( + [this](const ublox_msgs::msg::AidEPH &m) { + aid_eph_pub_->publish(m); + }, + 1); + } - updater.reset(new diagnostic_updater::Updater()); - updater->setHardwareID("ublox"); + if (getRosBoolean(this, "publish.aid.hui")) { + gps_->subscribe( + [this](const ublox_msgs::msg::AidHUI &m) { + aid_hui_pub_->publish(m); + }, + 1); + } - // configure diagnostic updater for frequency - freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin)); - for(int i = 0; i < components_.size(); i++) - components_[i]->initializeRosDiagnostics(); + if (getRosBoolean(this, "publish.nmea")) { + gps_->subscribe_nmea([this](const std::string &sentence) { + nmea_msgs::msg::Sentence m; + m.header.stamp = this->now(); + m.header.frame_id = frame_id_; + m.sentence = sentence; + nmea_pub_->publish(m); + }); + } + + for (const std::shared_ptr &component : components_) { + component->subscribe(gps_); + } } +void UbloxNode::initializeRosDiagnostics() { + for (const std::shared_ptr &component : components_) { + component->initializeRosDiagnostics(); + } +} void UbloxNode::processMonVer() { - ublox_msgs::MonVER monVer; - if (!gps.poll(monVer)) + ublox_msgs::msg::MonVER monVer; + if (!gps_->poll(monVer)) { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); + } - ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), - monVer.hwVersion.c_array()); + RCLCPP_INFO( + this->get_logger(), "%s, HW VER: %s", std::string(monVer.sw_version.begin(), monVer.sw_version.end()).c_str(), + std::string(monVer.hw_version.begin(), monVer.hw_version.end()).c_str()); // Convert extension to vector of strings - std::vector extension; - extension.reserve(monVer.extension.size()); - for(std::size_t i = 0; i < monVer.extension.size(); ++i) { - ROS_DEBUG("%s", monVer.extension[i].field.c_array()); + std::vector extensions; + extensions.reserve(monVer.extension.size()); + for (std::size_t i = 0; i < monVer.extension.size(); ++i) { // NOLINT(modernize-loop-convert) + RCLCPP_DEBUG(this->get_logger(), "%s", std::string(monVer.extension[i].field.begin(), monVer.extension[i].field.end()).c_str()); // Find the end of the string (null character) - unsigned char* end = std::find(monVer.extension[i].field.begin(), - monVer.extension[i].field.end(), '\0'); - extension.push_back(std::string(monVer.extension[i].field.begin(), end)); + unsigned char *end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); + extensions.emplace_back(std::string(monVer.extension[i].field.begin(), end)); } // Get the protocol version - for(std::size_t i = 0; i < extension.size(); ++i) { - std::size_t found = extension[i].find("PROTVER"); + for (const std::string &ext : extensions) { + std::size_t found = ext.find("PROTVER"); if (found != std::string::npos) { - protocol_version_ = ::atof( - extension[i].substr(8, extension[i].size()-8).c_str()); + const char *sub = ext.substr(8, ext.size() - 8).c_str(); + char *end{nullptr}; + protocol_version_ = std::strtof(sub, &end); + if (protocol_version_ == HUGE_VALF || (protocol_version_ == 0 && end == sub)) { + // strtof failed to convert either via overflow or no conversion possible. + // Throw an error. + throw std::runtime_error("Failed to parse protocol version from extensions"); + } break; } } - if (protocol_version_ == 0) - ROS_WARN("Failed to parse MonVER and determine protocol version. %s", - "Defaulting to firmware version 6."); + if (protocol_version_ == 0.0) { + RCLCPP_WARN(this->get_logger(), "Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); + } addFirmwareInterface(); - if(protocol_version_ < 18) { + if (protocol_version_ < 18.0) { // Final line contains supported GNSS delimited by ; std::vector strs; - if(extension.size() > 0) - boost::split(strs, extension[extension.size()-1], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) - supported.insert(strs[i]); - } else { - for(std::size_t i = 0; i < extension.size(); ++i) { + if (extensions.size() > 0) { + strs = stringSplit(extensions[extensions.size() - 1], ";"); + } + for (const std::string &str : strs) { + gnss_->add(str); + } + } + else { + for (std::size_t i = 0; i < extensions.size(); ++i) { std::vector strs; // Up to 2nd to last line - if(i <= extension.size() - 2) { - boost::split(strs, extension[i], boost::is_any_of("=")); - if(strs.size() > 1) { - if (strs[0].compare(std::string("FWVER")) == 0) { - if(strs[1].length() > 8) + if (i <= extensions.size() - 2) { + strs = stringSplit(extensions[i], "="); + if (strs.size() > 1) { + if (strs[0] == "FWVER") { + if (strs[1].length() > 8) { addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); - else + } + else { addProductInterface(strs[1].substr(0, 3)); + } continue; } + // u-blox F9 modules support additional positioning signals + else if (strs[0] == "MOD") { + std::vector moduleField; + moduleField = stringSplit(strs[1], "-"); + if (moduleField.size() > 1) { + if (moduleField[1].substr(0, 2) == "F9") { + gnss_->add("GPS_L2C"); + gnss_->add("GAL_E5B"); + gnss_->add("BDS_B2"); + gnss_->add("QZSS_L2C"); + gnss_->add("GLO_L2"); + } + } + } } } // Last 1-2 lines contain supported GNSS - if(i >= extension.size() - 2) { - boost::split(strs, extension[i], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) - supported.insert(strs[i]); + if (i >= extensions.size() - 2) { + strs = stringSplit(extensions[i], ";"); + for (const std::string &str : strs) { + gnss_->add(str); + } } } } @@ -414,1430 +751,198 @@ void UbloxNode::processMonVer() { bool UbloxNode::configureUblox() { try { - if (!gps.isInitialized()) + if (!gps_->isInitialized()) { throw std::runtime_error("Failed to initialize."); - if (load_.loadMask != 0) { - ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.loadMask); - if (!gps.configure(load_)) - throw std::runtime_error(std::string("Failed to load configuration ") + - "from memory"); - if (load_.loadMask & load_.MASK_IO_PORT) { - ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", - "communications."); - boost::posix_time::seconds wait(kResetWait); - gps.reset(wait); - if (!gps.isConfigured()) - throw std::runtime_error(std::string("Failed to reset serial I/O") + - "after loading I/O configurations from device memory."); + } + if (load_.load_mask != 0) { + RCLCPP_DEBUG(this->get_logger(), "Loading u-blox configuration from memory. %u", load_.load_mask); + if (!gps_->configure(load_)) { + throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); + } + if (load_.load_mask & ublox_msgs::msg::CfgCFG::MASK_IO_PORT) { + RCLCPP_DEBUG(this->get_logger(), "Loaded I/O configuration from memory, resetting serial %s", "communications."); + std::chrono::seconds wait(kResetWait); + gps_->reset(wait); + if (!gps_->isConfigured()) { + throw std::runtime_error(std::string("Failed to reset serial I/O") + "after loading I/O configurations from device memory."); + } } } - if (config_on_startup_flag_) { + if (getRosBoolean(this, "config_on_startup")) { if (set_usb_) { - gps.configUsb(usb_tx_, usb_in_, usb_out_); + gps_->configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps.configRate(meas_rate, nav_rate)) { + if (!gps_->configRate(meas_rate_, nav_rate_)) { std::stringstream ss; - ss << "Failed to set measurement rate to " << meas_rate - << "ms and navigation rate to " << nav_rate; + ss << "Failed to set measurement rate to " << meas_rate_ << "ms and navigation rate to " << nav_rate_; throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) - if(supportsGnss("SBAS")) { - if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + - " SBAS."); + if (gnss_->isSupported("SBAS")) { + if (!gps_->configSbas(getRosBoolean(this, "gnss.sbas"), sbas_usage_, max_sbas_)) { + throw std::runtime_error(std::string("Failed to ") + (getRosBoolean(this, "gnss.sbas") ? "enable" : "disable") + " SBAS."); } } - if (!gps.setPpp(enable_ppp_)) - throw std::runtime_error(std::string("Failed to ") + - ((enable_ppp_) ? "enable" : "disable") - + " PPP."); - if (!gps.setDynamicModel(dmodel_)) + if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"), protocol_version_)) { + throw std::runtime_error(std::string("Failed to ") + (getRosBoolean(this, "enable_ppp") ? "enable" : "disable") + " PPP."); + } + if (!gps_->setDynamicModel(dmodel_)) { throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); - if (!gps.setFixMode(fmode_)) + } + if (!gps_->setFixMode(fmode_)) { throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + "."); - if (!gps.setDeadReckonLimit(dr_limit_)) { + } + if (!gps_->setDeadReckonLimit(dr_limit_)) { std::stringstream ss; ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (set_dat_ && !gps.configure(cfg_dat_)) + if (getRosBoolean(this, "dat.set") && !gps_->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); + } // Configure each component - for (int i = 0; i < components_.size(); i++) { - if(!components_[i]->configureUblox()) + for (const std::shared_ptr &component : components_) { + if (!component->configureUblox(gps_)) { return false; + } } } - if (save_.saveMask != 0) { - ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", - save_.saveMask, save_.deviceMask); - if(!gps.configure(save_)) - ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); + if (save_.save_mask != 0) { + RCLCPP_DEBUG(this->get_logger(), "Saving the u-blox configuration, mask %u, device %u", save_.save_mask, save_.device_mask); + if (!gps_->configure(save_)) { + RCLCPP_ERROR(this->get_logger(), "u-blox unable to save configuration to non-volatile memory"); + } } - } catch (std::exception& e) { - ROS_FATAL("Error configuring u-blox: %s", e.what()); + } + catch (const std::exception &e) { + RCLCPP_FATAL(this->get_logger(), "Error configuring u-blox: %s", e.what()); return false; } return true; } void UbloxNode::configureInf() { - ublox_msgs::CfgINF msg; + ublox_msgs::msg::CfgINF msg; // Subscribe to UBX INF messages - ublox_msgs::CfgINF_Block block; - block.protocolID = block.PROTOCOL_ID_UBX; + ublox_msgs::msg::CfgINFBlock block; + block.protocol_id = ublox_msgs::msg::CfgINFBlock::PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port - uint8_t mask = (enabled["inf_error"] ? block.INF_MSG_ERROR : 0) | - (enabled["inf_warning"] ? block.INF_MSG_WARNING : 0) | - (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | - (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | - (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0); - for (int i = 0; i < block.infMsgMask.size(); i++) - block.infMsgMask[i] = mask; + uint8_t mask = (getRosBoolean(this, "inf.error") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_ERROR : 0) | + (getRosBoolean(this, "inf.warning") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_WARNING : 0) | + (getRosBoolean(this, "inf.notice") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_NOTICE : 0) | + (getRosBoolean(this, "inf.test") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_TEST : 0) | + (getRosBoolean(this, "inf.debug") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_DEBUG : 0); + for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { // NOLINT(modernize-loop-convert) + block.inf_msg_mask[i] = mask; + } msg.blocks.push_back(block); // IF NMEA is enabled - if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) { - ublox_msgs::CfgINF_Block block; - block.protocolID = block.PROTOCOL_ID_NMEA; + if (uart_in_ & ublox_msgs::msg::CfgPRT::PROTO_NMEA) { + ublox_msgs::msg::CfgINFBlock block; + block.protocol_id = ublox_msgs::msg::CfgINFBlock::PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port - for (int i = 0; i < block.infMsgMask.size(); i++) - block.infMsgMask[i] = mask; + for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { // NOLINT(modernize-loop-convert) + block.inf_msg_mask[i] = mask; + } msg.blocks.push_back(block); } - ROS_DEBUG("Configuring INF messages"); - if (!gps.configure(msg)) - ROS_WARN("Failed to configure INF messages"); + RCLCPP_DEBUG(this->get_logger(), "Configuring INF messages"); + if (!gps_->configure(msg)) { + RCLCPP_WARN(this->get_logger(), "Failed to configure INF messages"); + } } void UbloxNode::initializeIo() { - gps.setConfigOnStartup(config_on_startup_flag_); + gps_->setConfigOnStartup(getRosBoolean(this, "config_on_startup")); - boost::smatch match; - if (boost::regex_match(device_, match, - boost::regex("(tcp|udp)://(.+):(\\d+)"))) { + std::smatch match; + if (std::regex_match(device_, match, std::regex("(tcp|udp)://(.+):(\\d+)"))) { std::string proto(match[1]); if (proto == "tcp") { std::string host(match[2]); std::string port(match[3]); - ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); - gps.initializeTcp(host, port); - } else { + RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); + gps_->initializeTcp(host, port); + } + else if (proto == "udp") { + std::string host(match[2]); + std::string port(match[3]); + RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); + gps_->initializeUdp(host, port); + } + else { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } - } else { - gps.initializeSerial(device_, baudrate_, uart_in_, uart_out_); + } + else { + gps_->initializeSerial(device_, baudrate_, uart_in_, uart_out_); } // raw data stream logging - if (rawDataStreamPa_.isEnabled()) { - gps.setRawDataCallback( - boost::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, _1, _2)); - rawDataStreamPa_.initialize(); + if (getRosBoolean(this, "raw_data_stream.enable")) { + if (raw_data_stream_pa_->isEnabled()) { + gps_->setRawDataCallback(std::bind(&RawDataStreamPa::ubloxCallback, raw_data_stream_pa_.get(), std::placeholders::_1, std::placeholders::_2)); + raw_data_stream_pa_->initialize(); + } } } void UbloxNode::initialize() { // Params must be set before initializing IO getRosParams(); + + // configure diagnostic updater for frequency + freq_diag_ = std::make_shared(std::string("fix"), kFixFreqTol, kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); + initializeIo(); // Must process Mon VER before setting firmware/hardware params processMonVer(); - if(protocol_version_ <= 14) { - if(nh->param("raw_data", false)) - components_.push_back(ComponentPtr(new RawDataProduct)); + if (protocol_version_ <= 14.0) { + if (getRosBoolean(this, "raw_data")) { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, this)); + } } // Must set firmware & hardware params before initializing diagnostics - for (int i = 0; i < components_.size(); i++) - components_[i]->getRosParams(); + for (const std::shared_ptr &component : components_) { + component->getRosParams(); + } // Do this last initializeRosDiagnostics(); if (configureUblox()) { - ROS_INFO("U-Blox configured successfully."); + RCLCPP_INFO(this->get_logger(), "U-Blox configured successfully."); // Subscribe to all U-Blox messages subscribe(); // Configure INF messages (needs INF params, call after subscribing) configureInf(); - ros::Timer poller; - poller = nh->createTimer(ros::Duration(kPollDuration), - &UbloxNode::pollMessages, - this); - poller.start(); - ros::spin(); - } - shutdown(); -} - -void UbloxNode::shutdown() { - if (gps.isInitialized()) { - gps.close(); - ROS_INFO("Closed connection to %s.", device_.c_str()); - } -} - -// -// U-Blox Firmware (all versions) -// -void UbloxFirmware::initializeRosDiagnostics() { - updater->add("fix", this, &UbloxFirmware::fixDiagnostic); - updater->force_update(); -} - -// -// U-Blox Firmware Version 6 -// -UbloxFirmware6::UbloxFirmware6() {} - -void UbloxFirmware6::getRosParams() { - // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS; - - nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { - bool compat, consider; - - if (!getRosUint("nmea/version", cfg_nmea_.version)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); - if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); - if (!nh->getParam("nmea/consider", consider)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); - - // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - - // set filter - bool temp; - nh->param("nmea/filter/pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0; - nh->param("nmea/filter/msk_pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0; - nh->param("nmea/filter/time", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0; - nh->param("nmea/filter/date", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0; - nh->param("nmea/filter/sbas", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_SBAS_FILT : 0; - nh->param("nmea/filter/track", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; - } -} - -bool UbloxFirmware6::configureUblox() { - ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - - if (set_nmea_ && !gps.configure(cfg_nmea_)) - throw std::runtime_error("Failed to configure NMEA"); - - return true; -} - -void UbloxFirmware6::subscribe() { - // Whether or not to publish Nav POS LLH (always subscribes) - nh->param("publish/nav/posllh", enabled["nav_posllh"], enabled["nav"]); - nh->param("publish/nav/sol", enabled["nav_sol"], enabled["nav"]); - nh->param("publish/nav/velned", enabled["nav_velned"], enabled["nav"]); - - // Always subscribes to these messages, but may not publish to ROS topic - // Subscribe to Nav POSLLH - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); - gps.subscribe(boost::bind( - // Subscribe to Nav SOL - &UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); - // Subscribe to Nav VELNED - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); - - // Subscribe to Nav SVINFO - nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); - if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), - kNavSvInfoSubscribeRate); - - // Subscribe to Mon HW - nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); - if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); -} - -void UbloxFirmware6::fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - // Set the diagnostic level based on the fix status - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Dead reckoning only"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "2D fix"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "3D fix"; - } else if (last_nav_sol_.gpsFix == - ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Time fix only"; - } - // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message += ", fix not ok"; - } - // Raise diagnostic level to error if no fix - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "No fix"; - } - - // Add last fix position - stat.add("iTOW [ms]", last_nav_pos_.iTOW); - stat.add("Latitude [deg]", last_nav_pos_.lat * 1e-7); - stat.add("Longitude [deg]", last_nav_pos_.lon * 1e-7); - stat.add("Altitude [m]", last_nav_pos_.height * 1e-3); - stat.add("Height above MSL [m]", last_nav_pos_.hMSL * 1e-3); - stat.add("Horizontal Accuracy [m]", last_nav_pos_.hAcc * 1e-3); - stat.add("Vertical Accuracy [m]", last_nav_pos_.vAcc * 1e-3); - stat.add("# SVs used", (int)last_nav_sol_.numSV); -} - -void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if(enabled["nav_posllh"]) { - static ros::Publisher publisher = - nh->advertise("navposllh", kROSQueueSize); - publisher.publish(m); - } - - // Position message - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); - if (m.iTOW == last_nav_vel_.iTOW) - fix_.header.stamp = velocity_.header.stamp; // use last timestamp - else - fix_.header.stamp = ros::Time::now(); // new timestamp - - fix_.header.frame_id = frame_id; - fix_.latitude = m.lat * 1e-7; - fix_.longitude = m.lon * 1e-7; - fix_.altitude = m.height * 1e-3; - - if (last_nav_sol_.gpsFix >= last_nav_sol_.GPS_2D_FIX) - fix_.status.status = fix_.status.STATUS_FIX; - else - fix_.status.status = fix_.status.STATUS_NO_FIX; - - // Convert from mm to m - const double varH = pow(m.hAcc / 1000.0, 2); - const double varV = pow(m.vAcc / 1000.0, 2); - - fix_.position_covariance[0] = varH; - fix_.position_covariance[4] = varH; - fix_.position_covariance[8] = varV; - fix_.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - - fix_.status.service = fix_.status.SERVICE_GPS; - fixPublisher.publish(fix_); - last_nav_pos_ = m; - // update diagnostics - freq_diag->diagnostic->tick(fix_.header.stamp); - updater->update(); -} - -void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if(enabled["nav_velned"]) { - static ros::Publisher publisher = - nh->advertise("navvelned", kROSQueueSize); - publisher.publish(m); - } - - // Example geometry message - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); - if (m.iTOW == last_nav_pos_.iTOW) - velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh - else - velocity_.header.stamp = ros::Time::now(); // create a new timestamp - velocity_.header.frame_id = frame_id; - - // convert to XYZ linear velocity - velocity_.twist.twist.linear.x = m.velE / 100.0; - velocity_.twist.twist.linear.y = m.velN / 100.0; - velocity_.twist.twist.linear.z = -m.velD / 100.0; - - const double varSpeed = pow(m.sAcc / 100.0, 2); - - const int cols = 6; - velocity_.twist.covariance[cols * 0 + 0] = varSpeed; - velocity_.twist.covariance[cols * 1 + 1] = varSpeed; - velocity_.twist.covariance[cols * 2 + 2] = varSpeed; - velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - - velocityPublisher.publish(velocity_); - last_nav_vel_ = m; -} - -void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if(enabled["nav_sol"]) { - static ros::Publisher publisher = - nh->advertise("navsol", kROSQueueSize); - publisher.publish(m); - } - last_nav_sol_ = m; -} - -// -// Ublox Firmware Version 7 -// -UbloxFirmware7::UbloxFirmware7() {} - -void UbloxFirmware7::getRosParams() { - // - // GNSS configuration - // - // GNSS enable/disable - nh->param("gnss/gps", enable_gps_, true); - nh->param("gnss/glonass", enable_glonass_, false); - nh->param("gnss/qzss", enable_qzss_, false); - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); - nh->param("gnss/sbas", enable_sbas_, false); - - if(enable_gps_ && !supportsGnss("GPS")) - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); - if(enable_glonass_ && !supportsGnss("GLO")) - ROS_WARN("gnss/glonass is true, but GLONASS is not %s", - "supported by this device"); - if(enable_qzss_ && !supportsGnss("QZSS")) - ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if(enable_sbas_ && !supportsGnss("SBAS")) - ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - - if(nh->hasParam("gnss/galileo")) - ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); - if(nh->hasParam("gnss/beidou")) - ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); - if(nh->hasParam("gnss/imes")) - ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); - - // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; - - // - // NMEA Configuration - // - nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { - bool compat, consider; - - if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); - if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); - if (!nh->getParam("nmea/consider", consider)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); - - // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - // set filter - bool temp; - nh->param("nmea/filter/pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0; - nh->param("nmea/filter/msk_pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0; - nh->param("nmea/filter/time", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0; - nh->param("nmea/filter/date", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0; - nh->param("nmea/filter/gps_only", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_GPS_ONLY : 0; - nh->param("nmea/filter/track", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; - // set gnssToFilter - nh->param("nmea/gnssToFilter/gps", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - nh->param("nmea/gnssToFilter/sbas", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - nh->param("nmea/gnssToFilter/qzss", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - nh->param("nmea/gnssToFilter/glonass", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - - getRosUint("nmea/main_talker_id", cfg_nmea_.mainTalkerId); - getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsvTalkerId); - } -} - -bool UbloxFirmware7::configureUblox() { - /** Configure the GNSS **/ - ublox_msgs::CfgGNSS cfgGNSSRead; - if (gps.poll(cfgGNSSRead)) { - ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.numTrkChHw); - ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.numTrkChUse); - } else { - throw std::runtime_error("Failed to read the GNSS config."); - } - - ublox_msgs::CfgGNSS cfgGNSSWrite; - cfgGNSSWrite.numConfigBlocks = 1; // do services one by one - cfgGNSSWrite.numTrkChHw = cfgGNSSRead.numTrkChHw; - cfgGNSSWrite.numTrkChUse = cfgGNSSRead.numTrkChUse; - cfgGNSSWrite.msgVer = 0; - - // configure GLONASS - if(supportsGnss("GLO")) { - ublox_msgs::CfgGNSS_Block block; - block.gnssId = block.GNSS_ID_GLONASS; - block.resTrkCh = block.RES_TRK_CH_GLONASS; - block.maxTrkCh = block.MAX_TRK_CH_GLONASS; - block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; - cfgGNSSWrite.blocks.push_back(block); - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " GLONASS."); - } - } - - if(supportsGnss("QZSS")) { - // configure QZSS - ublox_msgs::CfgGNSS_Block block; - block.gnssId = block.GNSS_ID_QZSS; - block.resTrkCh = block.RES_TRK_CH_QZSS; - block.maxTrkCh = block.MAX_TRK_CH_QZSS; - block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; - cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " QZSS."); - } - } - - if(supportsGnss("SBAS")) { - // configure SBAS - ublox_msgs::CfgGNSS_Block block; - block.gnssId = block.GNSS_ID_SBAS; - block.resTrkCh = block.RES_TRK_CH_SBAS; - block.maxTrkCh = block.MAX_TRK_CH_SBAS; - block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0; - cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + - " SBAS."); - } - } - - if(set_nmea_ && !gps.configure(cfg_nmea_)) - throw std::runtime_error("Failed to configure NMEA"); - - return true; -} - -void UbloxFirmware7::subscribe() { - // Whether to publish Nav PVT messages to a ROS topic - nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); - // Subscribe to Nav PVT (always does so since fix information is published - // from this) - gps.subscribe(boost::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, _1), - kSubscribeRate); - - // Subscribe to Nav SVINFO - nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); - if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), - kNavSvInfoSubscribeRate); - - // Subscribe to Mon HW - nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); - if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); -} - -// -// Ublox Version 8 -// -UbloxFirmware8::UbloxFirmware8() {} - -void UbloxFirmware8::getRosParams() { - // UPD SOS configuration - nh->param("clear_bbr", clear_bbr_, false); - gps.setSaveOnShutdown(nh->param("save_on_shutdown", false)); - - // GNSS enable/disable - nh->param("gnss/gps", enable_gps_, true); - nh->param("gnss/galileo", enable_galileo_, false); - nh->param("gnss/beidou", enable_beidou_, false); - nh->param("gnss/imes", enable_imes_, false); - nh->param("gnss/glonass", enable_glonass_, false); - nh->param("gnss/qzss", enable_qzss_, false); - nh->param("gnss/sbas", enable_sbas_, false); - // QZSS Signal Configuration - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); - - if (enable_gps_ && !supportsGnss("GPS")) - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", - "this device"); - if (enable_glonass_ && !supportsGnss("GLO")) - ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", - "this device"); - if (enable_galileo_ && !supportsGnss("GAL")) - ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", - "by this device"); - if (enable_beidou_ && !supportsGnss("BDS")) - ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", - "by this device"); - if (enable_imes_ && !supportsGnss("IMES")) - ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", - "this device"); - if (enable_qzss_ && !supportsGnss("QZSS")) - ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if (enable_sbas_ && !supportsGnss("SBAS")) - ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - - // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS - + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS - + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; - - // - // NMEA Configuration - // - nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { - bool compat, consider; - cfg_nmea_.version = cfg_nmea_.VERSION; // message version - - // Verify that parameters are set - if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); - if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); - if (!nh->getParam("nmea/consider", consider)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); - - // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - bool temp; - nh->param("nmea/limit82", temp, false); - cfg_nmea_.flags |= temp ? cfg_nmea_.FLAGS_LIMIT82 : 0; - nh->param("nmea/high_prec", temp, false); - cfg_nmea_.flags |= temp ? cfg_nmea_.FLAGS_HIGH_PREC : 0; - // set filter - nh->param("nmea/filter/pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0; - nh->param("nmea/filter/msk_pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0; - nh->param("nmea/filter/time", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0; - nh->param("nmea/filter/date", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0; - nh->param("nmea/filter/gps_only", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_GPS_ONLY : 0; - nh->param("nmea/filter/track", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; - // set gnssToFilter - nh->param("nmea/gnssToFilter/gps", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - nh->param("nmea/gnssToFilter/sbas", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - nh->param("nmea/gnssToFilter/qzss", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - nh->param("nmea/gnssToFilter/glonass", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - nh->param("nmea/gnssToFilter/beidou", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; - - getRosUint("nmea/main_talker_id", cfg_nmea_.mainTalkerId); - getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsvTalkerId); - - std::vector bdsTalkerId; - getRosUint("nmea/bds_talker_id", bdsTalkerId); - cfg_nmea_.bdsTalkerId[0] = bdsTalkerId[0]; - cfg_nmea_.bdsTalkerId[1] = bdsTalkerId[1]; - } -} - - -bool UbloxFirmware8::configureUblox() { - if(clear_bbr_) { - // clear flash memory - if(!gps.clearBbr()) - ROS_ERROR("u-blox failed to clear flash memory"); - } - // - // Configure the GNSS, only if the configuration is different - // - // First, get the current GNSS configuration - ublox_msgs::CfgGNSS cfg_gnss; - if (gps.poll(cfg_gnss)) { - ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.numTrkChHw); - ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.numTrkChUse); - } else { - throw std::runtime_error("Failed to read the GNSS config."); - } - - // Then, check the configuration for each GNSS. If it is different, change it. - bool correct = true; - for (int i = 0; i < cfg_gnss.blocks.size(); i++) { - ublox_msgs::CfgGNSS_Block block = cfg_gnss.blocks[i]; - if (block.gnssId == block.GNSS_ID_GPS - && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; - ROS_DEBUG("GPS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_SBAS - && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; - ROS_DEBUG("SBAS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_GALILEO - && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; - ROS_DEBUG("Galileo GNSS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_BEIDOU - && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; - ROS_DEBUG("BeiDou Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_IMES - && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnssId == block.GNSS_ID_QZSS - && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) - || (enable_qzss_ - && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { - ROS_DEBUG("QZSS Configuration is different %u, %u", - block.flags & block.FLAGS_ENABLE, - enable_qzss_); - correct = false; - ROS_DEBUG("QZSS Configuration: %u", block.flags); - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; - ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); - if (enable_qzss_) - // Only change sig cfg if enabling - cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; - } else if (block.gnssId == block.GNSS_ID_GLONASS - && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; - ROS_DEBUG("GLONASS Configuration is different"); - } - } - - // If the GNSS is already configured correctly, do not re-configure GNSS - // since this requires a cold reset - if (correct) - ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); - else if (!gps.configGnss(cfg_gnss, boost::posix_time::seconds(15))) - throw std::runtime_error(std::string("Failed to cold reset device ") + - "after configuring GNSS"); - - // - // NMEA config - // - if (set_nmea_ && !gps.configure(cfg_nmea_)) - throw std::runtime_error("Failed to configure NMEA"); - - return true; -} - -void UbloxFirmware8::subscribe() { - // Whether to publish Nav PVT messages - nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); - // Subscribe to Nav PVT - gps.subscribe( - boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); - - // Subscribe to Nav SAT messages - nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); - if (enabled["nav_sat"]) - gps.subscribe(boost::bind( - publish, _1, "navsat"), kNavSvInfoSubscribeRate); - - // Subscribe to Mon HW - nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); - if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); - - // Subscribe to RTCM messages - nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); - if (enabled["rxm_rtcm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmrtcm"), kSubscribeRate); -} - -// -// Raw Data Products -// -void RawDataProduct::subscribe() { - // Defaults to true instead of to all - nh->param("publish/rxm/all", enabled["rxm"], true); - - // Subscribe to RXM Raw - nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); - if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); - - // Subscribe to RXM SFRB - nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); - if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); - - // Subscribe to RXM EPH - nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); - if (enabled["rxm_eph"]) - gps.subscribe(boost::bind( - publish, _1, "rxmeph"), kSubscribeRate); - - // Subscribe to RXM ALM - nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); - if (enabled["rxm_alm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmalm"), kSubscribeRate); -} - -void RawDataProduct::initializeRosDiagnostics() { - if (enabled["rxm_raw"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); - if (enabled["rxm_sfrb"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); - if (enabled["rxm_eph"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); - if (enabled["rxm_alm"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); -} - -// -// u-blox ADR devices, partially implemented -// -void AdrUdrProduct::getRosParams() { - nh->param("use_adr", use_adr_, true); - // Check the nav rate - float nav_rate_hz = 1000 / (meas_rate * nav_rate); - if(nav_rate_hz != 1) - ROS_WARN("Nav Rate recommended to be 1 Hz"); -} - -bool AdrUdrProduct::configureUblox() { - if(!gps.setUseAdr(use_adr_)) - throw std::runtime_error(std::string("Failed to ") - + (use_adr_ ? "enable" : "disable") + "use_adr"); - return true; -} - -void AdrUdrProduct::subscribe() { - nh->param("publish/esf/all", enabled["esf"], true); - - // Subscribe to NAV ATT messages - nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); - if (enabled["nav_att"]) - gps.subscribe(boost::bind( - publish, _1, "navatt"), kSubscribeRate); - - // Subscribe to ESF INS messages - nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); - if (enabled["esf_ins"]) - gps.subscribe(boost::bind( - publish, _1, "esfins"), kSubscribeRate); - - // Subscribe to ESF Meas messages - nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); - if (enabled["esf_meas"]) - gps.subscribe(boost::bind( - publish, _1, "esfmeas"), kSubscribeRate); - // also publish sensor_msgs::Imu - gps.subscribe(boost::bind( - &AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); - - // Subscribe to ESF Raw messages - nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); - if (enabled["esf_raw"]) - gps.subscribe(boost::bind( - publish, _1, "esfraw"), kSubscribeRate); - - // Subscribe to ESF Status messages - nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); - if (enabled["esf_status"]) - gps.subscribe(boost::bind( - publish, _1, "esfstatus"), kSubscribeRate); - - // Subscribe to HNR PVT messages - nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); - if (enabled["hnr_pvt"]) - gps.subscribe(boost::bind( - publish, _1, "hnrpvt"), kSubscribeRate); -} - -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (enabled["esf_meas"]) { - static ros::Publisher imu_pub = - nh->advertise("imu_meas", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - - imu_.header.stamp = ros::Time::now(); - imu_.header.frame_id = frame_id; - - float deg_per_sec = pow(2, -12); - float m_per_sec_sq = pow(2, -10); - float deg_c = 1e-2; - - std::vector imu_data = m.data; - for (int i=0; i < imu_data.size(); i++){ - unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data - double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data - unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits - - if (data_sign == 0) { - data_sign = -1; - } else { - data_sign = 1; - } - - //ROS_INFO("data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray - - imu_.orientation_covariance[0] = -1; - imu_.linear_acceleration_covariance[0] = -1; - imu_.angular_velocity_covariance[0] = -1; - - if (data_type == 14) { - if (data_sign == 1) { - imu_.angular_velocity.x = 2048 - data_value * deg_per_sec; - } else { - imu_.angular_velocity.x = data_sign * data_value * deg_per_sec; - } - } else if (data_type == 16) { - //ROS_INFO("data_sign: %f", data_sign); - //ROS_INFO("data_value: %u", data_value * m); - if (data_sign == 1) { - imu_.linear_acceleration.x = 8191 - data_value * m_per_sec_sq; - } else { - imu_.linear_acceleration.x = data_sign * data_value * m_per_sec_sq; - } - } else if (data_type == 13) { - if (data_sign == 1) { - imu_.angular_velocity.y = 2048 - data_value * deg_per_sec; - } else { - imu_.angular_velocity.y = data_sign * data_value * deg_per_sec; - } - } else if (data_type == 17) { - if (data_sign == 1) { - imu_.linear_acceleration.y = 8191 - data_value * m_per_sec_sq; - } else { - imu_.linear_acceleration.y = data_sign * data_value * m_per_sec_sq; - } - } else if (data_type == 5) { - if (data_sign == 1) { - imu_.angular_velocity.z = 2048 - data_value * deg_per_sec; - } else { - imu_.angular_velocity.z = data_sign * data_value * deg_per_sec; - } - } else if (data_type == 18) { - if (data_sign == 1) { - imu_.linear_acceleration.z = 8191 - data_value * m_per_sec_sq; - } else { - imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq; - } - } else if (data_type == 12) { - //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); - } else { - ROS_INFO("data_type: %u", data_type); - ROS_INFO("data_value: %u", data_value); - } - - // create time ref message and put in the data - //t_ref_.header.seq = m.risingEdgeCount; - //t_ref_.header.stamp = ros::Time::now(); - //t_ref_.header.frame_id = frame_id; - - //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); - - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp - t_ref_.header.frame_id = frame_id; - - time_ref_pub.publish(t_ref_); - imu_pub.publish(imu_); - } - } - - updater->force_update(); -} -// -// u-blox High Precision GNSS Reference Station -// -void HpgRefProduct::getRosParams() { - if (config_on_startup_flag_) { - if(nav_rate * meas_rate != 1000) - ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); - - if(!getRosUint("tmode3", tmode3_)) - throw std::runtime_error("Invalid settings: TMODE3 must be set"); - - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!nh->getParam("arp/position", arp_position_)) - throw std::runtime_error(std::string("Invalid settings: arp/position ") - + "must be set if TMODE3 is fixed"); - if(!getRosInt("arp/position_hp", arp_position_hp_)) - throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/acc", fixed_pos_acc_)) - throw std::runtime_error(std::string("Invalid settings: arp/acc ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/lla_flag", lla_flag_)) { - ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", - "in ECEF"); - lla_flag_ = false; - } - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - nh->param("sv_in/reset", svin_reset_, true); - if(!getRosUint("sv_in/min_dur", sv_in_min_dur_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") - + "must be set if TMODE3 is survey-in"); - if(!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") - + "must be set if TMODE3 is survey-in"); - } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") - + " flag constants for possible values."); + if (device_.substr(0, 6) == "udp://") { + // Setup timer to poll version message to keep UDP socket active + keep_alive_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(kKeepAlivePeriod * 1000.0)), std::bind(&UbloxNode::keepAlive, this)); } - } -} - -bool HpgRefProduct::configureUblox() { - // Configure TMODE3 - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if(!gps.disableTmode3()) - throw std::runtime_error("Failed to disable TMODE3."); - mode_ = DISABLED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, - fixed_pos_acc_)) - throw std::runtime_error("Failed to set TMODE3 to fixed."); - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) - throw std::runtime_error("Failed to set RTCM rates"); - mode_ = FIXED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - if(!svin_reset_) { - ublox_msgs::NavSVIN nav_svin; - if(!gps.poll(nav_svin)) - throw std::runtime_error(std::string("Failed to poll NavSVIN while") + - " configuring survey-in"); - // Don't reset survey-in if it's already active - if(nav_svin.active) { - mode_ = SURVEY_IN; - return true; - } - // Don't reset survey-in if it already has a valid value - if(nav_svin.valid) { - setTimeMode(); - return true; - } - ublox_msgs::NavPVT nav_pvt; - if(!gps.poll(nav_pvt)) - throw std::runtime_error(std::string("Failed to poll NavPVT while") + - " configuring survey-in"); - // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY - && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { - setTimeMode(); - return true; - } - } - // Reset the Survey In - // For Survey in, meas rate must be at least 1 Hz - uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] - // If measurement period isn't a factor of 1000, set to default - if(1000 % meas_rate_temp != 0) - meas_rate_temp = kDefaultMeasPeriod; - // Set nav rate to 1 Hz during survey in - if(!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) - throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + - "before setting TMODE3 to survey-in."); - // As recommended in the documentation, first disable, then set to survey in - if(!gps.disableTmode3()) - ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); - else - mode_ = DISABLED; - // Set to Survey in mode - if(!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) - throw std::runtime_error("Failed to set TMODE3 to survey-in."); - mode_ = SURVEY_IN; - } - return true; -} - -void HpgRefProduct::subscribe() { - // Whether to publish Nav Survey-In messages - nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); - // Subscribe to Nav Survey-In - gps.subscribe(boost::bind( - &HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); -} - -void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { - if(enabled["nav_svin"]) { - static ros::Publisher publisher = - nh->advertise("navsvin", kROSQueueSize); - publisher.publish(m); - } - - last_nav_svin_ = m; - - if(!m.active && m.valid && mode_ == SURVEY_IN) { - setTimeMode(); - } - - updater->update(); -} -bool HpgRefProduct::setTimeMode() { - ROS_INFO("Setting mode (internal state) to Time Mode"); - mode_ = TIME; - - // Set the Measurement & nav rate to user config - // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if(!gps.configRate(meas_rate, nav_rate)) - ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, - "navigation rate to ", nav_rate); - // Enable the RTCM out messages - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) { - ROS_ERROR("Failed to configure RTCM IDs"); - return false; - } - return true; -} - -void HpgRefProduct::initializeRosDiagnostics() { - updater->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); - updater->force_update(); -} - -void HpgRefProduct::tmode3Diagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - if (mode_ == INIT) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Not configured"; - } else if (mode_ == DISABLED){ - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Disabled"; - } else if (mode_ == SURVEY_IN) { - if (!last_nav_svin_.active && !last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "Survey-In inactive and invalid"; - } else if (last_nav_svin_.active && !last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Survey-In active but invalid"; - } else if (!last_nav_svin_.active && last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Survey-In complete"; - } else if (last_nav_svin_.active && last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Survey-In active and valid"; - } - - stat.add("iTOW [ms]", last_nav_svin_.iTOW); - stat.add("Duration [s]", last_nav_svin_.dur); - stat.add("# observations", last_nav_svin_.obs); - stat.add("Mean X [m]", last_nav_svin_.meanX * 1e-2); - stat.add("Mean Y [m]", last_nav_svin_.meanY * 1e-2); - stat.add("Mean Z [m]", last_nav_svin_.meanZ * 1e-2); - stat.add("Mean X HP [m]", last_nav_svin_.meanXHP * 1e-4); - stat.add("Mean Y HP [m]", last_nav_svin_.meanYHP * 1e-4); - stat.add("Mean Z HP [m]", last_nav_svin_.meanZHP * 1e-4); - stat.add("Mean Accuracy [m]", last_nav_svin_.meanAcc * 1e-4); - } else if(mode_ == FIXED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Fixed Position"; - } else if(mode_ == TIME) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Time"; + poller_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(kPollDuration * 1000.0)), std::bind(&UbloxNode::pollMessages, this)); } } -// -// U-Blox High Precision GNSS Rover -// -void HpgRovProduct::getRosParams() { - // default to float, see CfgDGNSS message for details - getRosUint("dgnss_mode", dgnss_mode_, - ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); -} - -bool HpgRovProduct::configureUblox() { - // Configure the DGNSS - if(!gps.setDgnss(dgnss_mode_)) - throw std::runtime_error(std::string("Failed to Configure DGNSS")); - return true; -} - -void HpgRovProduct::subscribe() { - // Whether to publish Nav Relative Position NED - nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); - // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); -} - -void HpgRovProduct::initializeRosDiagnostics() { - freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow); - updater->add("Carrier Phase Solution", this, - &HpgRovProduct::carrierPhaseDiagnostics); - updater->force_update(); -} - -void HpgRovProduct::carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; - stat.add("iTow", last_rel_pos_.iTow); - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || - !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && - last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "None"; - } else { - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Float"; - } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Fixed"; - } - stat.add("Ref Station ID", last_rel_pos_.refStationId); - - double rel_pos_n = (last_rel_pos_.relPosN - + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; - double rel_pos_e = (last_rel_pos_.relPosE - + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; - double rel_pos_d = (last_rel_pos_.relPosD - + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; - stat.add("Relative Position N [m]", rel_pos_n); - stat.add("Relative Accuracy N [m]", last_rel_pos_.accN * 1e-4); - stat.add("Relative Position E [m]", rel_pos_e); - stat.add("Relative Accuracy E [m]", last_rel_pos_.accE * 1e-4); - stat.add("Relative Position D [m]", rel_pos_d); - stat.add("Relative Accuracy D [m]", last_rel_pos_.accD * 1e-4); - } -} - -void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); - publisher.publish(m); - } - - last_rel_pos_ = m; - updater->update(); -} - -// -// U-Blox High Precision Positioning Receiver -// - -void HpPosRecProduct::subscribe() { - // Whether to publish Nav Relative Position NED - nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); - // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpPosRecProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); - - // Whether to publish the Heading info from Nav Relative Position NED - nh->param("publish/nav/heading", enabled["nav_heading"], enabled["nav"]); -} - -void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); - publisher.publish(m); - } - - if (enabled["nav_heading"]) { - static ros::Publisher imu_pub = - nh->advertise("navheading", kROSQueueSize); - - imu_.header.stamp = ros::Time::now(); - imu_.header.frame_id = frame_id; - - imu_.linear_acceleration_covariance[0] = -1; - imu_.angular_velocity_covariance[0] = -1; - - double heading = static_cast(m.relPosHeading) * 1e-5 / 180.0 * M_PI; - tf::Quaternion orientation; - orientation.setRPY(0, 0, heading); - imu_.orientation.x = orientation[0]; - imu_.orientation.y = orientation[1]; - imu_.orientation.z = orientation[2]; - imu_.orientation.w = orientation[3]; - // Only heading is reported with an accuracy in 0.1mm units - imu_.orientation_covariance[0] = 1000.0; - imu_.orientation_covariance[4] = 1000.0; - imu_.orientation_covariance[8] = pow(m.accHeading / 10000.0, 2); +void UbloxNode::shutdown() { + if (gps_->isInitialized()) { + gps_->close(); - imu_pub.publish(imu_); + RCLCPP_INFO(this->get_logger(), "Closed connection to %s.", device_.c_str()); } - - last_rel_pos_ = m; - updater->update(); -} - -// -// U-Blox Time Sync Products, partially implemented. -// -void TimProduct::getRosParams() { -} - -bool TimProduct::configureUblox() { - uint8_t r = 1; - // Configure the reciever - if(!gps.setUTCtime()) - throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); - - if(!gps.setTimtm2(r)) - throw std::runtime_error(std::string("Failed to Configure TIM Product")); - - return true; } -void TimProduct::subscribe() { - ROS_INFO("TIM is Enabled: %u", enabled["tim"]); - ROS_INFO("TIM-TM2 is Enabled: %u", enabled["tim_tm2"]); - // Subscribe to TIM-TM2 messages (Time mark messages) - nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); - - gps.subscribe(boost::bind( - &TimProduct::callbackTimTM2, this, _1), kSubscribeRate); - - ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); - - // Subscribe to SFRBX messages - nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); - if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); - - // Subscribe to RawX messages - nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); - if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); -} - -void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - - if (enabled["tim_tm2"]) { - static ros::Publisher publisher = - nh->advertise("timtm2", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - - // create time ref message and put in the data - t_ref_.header.seq = m.risingEdgeCount; - t_ref_.header.stamp = ros::Time::now(); - t_ref_.header.frame_id = frame_id; - - t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - std::ostringstream src; - src << "TIM" << int(m.ch); - t_ref_.source = src.str(); - - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp - t_ref_.header.frame_id = frame_id; - - publisher.publish(m); - time_ref_pub.publish(t_ref_); - } - - updater->force_update(); -} - -void TimProduct::initializeRosDiagnostics() { - updater->force_update(); +UbloxNode::~UbloxNode() { + shutdown(); } -int main(int argc, char** argv) { - ros::init(argc, argv, "ublox_gps"); - nh.reset(new ros::NodeHandle("~")); - nh->param("debug", ublox_gps::debug, 1); - if(ublox_gps::debug) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); +} // namespace ublox_node - } - UbloxNode node; - return 0; -} +RCLCPP_COMPONENTS_REGISTER_NODE(ublox_node::UbloxNode) diff --git a/ublox_gps/src/node_main.cpp b/ublox_gps/src/node_main.cpp new file mode 100644 index 00000000..dcdff1ff --- /dev/null +++ b/ublox_gps/src/node_main.cpp @@ -0,0 +1,24 @@ +#include + +#include + +#include + +int main(int argc, char** argv) { + // Force flush of the stdout buffer. + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + int exit_code = 0; + try { + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("ublox_gps_node"), "Fatal error: %s", e.what()); + exit_code = 1; + } + + rclcpp::shutdown(); + + return exit_code; +} diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 872fca9a..dedd7bb9 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -32,161 +32,169 @@ // the data as ros messages. This is used by our group to also evaluate the // measured data with the rtklib. -#include "ublox_gps/raw_data_pa.h" +#include #include +#include #include #include #include #include -#include -using namespace ublox_node; +#include + +#include +#include + +#include // // ublox_node namespace // -RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : - pnh_(ros::NodeHandle("~")), +namespace ublox_node { + +RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : rclcpp::Node("raw_data_pa"), flag_publish_(false), is_ros_subscriber_(is_ros_subscriber) { + raw_pub_ = this->create_publisher("raw_data_stream", 100); + + this->declare_parameter("dir", ""); + this->declare_parameter("raw_data_stream.dir", ""); + this->declare_parameter("raw_data_stream.publish", false); } void RawDataStreamPa::getRosParams() { - if (is_ros_subscriber_) { - pnh_.param("dir", file_dir_, ""); - } else { - pnh_.param("raw_data_stream/dir", file_dir_, ""); - pnh_.param("raw_data_stream/publish", flag_publish_, false); - } + if (is_ros_subscriber_) { + file_dir_ = this->get_parameter("dir").get_value(); + } else { + file_dir_ = this->get_parameter("raw_data_stream.dir").get_value(); + flag_publish_ = this->get_parameter("raw_data_stream.publish").get_value(); + } } bool RawDataStreamPa::isEnabled() { - if (is_ros_subscriber_) { - return !file_dir_.empty(); - } else { - return flag_publish_ || (!file_dir_.empty()); - } -} + if (is_ros_subscriber_) { + return !file_dir_.empty(); + } + return flag_publish_ || !file_dir_.empty(); +} void RawDataStreamPa::initialize() { - if (is_ros_subscriber_) { - ROS_INFO("Subscribing to raw data stream."); - static ros::Subscriber subscriber = - nh_.subscribe ("raw_data_stream", 100, - &RawDataStreamPa::msgCallback, this); - } else if (flag_publish_) { - ROS_INFO("Publishing raw data stream."); - RawDataStreamPa::publishMsg(std::string()); - } + if (is_ros_subscriber_) { + RCLCPP_INFO(this->get_logger(), "Subscribing to raw data stream."); + raw_data_stream_sub_ = + this->create_subscription("raw_data_stream", rclcpp::QoS(100), + std::bind(&RawDataStreamPa::msgCallback, this, std::placeholders::_1)); + } else if (flag_publish_) { + RCLCPP_INFO(this->get_logger(), "Publishing raw data stream."); + RawDataStreamPa::publishMsg(std::string()); + } + + if (!file_dir_.empty()) { + struct stat stat_info{}; + if (::stat(file_dir_.c_str(), &stat_info) != 0) { + RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " + "Directory \"%s\" does not exist.", file_dir_.c_str()); + + } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { + RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " + "\"%s\" exists, but is not a directory.", file_dir_.c_str()); - if (!file_dir_.empty()) { - struct stat stat_info; - if (stat(file_dir_.c_str(), &stat_info ) != 0) { - ROS_ERROR("Can't log raw data to file. " - "Directory \"%s\" does not exist.", file_dir_.c_str()); - - } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { - ROS_ERROR("Can't log raw data to file. " - "\"%s\" exists, but is not a directory.", file_dir_.c_str()); - - } else { - if (file_dir_.back() != '/') { - file_dir_ += '/'; - } - - time_t t = time(NULL); - struct tm time_struct = *localtime(&t); - - std::stringstream filename; - filename.width(4); filename.fill('0'); - filename << time_struct.tm_year + 1900; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mon + 1; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mday; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_hour; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_min ; - filename.width(0); filename << ".log"; - file_name_ = file_dir_ + filename.str(); - - try { - file_handle_.open(file_name_); - ROS_INFO("Logging raw data to file \"%s\"", - file_name_.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Can't log raw data to file. " - "Can't create file \"%s\".", file_name_.c_str()); - } - } + } else { + if (file_dir_.back() != '/') { + file_dir_ += '/'; + } + + time_t t = time(nullptr); + struct tm time_struct = *localtime(&t); + + std::stringstream filename; + filename.width(4); filename.fill('0'); + filename << time_struct.tm_year + 1900; + filename.width(0); filename << '_'; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_mon + 1; + filename.width(0); filename << '_'; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_mday; + filename.width(0); filename << '_'; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_hour; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_min ; + filename.width(0); filename << ".log"; + file_name_ = file_dir_ + filename.str(); + + try { + file_handle_.open(file_name_); + RCLCPP_INFO(this->get_logger(), "Logging raw data to file \"%s\"", + file_name_.c_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " + "Can't create file \"%s\".", file_name_.c_str()); + } } + } } void RawDataStreamPa::ubloxCallback(const unsigned char* data, - const std::size_t size) { + std::size_t size) { - std::string str((const char*) data, size); + std::string str(reinterpret_cast(data), size); - if (flag_publish_) { - publishMsg(str); - } + if (flag_publish_) { + publishMsg(str); + } - saveToFile(str); + saveToFile(str); } void RawDataStreamPa::msgCallback( - const std_msgs::UInt8MultiArray::ConstPtr& msg) { + const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { - std::string str(msg->data.size(), ' '); - std::copy(msg->data.begin(), msg->data.end(), str.begin()); - saveToFile(str); + std::string str(msg->data.size(), ' '); + std::copy(msg->data.begin(), msg->data.end(), str.begin()); + saveToFile(str); } -std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( - const std::string str) { +std_msgs::msg::UInt8MultiArray RawDataStreamPa::str2uint8( + const std::string & str) { - std_msgs::UInt8MultiArray msg; + std_msgs::msg::UInt8MultiArray msg; - msg.layout.data_offset = 0; - msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); - msg.layout.dim[0].size = str.length(); - msg.layout.dim[0].stride = 1; - msg.layout.dim[0].label = "raw_data_stream"; + msg.layout.data_offset = 0; + msg.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + msg.layout.dim[0].size = str.length(); + msg.layout.dim[0].stride = 1; + msg.layout.dim[0].label = "raw_data_stream"; - msg.data.resize(str.length()); - std::copy(str.begin(), str.end(), msg.data.begin()); + msg.data.resize(str.length()); + std::copy(str.begin(), str.end(), msg.data.begin()); - return msg; + return msg; } -void RawDataStreamPa::publishMsg(const std::string str) { - - static ros::Publisher publisher = - pnh_.advertise("raw_data_stream", 100); +void RawDataStreamPa::publishMsg(const std::string & str) { - publisher.publish(RawDataStreamPa::str2uint8(str)); + raw_pub_->publish(RawDataStreamPa::str2uint8(str)); } -void RawDataStreamPa::saveToFile(const std::string str) { +void RawDataStreamPa::saveToFile(const std::string & str) { - if (file_handle_.is_open()) { - try { - file_handle_ << str; - // file_handle_.flush(); - } catch(const std::exception& e) { - ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); - } + if (file_handle_.is_open()) { + try { + file_handle_ << str; + // file_handle_.flush(); + } catch (const std::exception& e) { + RCLCPP_WARN(this->get_logger(), "Error writing to file \"%s\"", file_name_.c_str()); } + } } +} // namespace ublox_node diff --git a/ublox_gps/src/raw_data_product.cpp b/ublox_gps/src/raw_data_product.cpp new file mode 100644 index 00000000..23c953aa --- /dev/null +++ b/ublox_gps/src/raw_data_product.cpp @@ -0,0 +1,82 @@ +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace ublox_node { + +// +// Raw Data Products +// +RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { + if (getRosBoolean(node_, "publish.rxm.raw")) { + rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + } + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + } + if (getRosBoolean(node_, "publish.rxm.eph")) { + rxm_eph_pub_ = node_->create_publisher("rxmeph", 1); + } + if (getRosBoolean(node_, "publish.rxm.almRaw")) { + rxm_alm_pub_ = node_->create_publisher("rxmalm", 1); + } +} + +void RawDataProduct::subscribe(std::shared_ptr gps) { + // Subscribe to RXM Raw + if (getRosBoolean(node_, "publish.rxm.raw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRAW &m) { rxm_raw_pub_->publish(m); }, + 1); + } + + // Subscribe to RXM SFRB + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + gps->subscribe([this](const ublox_msgs::msg::RxmSFRB &m) { rxm_sfrb_pub_->publish(m); }, + 1); + } + + // Subscribe to RXM EPH + if (getRosBoolean(node_, "publish.rxm.eph")) { + gps->subscribe([this](const ublox_msgs::msg::RxmEPH &m) { rxm_eph_pub_->publish(m); }, + 1); + } + + // Subscribe to RXM ALM + if (getRosBoolean(node_, "publish.rxm.almRaw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmALM &m) { rxm_alm_pub_->publish(m); }, + 1); + } +} + +void RawDataProduct::initializeRosDiagnostics() { + if (getRosBoolean(node_, "publish.rxm.raw")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } + if (getRosBoolean(node_, "publish.rxm.eph")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } + if (getRosBoolean(node_, "publish.rxm.almRaw")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } +} + +} // namespace ublox_node diff --git a/ublox_gps/src/tim_product.cpp b/ublox_gps/src/tim_product.cpp new file mode 100644 index 00000000..3f9000f1 --- /dev/null +++ b/ublox_gps/src/tim_product.cpp @@ -0,0 +1,99 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox Time Sync Products, partially implemented. +// +TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) : frame_id_(frame_id), updater_(updater), node_(node) +{ + timtm2_pub_ = + node_->create_publisher("timtm2", 1); + interrupt_time_pub_ = + node_->create_publisher("interrupt_time", 1); + + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + } + if (getRosBoolean(node_, "publish.rxm.raw")) { + rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + } +} + +void TimProduct::getRosParams() { +} + +bool TimProduct::configureUblox(std::shared_ptr gps) { + uint8_t r = 1; + // Configure the reciever + if (!gps->setUTCtime()) { + throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); + } + + if (!gps->setTimtm2(r)) { + throw std::runtime_error(std::string("Failed to Configure TIM Product")); + } + + return true; +} + +void TimProduct::subscribe(std::shared_ptr gps) { + gps->subscribe(std::bind( + &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); + + // RCLCPP_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); + + // Subscribe to SFRBX messages + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + gps->subscribe([this](const ublox_msgs::msg::RxmSFRBX &m) { rxm_sfrb_pub_->publish(m); }, + 1); + } + + // Subscribe to RawX messages + if (getRosBoolean(node_, "publish.rxm.raw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRAWX &m) { rxm_raw_pub_->publish(m); }, + 1); + } +} + +void TimProduct::callbackTimTM2(const ublox_msgs::msg::TimTM2 &m) { + if (getRosBoolean(node_, "publish.tim.tm2")) { + // create time ref message and put in the data + t_ref_.header.stamp = node_->now(); + t_ref_.header.frame_id = frame_id_; + + t_ref_.time_ref = rclcpp::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); + + std::ostringstream src; + src << "TIM" << int(m.ch); + t_ref_.source = src.str(); + + t_ref_.header.stamp = node_->now(); // create a new timestamp + t_ref_.header.frame_id = frame_id_; + + timtm2_pub_->publish(m); + interrupt_time_pub_->publish(t_ref_); + } + +} + +void TimProduct::initializeRosDiagnostics() { + updater_->force_update(); +} + +} // namespace ublox_node diff --git a/ublox_gps/src/ublox_firmware.cpp b/ublox_gps/src/ublox_firmware.cpp new file mode 100644 index 00000000..ab273e31 --- /dev/null +++ b/ublox_gps/src/ublox_firmware.cpp @@ -0,0 +1,23 @@ +#include + +#include +#include + +#include +#include + +namespace ublox_node { + +// +// U-Blox Firmware (all versions) +// +UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, rclcpp::Node* node) : updater_(updater), gnss_(gnss), node_(node) +{ +} + +void UbloxFirmware::initializeRosDiagnostics() { + updater_->add("fix", this, &UbloxFirmware::fixDiagnostic); + updater_->force_update(); +} + +} // namespace ublox_node diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp new file mode 100644 index 00000000..65922b82 --- /dev/null +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -0,0 +1,246 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox Firmware Version 6 +// +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) +{ + if (getRosBoolean(node_, "publish.nav.posllh")) { + nav_pos_llh_pub_ = + node_->create_publisher("navposllh", 1); + } + + fix_pub_ = + node_->create_publisher("~/fix", 1); + + if (getRosBoolean(node_, "publish.nav.velned")) { + nav_vel_ned_pub_ = + node_->create_publisher("navvelned", 1); + } + + vel_pub_ = + node_->create_publisher("~/fix_velocity", + 1); + + if (getRosBoolean(node_, "publish.nav.sol")) { + nav_sol_pub_ = + node_->create_publisher("navsol", 1); + } + if (getRosBoolean(node_, "publish.nav.svinfo")) { + nav_svinfo_pub_ = + node_->create_publisher("navinfo", 1); + } + if (getRosBoolean(node_, "publish.mon.hw")) { + mon_hw_pub_ = + node_->create_publisher("monhw", 1); + } +} + +void UbloxFirmware6::getRosParams() { + // Fix Service type, used when publishing fix status messages + fix_status_service_ = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + + if (getRosBoolean(node_, "nmea.set")) { + if (!getRosUint(node_, "nmea.version", cfg_nmea_.version)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.version must be set"); + } + if (!getRosUint(node_, "nmea.num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.num_sv must be set"); + } + + // set flags + cfg_nmea_.flags = getRosBoolean(node_, "nmea.compat") ? ublox_msgs::msg::CfgNMEA6::FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.consider") ? ublox_msgs::msg::CfgNMEA6::FLAGS_CONSIDER : 0; + + // set filter + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? ublox_msgs::msg::CfgNMEA6::FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? ublox_msgs::msg::CfgNMEA6::FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? ublox_msgs::msg::CfgNMEA6::FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? ublox_msgs::msg::CfgNMEA6::FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.sbas") ? ublox_msgs::msg::CfgNMEA6::FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? ublox_msgs::msg::CfgNMEA6::FILTER_TRACK : 0; + } +} + +bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 7, ignoring GNSS settings"); + + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +void UbloxFirmware6::subscribe(std::shared_ptr gps) { + // Always subscribes to these messages, but may not publish to ROS topic + // Subscribe to Nav POSLLH + gps->subscribe(std::bind( + &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), 1); + gps->subscribe(std::bind( + // Subscribe to Nav SOL + &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), 1); + // Subscribe to Nav VELNED + gps->subscribe(std::bind( + &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); + + // Subscribe to Nav SVINFO + if (getRosBoolean(node_, "publish.nav.svinfo")) { + gps->subscribe([this](const ublox_msgs::msg::NavSVINFO &m) { nav_svinfo_pub_->publish(m); }, + kNavSvInfoSubscribeRate); + } + + // Subscribe to Mon HW + if (getRosBoolean(node_, "publish.mon.hw")) { + gps->subscribe([this](const ublox_msgs::msg::MonHW6 &m) { mon_hw_pub_->publish(m); }, + 1); + } +} + +void UbloxFirmware6::fixDiagnostic( + diagnostic_updater::DiagnosticStatusWrapper& stat) { + // Set the diagnostic level based on the fix status + if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_DEAD_RECKONING_ONLY) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Dead reckoning only"; + } else if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_2D_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "2D fix"; + } else if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_3D_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "3D fix"; + } else if (last_nav_sol_.gps_fix == + ublox_msgs::msg::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "GPS and dead reckoning combined"; + } else if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_TIME_ONLY_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "Time fix only"; + } + // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level + if (!(last_nav_sol_.flags & ublox_msgs::msg::NavSOL::FLAGS_GPS_FIX_OK)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message += ", fix not ok"; + } + // Raise diagnostic level to error if no fix + if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_NO_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "No fix"; + } + + // Add last fix position + stat.add("iTOW [ms]", last_nav_pos_.i_tow); + stat.add("Latitude [deg]", last_nav_pos_.lat * 1e-7); + stat.add("Longitude [deg]", last_nav_pos_.lon * 1e-7); + stat.add("Altitude [m]", last_nav_pos_.height * 1e-3); + stat.add("Height above MSL [m]", last_nav_pos_.h_msl * 1e-3); + stat.add("Horizontal Accuracy [m]", last_nav_pos_.h_acc * 1e-3); + stat.add("Vertical Accuracy [m]", last_nav_pos_.v_acc * 1e-3); + stat.add("# SVs used", static_cast(last_nav_sol_.num_sv)); +} + +void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m) { + if (getRosBoolean(node_, "publish.nav.posllh")) { + nav_pos_llh_pub_->publish(m); + } + + // Position message + if (m.i_tow == last_nav_vel_.i_tow) { + fix_.header.stamp = velocity_.header.stamp; // use last timestamp + } else { + fix_.header.stamp = node_->now(); // new timestamp + } + + fix_.header.frame_id = frame_id_; + fix_.latitude = m.lat * 1e-7; + fix_.longitude = m.lon * 1e-7; + fix_.altitude = m.height * 1e-3; + + if (last_nav_sol_.gps_fix >= ublox_msgs::msg::NavSOL::GPS_2D_FIX) { + fix_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + } else { + fix_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } + + // Convert from mm to m + const double var_h = pow(m.h_acc / 1000.0, 2); + const double var_v = pow(m.v_acc / 1000.0, 2); + + fix_.position_covariance[0] = var_h; + fix_.position_covariance[4] = var_h; + fix_.position_covariance[8] = var_v; + fix_.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + fix_.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + fix_pub_->publish(fix_); + last_nav_pos_ = m; + // update diagnostics + freq_diag_->diagnostic->tick(fix_.header.stamp); +} + +void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::msg::NavVELNED& m) { + if (getRosBoolean(node_, "publish.nav.velned")) { + nav_vel_ned_pub_->publish(m); + } + + // Example geometry message + if (m.i_tow == last_nav_pos_.i_tow) { + velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh + } else { + velocity_.header.stamp = node_->now(); // create a new timestamp + } + velocity_.header.frame_id = frame_id_; + + // convert to XYZ linear velocity + velocity_.twist.twist.linear.x = m.vel_e / 100.0; + velocity_.twist.twist.linear.y = m.vel_n / 100.0; + velocity_.twist.twist.linear.z = -m.vel_d / 100.0; + + const double var_speed = pow(m.s_acc / 100.0, 2); + + const int cols = 6; + velocity_.twist.covariance[cols * 0 + 0] = var_speed; + velocity_.twist.covariance[cols * 1 + 1] = var_speed; + velocity_.twist.covariance[cols * 2 + 2] = var_speed; + velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported + + vel_pub_->publish(velocity_); + last_nav_vel_ = m; +} + +void UbloxFirmware6::callbackNavSol(const ublox_msgs::msg::NavSOL& m) { + if (getRosBoolean(node_, "publish.nav.sol")) { + nav_sol_pub_->publish(m); + } + last_nav_sol_ = m; +} + +} // namespace ublox_node diff --git a/ublox_gps/src/ublox_firmware7.cpp b/ublox_gps/src/ublox_firmware7.cpp new file mode 100644 index 00000000..03dc009c --- /dev/null +++ b/ublox_gps/src/ublox_firmware7.cpp @@ -0,0 +1,199 @@ +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +// +// Ublox Firmware Version 7 +// +void UbloxFirmware7::getRosParams() { + // + // GNSS configuration + // + // GNSS enable/disable + enable_gps_ = getRosBoolean(node_, "gnss.gps"); + enable_glonass_ = getRosBoolean(node_, "gnss.glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss.qzss"); + + getRosUint(node_, "gnss.qzss_sig_cfg", qzss_sig_cfg_, + ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); + + if (enable_gps_ && !gnss_->isSupported("GPS")) { + RCLCPP_WARN(node_->get_logger(), "gnss/gps is true, but GPS GNSS is not supported by this device"); + } + if (enable_glonass_ && !gnss_->isSupported("GLO")) { + RCLCPP_WARN(node_->get_logger(), "gnss/glonass is true, but GLONASS is not %s", + "supported by this device"); + } + if (enable_qzss_ && !gnss_->isSupported("QZSS")) { + RCLCPP_WARN(node_->get_logger(), "gnss/qzss is true, but QZSS is not supported by this device"); + } + if (getRosBoolean(node_, "gnss.sbas") && !gnss_->isSupported("SBAS")) { + RCLCPP_WARN(node_->get_logger(), "gnss/sbas is true, but SBAS is not supported by this device"); + } + + if (getRosBoolean(node_, "gnss.galileo")) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 8, ignoring Galileo GNSS Settings"); + } + if (getRosBoolean(node_, "gnss.beidou")) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 8, ignoring BeiDou Settings"); + } + if (getRosBoolean(node_, "gnss.imes")) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 8, ignoring IMES GNSS Settings"); + } + + // Fix Service type, used when publishing fix status messages + fix_status_service_ = sensor_msgs::msg::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS; + + // + // NMEA Configuration + // + if (getRosBoolean(node_, "nmea.set")) { + bool compat, consider; + + if (!getRosUint(node_, "nmea.version", cfg_nmea_.nmea_version)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.version must be set"); + } + if (!getRosUint(node_, "nmea.num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.num_sv must be set"); + } + if (!getRosUint(node_, "nmea.sv_numbering", cfg_nmea_.sv_numbering)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.sv_numbering must be set"); + } + if (!node_->get_parameter("nmea.compat", compat)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.compat must be set"); + } + if (!node_->get_parameter("nmea.consider", consider)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.consider must be set"); + } + + // set flags + cfg_nmea_.flags = compat ? ublox_msgs::msg::CfgNMEA7::FLAGS_COMPAT : 0; + cfg_nmea_.flags |= consider ? ublox_msgs::msg::CfgNMEA7::FLAGS_CONSIDER : 0; + // set filter + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? ublox_msgs::msg::CfgNMEA7::FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? ublox_msgs::msg::CfgNMEA7::FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? ublox_msgs::msg::CfgNMEA7::FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? ublox_msgs::msg::CfgNMEA7::FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? ublox_msgs::msg::CfgNMEA7::FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? ublox_msgs::msg::CfgNMEA7::FILTER_TRACK : 0; + // set gnssToFilter + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_GLONASS : 0; + + getRosUint(node_, "nmea.main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea.gsv_talker_id", cfg_nmea_.gsv_talker_id); + } +} + +bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { + /** Configure the GNSS **/ + ublox_msgs::msg::CfgGNSS cfgGNSSRead; + if (gps->poll(cfgGNSSRead)) { + RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config."); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); + } else { + throw std::runtime_error("Failed to read the GNSS config."); + } + + ublox_msgs::msg::CfgGNSS cfgGNSSWrite; + cfgGNSSWrite.num_config_blocks = 1; // do services one by one + cfgGNSSWrite.num_trk_ch_hw = cfgGNSSRead.num_trk_ch_hw; + cfgGNSSWrite.num_trk_ch_use = cfgGNSSRead.num_trk_ch_use; + cfgGNSSWrite.msg_ver = 0; + + // configure GLONASS + if (gnss_->isSupported("GLO")) { + ublox_msgs::msg::CfgGNSSBlock block; + block.gnss_id = ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GLONASS; + block.res_trk_ch = ublox_msgs::msg::CfgGNSSBlock::RES_TRK_CH_GLONASS; + block.max_trk_ch = ublox_msgs::msg::CfgGNSSBlock::MAX_TRK_CH_GLONASS; + block.flags = enable_glonass_ ? ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_GLONASS_L1OF : 0; + cfgGNSSWrite.blocks.push_back(block); + if (!gps->configure(cfgGNSSWrite)) { + throw std::runtime_error(std::string("Failed to ") + + ((enable_glonass_) ? "enable" : "disable") + + " GLONASS."); + } + } + + if (gnss_->isSupported("QZSS")) { + // configure QZSS + ublox_msgs::msg::CfgGNSSBlock block; + block.gnss_id = ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_QZSS; + block.res_trk_ch = ublox_msgs::msg::CfgGNSSBlock::RES_TRK_CH_QZSS; + block.max_trk_ch = ublox_msgs::msg::CfgGNSSBlock::MAX_TRK_CH_QZSS; + block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; + cfgGNSSWrite.blocks[0] = block; + if (!gps->configure(cfgGNSSWrite)) { + throw std::runtime_error(std::string("Failed to ") + + ((enable_glonass_) ? "enable" : "disable") + + " QZSS."); + } + } + + if (gnss_->isSupported("SBAS")) { + // configure SBAS + ublox_msgs::msg::CfgGNSSBlock block; + block.gnss_id = ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_SBAS; + block.res_trk_ch = ublox_msgs::msg::CfgGNSSBlock::RES_TRK_CH_SBAS; + block.max_trk_ch = ublox_msgs::msg::CfgGNSSBlock::MAX_TRK_CH_SBAS; + block.flags = getRosBoolean(node_, "gnss.sbas") ? ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_SBAS_L1CA : 0; + cfgGNSSWrite.blocks[0] = block; + if (!gps->configure(cfgGNSSWrite)) { + throw std::runtime_error(std::string("Failed to ") + + (getRosBoolean(node_, "gnss.sbas") ? "enable" : "disable") + + " SBAS."); + } + } + + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +void UbloxFirmware7::subscribe(std::shared_ptr gps) { + // Subscribe to Nav PVT (always does so since fix information is published + // from this) + gps->subscribe(std::bind( + &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), + 1); + + // Subscribe to Nav SVINFO + if (getRosBoolean(node_, "publish.nav.svinfo")) { + gps->subscribe([this](const ublox_msgs::msg::NavSVINFO &m) { nav_svinfo_pub_->publish(m); }, + kNavSvInfoSubscribeRate); + } + + // Subscribe to Mon HW + if (getRosBoolean(node_, "publish.mon.hw")) { + gps->subscribe([this](const ublox_msgs::msg::MonHW &m) { mon_hw_pub_->publish(m); }, + 1); + } +} + +} // namespace ublox_node diff --git a/ublox_gps/src/ublox_firmware8.cpp b/ublox_gps/src/ublox_firmware8.cpp new file mode 100644 index 00000000..eb7e3ba7 --- /dev/null +++ b/ublox_gps/src/ublox_firmware8.cpp @@ -0,0 +1,256 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +// +// Ublox Version 8 +// +void UbloxFirmware8::getRosParams() { + // UPD SOS configuration + clear_bbr_ = getRosBoolean(node_, "clear_bbr"); + save_on_shutdown_ = getRosBoolean(node_, "save_on_shutdown"); + + // GNSS enable/disable + enable_gps_ = getRosBoolean(node_, "gnss.gps"); + enable_galileo_ = getRosBoolean(node_, "gnss.galileo"); + enable_beidou_ = getRosBoolean(node_, "gnss.beidou"); + enable_imes_ = getRosBoolean(node_, "gnss.imes"); + enable_glonass_ = getRosBoolean(node_, "gnss.glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss.qzss"); + + // QZSS Signal Configuration + getRosUint(node_, "gnss.qzss_sig_cfg", qzss_sig_cfg_, + ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); + + if (enable_gps_ && !gnss_->isSupported("GPS")) { + RCLCPP_WARN(node_->get_logger(), "gnss.gps is true, but GPS GNSS is not supported by %s", + "this device"); + } + if (enable_glonass_ && !gnss_->isSupported("GLO")) { + RCLCPP_WARN(node_->get_logger(), "gnss.glonass is true, but GLONASS is not supported by %s", + "this device"); + } + if (enable_galileo_ && !gnss_->isSupported("GAL")) { + RCLCPP_WARN(node_->get_logger(), "gnss.galileo is true, but Galileo GNSS is not supported %s", + "by this device"); + } + if (enable_beidou_ && !gnss_->isSupported("BDS")) { + RCLCPP_WARN(node_->get_logger(), "gnss.beidou is true, but Beidou GNSS is not supported %s", + "by this device"); + } + if (enable_imes_ && !gnss_->isSupported("IMES")) { + RCLCPP_WARN(node_->get_logger(), "gnss.imes is true, but IMES GNSS is not supported by %s", + "this device"); + } + if (enable_qzss_ && !gnss_->isSupported("QZSS")) { + RCLCPP_WARN(node_->get_logger(), "gnss.qzss is true, but QZSS is not supported by this device"); + } + if (getRosBoolean(node_, "gnss.sbas") && !gnss_->isSupported("SBAS")) { + RCLCPP_WARN(node_->get_logger(), "gnss.sbas is true, but SBAS is not supported by this device"); + } + + // Fix Service type, used when publishing fix status messages + fix_status_service_ = sensor_msgs::msg::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS + + (enable_beidou_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_COMPASS + + (enable_galileo_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_GALILEO; + + // + // NMEA Configuration + // + if (getRosBoolean(node_, "nmea.set")) { + bool compat, consider; + cfg_nmea_.version = ublox_msgs::msg::CfgNMEA::VERSION; // message version + + // Verify that parameters are set + if (!getRosUint(node_, "nmea.version", cfg_nmea_.nmea_version)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.version must be set"); + } + if (!getRosUint(node_, "nmea.num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.num_sv must be set"); + } + if (!getRosUint(node_, "nmea.sv_numbering", cfg_nmea_.sv_numbering)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.sv_numbering must be set"); + } + if (!node_->get_parameter("nmea.compat", compat)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.compat must be set"); + } + if (!node_->get_parameter("nmea.consider", consider)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.consider must be set"); + } + + // set flags + cfg_nmea_.flags = compat ? ublox_msgs::msg::CfgNMEA::FLAGS_COMPAT : 0; + cfg_nmea_.flags |= consider ? ublox_msgs::msg::CfgNMEA::FLAGS_CONSIDER : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.limit82") ? ublox_msgs::msg::CfgNMEA::FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.high_prec") ? ublox_msgs::msg::CfgNMEA::FLAGS_HIGH_PREC : 0; + // set filter + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? ublox_msgs::msg::CfgNMEA::FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? ublox_msgs::msg::CfgNMEA::FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? ublox_msgs::msg::CfgNMEA::FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? ublox_msgs::msg::CfgNMEA::FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? ublox_msgs::msg::CfgNMEA::FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? ublox_msgs::msg::CfgNMEA::FILTER_TRACK : 0; + // set gnssToFilter + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.beidou") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_BEIDOU : 0; + + getRosUint(node_, "nmea.main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea.gsv_talker_id", cfg_nmea_.gsv_talker_id); + + std::vector bds_talker_id; + getRosUint(node_, "nmea.bds_talker_id", bds_talker_id); + cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; + cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; + } +} + +bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { + if (clear_bbr_) { + // clear flash memory + if (!gps->clearBbr()) { + RCLCPP_ERROR(node_->get_logger(), "u-blox failed to clear flash memory"); + } + } + + gps->setSaveOnShutdown(save_on_shutdown_); + + // + // Configure the GNSS, only if the configuration is different + // + // First, get the current GNSS configuration + ublox_msgs::msg::CfgGNSS cfg_gnss; + if (gps->poll(cfg_gnss)) { + RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config."); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); + } else { + throw std::runtime_error("Failed to read the GNSS config."); + } + + // Then, check the configuration for each GNSS. If it is different, change it. + bool correct = true; + for (size_t i = 0; i < cfg_gnss.blocks.size(); i++) { // NOLINT(modernize-loop-convert) + ublox_msgs::msg::CfgGNSSBlock block = cfg_gnss.blocks[i]; + if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GPS + && enable_gps_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_gps_; + RCLCPP_DEBUG(node_->get_logger(), "GPS Configuration is different"); + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_SBAS + && getRosBoolean(node_, "gnss.sbas") != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | getRosBoolean(node_, "gnss.sbas"); + RCLCPP_DEBUG(node_->get_logger(), "SBAS Configuration is different"); + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GALILEO + && enable_galileo_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_galileo_; + RCLCPP_DEBUG(node_->get_logger(), "Galileo GNSS Configuration is different"); + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_BEIDOU + && enable_beidou_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_beidou_; + RCLCPP_DEBUG(node_->get_logger(), "BeiDou Configuration is different"); + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_IMES + && enable_imes_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_imes_; + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_QZSS + && (enable_qzss_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) + || (enable_qzss_ + && qzss_sig_cfg_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_SIG_CFG_MASK)))) { + RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration is different %u, %u", + block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE, + enable_qzss_); + correct = false; + RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration: %u", block.flags); + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_qzss_; + RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration: %u", cfg_gnss.blocks[i].flags); + if (enable_qzss_) { + // Only change sig cfg if enabling + cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; + } + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GLONASS + && enable_glonass_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_glonass_; + RCLCPP_DEBUG(node_->get_logger(), "GLONASS Configuration is different"); + } + } + + // If the GNSS is already configured correctly, do not re-configure GNSS + // since this requires a cold reset + if (correct) { + RCLCPP_DEBUG(node_->get_logger(), "U-Blox GNSS configuration is correct. GNSS not re-configured."); + } else if (!gps->configGnss(cfg_gnss, std::chrono::seconds(15))) { + throw std::runtime_error(std::string("Failed to cold reset device ") + + "after configuring GNSS"); + } + + // + // NMEA config + // + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +void UbloxFirmware8::subscribe(std::shared_ptr gps) { + // Subscribe to Nav PVT + gps->subscribe( + std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); + + // Subscribe to Nav SAT messages + if (getRosBoolean(node_, "publish.nav.sat")) { + gps->subscribe([this](const ublox_msgs::msg::NavSAT &m) { nav_sat_pub_->publish(m); }, + kNavSvInfoSubscribeRate); + } + + // Subscribe to Mon HW + if (getRosBoolean(node_, "publish.mon.hw")) { + gps->subscribe([this](const ublox_msgs::msg::MonHW &m) { mon_hw_pub_->publish(m); }, + 1); + } + + // Subscribe to RTCM messages + if (getRosBoolean(node_, "publish.rxm.rtcm")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRTCM &m) { rxm_rtcm_pub_->publish(m); }, + 1); + } +} + +} // namespace ublox_node diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp new file mode 100644 index 00000000..1eb3ddf4 --- /dev/null +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -0,0 +1,122 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace ublox_node { + +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) +{ +} + +bool UbloxFirmware9::configureUblox(std::shared_ptr gps) +{ + if (clear_bbr_) + { + // clear flash memory + if (!gps->clearBbr()) + { + RCLCPP_ERROR(node_->get_logger(), "u-blox failed to clear flash memory"); + } + } + + gps->setSaveOnShutdown(save_on_shutdown_); + + // + // Configure the GNSS, only if the configuration is different + // + // First, get the current GNSS configuration + ublox_msgs::msg::CfgGNSS cfg_gnss; + if (gps->poll(cfg_gnss)) + { + RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config."); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); + } + else + { + throw std::runtime_error("Failed to read the GNSS config."); + } + + ublox_msgs::msg::CfgVALSET cfg_signal; + cfg_signal.layers = ublox_msgs::msg::CfgVALSET::LAYER_RAM; + + using signal = ublox_msgs::msg::CfgVALSETCfgdata; + + // Configure GPS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_ENABLE, enable_gps_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L1CA_ENABLE, enable_gps_)); + if (gnss_->isSupported("GPS_L2C")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L2C_ENABLE, enable_gps_)); + } + + // Configure SBAS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_ENABLE, enable_gps_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_L1CA_ENABLE, enable_gps_)); + + // Configure Galileo Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_ENABLE, enable_galileo_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E1_ENABLE, enable_galileo_)); + if (gnss_->isSupported("GAL_E5B")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E5B_ENABLE, enable_galileo_)); + } + + // Configure Beidou Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_ENABLE, enable_beidou_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B1_ENABLE, enable_beidou_)); + if (gnss_->isSupported("BDS_B2")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B2_ENABLE, enable_beidou_)); + } + + // Configure QZSS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_ENABLE, enable_qzss_ && enable_gps_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L1CA_ENABLE, enable_qzss_ && enable_gps_)); + if (gnss_->isSupported("QZSS_L2C")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L2C_ENABLE, enable_qzss_ && enable_gps_)); + } + + // Configure GLONASS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_ENABLE, enable_glonass_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L1_ENABLE, enable_glonass_)); + if (gnss_->isSupported("GLO_L2")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L2_ENABLE, enable_glonass_)); + } + + RCLCPP_DEBUG(node_->get_logger(), "Ready to configure"); + gps->configure(cfg_signal, false); + + // + // NMEA config + // + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) + { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t signalID, bool enable) +{ + ublox_msgs::msg::CfgVALSETCfgdata signalConfig; + signalConfig.key = signalID; + signalConfig.data.resize(1); + signalConfig.data[0] = enable; + return signalConfig; +} + + +} // namespace ublox_node diff --git a/ublox_msgs/CHANGELOG.rst b/ublox_msgs/CHANGELOG.rst index a1a4ebfd..b61a7f98 100644 --- a/ublox_msgs/CHANGELOG.rst +++ b/ublox_msgs/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package ublox_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.0 (2022-04-13) +------------------ +* Revamp the building of the driver for modern ROS 2 practices. +* Contributors: Chao Qu, Chris Lalancette + +2.0.0 (2020-10-13) +------------------ +* Fixes to compile on Foxy. +* Fix heading output to comply with REP-103 +* Fixes pointed out by clang-tidy. +* Small style fixups. +* Port ublox_msgs to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Move serialization into the ublox_serialization module. +* More rearrangement of header files to make a more sane structure. +* Move the directory structure around just a bit. +* Rearrange messages. +* Remove boost from ublox_msgs.hpp serialization. +* Remove trailing whitespace in all files. +* Rename header files to have .hpp extension. +* Contributors: Chao Qu, Chris Lalancette + 1.2.0 (2019-11-19) ------------------ * Add support for ZED-F9P new RELPOSNED message and provide heading output diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 3dbf3dab..4e3db305 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -1,37 +1,139 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation ublox_serialization std_msgs sensor_msgs) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_message_files(DIRECTORY msg) -generate_messages(DEPENDENCIES std_msgs sensor_msgs) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS message_runtime ublox_serialization std_msgs sensor_msgs) +find_package(ament_cmake_ros REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ublox_serialization REQUIRED) -include_directories(${PROJECT_SOURCE_DIR}/include) -include_directories(${ublox_serialization_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME} src/ublox_msgs.cpp) - -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +set(msg_files + "msg/Ack.msg" + "msg/AidALM.msg" + "msg/AidEPH.msg" + "msg/AidHUI.msg" + "msg/CfgANT.msg" + "msg/CfgCFG.msg" + "msg/CfgDAT.msg" + "msg/CfgDGNSS.msg" + "msg/CfgGNSSBlock.msg" + "msg/CfgGNSS.msg" + "msg/CfgHNR.msg" + "msg/CfgINFBlock.msg" + "msg/CfgINF.msg" + "msg/CfgMSG.msg" + "msg/CfgNAV5.msg" + "msg/CfgNAVX5.msg" + "msg/CfgNMEA6.msg" + "msg/CfgNMEA7.msg" + "msg/CfgNMEA.msg" + "msg/CfgPRT.msg" + "msg/CfgRATE.msg" + "msg/CfgRST.msg" + "msg/CfgSBAS.msg" + "msg/CfgTMODE3.msg" + "msg/CfgUSB.msg" + "msg/CfgVALDEL.msg" + "msg/CfgVALGET.msg" + "msg/CfgVALSET.msg" + "msg/CfgVALSETCfgdata.msg" + "msg/EsfINS.msg" + "msg/EsfMEAS.msg" + "msg/EsfRAWBlock.msg" + "msg/EsfRAW.msg" + "msg/EsfSTATUS.msg" + "msg/EsfSTATUSSens.msg" + "msg/HnrPVT.msg" + "msg/Inf.msg" + "msg/MgaGAL.msg" + "msg/MonGNSS.msg" + "msg/MonHW6.msg" + "msg/MonHW.msg" + "msg/MonVERExtension.msg" + "msg/MonVER.msg" + "msg/NavATT.msg" + "msg/NavCLOCK.msg" + "msg/NavCOV.msg" + "msg/NavDGPS.msg" + "msg/NavDGPSSV.msg" + "msg/NavDOP.msg" + "msg/NavPOSECEF.msg" + "msg/NavPOSLLH.msg" + "msg/NavPVT7.msg" + "msg/NavPVT.msg" + "msg/NavRELPOSNED9.msg" + "msg/NavRELPOSNED.msg" + "msg/NavSAT.msg" + "msg/NavSATSV.msg" + "msg/NavSBAS.msg" + "msg/NavSBASSV.msg" + "msg/NavSOL.msg" + "msg/NavSTATUS.msg" + "msg/NavSVINFO.msg" + "msg/NavSVINFOSV.msg" + "msg/NavSVIN.msg" + "msg/NavTIMEGPS.msg" + "msg/NavTIMEUTC.msg" + "msg/NavVELECEF.msg" + "msg/NavVELNED.msg" + "msg/RxmALM.msg" + "msg/RxmEPH.msg" + "msg/RxmRAW.msg" + "msg/RxmRAWSV.msg" + "msg/RxmRAWXMeas.msg" + "msg/RxmRAWX.msg" + "msg/RxmRTCM.msg" + "msg/RxmSFRB.msg" + "msg/RxmSFRBX.msg" + "msg/RxmSVSI.msg" + "msg/RxmSVSISV.msg" + "msg/TimTM2.msg" + "msg/UpdSOSAck.msg" + "msg/UpdSOS.msg" ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES + sensor_msgs + std_msgs ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") +if(cpp_typesupport_target) + add_library(${PROJECT_NAME}_lib src/ublox_msgs.cpp) + target_include_directories(${PROJECT_NAME}_lib PRIVATE + "$" + "$") + target_link_libraries(${PROJECT_NAME}_lib + ${cpp_typesupport_target} + ublox_serialization::ublox_serialization + ) -install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE -) + install(TARGETS ${PROJECT_NAME}_lib EXPORT ${PROJECT_NAME}_lib + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + install(DIRECTORY include/ + DESTINATION "include/${PROJECT_NAME}" + ) + + ament_export_include_directories("include/${PROJECT_NAME}") + ament_export_libraries(${PROJECT_NAME}_lib) + ament_export_targets(${PROJECT_NAME}_lib) +endif() + +ament_export_dependencies(rosidl_default_runtime sensor_msgs std_msgs ublox_serialization) + +ament_package() diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.h b/ublox_msgs/include/ublox/serialization/ublox_msgs.h deleted file mode 100644 index fc7b2c41..00000000 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.h +++ /dev/null @@ -1,883 +0,0 @@ -//============================================================================== -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * 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. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, 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 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 UBLOX_SERIALIZATION_UBLOX_MSGS_H -#define UBLOX_SERIALIZATION_UBLOX_MSGS_H - -#include -#include -#include - -/// -/// This file declares custom serializers for u-blox messages with dynamic -/// lengths and messages where the get/set messages have different sizes, but -/// share the same parameters, such as CfgDAT. -/// - -namespace ublox { - -/// -/// @brief Serializes the CfgDAT message which has a different length for -/// get/set. -/// -template -struct Serializer > { - typedef boost::call_traits > - CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.datumNum); - stream.next(m.datumName); - stream.next(m.majA); - stream.next(m.flat); - stream.next(m.dX); - stream.next(m.dY); - stream.next(m.dZ); - stream.next(m.rotX); - stream.next(m.rotY); - stream.next(m.rotZ); - stream.next(m.scale); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - // this is the size of CfgDAT set messages - // serializedLength is only used for writes so this is ok - return 44; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - // ignores datumNum & datumName - stream.next(m.majA); - stream.next(m.flat); - stream.next(m.dX); - stream.next(m.dY); - stream.next(m.dZ); - stream.next(m.rotX); - stream.next(m.rotY); - stream.next(m.rotZ); - stream.next(m.scale); - } -}; - -/// -/// @brief Serializes the CfgGNSS message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::CfgGNSS_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.msgVer); - stream.next(m.numTrkChHw); - stream.next(m.numTrkChUse); - stream.next(m.numConfigBlocks); - m.blocks.resize(m.numConfigBlocks); - for(std::size_t i = 0; i < m.blocks.size(); ++i) - ros::serialization::deserialize(stream, m.blocks[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 4 + 8 * m.numConfigBlocks; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.blocks.size() != m.numConfigBlocks) { - ROS_ERROR("CfgGNSS numConfigBlocks must equal blocks size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.msgVer); - stream.next(m.numTrkChHw); - stream.next(m.numTrkChUse); - stream.next( - static_cast(m.blocks.size())); - for(std::size_t i = 0; i < m.blocks.size(); ++i) - ros::serialization::serialize(stream, m.blocks[i]); - } -}; - -/// -/// @brief Serializes the CfgInf message which has a repeated block. -/// -template -struct Serializer > { - typedef boost::call_traits > - CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - int num_blocks = count / 10; - m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) - ros::serialization::deserialize(stream, m.blocks[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 10 * m.blocks.size(); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.blocks.size(); ++i) - ros::serialization::serialize(stream, m.blocks[i]); - } -}; - -/// -/// @brief Serializes the Inf message which has a dynamic length string. -/// -template -struct Serializer > { - typedef boost::call_traits > CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - m.str.resize(count); - for (int i = 0; i < count; ++i) - ros::serialization::deserialize(stream, m.str[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return m.str.size(); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.str.size(); ++i) - ros::serialization::serialize(stream, m.str[i]); - } -}; - -/// -/// @brief Serializes the MonVER message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::MonVER_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.swVersion); - stream.next(m.hwVersion); - - m.extension.clear(); - int N = (count - 40) / 30; - m.extension.reserve(N); - typename Msg::_extension_type::value_type ext; - for (int i = 0; i < N; i++) { - // Read each extension string - stream.next(ext); - m.extension.push_back(ext); - } - } - - static uint32_t serializedLength(typename CallTraits::param_type m) { - return 40 + (30 * m.extension.size()); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.swVersion); - stream.next(m.hwVersion); - for(std::size_t i = 0; i < m.extension.size(); ++i) - ros::serialization::serialize(stream, m.extension[i]); - } -}; - -/// -/// @brief Serializes the NavDGPS message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::NavDGPS_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.age); - stream.next(m.baseId); - stream.next(m.baseHealth); - stream.next(m.numCh); - stream.next(m.status); - stream.next(m.reserved1); - m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::deserialize(stream, m.sv[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 16 + 12 * m.numCh; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { - ROS_ERROR("NavDGPS numCh must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(m.age); - stream.next(m.baseId); - stream.next(m.baseHealth); - stream.next(static_cast(m.sv.size())); - stream.next(m.status); - stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::serialize(stream, m.sv[i]); - } -}; - - -/// -/// @brief Serializes the NavSBAS message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::NavSBAS_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.geo); - stream.next(m.mode); - stream.next(m.sys); - stream.next(m.service); - stream.next(m.cnt); - stream.next(m.reserved0); - m.sv.resize(m.cnt); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::deserialize(stream, m.sv[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 12 + 12 * m.cnt; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.cnt) { - ROS_ERROR("NavSBAS cnt must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(m.geo); - stream.next(m.mode); - stream.next(m.sys); - stream.next(m.service); - stream.next(static_cast(m.sv.size())); - stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::serialize(stream, m.sv[i]); - } -}; - -/// -/// @brief Serializes the NavSAT message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::NavSAT_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.version); - stream.next(m.numSvs); - stream.next(m.reserved0); - m.sv.resize(m.numSvs); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::deserialize(stream, m.sv[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + 12 * m.numSvs; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSvs) { - ROS_ERROR("NavSAT numSvs must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(m.version); - stream.next(static_cast(m.sv.size())); - stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::serialize(stream, m.sv[i]); - } -}; - -/// -/// @brief Serializes the NavDGPS message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::NavSVINFO_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.numCh); - stream.next(m.globalFlags); - stream.next(m.reserved2); - m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::deserialize(stream, m.sv[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + 12 * m.numCh; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { - ROS_ERROR("NavSVINFO numCh must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(static_cast(m.sv.size())); - stream.next(m.globalFlags); - stream.next(m.reserved2); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::serialize(stream, m.sv[i]); - } -}; - -/// -/// @brief Serializes the RxmRAW message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::RxmRAW_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.rcvTOW); - stream.next(m.week); - stream.next(m.numSV); - stream.next(m.reserved1); - m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::deserialize(stream, m.sv[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + 24 * m.numSV; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { - ROS_ERROR("RxmRAW numSV must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.rcvTOW); - stream.next(m.week); - stream.next(static_cast(m.sv.size())); - stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::serialize(stream, m.sv[i]); - } -}; - -/// -/// @brief Serializes the RxmRAWX message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::RxmRAWX_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.rcvTOW); - stream.next(m.week); - stream.next(m.leapS); - stream.next(m.numMeas); - stream.next(m.recStat); - stream.next(m.version); - stream.next(m.reserved1); - m.meas.resize(m.numMeas); - for(std::size_t i = 0; i < m.meas.size(); ++i) - ros::serialization::deserialize(stream, m.meas[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 16 + 32 * m.numMeas; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.meas.size() != m.numMeas) { - ROS_ERROR("RxmRAWX numMeas must equal meas size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.rcvTOW); - stream.next(m.week); - stream.next(m.leapS); - stream.next(static_cast(m.meas.size())); - stream.next(m.recStat); - stream.next(m.version); - stream.next(m.reserved1); - for(std::size_t i = 0; i < m.meas.size(); ++i) - ros::serialization::serialize(stream, m.meas[i]); - } -}; - -/// -/// @brief Serializes the RxmSFRBX message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::RxmSFRBX_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.gnssId); - stream.next(m.svId); - stream.next(m.reserved0); - stream.next(m.freqId); - stream.next(m.numWords); - stream.next(m.chn); - stream.next(m.version); - stream.next(m.reserved1); - m.dwrd.resize(m.numWords); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) - ros::serialization::deserialize(stream, m.dwrd[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + 4 * m.numWords; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.dwrd.size() != m.numWords) { - ROS_ERROR("RxmSFRBX numWords must equal dwrd size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.gnssId); - stream.next(m.svId); - stream.next(m.reserved0); - stream.next(m.freqId); - stream.next(static_cast(m.dwrd.size())); - stream.next(m.chn); - stream.next(m.version); - stream.next(m.reserved1); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) - ros::serialization::serialize(stream, m.dwrd[i]); - } -}; - -/// -/// @brief Serializes the RxmSVSI message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::RxmSVSI_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.week); - stream.next(m.numVis); - stream.next(m.numSV); - m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::deserialize(stream, m.sv[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + 6 * m.numSV; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { - ROS_ERROR("RxmSVSI numSV must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(m.week); - stream.next(m.numVis); - stream.next(static_cast(m.sv.size())); - for(std::size_t i = 0; i < m.sv.size(); ++i) - ros::serialization::serialize(stream, m.sv[i]); - } -}; - -/// -/// @brief Serializes the RxmALM message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::RxmALM_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.svid); - stream.next(m.week); - - m.dwrd.clear(); - if(count == 40) { - typename Msg::_dwrd_type::value_type temp; - m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp); - m.dwrd.push_back(temp); - } - } - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + (4 * m.dwrd.size()); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.svid); - stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) - ros::serialization::serialize(stream, m.dwrd[i]); - } -}; - -/// -/// @brief Serializes the RxmEPH message which has a repeated block. -/// -template -struct Serializer > -{ - typedef ublox_msgs::RxmEPH_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.svid); - stream.next(m.how); - m.sf1d.clear(); - m.sf2d.clear(); - m.sf3d.clear(); - - if (count == 104) { - typename Msg::_sf1d_type::value_type temp1; - typename Msg::_sf2d_type::value_type temp2; - typename Msg::_sf3d_type::value_type temp3; - - m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp1); - m.sf1d.push_back(temp1); - } - m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp2); - m.sf2d.push_back(temp2); - } - m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp3); - m.sf3d.push_back(temp3); - } - } - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.svid); - stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) - ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) - ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) - ros::serialization::serialize(stream, m.sf3d[i]); - } -}; - -/// -/// @brief Serializes the AidALM message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::AidALM_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.svid); - stream.next(m.week); - - m.dwrd.clear(); - if (count == 40) { - typename Msg::_dwrd_type::value_type temp; - m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp); - m.dwrd.push_back(temp); - } - } - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + (4 * m.dwrd.size()); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.svid); - stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) - ros::serialization::serialize(stream, m.dwrd[i]); - } -}; - -/// -/// @brief Serializes the AidEPH message which has a repeated block. -/// -template -struct Serializer > -{ - typedef ublox_msgs::AidEPH_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.svid); - stream.next(m.how); - m.sf1d.clear(); - m.sf2d.clear(); - m.sf3d.clear(); - - if (count == 104) { - typename Msg::_sf1d_type::value_type temp1; - typename Msg::_sf2d_type::value_type temp2; - typename Msg::_sf3d_type::value_type temp3; - m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp1); - m.sf1d.push_back(temp1); - } - m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp2); - m.sf2d.push_back(temp2); - } - m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { - stream.next(temp3); - m.sf3d.push_back(temp3); - } - } - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.svid); - stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) - ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) - ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) - ros::serialization::serialize(stream, m.sf3d[i]); - } -}; - -/// -/// @brief Serializes the EsfMEAS message which has a repeated block and an -/// optional block. -/// -template -struct Serializer > { - typedef boost::call_traits > - CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.timeTag); - stream.next(m.flags); - stream.next(m.id); - - bool calib_valid = m.flags & m.FLAGS_CALIB_T_TAG_VALID; - int data_size = (count - (calib_valid ? 12 : 8)) / 4; - // Repeating block - m.data.resize(data_size); - for(std::size_t i = 0; i < data_size; ++i) - ros::serialization::deserialize(stream, m.data[i]); - // Optional block - if(calib_valid) { - m.calibTtag.resize(1); - ros::serialization::deserialize(stream, m.calibTtag[0]); - } - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 4 + 8 * m.data.size() + 4 * m.calibTtag.size(); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.timeTag); - stream.next(m.flags); - stream.next(m.id); - for(std::size_t i = 0; i < m.data.size(); ++i) - ros::serialization::serialize(stream, m.data[i]); - for(std::size_t i = 0; i < m.calibTtag.size(); ++i) - ros::serialization::serialize(stream, m.calibTtag[i]); - } -}; - -/// -/// @brief Serializes the EsfRAW message which has a repeated block. -/// -template -struct Serializer > { - typedef boost::call_traits > - CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.reserved0); - m.blocks.clear(); - int num_blocks = (count - 4) / 8; - m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) - ros::serialization::deserialize(stream, m.blocks[i]); - } - - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 4 + 8 * m.blocks.size(); - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - ros::serialization::OStream stream(data, size); - stream.next(m.reserved0); - for(std::size_t i = 0; i < m.blocks.size(); ++i) - ros::serialization::serialize(stream, m.blocks[i]); - } -}; - -/// -/// @brief Serializes the EsfSTATUS message which has a repeated block. -/// -template -struct Serializer > { - typedef ublox_msgs::EsfSTATUS_ Msg; - typedef boost::call_traits CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.version); - stream.next(m.fusionMode); - stream.next(m.reserved2); - stream.next(m.numSens); - m.sens.resize(m.numSens); - for(std::size_t i = 0; i < m.sens.size(); ++i) - ros::serialization::deserialize(stream, m.sens[i]); - } - - static uint32_t serializedLength (typename CallTraits::param_type m) { - return 16 + 4 * m.numSens; - } - - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sens.size() != m.numSens) { - ROS_ERROR("Writing EsfSTATUS message: numSens must equal size of sens"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(m.version); - stream.next(m.fusionMode); - stream.next(m.reserved2); - stream.next(static_cast(m.sens.size())); - for(std::size_t i = 0; i < m.sens.size(); ++i) - ros::serialization::serialize(stream, m.sens[i]); - } -}; - - -} // namespace ublox - -#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_H diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp new file mode 100644 index 00000000..14418e39 --- /dev/null +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -0,0 +1,3061 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 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 UBLOX_MSGS_SERIALIZATION_HPP +#define UBLOX_MSGS_SERIALIZATION_HPP + +#include + +#include +#include + +/// +/// This file declares custom serializers for u-blox messages with dynamic +/// lengths and messages where the get/set messages have different sizes, but +/// share the same parameters, such as CfgDAT. +/// + +namespace ublox { + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::Ack_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.cls_id); + stream.next(m.msg_id); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::Ack_ &m) { + (void)m; + return 2; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::Ack_ &m) { + UbloxOStream stream(data, size); + stream.next(m.cls_id); + stream.next(m.msg_id); + } +}; + +/// +/// @brief Serializes the AidALM message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::AidALM_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.svid); + stream.next(m.week); + + m.dwrd.clear(); + if (count == 40) { + typename ublox_msgs::msg::AidALM_::_dwrd_type::value_type temp; + m.dwrd.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp); + m.dwrd.push_back(temp); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::AidALM_ &m) { + return 8 + (4 * m.dwrd.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::AidALM_ &m) { + UbloxOStream stream(data, size); + stream.next(m.svid); + stream.next(m.week); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + serialize(stream, m.dwrd[i]); + } + } +}; + +/// +/// @brief Serializes the AidEPH message which has a repeated block. +/// +template +struct UbloxSerializer > +{ + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::AidEPH_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.svid); + stream.next(m.how); + m.sf1d.clear(); + m.sf2d.clear(); + m.sf3d.clear(); + + if (count == 104) { + typename ublox_msgs::msg::AidEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::msg::AidEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::msg::AidEPH_::_sf3d_type::value_type temp3; + m.sf1d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp1); + m.sf1d.push_back(temp1); + } + m.sf2d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp2); + m.sf2d.push_back(temp2); + } + m.sf3d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp3); + m.sf3d.push_back(temp3); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::AidEPH_ &m) { + return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::AidEPH_ &m) { + UbloxOStream stream(data, size); + stream.next(m.svid); + stream.next(m.how); + for (std::size_t i = 0; i < m.sf1d.size(); ++i) { + serialize(stream, m.sf1d[i]); + } + for (std::size_t i = 0; i < m.sf2d.size(); ++i) { + serialize(stream, m.sf2d[i]); + } + for (std::size_t i = 0; i < m.sf3d.size(); ++i) { + serialize(stream, m.sf3d[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::AidHUI_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.health); + stream.next(m.utc_a0); + stream.next(m.utc_a1); + stream.next(m.utc_tow); + stream.next(m.utc_wnt); + stream.next(m.utc_dn); + stream.next(m.utc_lsf); + stream.next(m.utc_spare); + stream.next(m.klob_a0); + stream.next(m.klob_a1); + stream.next(m.klob_a2); + stream.next(m.klob_a3); + stream.next(m.klob_b0); + stream.next(m.klob_b1); + stream.next(m.klob_b2); + stream.next(m.klob_b3); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::AidHUI_ &m) { + (void)m; + return 72; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::AidHUI_ &m) { + UbloxOStream stream(data, size); + stream.next(m.health); + stream.next(m.utc_a0); + stream.next(m.utc_a1); + stream.next(m.utc_tow); + stream.next(m.utc_wnt); + stream.next(m.utc_dn); + stream.next(m.utc_lsf); + stream.next(m.utc_spare); + stream.next(m.klob_a0); + stream.next(m.klob_a1); + stream.next(m.klob_a2); + stream.next(m.klob_a3); + stream.next(m.klob_b0); + stream.next(m.klob_b1); + stream.next(m.klob_b2); + stream.next(m.klob_b3); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgCFG_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.clear_mask); + stream.next(m.save_mask); + stream.next(m.load_mask); + stream.next(m.device_mask); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgCFG_ &m) { + (void)m; + return 13; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgCFG_ &m) { + UbloxOStream stream(data, size); + stream.next(m.clear_mask); + stream.next(m.save_mask); + stream.next(m.load_mask); + stream.next(m.device_mask); + } +}; + +/// +/// @brief Serializes the CfgDAT message which has a different length for +/// get/set. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgDAT_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.datum_num); + stream.next(m.datum_name); + stream.next(m.maj_a); + stream.next(m.flat); + stream.next(m.d_x); + stream.next(m.d_y); + stream.next(m.d_z); + stream.next(m.rot_x); + stream.next(m.rot_y); + stream.next(m.rot_z); + stream.next(m.scale); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgDAT_ &m) { + // this is the size of CfgDAT set messages + // serializedLength is only used for writes so this is ok + (void)m; + return 44; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgDAT_ &m) { + UbloxOStream stream(data, size); + // ignores datumNum & datumName + stream.next(m.maj_a); + stream.next(m.flat); + stream.next(m.d_x); + stream.next(m.d_y); + stream.next(m.d_z); + stream.next(m.rot_x); + stream.next(m.rot_y); + stream.next(m.rot_z); + stream.next(m.scale); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgDGNSS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.dgnss_mode); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgDGNSS_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgDGNSS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.dgnss_mode); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::CfgGNSSBlock_ & m) { + stream.next(m.gnss_id); + stream.next(m.res_trk_ch); + stream.next(m.max_trk_ch); + stream.next(m.reserved1); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgGNSSBlock_ & m) { + (void)m; + return 8; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::CfgGNSSBlock_ & m) { + stream.next(m.gnss_id); + stream.next(m.res_trk_ch); + stream.next(m.max_trk_ch); + stream.next(m.reserved1); + stream.next(m.flags); + } +}; + +/// +/// @brief Serializes the CfgGNSS message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgGNSS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.msg_ver); + stream.next(m.num_trk_ch_hw); + stream.next(m.num_trk_ch_use); + stream.next(m.num_config_blocks); + m.blocks.resize(m.num_config_blocks); + for (std::size_t i = 0; i < m.blocks.size(); ++i) { + deserialize(stream, m.blocks[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgGNSS_ &m) { + return 4 + 8 * m.num_config_blocks; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgGNSS_ &m) { + if (m.blocks.size() != m.num_config_blocks) { + // ROS_ERROR("CfgGNSS num_config_blocks must equal blocks size"); + } + UbloxOStream stream(data, size); + stream.next(m.msg_ver); + stream.next(m.num_trk_ch_hw); + stream.next(m.num_trk_ch_use); + stream.next( + static_cast::_num_config_blocks_type>(m.blocks.size())); + for (std::size_t i = 0; i < m.blocks.size(); ++i) { + serialize(stream, m.blocks[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::CfgINFBlock_ & m) { + stream.next(m.protocol_id); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.inf_msg_mask[0]); + stream.next(m.inf_msg_mask[1]); + stream.next(m.inf_msg_mask[2]); + stream.next(m.inf_msg_mask[3]); + stream.next(m.inf_msg_mask[4]); + stream.next(m.inf_msg_mask[5]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgINFBlock_ & m) { + (void)m; + return 10; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::CfgINFBlock_ & m) { + stream.next(m.protocol_id); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.inf_msg_mask[0]); + stream.next(m.inf_msg_mask[1]); + stream.next(m.inf_msg_mask[2]); + stream.next(m.inf_msg_mask[3]); + stream.next(m.inf_msg_mask[4]); + stream.next(m.inf_msg_mask[5]); + } +}; + +/// +/// @brief Serializes the CfgInf message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgINF_ &m) { + UbloxIStream stream(const_cast(data), count); + uint32_t num_blocks = count / 10; + m.blocks.resize(num_blocks); + for (std::size_t i = 0; i < num_blocks; ++i) { + deserialize(stream, m.blocks[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgINF_ &m) { + return 10 * m.blocks.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgINF_ &m) { + UbloxOStream stream(data, size); + for (std::size_t i = 0; i < m.blocks.size(); ++i) { + serialize(stream, m.blocks[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgMSG_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.msg_class); + stream.next(m.msg_id); + stream.next(m.rate); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgMSG_ &m) { + (void)m; + return 3; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgMSG_ &m) { + UbloxOStream stream(data, size); + stream.next(m.msg_class); + stream.next(m.msg_id); + stream.next(m.rate); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgNAV5_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.mask); + stream.next(m.dyn_model); + stream.next(m.fix_mode); + stream.next(m.fixed_alt); + stream.next(m.fixed_alt_var); + stream.next(m.min_elev); + stream.next(m.dr_limit); + stream.next(m.p_dop); + stream.next(m.t_dop); + stream.next(m.p_acc); + stream.next(m.t_acc); + stream.next(m.static_hold_thresh); + stream.next(m.dgnss_time_out); + stream.next(m.cno_thresh_num_svs); + stream.next(m.cno_thresh); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.static_hold_max_dist); + stream.next(m.utc_standard); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.reserved2[4]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNAV5_ &m) { + (void)m; + return 36; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgNAV5_ &m) { + UbloxOStream stream(data, size); + stream.next(m.mask); + stream.next(m.dyn_model); + stream.next(m.fix_mode); + stream.next(m.fixed_alt); + stream.next(m.fixed_alt_var); + stream.next(m.min_elev); + stream.next(m.dr_limit); + stream.next(m.p_dop); + stream.next(m.t_dop); + stream.next(m.p_acc); + stream.next(m.t_acc); + stream.next(m.static_hold_thresh); + stream.next(m.dgnss_time_out); + stream.next(m.cno_thresh_num_svs); + stream.next(m.cno_thresh); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.static_hold_max_dist); + stream.next(m.utc_standard); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.reserved2[4]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgNAVX5_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.mask1); + stream.next(m.mask2); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.min_svs); + stream.next(m.max_svs); + stream.next(m.min_cno); + stream.next(m.reserved2); + stream.next(m.ini_fix3d); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.ack_aiding); + stream.next(m.wkn_rollover); + stream.next(m.sig_atten_comp_mode); + stream.next(m.reserved4[0]); + stream.next(m.reserved4[1]); + stream.next(m.reserved4[2]); + stream.next(m.reserved4[3]); + stream.next(m.reserved4[4]); + stream.next(m.use_ppp); + stream.next(m.aop_cfg); + stream.next(m.reserved5[0]); + stream.next(m.reserved5[1]); + stream.next(m.aop_orb_max_err); + stream.next(m.reserved6[0]); + stream.next(m.reserved6[1]); + stream.next(m.reserved6[2]); + stream.next(m.reserved6[3]); + stream.next(m.reserved6[4]); + stream.next(m.reserved6[5]); + stream.next(m.reserved6[6]); + stream.next(m.use_adr); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNAVX5_ &m) { + (void)m; + return 40; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgNAVX5_ &m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.mask1); + stream.next(m.mask2); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.min_svs); + stream.next(m.max_svs); + stream.next(m.min_cno); + stream.next(m.reserved2); + stream.next(m.ini_fix3d); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.ack_aiding); + stream.next(m.wkn_rollover); + stream.next(m.sig_atten_comp_mode); + stream.next(m.reserved4[0]); + stream.next(m.reserved4[1]); + stream.next(m.reserved4[2]); + stream.next(m.reserved4[3]); + stream.next(m.reserved4[4]); + stream.next(m.use_ppp); + stream.next(m.aop_cfg); + stream.next(m.reserved5[0]); + stream.next(m.reserved5[1]); + stream.next(m.aop_orb_max_err); + stream.next(m.reserved6[0]); + stream.next(m.reserved6[1]); + stream.next(m.reserved6[2]); + stream.next(m.reserved6[3]); + stream.next(m.reserved6[4]); + stream.next(m.reserved6[5]); + stream.next(m.reserved6[6]); + stream.next(m.use_adr); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgNMEA_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.version); + stream.next(m.bds_talker_id[0]); + stream.next(m.bds_talker_id[1]); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNMEA_ &m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgNMEA_ &m) { + UbloxOStream stream(data, size); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.version); + stream.next(m.bds_talker_id[0]); + stream.next(m.bds_talker_id[1]); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgNMEA6_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.filter); + stream.next(m.version); + stream.next(m.num_sv); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNMEA6_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgNMEA6_ &m) { + UbloxOStream stream(data, size); + stream.next(m.filter); + stream.next(m.version); + stream.next(m.num_sv); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgNMEA7_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.reserved); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNMEA7_ &m) { + (void)m; + return 12; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgNMEA7_ &m) { + UbloxOStream stream(data, size); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.reserved); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgPRT_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.port_id); + stream.next(m.reserved0); + stream.next(m.tx_ready); + stream.next(m.mode); + stream.next(m.baud_rate); + stream.next(m.in_proto_mask); + stream.next(m.out_proto_mask); + stream.next(m.flags); + stream.next(m.reserved1); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgPRT_ &m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgPRT_ &m) { + UbloxOStream stream(data, size); + stream.next(m.port_id); + stream.next(m.reserved0); + stream.next(m.tx_ready); + stream.next(m.mode); + stream.next(m.baud_rate); + stream.next(m.in_proto_mask); + stream.next(m.out_proto_mask); + stream.next(m.flags); + stream.next(m.reserved1); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgRATE_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.meas_rate); + stream.next(m.nav_rate); + stream.next(m.time_ref); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgRATE_ &m) { + (void)m; + return 6; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgRATE_ &m) { + UbloxOStream stream(data, size); + stream.next(m.meas_rate); + stream.next(m.nav_rate); + stream.next(m.time_ref); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgRST_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.nav_bbr_mask); + stream.next(m.reset_mode); + stream.next(m.reserved1); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgRST_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgRST_ &m) { + UbloxOStream stream(data, size); + stream.next(m.nav_bbr_mask); + stream.next(m.reset_mode); + stream.next(m.reserved1); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgSBAS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.mode); + stream.next(m.usage); + stream.next(m.max_sbas); + stream.next(m.scanmode2); + stream.next(m.scanmode1); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgSBAS_ &m) { + (void)m; + return 8; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgSBAS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.mode); + stream.next(m.usage); + stream.next(m.max_sbas); + stream.next(m.scanmode2); + stream.next(m.scanmode1); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgTMODE3_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.flags); + stream.next(m.ecef_x_or_lat); + stream.next(m.ecef_y_or_lon); + stream.next(m.ecef_z_or_alt); + stream.next(m.ecef_x_or_lat_hp); + stream.next(m.ecef_y_or_lon_hp); + stream.next(m.ecef_z_or_alt_hp); + stream.next(m.reserved2); + stream.next(m.fixed_pos_acc); + stream.next(m.svin_min_dur); + stream.next(m.svin_acc_limit); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.reserved3[4]); + stream.next(m.reserved3[5]); + stream.next(m.reserved3[6]); + stream.next(m.reserved3[7]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgTMODE3_ &m) { + (void)m; + return 40; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgTMODE3_ &m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.flags); + stream.next(m.ecef_x_or_lat); + stream.next(m.ecef_y_or_lon); + stream.next(m.ecef_z_or_alt); + stream.next(m.ecef_x_or_lat_hp); + stream.next(m.ecef_y_or_lon_hp); + stream.next(m.ecef_z_or_alt_hp); + stream.next(m.reserved2); + stream.next(m.fixed_pos_acc); + stream.next(m.svin_min_dur); + stream.next(m.svin_acc_limit); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.reserved3[4]); + stream.next(m.reserved3[5]); + stream.next(m.reserved3[6]); + stream.next(m.reserved3[7]); + } +}; + +/// +/// @brief Serializes the CfgVALDEL message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgVALDEL_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.keys.size(); ++i) { + deserialize(stream, m.keys[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgVALDEL_ &m) { + return 4 + 4 * m.keys.size(); + } + + inline static void write(const uint8_t *data, uint32_t count, + const ublox_msgs::msg::CfgVALDEL_ &m) { + UbloxOStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.keys.size(); ++i) { + serialize(stream, m.keys[i]); + } + } +}; + +/// +/// @brief Serializes the CfgVALGET message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgVALGET_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.position); + uint8_t data_size = (count - 4) / 4; + m.keys.resize(data_size); + // the key and values are all together in the keys array + for (std::size_t i = 0; i < data_size; ++i) { + deserialize(stream, m.keys[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgVALGET_ &m) { + return 4 + 4 * m.keys.size(); + } + + inline static void write(const uint8_t *data, uint32_t count, + const ublox_msgs::msg::CfgVALGET_ &m) { + UbloxOStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.position); + for (std::size_t i = 0; i < m.keys.size(); ++i) { + serialize(stream, m.keys[i]); + } + } +}; + +/// +/// @brief Serializes the CfgVALSET message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgVALSET_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.cfgdata.size(); ++i) { + deserialize(stream, m.cfgdata[i].key); + for (std::size_t j = 0; j < m.cfgdata[i].data.size(); ++j) { + deserialize(stream, m.cfgdata[i].data[j]); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgVALSET_ &m) { + int data_size = 0; + for (std::size_t i = 0; i < m.cfgdata.size(); ++i) { + data_size += m.cfgdata[i].data.size(); + data_size += 4; + } + return 4 + data_size; + } + + inline static void write(const uint8_t *data, uint32_t count, + const ublox_msgs::msg::CfgVALSET_ &m) { + UbloxOStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.cfgdata.size(); ++i) { + serialize(stream, m.cfgdata[i].key); + for (std::size_t j = 0; j < m.cfgdata[i].data.size(); ++j) { + serialize(stream, m.cfgdata[i].data[j]); + } + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::EsfINS_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.bitfield0); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.i_tow); + stream.next(m.x_ang_rate); + stream.next(m.y_ang_rate); + stream.next(m.z_ang_rate); + stream.next(m.x_accel); + stream.next(m.y_accel); + stream.next(m.z_accel); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfINS_ & m) { + (void)m; + return 36; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::EsfINS_ & m) { + UbloxOStream stream(data, size); + stream.next(m.bitfield0); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.i_tow); + stream.next(m.x_ang_rate); + stream.next(m.y_ang_rate); + stream.next(m.z_ang_rate); + stream.next(m.x_accel); + stream.next(m.y_accel); + stream.next(m.z_accel); + } +}; + +/// +/// @brief Serializes the EsfMEAS message which has a repeated block and an +/// optional block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::EsfMEAS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.time_tag); + stream.next(m.flags); + stream.next(m.id); + + bool calib_valid = m.flags & m.FLAGS_CALIB_T_TAG_VALID; + uint32_t data_size = (count - (calib_valid ? 12 : 8)) / 4; + // Repeating block + m.data.resize(data_size); + for (std::size_t i = 0; i < data_size; ++i) { + deserialize(stream, m.data[i]); + } + // Optional block + if (calib_valid) { + m.calib_t_tag.resize(1); + deserialize(stream, m.calib_t_tag[0]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfMEAS_ &m) { + return 4 + 8 * m.data.size() + 4 * m.calib_t_tag.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::EsfMEAS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.time_tag); + stream.next(m.flags); + stream.next(m.id); + for (std::size_t i = 0; i < m.data.size(); ++i) { + serialize(stream, m.data[i]); + } + for (std::size_t i = 0; i < m.calib_t_tag.size(); ++i) { + serialize(stream, m.calib_t_tag[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::EsfRAWBlock_ & m) { + stream.next(m.data); + stream.next(m.s_t_tag); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfRAWBlock_ & m) { + (void)m; + return 8; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::EsfRAWBlock_ & m) { + stream.next(m.data); + stream.next(m.s_t_tag); + } +}; + +/// +/// @brief Serializes the EsfRAW message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::EsfRAW_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.reserved0); + m.blocks.clear(); + uint32_t num_blocks = (count - 4) / 8; + m.blocks.resize(num_blocks); + for (std::size_t i = 0; i < num_blocks; ++i) { + deserialize(stream, m.blocks[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfRAW_ &m) { + return 4 + 8 * m.blocks.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::EsfRAW_ &m) { + UbloxOStream stream(data, size); + stream.next(m.reserved0); + for (std::size_t i = 0; i < m.blocks.size(); ++i) { + serialize(stream, m.blocks[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::EsfSTATUSSens_ & m) { + stream.next(m.sens_status1); + stream.next(m.sens_status2); + stream.next(m.freq); + stream.next(m.faults); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfSTATUSSens_ & m) { + (void)m; + return 4; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::EsfSTATUSSens_ & m) { + stream.next(m.sens_status1); + stream.next(m.sens_status2); + stream.next(m.freq); + stream.next(m.faults); + } +}; + +/// +/// @brief Serializes the EsfSTATUS message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::EsfSTATUS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.fusion_mode); + stream.next(m.reserved2); + stream.next(m.num_sens); + m.sens.resize(m.num_sens); + for (std::size_t i = 0; i < m.sens.size(); ++i) { + deserialize(stream, m.sens[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfSTATUS_ &m) { + return 16 + 4 * m.num_sens; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::EsfSTATUS_ &m) { + if (m.sens.size() != m.num_sens) { + // ROS_ERROR("Writing EsfSTATUS message: num_sens must equal size of sens"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.fusion_mode); + stream.next(m.reserved2); + stream.next(static_cast::_num_sens_type>(m.sens.size())); + for (std::size_t i = 0; i < m.sens.size(); ++i) { + serialize(stream, m.sens[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::HnrPVT_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.nano); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.g_speed); + stream.next(m.speed); + stream.next(m.head_mot); + stream.next(m.head_veh); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::HnrPVT_ & m) { + (void)m; + return 72; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::HnrPVT_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.nano); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.g_speed); + stream.next(m.speed); + stream.next(m.head_mot); + stream.next(m.head_veh); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + } +}; + +/// +/// @brief Serializes the Inf message which has a dynamic length string. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::Inf_ &m) { + UbloxIStream stream(const_cast(data), count); + m.str.resize(count); + for (uint32_t i = 0; i < count; ++i) { + deserialize(stream, m.str[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::Inf_ &m) { + return m.str.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::Inf_ &m) { + UbloxOStream stream(data, size); + for (std::size_t i = 0; i < m.str.size(); ++i) { + serialize(stream, m.str[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::MonHW_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::MonHW_ & m) { + (void)m; + return 60; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::MonHW_ & m) { + UbloxOStream stream(data, size); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::MonHW6_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.vp[17]); + stream.next(m.vp[18]); + stream.next(m.vp[19]); + stream.next(m.vp[20]); + stream.next(m.vp[21]); + stream.next(m.vp[22]); + stream.next(m.vp[23]); + stream.next(m.vp[24]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::MonHW6_ & m) { + (void)m; + return 68; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::MonHW6_ & m) { + UbloxOStream stream(data, size); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.vp[17]); + stream.next(m.vp[18]); + stream.next(m.vp[19]); + stream.next(m.vp[20]); + stream.next(m.vp[21]); + stream.next(m.vp[22]); + stream.next(m.vp[23]); + stream.next(m.vp[24]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::MonVERExtension_ & m) { + stream.next(m.field); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::MonVERExtension_ & m) { + (void)m; + return 30; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::MonVERExtension_ & m) { + stream.next(m.field); + } +}; + +/// +/// @brief Serializes the MonVER message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::MonVER_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.sw_version); + stream.next(m.hw_version); + + m.extension.clear(); + int N = (count - 40) / 30; + m.extension.resize(N); + typename ublox_msgs::msg::MonVER_::_extension_type::value_type ext; + for (int i = 0; i < N; i++) { + // Read each extension string + deserialize(stream, m.extension[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::MonVER_ &m) { + return 40 + (30 * m.extension.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::MonVER_ &m) { + UbloxOStream stream(data, size); + stream.next(m.sw_version); + stream.next(m.hw_version); + for (std::size_t i = 0; i < m.extension.size(); ++i) { + serialize(stream, m.extension[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavATT_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.roll); + stream.next(m.pitch); + stream.next(m.heading); + stream.next(m.acc_roll); + stream.next(m.acc_pitch); + stream.next(m.acc_heading); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavATT_ & m) { + (void)m; + return 32; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavATT_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.roll); + stream.next(m.pitch); + stream.next(m.heading); + stream.next(m.acc_roll); + stream.next(m.acc_pitch); + stream.next(m.acc_heading); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavCLOCK_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.clk_b); + stream.next(m.clk_d); + stream.next(m.t_acc); + stream.next(m.f_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavCLOCK_ & m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavCLOCK_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.clk_b); + stream.next(m.clk_d); + stream.next(m.t_acc); + stream.next(m.f_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavCOV_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.pos_cov_valid); + stream.next(m.vel_cov_valid); + stream.next(m.reserved_0[0]); + stream.next(m.reserved_0[1]); + stream.next(m.reserved_0[2]); + stream.next(m.reserved_0[3]); + stream.next(m.reserved_0[4]); + stream.next(m.reserved_0[5]); + stream.next(m.reserved_0[6]); + stream.next(m.reserved_0[7]); + stream.next(m.reserved_0[8]); + stream.next(m.pos_cov_nn); + stream.next(m.pos_cov_ne); + stream.next(m.pos_cov_nd); + stream.next(m.pos_cov_ee); + stream.next(m.pos_cov_ed); + stream.next(m.pos_cov_dd); + stream.next(m.vel_cov_nn); + stream.next(m.vel_cov_ne); + stream.next(m.vel_cov_nd); + stream.next(m.vel_cov_ee); + stream.next(m.vel_cov_ed); + stream.next(m.vel_cov_dd); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavCOV_ & m) { + (void)m; + return 64; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavCOV_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.pos_cov_valid); + stream.next(m.vel_cov_valid); + stream.next(m.reserved_0[0]); + stream.next(m.reserved_0[1]); + stream.next(m.reserved_0[2]); + stream.next(m.reserved_0[3]); + stream.next(m.reserved_0[4]); + stream.next(m.reserved_0[5]); + stream.next(m.reserved_0[6]); + stream.next(m.reserved_0[7]); + stream.next(m.reserved_0[8]); + stream.next(m.pos_cov_nn); + stream.next(m.pos_cov_ne); + stream.next(m.pos_cov_nd); + stream.next(m.pos_cov_ee); + stream.next(m.pos_cov_ed); + stream.next(m.pos_cov_dd); + stream.next(m.vel_cov_nn); + stream.next(m.vel_cov_ne); + stream.next(m.vel_cov_nd); + stream.next(m.vel_cov_ee); + stream.next(m.vel_cov_ed); + stream.next(m.vel_cov_dd); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavDGPSSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.age_c); + stream.next(m.prc); + stream.next(m.prrc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavDGPSSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavDGPSSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.age_c); + stream.next(m.prc); + stream.next(m.prrc); + } +}; + +/// +/// @brief Serializes the NavDGPS message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavDGPS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.age); + stream.next(m.base_id); + stream.next(m.base_health); + stream.next(m.num_ch); + stream.next(m.status); + stream.next(m.reserved1); + m.sv.resize(m.num_ch); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavDGPS_ &m) { + return 16 + 12 * m.num_ch; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavDGPS_ &m) { + if (m.sv.size() != static_cast(m.num_ch)) { + // ROS_ERROR("NavDGPS numCh must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.age); + stream.next(m.base_id); + stream.next(m.base_health); + stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(m.status); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavPOSECEF_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPOSECEF_ & m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavPOSECEF_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavPOSLLH_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPOSLLH_ & m) { + (void)m; + return 28; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavPOSLLH_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavPVT_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + stream.next(m.head_veh); + stream.next(m.mag_dec); + stream.next(m.mag_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPVT_ & m) { + (void)m; + return 92; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavPVT_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + stream.next(m.head_veh); + stream.next(m.mag_dec); + stream.next(m.mag_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavPVT7_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPVT7_ & m) { + (void)m; + return 84; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavPVT7_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavRELPOSNED_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved0); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); + stream.next(m.reserved1); + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavRELPOSNED_ & m) { + (void)m; + return 40; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavRELPOSNED_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved0); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); + stream.next(m.reserved1); + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavRELPOSNED9_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_length); + stream.next(m.rel_pos_heading); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); + stream.next(m.rel_pos_hp_length); + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.acc_length); + stream.next(m.acc_heading); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavRELPOSNED9_ & m) { + (void)m; + return 64; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavRELPOSNED9_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_length); + stream.next(m.rel_pos_heading); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); + stream.next(m.rel_pos_hp_length); + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.acc_length); + stream.next(m.acc_heading); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavSATSV_ & m) { + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSATSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavSATSV_ & m) { + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + stream.next(m.flags); + } +}; + +/// +/// @brief Serializes the NavSAT message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavSAT_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.num_svs); + stream.next(m.reserved0); + m.sv.resize(m.num_svs); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSAT_ &m) { + return 8 + 12 * m.num_svs; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavSAT_ &m) { + if (m.sv.size() != m.num_svs) { + // ROS_ERROR("NavSAT num_svs must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(static_cast::_num_svs_type>(m.sv.size())); + stream.next(m.reserved0); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavSBASSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.udre); + stream.next(m.sv_sys); + stream.next(m.sv_service); + stream.next(m.reserved1); + stream.next(m.prc); + stream.next(m.reserved2); + stream.next(m.ic); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSBASSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavSBASSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.udre); + stream.next(m.sv_sys); + stream.next(m.sv_service); + stream.next(m.reserved1); + stream.next(m.prc); + stream.next(m.reserved2); + stream.next(m.ic); + } +}; + +/// +/// @brief Serializes the NavSBAS message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavSBAS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.geo); + stream.next(m.mode); + stream.next(m.sys); + stream.next(m.service); + stream.next(m.cnt); + stream.next(m.reserved0); + m.sv.resize(m.cnt); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSBAS_ &m) { + return 12 + 12 * m.cnt; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavSBAS_ &m) { + if (m.sv.size() != m.cnt) { + // ROS_ERROR("NavSBAS cnt must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.geo); + stream.next(m.mode); + stream.next(m.sys); + stream.next(m.service); + stream.next(static_cast::_cnt_type>(m.sv.size())); + stream.next(m.reserved0); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavSOL_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + stream.next(m.ecef_vx); + stream.next(m.ecef_vy); + stream.next(m.ecef_vz); + stream.next(m.s_acc); + stream.next(m.p_dop); + stream.next(m.reserved1); + stream.next(m.num_sv); + stream.next(m.reserved2); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSOL_ & m) { + (void)m; + return 52; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavSOL_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + stream.next(m.ecef_vx); + stream.next(m.ecef_vy); + stream.next(m.ecef_vz); + stream.next(m.s_acc); + stream.next(m.p_dop); + stream.next(m.reserved1); + stream.next(m.num_sv); + stream.next(m.reserved2); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavSTATUS_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.fix_stat); + stream.next(m.flags2); + stream.next(m.ttff); + stream.next(m.msss); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSTATUS_ & m) { + (void)m; + return 16; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavSTATUS_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.fix_stat); + stream.next(m.flags2); + stream.next(m.ttff); + stream.next(m.msss); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavSVIN_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.i_tow); + stream.next(m.dur); + stream.next(m.mean_x); + stream.next(m.mean_y); + stream.next(m.mean_z); + stream.next(m.mean_xhp); + stream.next(m.mean_yhp); + stream.next(m.mean_zhp); + stream.next(m.reserved1); + stream.next(m.mean_acc); + stream.next(m.obs); + stream.next(m.valid); + stream.next(m.active); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSVIN_ & m) { + (void)m; + return 40; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavSVIN_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.i_tow); + stream.next(m.dur); + stream.next(m.mean_x); + stream.next(m.mean_y); + stream.next(m.mean_z); + stream.next(m.mean_xhp); + stream.next(m.mean_yhp); + stream.next(m.mean_zhp); + stream.next(m.reserved1); + stream.next(m.mean_acc); + stream.next(m.obs); + stream.next(m.valid); + stream.next(m.active); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavSVINFOSV_ & m) { + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.quality); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSVINFOSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavSVINFOSV_ & m) { + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.quality); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + } +}; + +/// +/// @brief Serializes the NavSVINFO message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavSVINFO_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.num_ch); + stream.next(m.global_flags); + stream.next(m.reserved2); + m.sv.resize(m.num_ch); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSVINFO_ &m) { + return 8 + 12 * m.num_ch; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavSVINFO_ &m) { + if (m.sv.size() != m.num_ch) { + // ROS_ERROR("NavSVINFO num_ch must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(m.global_flags); + stream.next(m.reserved2); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavVELNED_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.speed); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.c_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavVELNED_ & m) { + (void)m; + return 36; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavVELNED_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.speed); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.c_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavTIMEGPS_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(m.valid); + stream.next(m.t_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavTIMEGPS_ & m) { + (void)m; + return 16; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavTIMEGPS_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(m.valid); + stream.next(m.t_acc); + } +}; + +/// +/// @brief Serializes the RxmALM message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmALM_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.svid); + stream.next(m.week); + + m.dwrd.clear(); + if (count == 40) { + typename ublox_msgs::msg::RxmALM_::_dwrd_type::value_type temp; + m.dwrd.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp); + m.dwrd.push_back(temp); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmALM_ &m) { + return 8 + (4 * m.dwrd.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmALM_ &m) { + UbloxOStream stream(data, size); + stream.next(m.svid); + stream.next(m.week); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + serialize(stream, m.dwrd[i]); + } + } +}; + +/// +/// @brief Serializes the RxmEPH message which has a repeated block. +/// +template +struct UbloxSerializer > +{ + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmEPH_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.svid); + stream.next(m.how); + m.sf1d.clear(); + m.sf2d.clear(); + m.sf3d.clear(); + + if (count == 104) { + typename ublox_msgs::msg::RxmEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::msg::RxmEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::msg::RxmEPH_::_sf3d_type::value_type temp3; + + m.sf1d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp1); + m.sf1d.push_back(temp1); + } + m.sf2d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp2); + m.sf2d.push_back(temp2); + } + m.sf3d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp3); + m.sf3d.push_back(temp3); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmEPH_ &m) { + return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmEPH_ &m) { + UbloxOStream stream(data, size); + stream.next(m.svid); + stream.next(m.how); + for (std::size_t i = 0; i < m.sf1d.size(); ++i) { + serialize(stream, m.sf1d[i]); + } + for (std::size_t i = 0; i < m.sf2d.size(); ++i) { + serialize(stream, m.sf2d[i]); + } + for (std::size_t i = 0; i < m.sf3d.size(); ++i) { + serialize(stream, m.sf3d[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::RxmRAWSV_ & m) { + stream.next(m.cp_mes); + stream.next(m.pr_mes); + stream.next(m.do_mes); + stream.next(m.sv); + stream.next(m.mes_qi); + stream.next(m.cno); + stream.next(m.lli); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAWSV_ & m) { + (void)m; + return 24; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::RxmRAWSV_ & m) { + stream.next(m.cp_mes); + stream.next(m.pr_mes); + stream.next(m.do_mes); + stream.next(m.sv); + stream.next(m.mes_qi); + stream.next(m.cno); + stream.next(m.lli); + } +}; + +/// +/// @brief Serializes the RxmRAW message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmRAW_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.rcv_tow); + stream.next(m.week); + stream.next(m.num_sv); + stream.next(m.reserved1); + m.sv.resize(m.num_sv); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAW_ &m) { + return 8 + 24 * m.num_sv; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmRAW_ &m) { + if (m.sv.size() != m.num_sv) { + // ROS_ERROR("RxmRAW num_sv must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.rcv_tow); + stream.next(m.week); + stream.next(static_cast::_num_sv_type>(m.sv.size())); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::RxmRAWXMeas_ & m) { + stream.next(m.pr_mes); + stream.next(m.cp_mes); + stream.next(m.do_mes); + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.reserved0); + stream.next(m.freq_id); + stream.next(m.locktime); + stream.next(m.cno); + stream.next(m.pr_stdev); + stream.next(m.cp_stdev); + stream.next(m.do_stdev); + stream.next(m.trk_stat); + stream.next(m.reserved1); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAWXMeas_ & m) { + (void)m; + return 32; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::RxmRAWXMeas_ & m) { + stream.next(m.pr_mes); + stream.next(m.cp_mes); + stream.next(m.do_mes); + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.reserved0); + stream.next(m.freq_id); + stream.next(m.locktime); + stream.next(m.cno); + stream.next(m.pr_stdev); + stream.next(m.cp_stdev); + stream.next(m.do_stdev); + stream.next(m.trk_stat); + stream.next(m.reserved1); + } +}; + +/// +/// @brief Serializes the RxmRAWX message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmRAWX_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.rcv_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(m.num_meas); + stream.next(m.rec_stat); + stream.next(m.version); + stream.next(m.reserved1); + m.meas.resize(m.num_meas); + for (std::size_t i = 0; i < m.meas.size(); ++i) { + deserialize(stream, m.meas[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAWX_ &m) { + return 16 + 32 * m.num_meas; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmRAWX_ &m) { + if (m.meas.size() != m.num_meas) { + // ROS_ERROR("RxmRAWX num_meas must equal meas size"); + } + UbloxOStream stream(data, size); + stream.next(m.rcv_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(static_cast::_num_meas_type>(m.meas.size())); + stream.next(m.rec_stat); + stream.next(m.version); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.meas.size(); ++i) { + serialize(stream, m.meas[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmRTCM_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.flags); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.ref_station); + stream.next(m.msg_type); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRTCM_ & m) { + (void)m; + return 8; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmRTCM_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.flags); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.ref_station); + stream.next(m.msg_type); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmSFRB_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.dwrd[0]); + stream.next(m.dwrd[1]); + stream.next(m.dwrd[2]); + stream.next(m.dwrd[3]); + stream.next(m.dwrd[4]); + stream.next(m.dwrd[5]); + stream.next(m.dwrd[6]); + stream.next(m.dwrd[7]); + stream.next(m.dwrd[8]); + stream.next(m.dwrd[9]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSFRB_ & m) { + (void)m; + return 42; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmSFRB_ & m) { + UbloxOStream stream(data, size); + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.dwrd[0]); + stream.next(m.dwrd[1]); + stream.next(m.dwrd[2]); + stream.next(m.dwrd[3]); + stream.next(m.dwrd[4]); + stream.next(m.dwrd[5]); + stream.next(m.dwrd[6]); + stream.next(m.dwrd[7]); + stream.next(m.dwrd[8]); + stream.next(m.dwrd[9]); + } +}; + +/// +/// @brief Serializes the RxmSFRBX message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmSFRBX_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.reserved0); + stream.next(m.freq_id); + stream.next(m.num_words); + stream.next(m.chn); + stream.next(m.version); + stream.next(m.reserved1); + m.dwrd.resize(m.num_words); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + deserialize(stream, m.dwrd[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSFRBX_ &m) { + return 8 + 4 * m.num_words; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmSFRBX_ &m) { + if (m.dwrd.size() != m.num_words) { + // ROS_ERROR("RxmSFRBX num_words must equal dwrd size"); + } + UbloxOStream stream(data, size); + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.reserved0); + stream.next(m.freq_id); + stream.next(static_cast::_num_words_type>(m.dwrd.size())); + stream.next(m.chn); + stream.next(m.version); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + serialize(stream, m.dwrd[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::RxmSVSISV_ & m) { + stream.next(m.svid); + stream.next(m.sv_flag); + stream.next(m.azim); + stream.next(m.elev); + stream.next(m.age); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSVSISV_ & m) { + (void)m; + return 6; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::RxmSVSISV_ & m) { + stream.next(m.svid); + stream.next(m.sv_flag); + stream.next(m.azim); + stream.next(m.elev); + stream.next(m.age); + } +}; + +/// +/// @brief Serializes the RxmSVSI message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::RxmSVSI_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.week); + stream.next(m.num_vis); + stream.next(m.num_sv); + m.sv.resize(m.num_sv); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSVSI_ &m) { + return 8 + 6 * m.num_sv; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::RxmSVSI_ &m) { + if (m.sv.size() != m.num_sv) { + // ROS_ERROR("RxmSVSI num_sv must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.week); + stream.next(m.num_vis); + stream.next(static_cast::_num_sv_type>(m.sv.size())); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::TimTM2_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.ch); + stream.next(m.flags); + stream.next(m.rising_edge_count); + stream.next(m.wn_r); + stream.next(m.wn_f); + stream.next(m.tow_ms_r); + stream.next(m.tow_sub_ms_r); + stream.next(m.tow_ms_f); + stream.next(m.tow_sub_ms_f); + stream.next(m.acc_est); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::TimTM2_ &m) { + (void)m; + return 28; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::TimTM2_ &m) { + UbloxOStream stream(data, size); + stream.next(m.ch); + stream.next(m.flags); + stream.next(m.rising_edge_count); + stream.next(m.wn_r); + stream.next(m.wn_f); + stream.next(m.tow_ms_r); + stream.next(m.tow_sub_ms_r); + stream.next(m.tow_ms_f); + stream.next(m.tow_sub_ms_f); + stream.next(m.acc_est); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::UpdSOS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.cmd); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::UpdSOS_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::UpdSOS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.cmd); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::UpdSOSAck_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.cmd); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.response); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::UpdSOSAck_ &m) { + (void)m; + return 8; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::UpdSOSAck_ &m) { + UbloxOStream stream(data, size); + stream.next(m.cmd); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.response); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } +}; + +} // namespace ublox + +#endif // UBLOX_MSGS_SERIALIZATION_HPP diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.h b/ublox_msgs/include/ublox_msgs/ublox_msgs.h deleted file mode 100644 index 09c2e80e..00000000 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.h +++ /dev/null @@ -1,259 +0,0 @@ -//============================================================================== -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * 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. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, 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 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 UBLOX_MSGS_H -#define UBLOX_MSGS_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include - -namespace ublox_msgs { - -namespace Class { - static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, - //!< Speed, Time, Acceleration, Heading, - //!< DOP, SVs used - static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: - //!< Satellite Status, RTC Status - static const uint8_t INF = 0x04; //!< Information Messages: - //!< Printf-Style Messages, with IDs such as - //!< Error, Warning, Notice - static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject - //!< messages to CFG input messages - static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set - //!< Dynamic Model, Set DOP Mask, Set Baud - //!< Rate, etc. - static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. - //!< Memory/Flash erase/write, Reboot, Flash - //!< identification, etc. - //!< Used to update the firmware and identify - //!< any attached flash device - static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication - //!< Status, CPU Load, Stack Usage, - //!< Task Status - static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, - //!< Almanac, other A-GPS data input - static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, - //!< Timemark Results - static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: - //!< External sensor measurements and status - //!< information - static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: - //!< Assistance data for various GNSS - static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, - //!< deletion, info and retrieval - static const uint8_t SEC = 0x27; //!< Security Feature Messages - static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: - //!< High rate time, position, speed, heading - static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages -} - -namespace Message { - namespace NAV { - static const uint8_t ATT = NavATT::MESSAGE_ID; - static const uint8_t CLOCK = NavCLOCK::MESSAGE_ID; - static const uint8_t DGPS = NavDGPS::MESSAGE_ID; - static const uint8_t DOP = NavDOP::MESSAGE_ID; - static const uint8_t POSECEF = NavPOSECEF::MESSAGE_ID; - static const uint8_t POSLLH = NavPOSLLH::MESSAGE_ID; - static const uint8_t RELPOSNED = NavRELPOSNED::MESSAGE_ID; - static const uint8_t RELPOSNED9 = NavRELPOSNED9::MESSAGE_ID; - static const uint8_t SBAS = NavSBAS::MESSAGE_ID; - static const uint8_t SOL = NavSOL::MESSAGE_ID; - static const uint8_t PVT = NavPVT::MESSAGE_ID; - static const uint8_t SAT = NavSAT::MESSAGE_ID; - static const uint8_t STATUS = NavSTATUS::MESSAGE_ID; - static const uint8_t SVINFO = NavSVINFO::MESSAGE_ID; - static const uint8_t SVIN = NavSVIN::MESSAGE_ID; - static const uint8_t TIMEGPS = NavTIMEGPS::MESSAGE_ID; - static const uint8_t TIMEUTC = NavTIMEUTC::MESSAGE_ID; - static const uint8_t VELECEF = NavVELECEF::MESSAGE_ID; - static const uint8_t VELNED = NavVELNED::MESSAGE_ID; - } - - namespace RXM { - static const uint8_t ALM = RxmALM::MESSAGE_ID; - static const uint8_t EPH = RxmEPH::MESSAGE_ID; - static const uint8_t RAW = RxmRAW::MESSAGE_ID; - static const uint8_t RAWX = RxmRAWX::MESSAGE_ID; - static const uint8_t RTCM = RxmRTCM::MESSAGE_ID; - static const uint8_t SFRB = RxmSFRB::MESSAGE_ID; - static const uint8_t SFRBX = RxmSFRBX::MESSAGE_ID; - static const uint8_t SVSI = RxmSVSI::MESSAGE_ID; - } - - namespace INF { - static const uint8_t ERROR = 0x00; - static const uint8_t WARNING = 0x01; - static const uint8_t NOTICE = 0x02; - static const uint8_t TEST = 0x03; - static const uint8_t DEBUG = 0x04; - } - - namespace ACK { - static const uint8_t NACK = 0x00; - static const uint8_t ACK = 0x01; - } - - namespace AID { - static const uint8_t ALM = AidALM::MESSAGE_ID; - static const uint8_t EPH = AidEPH::MESSAGE_ID; - static const uint8_t HUI = AidHUI::MESSAGE_ID; - } - - namespace CFG { - static const uint8_t ANT = CfgANT::MESSAGE_ID; - static const uint8_t CFG = CfgCFG::MESSAGE_ID; - static const uint8_t DAT = CfgDAT::MESSAGE_ID; - static const uint8_t GNSS = CfgGNSS::MESSAGE_ID; - static const uint8_t HNR = CfgHNR::MESSAGE_ID; - static const uint8_t INF = CfgINF::MESSAGE_ID; - static const uint8_t DGNSS = CfgDGNSS::MESSAGE_ID; - static const uint8_t MSG = CfgMSG::MESSAGE_ID; - static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; - static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; - static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; - static const uint8_t PRT = CfgPRT::MESSAGE_ID; - static const uint8_t RATE = CfgRATE::MESSAGE_ID; - static const uint8_t RST = CfgRST::MESSAGE_ID; - static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; - static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; - static const uint8_t USB = CfgUSB::MESSAGE_ID; - } - - namespace UPD { - //! SOS and SOS_Ack have the same message ID, but different lengths - static const uint8_t SOS = UpdSOS::MESSAGE_ID; - } - - namespace MON { - static const uint8_t GNSS = MonGNSS::MESSAGE_ID; - static const uint8_t HW = MonHW::MESSAGE_ID; - static const uint8_t VER = MonVER::MESSAGE_ID; - } - - namespace ESF { - static const uint8_t INS = EsfINS::MESSAGE_ID; - static const uint8_t MEAS = EsfMEAS::MESSAGE_ID; - static const uint8_t RAW = EsfRAW::MESSAGE_ID; - static const uint8_t STATUS = EsfSTATUS::MESSAGE_ID; - } - - namespace MGA { - static const uint8_t GAL = MgaGAL::MESSAGE_ID; - } - - namespace HNR { - static const uint8_t PVT = HnrPVT::MESSAGE_ID; - } - - namespace TIM { - static const uint8_t TM2 = TimTM2::MESSAGE_ID; - } -} - -} //!< namespace ublox_msgs - -#endif //!< UBLOX_MSGS_H diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp new file mode 100644 index 00000000..b1ac7155 --- /dev/null +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -0,0 +1,268 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 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 UBLOX_MSGS_UBLOX_MSGS_HPP +#define UBLOX_MSGS_UBLOX_MSGS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace ublox_msgs { + +namespace Class { + static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, + //!< Speed, Time, Acceleration, Heading, + //!< DOP, SVs used + static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: + //!< Satellite Status, RTC Status + static const uint8_t INF = 0x04; //!< Information Messages: + //!< Printf-Style Messages, with IDs such as + //!< Error, Warning, Notice + static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject + //!< messages to CFG input messages + static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set + //!< Dynamic Model, Set DOP Mask, Set Baud + //!< Rate, etc. + static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. + //!< Memory/Flash erase/write, Reboot, Flash + //!< identification, etc. + //!< Used to update the firmware and identify + //!< any attached flash device + static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication + //!< Status, CPU Load, Stack Usage, + //!< Task Status + static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, + //!< Almanac, other A-GPS data input + static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, + //!< Timemark Results + static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: + //!< External sensor measurements and status + //!< information + static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: + //!< Assistance data for various GNSS + static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, + //!< deletion, info and retrieval + static const uint8_t SEC = 0x27; //!< Security Feature Messages + static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: + //!< High rate time, position, speed, heading + static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages +} // namespace Class + +namespace Message { + namespace NAV { + static const uint8_t ATT = ublox_msgs::msg::NavATT::MESSAGE_ID; + static const uint8_t CLOCK = ublox_msgs::msg::NavCLOCK::MESSAGE_ID; + static const uint8_t COV = ublox_msgs::msg::NavCOV::MESSAGE_ID; + static const uint8_t DGPS = ublox_msgs::msg::NavDGPS::MESSAGE_ID; + static const uint8_t DOP = ublox_msgs::msg::NavDOP::MESSAGE_ID; + static const uint8_t POSECEF = ublox_msgs::msg::NavPOSECEF::MESSAGE_ID; + static const uint8_t POSLLH = ublox_msgs::msg::NavPOSLLH::MESSAGE_ID; + static const uint8_t RELPOSNED = ublox_msgs::msg::NavRELPOSNED::MESSAGE_ID; + static const uint8_t RELPOSNED9 = ublox_msgs::msg::NavRELPOSNED9::MESSAGE_ID; + static const uint8_t SBAS = ublox_msgs::msg::NavSBAS::MESSAGE_ID; + static const uint8_t SOL = ublox_msgs::msg::NavSOL::MESSAGE_ID; + static const uint8_t PVT = ublox_msgs::msg::NavPVT::MESSAGE_ID; + static const uint8_t SAT = ublox_msgs::msg::NavSAT::MESSAGE_ID; + static const uint8_t STATUS = ublox_msgs::msg::NavSTATUS::MESSAGE_ID; + static const uint8_t SVINFO = ublox_msgs::msg::NavSVINFO::MESSAGE_ID; + static const uint8_t SVIN = ublox_msgs::msg::NavSVIN::MESSAGE_ID; + static const uint8_t TIMEGPS = ublox_msgs::msg::NavTIMEGPS::MESSAGE_ID; + static const uint8_t TIMEUTC = ublox_msgs::msg::NavTIMEUTC::MESSAGE_ID; + static const uint8_t VELECEF = ublox_msgs::msg::NavVELECEF::MESSAGE_ID; + static const uint8_t VELNED = ublox_msgs::msg::NavVELNED::MESSAGE_ID; + } // namespace NAV + + namespace RXM { + static const uint8_t ALM = ublox_msgs::msg::RxmALM::MESSAGE_ID; + static const uint8_t EPH = ublox_msgs::msg::RxmEPH::MESSAGE_ID; + static const uint8_t RAW = ublox_msgs::msg::RxmRAW::MESSAGE_ID; + static const uint8_t RAWX = ublox_msgs::msg::RxmRAWX::MESSAGE_ID; + static const uint8_t RTCM = ublox_msgs::msg::RxmRTCM::MESSAGE_ID; + static const uint8_t SFRB = ublox_msgs::msg::RxmSFRB::MESSAGE_ID; + static const uint8_t SFRBX = ublox_msgs::msg::RxmSFRBX::MESSAGE_ID; + static const uint8_t SVSI = ublox_msgs::msg::RxmSVSI::MESSAGE_ID; + } // namespace RXM + + namespace INF { + static const uint8_t ERROR = 0x00; + static const uint8_t WARNING = 0x01; + static const uint8_t NOTICE = 0x02; + static const uint8_t TEST = 0x03; + static const uint8_t DEBUG = 0x04; + } // namespace INF + + namespace ACK { + static const uint8_t NACK = 0x00; + static const uint8_t ACK = 0x01; + } // namespace ACK + + namespace AID { + static const uint8_t ALM = ublox_msgs::msg::AidALM::MESSAGE_ID; + static const uint8_t EPH = ublox_msgs::msg::AidEPH::MESSAGE_ID; + static const uint8_t HUI = ublox_msgs::msg::AidHUI::MESSAGE_ID; + } // namespace AID + + namespace CFG { + static const uint8_t ANT = ublox_msgs::msg::CfgANT::MESSAGE_ID; + static const uint8_t CFG = ublox_msgs::msg::CfgCFG::MESSAGE_ID; + static const uint8_t DAT = ublox_msgs::msg::CfgDAT::MESSAGE_ID; + static const uint8_t GNSS = ublox_msgs::msg::CfgGNSS::MESSAGE_ID; + static const uint8_t HNR = ublox_msgs::msg::CfgHNR::MESSAGE_ID; + static const uint8_t INF = ublox_msgs::msg::CfgINF::MESSAGE_ID; + static const uint8_t DGNSS = ublox_msgs::msg::CfgDGNSS::MESSAGE_ID; + static const uint8_t MSG = ublox_msgs::msg::CfgMSG::MESSAGE_ID; + static const uint8_t NAV5 = ublox_msgs::msg::CfgNAV5::MESSAGE_ID; + static const uint8_t NAVX5 = ublox_msgs::msg::CfgNAVX5::MESSAGE_ID; + static const uint8_t NMEA = ublox_msgs::msg::CfgNMEA::MESSAGE_ID; + static const uint8_t PRT = ublox_msgs::msg::CfgPRT::MESSAGE_ID; + static const uint8_t RATE = ublox_msgs::msg::CfgRATE::MESSAGE_ID; + static const uint8_t RST = ublox_msgs::msg::CfgRST::MESSAGE_ID; + static const uint8_t SBAS = ublox_msgs::msg::CfgSBAS::MESSAGE_ID; + static const uint8_t TMODE3 = ublox_msgs::msg::CfgTMODE3::MESSAGE_ID; + static const uint8_t USB = ublox_msgs::msg::CfgUSB::MESSAGE_ID; + static const uint8_t VALDEL = ublox_msgs::msg::CfgVALDEL::MESSAGE_ID; + static const uint8_t VALGET = ublox_msgs::msg::CfgVALGET::MESSAGE_ID; + static const uint8_t VALSET = ublox_msgs::msg::CfgVALSET::MESSAGE_ID; + } // namespace CFG + + namespace UPD { + //! SOS and SOS_Ack have the same message ID, but different lengths + static const uint8_t SOS = ublox_msgs::msg::UpdSOS::MESSAGE_ID; + } // namespace UPD + + namespace MON { + static const uint8_t GNSS = ublox_msgs::msg::MonGNSS::MESSAGE_ID; + static const uint8_t HW = ublox_msgs::msg::MonHW::MESSAGE_ID; + static const uint8_t VER = ublox_msgs::msg::MonVER::MESSAGE_ID; + } // namespace MON + + namespace ESF { + static const uint8_t INS = ublox_msgs::msg::EsfINS::MESSAGE_ID; + static const uint8_t MEAS = ublox_msgs::msg::EsfMEAS::MESSAGE_ID; + static const uint8_t RAW = ublox_msgs::msg::EsfRAW::MESSAGE_ID; + static const uint8_t STATUS = ublox_msgs::msg::EsfSTATUS::MESSAGE_ID; + } // namespace ESF + + namespace MGA { + static const uint8_t GAL = ublox_msgs::msg::MgaGAL::MESSAGE_ID; + } // namespace MGA + + namespace HNR { + static const uint8_t PVT = ublox_msgs::msg::HnrPVT::MESSAGE_ID; + } // namespace HNR + + namespace TIM { + static const uint8_t TM2 = ublox_msgs::msg::TimTM2::MESSAGE_ID; + } // namespace TIM +} // namespace Message + +} // namespace ublox_msgs + +#endif //!< UBLOX_MSGS_UBLOX_MSGS_HPP diff --git a/ublox_msgs/msg/Ack.msg b/ublox_msgs/msg/Ack.msg index fb7aba6b..85b24831 100644 --- a/ublox_msgs/msg/Ack.msg +++ b/ublox_msgs/msg/Ack.msg @@ -8,5 +8,5 @@ uint8 CLASS_ID = 5 uint8 NACK_MESSAGE_ID = 0 uint8 ACK_MESSAGE_ID = 1 -uint8 clsID # Class ID of the (Not-)Acknowledged Message -uint8 msgID # Message ID of the (Not-)Acknowledged Message \ No newline at end of file +uint8 cls_id # Class ID of the (Not-)Acknowledged Message +uint8 msg_id # Message ID of the (Not-)Acknowledged Message diff --git a/ublox_msgs/msg/AidALM.msg b/ublox_msgs/msg/AidALM.msg index 1997ee54..b770f8f0 100644 --- a/ublox_msgs/msg/AidALM.msg +++ b/ublox_msgs/msg/AidALM.msg @@ -2,27 +2,27 @@ # GPS Aiding Almanach Input/Output Message # # All UBX-AID messages are deprecated; use UBX-MGA messages instead -# - If the WEEK Value is 0, DWRD0 to DWRD7 are not sent as the almanach is not -# available for the given SV. This may happen even if NAV-SVINFO and RXM-SVSI -# are indicating almanac availability as the internal data may not represent +# - If the WEEK Value is 0, DWRD0 to DWRD7 are not sent as the almanach is not +# available for the given SV. This may happen even if NAV-SVINFO and RXM-SVSI +# are indicating almanac availability as the internal data may not represent # the content of an original broadcast almanac (or only parts thereof). # - DWORD0 to DWORD7 contain the 8 words following the Hand-Over Word ( HOW ) -# from the GPS navigation message, either pages 1 to 24 of sub-frame 5 or -# pages 2 to 10 of subframe 4. See IS-GPS-200 for a full description of the +# from the GPS navigation message, either pages 1 to 24 of sub-frame 5 or +# pages 2 to 10 of subframe 4. See IS-GPS-200 for a full description of the # contents of the Almanac pages. -# - In DWORD0 to DWORD7, the parity bits have been removed, and the 24 bits of +# - In DWORD0 to DWORD7, the parity bits have been removed, and the 24 bits of # data are located in Bits 0 to 23. Bits 24 to 31 shall be ignored. -# - Example: Parameter e (Eccentricity) from Almanach Subframe 4/5, Word 3, -# Bits 69-84 within the subframe can be found in DWRD0, Bits 15-0 whereas +# - Example: Parameter e (Eccentricity) from Almanach Subframe 4/5, Word 3, +# Bits 69-84 within the subframe can be found in DWRD0, Bits 15-0 whereas # Bit 0 is the LSB. uint8 CLASS_ID = 11 uint8 MESSAGE_ID = 48 -uint32 svid # SV ID for which the receiver shall return its +uint32 svid # SV ID for which the receiver shall return its # Almanac Data (Valid Range: 1 .. 32 or 51, 56, 63). uint32 week # Issue Date of Almanach (GPS week number) # Start of optional block -uint32[] dwrd # Almanach Words +uint32[] dwrd # Almanac Words # End of optional block diff --git a/ublox_msgs/msg/AidEPH.msg b/ublox_msgs/msg/AidEPH.msg index 81ffd014..45f3a8f2 100644 --- a/ublox_msgs/msg/AidEPH.msg +++ b/ublox_msgs/msg/AidEPH.msg @@ -20,14 +20,14 @@ uint8 CLASS_ID = 11 uint8 MESSAGE_ID = 49 -uint32 svid # SV ID for which this ephemeris data is +uint32 svid # SV ID for which this ephemeris data is # (Valid Range: 1 .. 32). uint32 how # Hand-Over Word of first Subframe. This is # required if data is sent to the receiver. # 0 indicates that no Ephemeris Data is following. # Start of optional block -uint32[] sf1d # Subframe 1 Words 3..10 (SF1D0..SF1D7) -uint32[] sf2d # Subframe 2 Words 3..10 (SF2D0..SF2D7) -uint32[] sf3d # Subframe 3 Words 3..10 (SF3D0..SF3D7) +uint32[] sf1d # Subframe 1 Words 3..10 (SF1D0..SF1D7) +uint32[] sf2d # Subframe 2 Words 3..10 (SF2D0..SF2D7) +uint32[] sf3d # Subframe 3 Words 3..10 (SF3D0..SF3D7) # End of optional block diff --git a/ublox_msgs/msg/AidHUI.msg b/ublox_msgs/msg/AidHUI.msg index ee9c6aba..a76d945f 100644 --- a/ublox_msgs/msg/AidHUI.msg +++ b/ublox_msgs/msg/AidHUI.msg @@ -8,28 +8,28 @@ uint8 CLASS_ID = 11 uint8 MESSAGE_ID = 2 -uint32 health # Bitmask, every bit represents a GPS SV (1-32). - # If the bit is set the SV is healthy. -float64 utcA0 # UTC - parameter A0 -float64 utcA1 # UTC - parameter A1 -int32 utcTOW # UTC - reference time of week -int16 utcWNT # UTC - reference week number -int16 utcLS # UTC - time difference due to leap seconds before event -int16 utcWNF # UTC - week number when next leap second event occurs -int16 utcDN # UTC - day of week when next leap second event occurs -int16 utcLSF # UTC - time difference due to leap seconds after event -int16 utcSpare # UTC - Spare to ensure structure is a multiple of 4 - # bytes -float32 klobA0 # Klobuchar - alpha 0 [s] -float32 klobA1 # Klobuchar - alpha 1 [s/semicircle] -float32 klobA2 # Klobuchar - alpha 2 [s/semicircle^2] -float32 klobA3 # Klobuchar - alpha 3 [s/semicircle^3] -float32 klobB0 # Klobuchar - beta 0 [s] -float32 klobB1 # Klobuchar - beta 1 [s/semicircle] -float32 klobB2 # Klobuchar - beta 2 [s/semicircle^2] -float32 klobB3 # Klobuchar - beta 3 [s/semicircle^3] -uint32 flags # flags -uint32 FLAGS_HEALTH = 1 # Healthmask field in this message is valid -uint32 FLAGS_UTC = 2 # UTC parameter fields in this message are valid -uint32 FLAGS_KLOB = 4 # Klobuchar parameter fields in this message are - # valid +uint32 health # Bitmask, every bit represents a GPS SV (1-32). + # If the bit is set the SV is healthy. +float64 utc_a0 # UTC - parameter A0 +float64 utc_a1 # UTC - parameter A1 +int32 utc_tow # UTC - reference time of week +int16 utc_wnt # UTC - reference week number +int16 utc_ls # UTC - time difference due to leap seconds before event +int16 utc_wnf # UTC - week number when next leap second event occurs +int16 utc_dn # UTC - day of week when next leap second event occurs +int16 utc_lsf # UTC - time difference due to leap seconds after event +int16 utc_spare # UTC - Spare to ensure structure is a multiple of 4 + # bytes +float32 klob_a0 # Klobuchar - alpha 0 [s] +float32 klob_a1 # Klobuchar - alpha 1 [s/semicircle] +float32 klob_a2 # Klobuchar - alpha 2 [s/semicircle^2] +float32 klob_a3 # Klobuchar - alpha 3 [s/semicircle^3] +float32 klob_b0 # Klobuchar - beta 0 [s] +float32 klob_b1 # Klobuchar - beta 1 [s/semicircle] +float32 klob_b2 # Klobuchar - beta 2 [s/semicircle^2] +float32 klob_b3 # Klobuchar - beta 3 [s/semicircle^3] +uint32 flags # flags +uint32 FLAGS_HEALTH = 1 # Healthmask field in this message is valid +uint32 FLAGS_UTC = 2 # UTC parameter fields in this message are valid +uint32 FLAGS_KLOB = 4 # Klobuchar parameter fields in this message are + # valid diff --git a/ublox_msgs/msg/CfgANT.msg b/ublox_msgs/msg/CfgANT.msg index c26b1252..c7e24320 100644 --- a/ublox_msgs/msg/CfgANT.msg +++ b/ublox_msgs/msg/CfgANT.msg @@ -5,21 +5,21 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 19 -uint16 flags # Antenna Flag Mask +uint16 flags # Antenna Flag Mask uint16 FLAGS_SVCS = 1 # Enable Antenna Supply Voltage Control Signal uint16 FLAGS_SCD = 2 # Enable Short Circuit Detection uint16 FLAGS_OCD = 4 # Enable Open Circuit Detection -uint16 FLAGS_PDWN_ON_SCD = 8 # Power Down Antenna supply if Short Circuit is +uint16 FLAGS_PDWN_ON_SCD = 8 # Power Down Antenna supply if Short Circuit is # detected. (only in combination with Bit 1) uint16 FLAGS_RECOVERY = 16 # Enable automatic recovery from short state -uint16 pins # Antenna Pin Configuration -uint16 PIN_SWITCH_MASK = 31 # PIO-Pin used for switching antenna supply - # (internal to TIM-LP/TIM-LF) -uint16 PIN_SCD_MASK = 992 # PIO-Pin used for detecting a short in the - # antenna supply -uint16 PIN_OCD_MASK = 31744 # PIO-Pin used for detecting open/not connected - # antenna -uint16 PIN_RECONFIG = 32678 # if set to one, and this command is sent to the - # receiver, the receiver will reconfigure the - # pins as specified. +uint16 pins # Antenna Pin Configuration +uint16 PIN_SWITCH_MASK = 31 # PIO-Pin used for switching antenna supply + # (internal to TIM-LP/TIM-LF) +uint16 PIN_SCD_MASK = 992 # PIO-Pin used for detecting a short in the + # antenna supply +uint16 PIN_OCD_MASK = 31744 # PIO-Pin used for detecting open/not connected + # antenna +uint16 PIN_RECONFIG = 32678 # if set to one, and this command is sent to the + # receiver, the receiver will reconfigure the + # pins as specified. diff --git a/ublox_msgs/msg/CfgCFG.msg b/ublox_msgs/msg/CfgCFG.msg index 6f9cdf61..a83b7359 100644 --- a/ublox_msgs/msg/CfgCFG.msg +++ b/ublox_msgs/msg/CfgCFG.msg @@ -5,19 +5,19 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 9 -uint32 clearMask # Mask with configuration sub-sections to Clear - # (=Load Default Configurations to Permanent - # Configurations in non-volatile memory) -uint32 saveMask # Mask with configuration sub-section to Save - # (=Save Current Configuration to Non-volatile - # Memory) -uint32 loadMask # Mask with configuration sub-sections to Load - # (=Load Permanent Configurations from - # Non-volatile Memory to Current Configurations) +uint32 clear_mask # Mask with configuration sub-sections to Clear + # (=Load Default Configurations to Permanent + # Configurations in non-volatile memory) +uint32 save_mask # Mask with configuration sub-section to Save + # (=Save Current Configuration to Non-volatile + # Memory) +uint32 load_mask # Mask with configuration sub-sections to Load + # (=Load Permanent Configurations from + # Non-volatile Memory to Current Configurations) -uint32 MASK_IO_PORT = 1 # Communications port settings. Modifying this - # sub-section results in an IO system reset. - # Because of this undefined data may be output +uint32 MASK_IO_PORT = 1 # Communications port settings. Modifying this + # sub-section results in an IO system reset. + # Because of this undefined data may be output # for a short period of time after receiving the # message. uint32 MASK_MSG_CONF = 2 # Message Configuration @@ -28,10 +28,10 @@ uint32 MASK_SEN_CONF = 256 # Sensor Interface Configuration, protocol >= 19 uint32 MASK_RINV_CONF = 512 # Remote Inventory Configuration uint32 MASK_ANT_CONF = 1024 # Antenna Configuration uint32 MASK_LOG_CONF = 2048 # Logging Configuration -uint32 MASK_FTS_CONF = 4096 # FTS Configuration. Only applicable to the +uint32 MASK_FTS_CONF = 4096 # FTS Configuration. Only applicable to the # FTS product variant. -uint8 deviceMask # Mask which selects the devices for this command +uint8 device_mask # Mask which selects the devices for this command uint8 DEV_BBR = 1 # device battery backed RAM uint8 DEV_FLASH = 2 # device Flash uint8 DEV_EEPROM = 4 # device EEPROM diff --git a/ublox_msgs/msg/CfgDAT.msg b/ublox_msgs/msg/CfgDAT.msg index a659d7bb..cd8d57fa 100644 --- a/ublox_msgs/msg/CfgDAT.msg +++ b/ublox_msgs/msg/CfgDAT.msg @@ -1,37 +1,37 @@ # CFG-DAT (0x06 0x06) # Set User-defined Datum -# +# # For more information see the description of Geodetic Systems and Frames. -# +# uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 6 # Only for GET, these values are not used for write -uint16 datumNum # Datum Number: 0 = WGS84, 0xFFFF = user-defined +uint16 datum_num # Datum Number: 0 = WGS84, 0xFFFF = user-defined uint16 DATUM_NUM_WGS84 = 0 uint16 DATUM_NUM_USER = 65535 -uint8[6] datumName # ASCII String: WGS84 or USER +uint8[6] datum_name # ASCII String: WGS84 or USER -float64 majA # Semi-major Axis [m] - # accepted range = 6,300,000.0 to 6,500,000.0 meters -float64 flat # 1.0 / Flattening - # accepted range is 0.0 to 500.0 +float64 maj_a # Semi-major Axis [m] + # accepted range = 6,300,000.0 to 6,500,000.0 meters +float64 flat # 1.0 / Flattening + # accepted range is 0.0 to 500.0 -float32 dX # X Axis shift at the origin [m] - # accepted range is +/- 5000.0 meters -float32 dY # Y Axis shift at the origin [m] - # accepted range is +/- 5000.0 meters -float32 dZ # Z Axis shift at the origin [m] - # accepted range is +/- 5000.0 meters +float32 d_x # X Axis shift at the origin [m] + # accepted range is +/- 5000.0 meters +float32 d_y # Y Axis shift at the origin [m] + # accepted range is +/- 5000.0 meters +float32 d_z # Z Axis shift at the origin [m] + # accepted range is +/- 5000.0 meters -float32 rotX # Rotation about the X Axis [s] - # accepted range is +/- 20.0 milli-arc seconds -float32 rotY # Rotation about the Y Axis [s] - # accepted range is +/- 20.0 milli-arc seconds -float32 rotZ # Rotation about the Z Axis [s] - # accepted range is +/- 20.0 milli-arc seconds +float32 rot_x # Rotation about the X Axis [s] + # accepted range is +/- 20.0 milli-arc seconds +float32 rot_y # Rotation about the Y Axis [s] + # accepted range is +/- 20.0 milli-arc seconds +float32 rot_z # Rotation about the Z Axis [s] + # accepted range is +/- 20.0 milli-arc seconds -float32 scale # Scale change [ppm] - # accepted range is 0.0 to 50.0 parts per million \ No newline at end of file +float32 scale # Scale change [ppm] + # accepted range is 0.0 to 50.0 parts per million diff --git a/ublox_msgs/msg/CfgDGNSS.msg b/ublox_msgs/msg/CfgDGNSS.msg index 6baa38e8..7adbce74 100644 --- a/ublox_msgs/msg/CfgDGNSS.msg +++ b/ublox_msgs/msg/CfgDGNSS.msg @@ -1,7 +1,7 @@ # CFG-DGNSS (0x06 0x70) # DGNSS configuration # -# This message allows the user to configure the DGNSS configuration of the +# This message allows the user to configure the DGNSS configuration of the # receiver. # Supported on: # - u-blox 8 / u-blox M8 from protocol version 20.01 up to version 23.01 (only @@ -10,9 +10,9 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 112 -uint8 dgnssMode # Specifies differential mode: +uint8 dgnss_mode # Specifies differential mode: uint8 DGNSS_MODE_RTK_FLOAT = 2 # RTK float: No attempts are made to fix # ambiguities. uint8 DGNSS_MODE_RTK_FIXED = 3 # RTK fixed: Ambiguities are fixed whenever # possible. -uint8[3] reserved0 # Reserved \ No newline at end of file +uint8[3] reserved0 # Reserved diff --git a/ublox_msgs/msg/CfgGNSS.msg b/ublox_msgs/msg/CfgGNSS.msg index f7a8c4aa..50a20989 100644 --- a/ublox_msgs/msg/CfgGNSS.msg +++ b/ublox_msgs/msg/CfgGNSS.msg @@ -2,43 +2,43 @@ # GNSS Configuration # # Gets or sets the GNSS system channel sharing configuration. -# If the receiver is sent a valid new configuration, it will respond with a +# If the receiver is sent a valid new configuration, it will respond with a # UBX-ACK-ACK message and immediately change to the new configuration. Otherwise -# the receiver will reject the request, by issuing a UBX-ACK-NAK and continuing +# the receiver will reject the request, by issuing a UBX-ACK-NAK and continuing # operation with the previous configuration. # Configuration requirements: -# - It is necessary for at least one major GNSS to be enabled, after applying +# - It is necessary for at least one major GNSS to be enabled, after applying # the new configuration to the current one. -# - It is also required that at least 4 tracking channels are available to each +# - It is also required that at least 4 tracking channels are available to each # enabled major GNSS, i.e. maxTrkCh must have a minimum value of 4 for each # enabled major GNSS. -# - The number of tracking channels in use must not exceed the number of -# tracking channels available in hardware, and the sum of all reserved +# - The number of tracking channels in use must not exceed the number of +# tracking channels available in hardware, and the sum of all reserved # tracking channels needs to be less than or equal to the number of tracking # channels in use. # Notes: # - To avoid cross-correlation issues, it is recommended that GPS and QZSS are # always both enabled or both disabled. -# - Polling this message returns the configuration of all supported GNSS, -# whether enabled or not; it may also include GNSS unsupported by the +# - Polling this message returns the configuration of all supported GNSS, +# whether enabled or not; it may also include GNSS unsupported by the # particular product, but in such cases the enable flag will always be unset. -# - See section GNSS Configuration for a discussion of the use of this message +# - See section GNSS Configuration for a discussion of the use of this message # and section Satellite Numbering for a description of the GNSS IDs available -# - Configuration specific to the GNSS system can be done via other messages +# - Configuration specific to the GNSS system can be done via other messages # (e.g. UBX-CFG-SBAS). # uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 62 -uint8 msgVer # Message version (= 0 for this version) -uint8 numTrkChHw # Number of tracking channels in hardware (read only) -uint8 numTrkChUse # (Read only in protocol versions greater than 23) - # Number of tracking channels to use (<= numTrkChHw) - # If 0xFF, then number of tracking channels to use will - # be set to numTrkChHw -uint8 numConfigBlocks # Number of configuration blocks following +uint8 msg_ver # Message version (= 0 for this version) +uint8 num_trk_ch_hw # Number of tracking channels in hardware (read only) +uint8 num_trk_ch_use # (Read only in protocol versions greater than 23) + # Number of tracking channels to use (<= numTrkChHw) + # If 0xFF, then number of tracking channels to use will + # be set to numTrkChHw +uint8 num_config_blocks # Number of configuration blocks following # Start of repeated block (numConfigBlocks times) -CfgGNSS_Block[] blocks -# End of repeated block \ No newline at end of file +CfgGNSSBlock[] blocks +# End of repeated block diff --git a/ublox_msgs/msg/CfgGNSS_Block.msg b/ublox_msgs/msg/CfgGNSSBlock.msg similarity index 56% rename from ublox_msgs/msg/CfgGNSS_Block.msg rename to ublox_msgs/msg/CfgGNSSBlock.msg index fcc36c66..4e4264fc 100644 --- a/ublox_msgs/msg/CfgGNSS_Block.msg +++ b/ublox_msgs/msg/CfgGNSSBlock.msg @@ -1,7 +1,7 @@ # see Cfg-GNSS message # -uint8 gnssId # System identifier (see Satellite Numbering) +uint8 gnss_id # System identifier (see Satellite Numbering) uint8 GNSS_ID_GPS = 0 uint8 GNSS_ID_SBAS = 1 @@ -11,29 +11,29 @@ uint8 GNSS_ID_IMES = 4 uint8 GNSS_ID_QZSS = 5 uint8 GNSS_ID_GLONASS = 6 -uint8 resTrkCh # (Read only in protocol versions greater than 23) - # Number of reserved (minimum) tracking channels - # for this GNSS system +uint8 res_trk_ch # (Read only in protocol versions greater than 23) + # Number of reserved (minimum) tracking channels + # for this GNSS system uint8 RES_TRK_CH_GPS = 8 uint8 RES_TRK_CH_QZSS = 0 uint8 RES_TRK_CH_SBAS = 0 uint8 RES_TRK_CH_GLONASS = 8 -uint8 maxTrkCh # (Read only in protocol versions greater than 23) - # Maximum number of tracking channels used for this - # system. Must be > 0, >= resTrkChn, <= numTrkChUse and - # <= maximum number of tracking channels supported for - # this system -uint8 MAX_TRK_CH_MAJOR_MIN = 4 # maxTrkCh must have this minimum value +uint8 max_trk_ch # (Read only in protocol versions greater than 23) + # Maximum number of tracking channels used for this + # system. Must be > 0, >= resTrkChn, <= numTrkChUse and + # <= maximum number of tracking channels supported for + # this system +uint8 MAX_TRK_CH_MAJOR_MIN = 4 # max_trk_ch must have this minimum value # for each enabled major GNSS uint8 MAX_TRK_CH_GPS = 16 uint8 MAX_TRK_CH_GLONASS = 14 uint8 MAX_TRK_CH_QZSS = 3 uint8 MAX_TRK_CH_SBAS = 3 -uint8 reserved1 # Reserved +uint8 reserved1 # Reserved -uint32 flags # Bitfield of flags. At least one signal must be - # configured in every enabled system. +uint32 flags # Bitfield of flags. At least one signal must be + # configured in every enabled system. uint32 FLAGS_ENABLE = 1 # Enable this system uint32 FLAGS_SIG_CFG_MASK = 16711680 # Signal configuration mask uint32 SIG_CFG_GPS_L1CA = 65536 # When gnssId is 0 (GPS) @@ -41,14 +41,14 @@ uint32 SIG_CFG_GPS_L1CA = 65536 # When gnssId is 0 (GPS) uint32 SIG_CFG_SBAS_L1CA = 65536 # When gnssId is 1 (SBAS) # * 0x01 = SBAS L1C/A uint32 SIG_CFG_GALILEO_E1OS = 65536 # When gnssId is 2 (Galileo) - # * 0x01 = Galileo E1OS (not supported in + # * 0x01 = Galileo E1OS (not supported in # protocol versions less than 18) uint32 SIG_CFG_BEIDOU_B1I = 65536 # When gnssId is 3 (BeiDou) - # * 0x01 = BeiDou B1I + # * 0x01 = BeiDou B1I uint32 SIG_CFG_IMES_L1 = 65536 # When gnssId is 4 (IMES) # * 0x01 = IMES L1 uint32 SIG_CFG_QZSS_L1CA = 65536 # When gnssId is 5 (QZSS) # * 0x01 = QZSS L1C/A uint32 SIG_CFG_QZSS_L1SAIF = 262144 # * 0x04 = QZSS L1SAIF uint32 SIG_CFG_GLONASS_L1OF = 65536 # When gnssId is 6 (GLONASS) - # * 0x01 = GLONASS L1OF \ No newline at end of file + # * 0x01 = GLONASS L1OF diff --git a/ublox_msgs/msg/CfgHNR.msg b/ublox_msgs/msg/CfgHNR.msg index e81e8adb..ee6faca6 100644 --- a/ublox_msgs/msg/CfgHNR.msg +++ b/ublox_msgs/msg/CfgHNR.msg @@ -1,11 +1,11 @@ # CFG-HNR (0x06 0x5C) # High Navigation Rate Settings # -# The u-blox receivers support high rates of navigation update up to 30 Hz. +# The u-blox receivers support high rates of navigation update up to 30 Hz. # The navigation solution output (NAV-HNR) will not be aligned to the top of a # second. # The update rate has a direct influence on the power consumption. The more -# fixes that are required, the more CPU power and communication resources are +# fixes that are required, the more CPU power and communication resources are # required. # For most applications a 1 Hz update rate would be sufficient. # @@ -15,5 +15,5 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 92 -uint8 highNavRate # Rate of navigation solution output [Hz] -uint8[3] reserved0 # Reserved \ No newline at end of file +uint8 high_nav_rate # Rate of navigation solution output [Hz] +uint8[3] reserved0 # Reserved diff --git a/ublox_msgs/msg/CfgINF.msg b/ublox_msgs/msg/CfgINF.msg index 8d93dae1..ae11b93d 100644 --- a/ublox_msgs/msg/CfgINF.msg +++ b/ublox_msgs/msg/CfgINF.msg @@ -1,13 +1,13 @@ # CFG-INF (0x06 0x02) # Information message configuration # -# The value of infMsgMask[x] below are that each bit represents one of the INF -# class messages (Bit 0 for ERROR, Bit 1 for WARNING and so on.). For a complete -# list, see the Message Class INF. Several configurations can be concatenated to +# The value of infMsgMask[x] below are that each bit represents one of the INF +# class messages (Bit 0 for ERROR, Bit 1 for WARNING and so on.). For a complete +# list, see the Message Class INF. Several configurations can be concatenated to # one input message. -# In this case the payload length can be a multiple of the normal length. Output -# messages from the module contain only one configuration unit. Note that I/O -# Ports 1 and 2 correspond to serial ports 1 and 2. I/O port 0 is DDC. I/O port +# In this case the payload length can be a multiple of the normal length. Output +# messages from the module contain only one configuration unit. Note that I/O +# Ports 1 and 2 correspond to serial ports 1 and 2. I/O port 0 is DDC. I/O port # 3 is USB. I/O port 4 is SPI. I/O port 5 is reserved for future use # @@ -15,5 +15,5 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 2 # start of repeated block -CfgINF_Block[] blocks +CfgINFBlock[] blocks # end of repeated block diff --git a/ublox_msgs/msg/CfgINFBlock.msg b/ublox_msgs/msg/CfgINFBlock.msg new file mode 100644 index 00000000..d18c6d37 --- /dev/null +++ b/ublox_msgs/msg/CfgINFBlock.msg @@ -0,0 +1,21 @@ +# See CfgINF message +# + +uint8 protocol_id # Protocol Identifier, identifying for which + # protocol the configuration is set/get. The + # following are valid Protocol Identifiers: + # 0: UBX Protocol + # 1: NMEA Protocol + # 2-255: Reserved +uint8 PROTOCOL_ID_UBX = 0 +uint8 PROTOCOL_ID_NMEA = 1 + +uint8[3] reserved1 # Reserved + +uint8[6] inf_msg_mask # A bit mask, saying which information messages + # are enabled on each I/O port +uint8 INF_MSG_ERROR = 1 # enable ERROR +uint8 INF_MSG_WARNING = 2 # enable WARNING +uint8 INF_MSG_NOTICE = 4 # enable NOTICE +uint8 INF_MSG_TEST = 8 # enable TEST +uint8 INF_MSG_DEBUG = 16 # enable DEBUG diff --git a/ublox_msgs/msg/CfgINF_Block.msg b/ublox_msgs/msg/CfgINF_Block.msg deleted file mode 100644 index 4929438c..00000000 --- a/ublox_msgs/msg/CfgINF_Block.msg +++ /dev/null @@ -1,21 +0,0 @@ -# See CfgINF message -# - -uint8 protocolID # Protocol Identifier, identifying for which - # protocol the configuration is set/get. The - # following are valid Protocol Identifiers: - # 0: UBX Protocol - # 1: NMEA Protocol - # 2-255: Reserved -uint8 PROTOCOL_ID_UBX = 0 -uint8 PROTOCOL_ID_NMEA = 1 - -uint8[3] reserved1 # Reserved - -uint8[6] infMsgMask # A bit mask, saying which information messages - # are enabled on each I/O port -uint8 INF_MSG_ERROR = 1 # enable ERROR -uint8 INF_MSG_WARNING = 2 # enable WARNING -uint8 INF_MSG_NOTICE = 4 # enable NOTICE -uint8 INF_MSG_TEST = 8 # enable TEST -uint8 INF_MSG_DEBUG = 16 # enable DEBUG \ No newline at end of file diff --git a/ublox_msgs/msg/CfgMSG.msg b/ublox_msgs/msg/CfgMSG.msg index 64065e8f..d6bc5637 100644 --- a/ublox_msgs/msg/CfgMSG.msg +++ b/ublox_msgs/msg/CfgMSG.msg @@ -1,12 +1,12 @@ # CFG-MSG (0x06 0x01) # Message Rate(s) -# +# # Set message rate for the current port uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 1 -uint8 msgClass # Message Class -uint8 msgID # Message Identifier -uint8 rate # Send rate on current port - # [number of navigation solutions] +uint8 msg_class # Message Class +uint8 msg_id # Message Identifier +uint8 rate # Send rate on current port + # [number of navigation solutions] diff --git a/ublox_msgs/msg/CfgNAV5.msg b/ublox_msgs/msg/CfgNAV5.msg index 0010b6f4..3f044a12 100644 --- a/ublox_msgs/msg/CfgNAV5.msg +++ b/ublox_msgs/msg/CfgNAV5.msg @@ -4,8 +4,8 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 36 -uint16 mask # Parameters Bitmask. Only the masked - # parameters will be applied. +uint16 mask # Parameters Bitmask. Only the masked + # parameters will be applied. uint16 MASK_DYN = 1 # Apply dynamic model settings uint16 MASK_MIN_EL = 2 # Apply minimum elevation settings uint16 MASK_FIX_MODE = 4 # Apply fix mode settings @@ -17,7 +17,7 @@ uint16 MASK_DGPS_MASK = 128 # Apply DGPS settings, firmware >= 7 only uint16 MASK_CNO = 256 # Apply CNO threshold settings uint16 MASK_UTC = 1024 # Apply UTC settings, protocol >= 16 only -uint8 dynModel # Dynamic Platform model: +uint8 dyn_model # Dynamic Platform model: uint8 DYN_MODEL_PORTABLE = 0 # Portable uint8 DYN_MODEL_STATIONARY = 2 # Stationary uint8 DYN_MODEL_PEDESTRIAN = 3 # Pedestrian @@ -28,39 +28,39 @@ uint8 DYN_MODEL_AIRBORNE_2G = 7 # Airborne with <2g Acceleration uint8 DYN_MODEL_AIRBORNE_4G = 8 # Airborne with <4g Acceleration uint8 DYN_MODEL_WRIST_WATCH = 9 # Wrist watch, protocol >= 18 -uint8 fixMode # Position Fixing Mode. +uint8 fix_mode # Position Fixing Mode. uint8 FIX_MODE_2D_ONLY = 1 # 2D only uint8 FIX_MODE_3D_ONLY = 2 # 3D only uint8 FIX_MODE_AUTO = 3 # Auto 2D/3D -int32 fixedAlt # Fixed altitude (mean sea level) for 2D fix mode. - # [m / 0.01] -uint32 fixedAltVar # Fixed altitude variance for 2D mode. [m^2 / 0.0001] -int8 minElev # Minimum Elevation for a GNSS satellite to be used in - # NAV [deg] -uint8 drLimit # Maximum time to perform dead reckoning [s] - # (linear extrapolation) in case of GPS signal loss -uint16 pDop # Position DOP Mask to use [1 / 0.1] -uint16 tDop # Time DOP Mask to use [1 / 0.1] -uint16 pAcc # Position Accuracy Mask [m] -uint16 tAcc # Time Accuracy Mask [m] -uint8 staticHoldThresh # Static hold threshold [cm/s] -uint8 dgnssTimeOut # DGNSS timeout, firmware 7 and newer only [s] +int32 fixed_alt # Fixed altitude (mean sea level) for 2D fix mode. + # [m / 0.01] +uint32 fixed_alt_var # Fixed altitude variance for 2D mode. [m^2 / 0.0001] +int8 min_elev # Minimum Elevation for a GNSS satellite to be used in + # NAV [deg] +uint8 dr_limit # Maximum time to perform dead reckoning [s] + # (linear extrapolation) in case of GPS signal loss +uint16 p_dop # Position DOP Mask to use [1 / 0.1] +uint16 t_dop # Time DOP Mask to use [1 / 0.1] +uint16 p_acc # Position Accuracy Mask [m] +uint16 t_acc # Time Accuracy Mask [m] +uint8 static_hold_thresh # Static hold threshold [cm/s] +uint8 dgnss_time_out # DGNSS timeout, firmware 7 and newer only [s] -uint8 cnoThreshNumSvs # Number of satellites required to have C/N0 above - # cnoThresh for a fix to be attempted -uint8 cnoThresh # C/N0 threshold for deciding whether to attempt a fix - # [dBHz] -uint8[2] reserved1 # Reserved +uint8 cno_thresh_num_svs # Number of satellites required to have C/N0 above + # cnoThresh for a fix to be attempted +uint8 cno_thresh # C/N0 threshold for deciding whether to attempt a fix + # [dBHz] +uint8[2] reserved1 # Reserved -uint16 staticHoldMaxDist # Static hold distance threshold (before quitting - # static hold) [m] -uint8 utcStandard # UTC standard to be used: -uint8 UTC_STANDARD_AUTOMATIC = 0 # receiver selects based on GNSS configuration -uint8 UTC_STANDARD_GPS = 3 # UTC as operated by the U.S. Naval Observatory - # (USNO); derived from GPS time -uint8 UTC_STANDARD_GLONASS = 6 # UTC as operated by the former Soviet Union; - # derived from GLONASS time -uint8 UTC_STANDARD_BEIDOU = 7 # UTC as operated by the National Time Service - # Center, China; derived from BeiDou time -uint8[5] reserved2 # Reserved \ No newline at end of file +uint16 static_hold_max_dist # Static hold distance threshold (before quitting + # static hold) [m] +uint8 utc_standard # UTC standard to be used: +uint8 UTC_STANDARD_AUTOMATIC = 0 # receiver selects based on GNSS configuration +uint8 UTC_STANDARD_GPS = 3 # UTC as operated by the U.S. Naval Observatory + # (USNO); derived from GPS time +uint8 UTC_STANDARD_GLONASS = 6 # UTC as operated by the former Soviet Union; + # derived from GLONASS time +uint8 UTC_STANDARD_BEIDOU = 7 # UTC as operated by the National Time Service + # Center, China; derived from BeiDou time +uint8[5] reserved2 # Reserved diff --git a/ublox_msgs/msg/CfgNAVX5.msg b/ublox_msgs/msg/CfgNAVX5.msg index dde005d9..74ff9225 100644 --- a/ublox_msgs/msg/CfgNAVX5.msg +++ b/ublox_msgs/msg/CfgNAVX5.msg @@ -6,62 +6,62 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 35 -uint16 version # Message version (set to 0) +uint16 version # Message version (set to 0) -uint16 mask1 # First parameters bitmask (possible values below) +uint16 mask1 # First parameters bitmask (possible values below) uint16 MASK1_MIN_MAX = 4 # apply min/max SVs settings uint16 MASK1_MIN_CNO = 8 # apply minimum C/N0 setting uint16 MASK1_INITIAL_FIX_3D = 64 # apply initial 3D fix settings uint16 MASK1_WKN_ROLL = 512 # apply GPS week number rollover settings -uint16 MASK1_ACK_AID = 1024 # apply assistance acknowledgment +uint16 MASK1_ACK_AID = 1024 # apply assistance acknowledgment # settings uint16 MASK1_PPP = 8192 # apply usePPP flag -uint16 MASK1_AOP = 16384 # apply aopCfg (useAOP flag) and +uint16 MASK1_AOP = 16384 # apply aopCfg (useAOP flag) and # aopOrbMaxErr settings # (AssistNow Autonomous) -uint32 mask2 # Second parameters bitmask (possible values below) - # Firmware >=8 only -uint32 MASK2_ADR = 64 # Apply ADR sensor fusion on/off - # setting -uint32 MASK2_SIG_ATTEN_COMP_MODE = 128 # Apply signal attenuation - # compensation feature settings +uint32 mask2 # Second parameters bitmask (possible values below) + # Firmware >=8 only +uint32 MASK2_ADR = 64 # Apply ADR sensor fusion on/off + # setting +uint32 MASK2_SIG_ATTEN_COMP_MODE = 128 # Apply signal attenuation + # compensation feature settings -uint8[2] reserved1 # Always set to zero +uint8[2] reserved1 # Always set to zero -uint8 minSVs # Minimum number of satellites for navigation -uint8 maxSVs # Maximum number of satellites for navigation -uint8 minCNO # Minimum satellite signal level for navigation [dBHz] +uint8 min_svs # Minimum number of satellites for navigation +uint8 max_svs # Maximum number of satellites for navigation +uint8 min_cno # Minimum satellite signal level for navigation [dBHz] -uint8 reserved2 # Always set to zero +uint8 reserved2 # Always set to zero -uint8 iniFix3D # If set to 1, initial fix must be 3D +uint8 ini_fix3d # If set to 1, initial fix must be 3D -uint8[2] reserved3 # Always set to zero +uint8[2] reserved3 # Always set to zero -uint8 ackAiding # If set to 1, issue acknowledgments for assistance -uint16 wknRollover # GPS week rollover number, GPS week numbers will be set - # correctly from this week up to 1024 weeks after this - # week -uint8 sigAttenCompMode # Permanently attenuated signal compensation [dBHz] - # 0 = disabled, 255 = automatic - # 1..63 = maximum expected C/N0 value - # Firmware 8 only +uint8 ack_aiding # If set to 1, issue acknowledgments for assistance +uint16 wkn_rollover # GPS week rollover number, GPS week numbers will be set + # correctly from this week up to 1024 weeks after this + # week +uint8 sig_atten_comp_mode # Permanently attenuated signal compensation [dBHz] + # 0 = disabled, 255 = automatic + # 1..63 = maximum expected C/N0 value + # Firmware 8 only -uint8[5] reserved4 # Always set to zero +uint8[5] reserved4 # Always set to zero -uint8 usePPP # Enable/disable PPP (on supported units) -uint8 aopCfg # AssistNow Autonomous configuration, 1: enabled +uint8 use_ppp # Enable/disable PPP (on supported units) +uint8 aop_cfg # AssistNow Autonomous configuration, 1: enabled -uint8[2] reserved5 # Always set to zero +uint8[2] reserved5 # Always set to zero -uint16 aopOrbMaxErr # Maximum acceptable (modeled) autonomous orbit - # error [m] - # valid range = 5..1000 - # 0 = reset to firmware default +uint16 aop_orb_max_err # Maximum acceptable (modeled) autonomous orbit + # error [m] + # valid range = 5..1000 + # 0 = reset to firmware default -uint8[7] reserved6 # Always set to zero +uint8[7] reserved6 # Always set to zero -uint8 useAdr # Enable/disable ADR sensor fusion - # 1: enabled, 0: disabled - # Only supported on certain products +uint8 use_adr # Enable/disable ADR sensor fusion + # 1: enabled, 0: disabled + # Only supported on certain products diff --git a/ublox_msgs/msg/CfgNMEA.msg b/ublox_msgs/msg/CfgNMEA.msg index e029e5c5..e3486e15 100644 --- a/ublox_msgs/msg/CfgNMEA.msg +++ b/ublox_msgs/msg/CfgNMEA.msg @@ -1,16 +1,16 @@ # CFG-NMEA (0x06 0x17) # NMEA protocol configuration V1 # -# Set/Get the NMEA protocol configuration. See section NMEA Protocol -# Configuration for a detailed description of the configuration effects on +# Set/Get the NMEA protocol configuration. See section NMEA Protocol +# Configuration for a detailed description of the configuration effects on # NMEA output # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 23 +uint8 MESSAGE_ID = 23 uint8 filter # filter flags -uint8 FILTER_POS = 1 # Enable position output for failed or +uint8 FILTER_POS = 1 # Enable position output for failed or # invalid fixes uint8 FILTER_MSK_POS = 2 # Enable position output for invalid fixes uint8 FILTER_TIME = 4 # Enable time output for invalid times @@ -18,30 +18,30 @@ uint8 FILTER_DATE = 8 # Enable date output for invalid dates uint8 FILTER_GPS_ONLY = 16 # Restrict output to GPS satellites only uint8 FILTER_TRACK = 32 # Enable COG output even if COG is frozen -uint8 nmeaVersion # NMEA version +uint8 nmea_version # NMEA version uint8 NMEA_VERSION_4_1 = 65 # Version 4.1 uint8 NMEA_VERSION_4_0 = 64 # Version 4.0 uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 -uint8 numSV # Maximum Number of SVs to report per TalkerId: +uint8 num_sv # Maximum Number of SVs to report per TalkerId: # unlimited (0) or 8, 12, 16 -uint8 NUM_SV_UNLIMITED = 0 +uint8 NUM_SV_UNLIMITED = 0 uint8 flags # flags uint8 FLAGS_COMPAT = 1 # enable compatibility mode. - # This might be needed for certain applications - # when customer's NMEA parser expects a fixed + # This might be needed for certain applications + # when customer's NMEA parser expects a fixed # number of digits in position coordinates uint8 FLAGS_CONSIDER = 2 # enable considering mode uint8 FLAGS_LIMIT82 = 4 # enable strict limit to 82 characters maximum uint8 FLAGS_HIGH_PREC = 8 # enable high precision mode - # This flag cannot be set in conjunction with + # This flag cannot be set in conjunction with # either Compatibility Mode or Limit82 Mode. # (not supported in protocol versions < 20.01) -uint32 gnssToFilter # Filters out satellites based on their GNSS. - # If a bitfield is enabled, the corresponding +uint32 gnss_to_filter # Filters out satellites based on their GNSS. + # If a bitfield is enabled, the corresponding # satellites will be not output. uint32 GNSS_TO_FILTER_GPS = 1 # Disable reporting of GPS satellites uint32 GNSS_TO_FILTER_SBAS = 2 # Disable reporting of SBAS satellites @@ -49,17 +49,17 @@ uint32 GNSS_TO_FILTER_QZSS = 16 # Disable reporting of QZSS satellites uint32 GNSS_TO_FILTER_GLONASS = 32 # Disable reporting of GLONASS satellites uint32 GNSS_TO_FILTER_BEIDOU = 64 # Disable reporting of BeiDou satellites -uint8 svNumbering # Configures the display of satellites that do not +uint8 sv_numbering # Configures the display of satellites that do not # have an NMEA-defined value. Note: this does not # apply to satellites with an unknown ID. uint8 SV_NUMBERING_STRICT = 0 # Strict - Satellites are not output uint8 SV_NUMBERING_EXTENDED = 1 # Extended - Use proprietary numbering -uint8 mainTalkerId # By default the main Talker ID (i.e. the Talker - # ID used for all messages other than GSV) is - # determined by the GNSS assignment of the - # receiver's channels (see CfgGNSS). - # This field enables the main Talker ID to be +uint8 main_talker_id # By default the main Talker ID (i.e. the Talker + # ID used for all messages other than GSV) is + # determined by the GNSS assignment of the + # receiver's channels (see CfgGNSS). + # This field enables the main Talker ID to be # overridden uint8 MAIN_TALKER_ID_NOT_OVERRIDDEN = 0 # Main Talker ID is not overridden uint8 MAIN_TALKER_ID_GP = 1 # Set main Talker ID to 'GP' @@ -68,18 +68,18 @@ uint8 MAIN_TALKER_ID_GN = 3 # Set main Talker ID to 'GN' uint8 MAIN_TALKER_ID_GA = 4 # Set main Talker ID to 'GA' uint8 MAIN_TALKER_ID_GB = 5 # Set main Talker ID to 'GB' -uint8 gsvTalkerId # By default the Talker ID for GSV messages is - # GNSS specific (as defined by NMEA). This field +uint8 gsv_talker_id # By default the Talker ID for GSV messages is + # GNSS specific (as defined by NMEA). This field # enables the GSV Talker ID to be overridden. -uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID +uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID # (as defined by NMEA) uint8 GSV_TALKER_ID_MAIN = 1 # Use the main Talker ID uint8 version # Message version (set to 1 for this version) uint8 VERSION = 1 -uint8[2] bdsTalkerId # Sets the two characters that should be used - # for the BeiDou Talker ID. If these are set to +uint8[2] bds_talker_id # Sets the two characters that should be used + # for the BeiDou Talker ID. If these are set to # zero, the default BeiDou TalkerId will be used -uint8[6] reserved1 # Reserved \ No newline at end of file +uint8[6] reserved1 # Reserved diff --git a/ublox_msgs/msg/CfgNMEA6.msg b/ublox_msgs/msg/CfgNMEA6.msg index a62cc358..94e83e06 100644 --- a/ublox_msgs/msg/CfgNMEA6.msg +++ b/ublox_msgs/msg/CfgNMEA6.msg @@ -1,15 +1,15 @@ # CFG-NMEA (0x06 0x17) # NMEA protocol configuration # -# Set/Get the NMEA protocol configuration. See section NMEA Protocol -# Configuration for a detailed description of the configuration effects on +# Set/Get the NMEA protocol configuration. See section NMEA Protocol +# Configuration for a detailed description of the configuration effects on # NMEA output # # Supported on u-blox 6 from firmware version 6.00 up to version 7.03. # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 23 +uint8 MESSAGE_ID = 23 uint8 filter # filter flags uint8 FILTER_POS = 1 # Disable position filtering @@ -23,7 +23,7 @@ uint8 version # NMEA version uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 -uint8 numSV # Maximum Number of SVs to report in NMEA +uint8 num_sv # Maximum Number of SVs to report in NMEA # protocol. # This does not affect the receiver's operation. # It only limits the number of SVs reported in @@ -33,7 +33,7 @@ uint8 numSV # Maximum Number of SVs to report in NMEA uint8 flags # flags uint8 FLAGS_COMPAT = 1 # enable compatibility mode. - # This might be needed for certain applications - # when customer's NMEA parser expects a fixed + # This might be needed for certain applications + # when customer's NMEA parser expects a fixed # number of digits in position coordinates uint8 FLAGS_CONSIDER = 2 # enable considering mode diff --git a/ublox_msgs/msg/CfgNMEA7.msg b/ublox_msgs/msg/CfgNMEA7.msg index ffb094f9..7a00e99f 100644 --- a/ublox_msgs/msg/CfgNMEA7.msg +++ b/ublox_msgs/msg/CfgNMEA7.msg @@ -1,18 +1,18 @@ # CFG-NMEA (0x06 0x17) # NMEA protocol configuration V0 # -# Set/Get the NMEA protocol configuration. See section NMEA Protocol -# Configuration for a detailed description of the configuration effects on +# Set/Get the NMEA protocol configuration. See section NMEA Protocol +# Configuration for a detailed description of the configuration effects on # NMEA output # # Supported on: u-blox 7 firmware version 1.00 # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 23 +uint8 MESSAGE_ID = 23 uint8 filter # filter flags -uint8 FILTER_POS = 1 # Enable position output for failed or +uint8 FILTER_POS = 1 # Enable position output for failed or # invalid fixes uint8 FILTER_MSK_POS = 2 # Enable position output for invalid fixes uint8 FILTER_TIME = 4 # Enable time output for invalid times @@ -20,51 +20,51 @@ uint8 FILTER_DATE = 8 # Enable date output for invalid dates uint8 FILTER_GPS_ONLY = 16 # Restrict output to GPS satellites only uint8 FILTER_TRACK = 32 # Enable COG output even if COG is frozen -uint8 nmeaVersion # NMEA version +uint8 nmea_version # NMEA version uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 -uint8 numSV # Maximum Number of SVs to report per TalkerId: +uint8 num_sv # Maximum Number of SVs to report per TalkerId: # unlimited (0) or 8, 12, 16 -uint8 NUM_SV_UNLIMITED = 0 +uint8 NUM_SV_UNLIMITED = 0 uint8 flags # flags uint8 FLAGS_COMPAT = 1 # enable compatibility mode. - # This might be needed for certain applications - # when customer's NMEA parser expects a fixed + # This might be needed for certain applications + # when customer's NMEA parser expects a fixed # number of digits in position coordinates uint8 FLAGS_CONSIDER = 2 # enable considering mode -uint32 gnssToFilter # Filters out satellites based on their GNSS. - # If a bitfield is enabled, the corresponding +uint32 gnss_to_filter # Filters out satellites based on their GNSS. + # If a bitfield is enabled, the corresponding # satellites will be not output. uint32 GNSS_TO_FILTER_GPS = 1 # Disable reporting of GPS satellites uint32 GNSS_TO_FILTER_SBAS = 2 # Disable reporting of SBAS satellites uint32 GNSS_TO_FILTER_QZSS = 16 # Disable reporting of QZSS satellites uint32 GNSS_TO_FILTER_GLONASS = 32 # Disable reporting of GLONASS satellites -uint8 svNumbering # Configures the display of satellites that do not +uint8 sv_numbering # Configures the display of satellites that do not # have an NMEA-defined value. Note: this does not # apply to satellites with an unknown ID. uint8 SV_NUMBERING_STRICT = 0 # Strict - Satellites are not output uint8 SV_NUMBERING_EXTENDED = 1 # Extended - Use proprietary numbering -uint8 mainTalkerId # By default the main Talker ID (i.e. the Talker - # ID used for all messages other than GSV) is - # determined by the GNSS assignment of the - # receiver's channels (see CfgGNSS). - # This field enables the main Talker ID to be +uint8 main_talker_id # By default the main Talker ID (i.e. the Talker + # ID used for all messages other than GSV) is + # determined by the GNSS assignment of the + # receiver's channels (see CfgGNSS). + # This field enables the main Talker ID to be # overridden uint8 MAIN_TALKER_ID_NOT_OVERRIDDEN = 0 # Main Talker ID is not overridden uint8 MAIN_TALKER_ID_GP = 1 # Set main Talker ID to 'GP' uint8 MAIN_TALKER_ID_GL = 2 # Set main Talker ID to 'GL' uint8 MAIN_TALKER_ID_GN = 3 # Set main Talker ID to 'GN' -uint8 gsvTalkerId # By default the Talker ID for GSV messages is - # GNSS specific (as defined by NMEA). This field +uint8 gsv_talker_id # By default the Talker ID for GSV messages is + # GNSS specific (as defined by NMEA). This field # enables the GSV Talker ID to be overridden. -uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID +uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID # (as defined by NMEA) uint8 GSV_TALKER_ID_MAIN = 1 # Use the main Talker ID -uint8 reserved # Reserved \ No newline at end of file +uint8 reserved # Reserved diff --git a/ublox_msgs/msg/CfgPRT.msg b/ublox_msgs/msg/CfgPRT.msg index 97580cb3..cb0c0903 100644 --- a/ublox_msgs/msg/CfgPRT.msg +++ b/ublox_msgs/msg/CfgPRT.msg @@ -1,7 +1,7 @@ # CFG-PRT (0x06 0x00) # Port Configuration for DDC, UART, USB, SPI # -# Several configurations can be concatenated to one input message. In this case +# Several configurations can be concatenated to one input message. In this case # the payload length can be a multiple of the normal length (see the other # versions of CFG-PRT). Output messages from the module contain only one # configuration unit. @@ -10,7 +10,7 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 0 -uint8 portID # Port Identifier Number +uint8 port_id # Port Identifier Number uint8 PORT_ID_DDC = 0 uint8 PORT_ID_UART1 = 1 uint8 PORT_ID_UART2 = 2 @@ -19,10 +19,10 @@ uint8 PORT_ID_SPI = 4 uint8 reserved0 # Reserved -uint16 txReady # TX ready PIN configuration (since Firmware 7.01) +uint16 tx_ready # TX ready PIN configuration (since Firmware 7.01) # reserved (Always set to zero) up to Firmware 7.01 - -uint16 TX_READY_EN = 1 # Enable TX ready feature for this + +uint16 TX_READY_EN = 1 # Enable TX ready feature for this # port uint16 TX_READY_POLARITY_HIGH_ACTIVE = 0 # Polarity High-active uint16 TX_READY_POLARITY_LOW_ACTIVE = 2 # Polarity Low-active @@ -32,19 +32,19 @@ uint16 TX_READY_PIN_MASK = 124 # uint16 TX_READY_THRES_SHIFT = 7 # Threshold uint16 TX_READY_THRES_MASK = 65408 # The given threshold is multiplied by # 8 bytes. - # The TX ready PIN goes active + # The TX ready PIN goes active # after >= thres*8 bytes are pending # for the port and going inactive # after the last pending bytes have # been written to hardware (0-4 bytes # before end of stream). -uint32 mode # A bit mask describing the DDC, UART or SPI mode +uint32 mode # A bit mask describing the DDC, UART or SPI mode # Reserved for USB # DDC Mode Constants -uint32 MODE_DDC_SLAVE_ADDR_SHIFT = 1 +uint32 MODE_DDC_SLAVE_ADDR_SHIFT = 1 uint32 MODE_DDC_SLAVE_ADDR_MASK = 254 # Slave address - # Range: 0x07 < slaveAddr < 0x78. + # Range: 0x07 < slaveAddr < 0x78. # UART Mode Constants uint32 MODE_RESERVED1 = 16 # Default 1 for compatibility with A4 uint32 MODE_CHAR_LEN_MASK = 192 # Character Length @@ -70,21 +70,21 @@ uint32 MODE_SPI_FLOW_CONTROL = 64 # (u-blox 6 only) # 1 Flow control enabled (9-bit mode) uint32 MODE_SPI_FF_COUNT_SHIFT = 8 uint32 MODE_SPI_FF_COUNT_MASK = 16128 # Number of bytes containing 0xFF to - # receive before switching off - # reception. + # receive before switching off + # reception. # Range: 0(mechanism off)-63 -uint32 baudRate # UART Baudrate in bits/second [bits/s] +uint32 baud_rate # UART Baudrate in bits/second [bits/s] # Reserved for USB, SPI, DDC - # Possible values: 4800, 9600, 19200, 38400, 57600, + # Possible values: 4800, 9600, 19200, 38400, 57600, # 115200, 230400, 460800 -uint16 inProtoMask # A mask describing which input protocols are active. +uint16 in_proto_mask # A mask describing which input protocols are active. # Each bit of this mask is used for a protocol. # Through that, multiple protocols can be defined # on a single port. -uint16 outProtoMask # A mask describing which output protocols are active. +uint16 out_proto_mask # A mask describing which output protocols are active. # Each bit of this mask is used for a protocol. # Through that, multiple protocols can be defined # on a single port. @@ -95,11 +95,11 @@ uint16 PROTO_RTCM = 4 # only for inProtoMask uint16 PROTO_RTCM3 = 32 # (not supported in protocol versions less than 20) uint16 flags # Flags for UART & SPI, Reserved for USB -uint16 FLAGS_EXTENDED_TX_TIMEOUT = 2 # if set, the port will timeout if - # allocated TX memory >=4 kB and no - # activity for 1.5s. If not set the port - # will timoout if no activity for 1.5s - # regardless on the amount of allocated +uint16 FLAGS_EXTENDED_TX_TIMEOUT = 2 # if set, the port will timeout if + # allocated TX memory >=4 kB and no + # activity for 1.5s. If not set the port + # will timoout if no activity for 1.5s + # regardless on the amount of allocated # TX memory. uint16 reserved1 # Always set to zero diff --git a/ublox_msgs/msg/CfgRATE.msg b/ublox_msgs/msg/CfgRATE.msg index f8beda7b..dc89f34c 100644 --- a/ublox_msgs/msg/CfgRATE.msg +++ b/ublox_msgs/msg/CfgRATE.msg @@ -1,17 +1,17 @@ # CFG-RATE (0x06 0x08) # Navigation/Measurement Rate Settings # -# This message allows the user to alter the rate at which navigation solutions -# (and the measurements that they depend on) are generated by the receiver. The -# calculation of the navigation solution will always be aligned to the top of a -# second zero (first second of the week) of the configured reference time -# system. For protocol version 18 and later the navigation period is an integer +# This message allows the user to alter the rate at which navigation solutions +# (and the measurements that they depend on) are generated by the receiver. The +# calculation of the navigation solution will always be aligned to the top of a +# second zero (first second of the week) of the configured reference time +# system. For protocol version 18 and later the navigation period is an integer # multiple of the measurement period. # - Each measurement triggers the measurements generation and raw data output. -# - The navRate value defines that every nth measurement triggers a navigation +# - The navRate value defines that every nth measurement triggers a navigation # epoch. -# - The update rate has a direct influence on the power consumption. The more -# fixes that are required, the more CPU power and communication resources +# - The update rate has a direct influence on the power consumption. The more +# fixes that are required, the more CPU power and communication resources # are required. # - For most applications a 1 Hz update rate would be sufficient. # - When using Power Save Mode, measurement and navigation rate can differ from @@ -20,12 +20,12 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 8 -uint16 measRate # Measurement Rate, GPS measurements are - # taken every measRate milliseconds [ms] -uint16 navRate # Navigation Rate, in number of measurement - # cycles. On u-blox 5 and u-blox 6, this parameter - # cannot be changed, and always equals 1. -uint16 timeRef # The time system to which measurements are aligned +uint16 meas_rate # Measurement Rate, GPS measurements are + # taken every measRate milliseconds [ms] +uint16 nav_rate # Navigation Rate, in number of measurement + # cycles. On u-blox 5 and u-blox 6, this parameter + # cannot be changed, and always equals 1. +uint16 time_ref # The time system to which measurements are aligned uint16 TIME_REF_UTC = 0 uint16 TIME_REF_GPS = 1 uint16 TIME_REF_GLONASS = 2 # not supported in protocol versions less than 18 diff --git a/ublox_msgs/msg/CfgRST.msg b/ublox_msgs/msg/CfgRST.msg index 76844e88..2d5683ee 100644 --- a/ublox_msgs/msg/CfgRST.msg +++ b/ublox_msgs/msg/CfgRST.msg @@ -8,9 +8,9 @@ # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 4 +uint8 MESSAGE_ID = 4 -uint16 navBbrMask # BBR Sections to clear. +uint16 nav_bbr_mask # BBR Sections to clear. # The following Special Sets apply: uint16 NAV_BBR_HOT_START = 0 # Hot start the device uint16 NAV_BBR_WARM_START = 1 # Warm start the device @@ -26,13 +26,13 @@ uint16 NAV_BBR_UTC = 128 # UTC Correction + GPS Leap Seconds Parameters uint16 NAV_BBR_RTC = 256 # RTC uint16 NAV_BBR_AOP = 32768 # Autonomous Orbit Parameters -uint8 resetMode # Reset Type: +uint8 reset_mode # Reset Type: uint8 RESET_MODE_HW_IMMEDIATE = 0 # Hardware reset (Watchdog) immediately uint8 RESET_MODE_SW = 1 # Controlled Software reset uint8 RESET_MODE_GNSS = 2 # Controlled Software reset (GNSS only) -uint8 RESET_MODE_HW_AFTER_SHUTDOWN = 4 # Hardware reset (Watchdog) after +uint8 RESET_MODE_HW_AFTER_SHUTDOWN = 4 # Hardware reset (Watchdog) after # shutdown uint8 RESET_MODE_GNSS_STOP = 8 # Controlled GNSS stop uint8 RESET_MODE_GNSS_START = 9 # Controlled GNSS start -uint8 reserved1 # Reserved \ No newline at end of file +uint8 reserved1 # Reserved diff --git a/ublox_msgs/msg/CfgSBAS.msg b/ublox_msgs/msg/CfgSBAS.msg index 0211d3fc..a46fe1ff 100644 --- a/ublox_msgs/msg/CfgSBAS.msg +++ b/ublox_msgs/msg/CfgSBAS.msg @@ -11,9 +11,9 @@ uint8 MESSAGE_ID = 22 uint8 mode # SBAS Mode uint8 MODE_ENABLED = 1 # SBAS Enabled (1) / Disabled (0) - # This field is deprecated; use UBX-CFG-GNSS to + # This field is deprecated; use UBX-CFG-GNSS to # enable/disable SBAS operation -uint8 MODE_TEST = 2 # SBAS Testbed: Use data anyhow (1) / Ignore data when +uint8 MODE_TEST = 2 # SBAS Testbed: Use data anyhow (1) / Ignore data when # in Test Mode (SBAS Msg 0) uint8 usage # SBAS Usage @@ -21,7 +21,7 @@ uint8 USAGE_RANGE = 1 # Use SBAS GEOs as a ranging source (for navigation) uint8 USAGE_DIFF_CORR = 2 # Use SBAS Differential Corrections uint8 USAGE_INTEGRITY = 4 # Use SBAS Integrity Information -uint8 maxSBAS # Maximum Number of SBAS prioritized tracking +uint8 max_sbas # Maximum Number of SBAS prioritized tracking # channels (valid range: 0 - 3) to use # (obsolete and superseeded by UBX-CFG-GNSS in protocol # versions 14+). @@ -31,6 +31,6 @@ uint8 scanmode2 # Continuation of scanmode bitmask below # PRN 152...158 uint32 scanmode1 # Which SBAS PRN numbers to search for (Bitmask) # If all Bits are set to zero, auto-scan (i.e. all valid - # PRNs) are searched. Every bit corresponds to a PRN + # PRNs) are searched. Every bit corresponds to a PRN # number. # PRN 120..151 diff --git a/ublox_msgs/msg/CfgTMODE3.msg b/ublox_msgs/msg/CfgTMODE3.msg index a4954439..9c31cb62 100644 --- a/ublox_msgs/msg/CfgTMODE3.msg +++ b/ublox_msgs/msg/CfgTMODE3.msg @@ -2,9 +2,9 @@ # Time Mode Settings 3 # # Configures the receiver to be in Time Mode. The position referred to in this -# message is that of the Antenna Reference Point (ARP). See the Time Mode +# message is that of the Antenna Reference Point (ARP). See the Time Mode # Description for details. -# +# # Supported on: # - u-blox 8 / u-blox M8 with protocol version 20 (only with High Precision # GNSS products) @@ -21,19 +21,19 @@ uint16 FLAGS_MODE_MASK = 255 # Receiver Mode: uint16 FLAGS_MODE_DISABLED = 0 # Disabled uint16 FLAGS_MODE_SURVEY_IN = 1 # Survey In uint16 FLAGS_MODE_FIXED = 2 # Fixed Mode (true ARP position required) -uint16 FLAGS_LLA = 256 # Position is given in LAT/LON/ALT +uint16 FLAGS_LLA = 256 # Position is given in LAT/LON/ALT # (default is ECEF) -int32 ecefXOrLat # WGS84 ECEF X coordinate (or latitude) of +int32 ecef_x_or_lat # WGS84 ECEF X coordinate (or latitude) of # the ARP position, depending on flags above # [cm] or [deg / 1e-7] -int32 ecefYOrLon # WGS84 ECEF Y coordinate (or longitude) of +int32 ecef_y_or_lon # WGS84 ECEF Y coordinate (or longitude) of # the ARP position, depending on flags above # [cm] or [deg / 1e-7] -int32 ecefZOrAlt # WGS84 ECEF Z coordinate (or altitude) of +int32 ecef_z_or_alt # WGS84 ECEF Z coordinate (or altitude) of # the ARP position, depending on flags above # [cm] -int8 ecefXOrLatHP # High-precision WGS84 ECEF X coordinate (or +int8 ecef_x_or_lat_hp # High-precision WGS84 ECEF X coordinate (or # latitude) of the ARP position, depending on # flags above. Must be in the range -99..+99. # The precise WGS84 ECEF X coordinate in units @@ -41,15 +41,15 @@ int8 ecefXOrLatHP # High-precision WGS84 ECEF X coordinate (or # units of 1e-7 degrees, is given by # ecefXOrLat + (ecefXOrLatHP * 1e-2) # [0.1 mm] or [deg * 1e-9] -int8 ecefYOrLonHP # High-precision WGS84 ECEF Y coordinate (or +int8 ecef_y_or_lon_hp # High-precision WGS84 ECEF Y coordinate (or # longitude) of the ARP position, depending on # flags above. Must be in the range -99..+99. # The precise WGS84 ECEF Y coordinate in units - # of cm, or the precise WGS84 ECEF longitude + # of cm, or the precise WGS84 ECEF longitude # in units of 1e-7 degrees, is given by # ecefYOrLon + (ecefYOrLonHP * 1e-2) # [0.1 mm] or [deg * 1e-9] -int8 ecefZOrAltHP # High-precision WGS84 ECEF Z coordinate (or +int8 ecef_z_or_alt_hp # High-precision WGS84 ECEF Z coordinate (or # altitude) of the ARP position, depending on # flags above. Must be in the range -99..+99. # The precise WGS84 ECEF Z coordinate, or @@ -58,11 +58,11 @@ int8 ecefZOrAltHP # High-precision WGS84 ECEF Z coordinate (or # [0.1 mm] uint8 reserved2 # Reserved -uint32 fixedPosAcc # Fixed position 3D accuracy +uint32 fixed_pos_acc # Fixed position 3D accuracy # [0.1 mm] -uint32 svinMinDur # Survey-in minimum duration +uint32 svin_min_dur # Survey-in minimum duration # [s] -uint32 svinAccLimit # Survey-in position accuracy limit +uint32 svin_acc_limit # Survey-in position accuracy limit # [0.1 mm] uint8[8] reserved3 # Reserved diff --git a/ublox_msgs/msg/CfgUSB.msg b/ublox_msgs/msg/CfgUSB.msg index 2e812d14..c44249b0 100644 --- a/ublox_msgs/msg/CfgUSB.msg +++ b/ublox_msgs/msg/CfgUSB.msg @@ -3,28 +3,28 @@ # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 27 +uint8 MESSAGE_ID = 27 -uint16 vendorID # Only set to registered Vendor IDs. +uint16 vendor_id # Only set to registered Vendor IDs. # Changing this field requires special Host drivers. -uint16 productID # Product ID. Changing this field requires special +uint16 product_id # Product ID. Changing this field requires special # Host drivers. uint8[2] reserved1 # Reserved uint8[2] reserved2 # Reserved -uint16 powerConsumption # Power consumed by the device [mA] +uint16 power_consumption # Power consumed by the device [mA] uint16 flags # various configuration flags (see graphic below) uint16 FLAGS_RE_ENUM = 0 # force re-enumeration uint16 FLAGS_POWER_MODE = 2 # self-powered (1), bus-powered (0) -int8[32] vendorString # String containing the vendor name. +int8[32] vendor_string # String containing the vendor name. # 32 ASCII bytes including 0-termination. -int8[32] productString # String containing the product name. +int8[32] product_string # String containing the product name. # 32 ASCII bytes including 0-termination. -int8[32] serialNumber # String containing the serial number. - # 32 ASCII bytes including 0-termination. - # Changing the String fields requires special Host - # drivers. \ No newline at end of file +int8[32] serial_number # String containing the serial number. + # 32 ASCII bytes including 0-termination. + # Changing the String fields requires special Host + # drivers. diff --git a/ublox_msgs/msg/CfgVALDEL.msg b/ublox_msgs/msg/CfgVALDEL.msg new file mode 100644 index 00000000..2c30d8b2 --- /dev/null +++ b/ublox_msgs/msg/CfgVALDEL.msg @@ -0,0 +1,21 @@ +# CFG-VALDEL (0x06 0x8c) +# Delete configuration item values +# +# This message is used to delete saved configurations to effectively revert the items values to +# defaults. Maximum of 64 Key IDs. +# + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 140 + +uint8 version # Message Version (0x00 for this version) + +uint8 layers # Bit mask: the layers where the configuration should be applied +uint8 LAYER_BBR = 1 # BBR layer +uint8 LAYER_FLASH = 2 # Flash Layer + +uint8[2] reserved0 + +# Start of repeated block +uint32[] keys # Configuration key IDs of configuration items to be retrieved +# End of repeated block diff --git a/ublox_msgs/msg/CfgVALGET.msg b/ublox_msgs/msg/CfgVALGET.msg new file mode 100644 index 00000000..4780b807 --- /dev/null +++ b/ublox_msgs/msg/CfgVALGET.msg @@ -0,0 +1,23 @@ +# CFG-VALGET (0x06 0x8b) +# Get configuration items +# +# This message is used to get configuration values by providing a list of configuration key IDs, +# which identify the configuration items to retrieve. Maximum of 64 Key IDs. +# + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 139 + +uint8 version # Message Version (0x00 for this version) + +uint8 layers # The layer which the configuration items should be retrieved +uint8 LAYER_RAM = 0 # RAM layer +uint8 LAYER_BBR = 1 # BBR layer +uint8 LAYER_FLASH = 2 # Flash Layer +uint8 LAYER_DEFAULT = 7 # Default Layer + +uint16 position # Skip this many key values before constructing output message + +# Start of repeated block +uint32[] keys # Configuration key IDs of configuration items to be retrieved +# End of repeated block diff --git a/ublox_msgs/msg/CfgVALSET.msg b/ublox_msgs/msg/CfgVALSET.msg new file mode 100644 index 00000000..daec4522 --- /dev/null +++ b/ublox_msgs/msg/CfgVALSET.msg @@ -0,0 +1,23 @@ +# CFG-VALSET (0x06 0x8a) +# Set configuration item values +# +# This message is used to set a configuration by providing configuration data ( a list of +# key and value pairs), which identify the configuration items to change, and their new values. +# Maximum of 64 Key IDs. +# + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 138 + +uint8 version # Message Version (0x00 for this version) + +uint8 layers # Bit mask: the layers where the configuration should be applied +uint8 LAYER_RAM = 1 # RAM layer +uint8 LAYER_BBR = 2 # BBR layer +uint8 LAYER_FLASH = 4 # Flash Layer + +uint8[2] reserved0 + +# Start of repeated block +CfgVALSETCfgdata[] cfgdata # Configuration data (key and value pairs) +# End of repeated block diff --git a/ublox_msgs/msg/CfgVALSETCfgdata.msg b/ublox_msgs/msg/CfgVALSETCfgdata.msg new file mode 100644 index 00000000..83d73dbe --- /dev/null +++ b/ublox_msgs/msg/CfgVALSETCfgdata.msg @@ -0,0 +1,32 @@ +# CFG-SIGNAL Keys +uint32 GPS_ENABLE = 271646751 # 0x1031001f +uint32 GPS_L1CA_ENABLE = 271646721 # 0x10310001 +uint32 GPS_L2C_ENABLE = 271646723 # 0x10310003 + +uint32 SBAS_ENABLE = 271646752 # 0x10310020 +uint32 SBAS_L1CA_ENABLE = 271646725 # 0x10310005 + +uint32 GAL_ENABLE = 271646753 # 0x10310021 +uint32 GAL_E1_ENABLE = 271646727 # 0x10310007 +uint32 GAL_E5B_ENABLE = 271646730 # 0x1031000a + +uint32 BDS_ENABLE = 271646754 # 0x10310022 +uint32 BDS_B1_ENABLE = 271646733 # 0x1031000d +uint32 BDS_B2_ENABLE = 271646734 # 0x1031000e + +uint32 QZSS_ENABLE = 271646756 # 0x10310024 +uint32 QZSS_L1CA_ENABLE = 271646738 # 0x10310012 +uint32 QZSS_L1S_ENABLE = 271646740 # 0x10310014 +uint32 QZSS_L2C_ENABLE = 271646741 # 0x10310015 + +uint32 GLO_ENABLE = 271646757 # 0x10310025 +uint32 GLO_L1_ENABLE = 271646744 # 0x10310018 +uint32 GLO_L2_ENABLE = 271646746 # 0x1031001a + +# See Cfg-VALSET +# + +uint32 key # Key for configuration item being changed + +uint8[] data # Data for configuration item being changed, + # array can be of length 1 - 8 (i.e. 1 - 8 bytes long) diff --git a/ublox_msgs/msg/EsfINS.msg b/ublox_msgs/msg/EsfINS.msg index 423c076d..706ae1a8 100644 --- a/ublox_msgs/msg/EsfINS.msg +++ b/ublox_msgs/msg/EsfINS.msg @@ -1,11 +1,11 @@ # ESF-INS (0x10 0x15) # Vehicle dynamics information -# -# This message outputs information about vehicle dynamics computed by the +# +# This message outputs information about vehicle dynamics computed by the # Inertial Navigation System (INS) during ESF-based navigation. -# For ADR products, the output dynamics information (angular rates and +# For ADR products, the output dynamics information (angular rates and # accelerations) is expressed with respect to the vehicle-frame. -# For UDR products, the output dynamics information (angular rates and +# For UDR products, the output dynamics information (angular rates and # accelerations) is expressed with respect to the body-frame. # @@ -14,25 +14,25 @@ uint8 MESSAGE_ID = 21 uint32 bitfield0 # Bitfield (see graphic below) uint32 BITFIELD0_VERSION = 255 # Message version (1 for this version). -uint32 BITFIELD0_X_ANG_RATE_VALID = 256 # Compensated x-axis angular rate data +uint32 BITFIELD0_X_ANG_RATE_VALID = 256 # Compensated x-axis angular rate data # validity flag -uint32 BITFIELD0_Y_ANG_RATE_VALID = 512 # Compensated y-axis angular rate data +uint32 BITFIELD0_Y_ANG_RATE_VALID = 512 # Compensated y-axis angular rate data # validity flag -uint32 BITFIELD0_Z_ANG_RATE_VALID = 1024 # Compensated z-axis angular rate data +uint32 BITFIELD0_Z_ANG_RATE_VALID = 1024 # Compensated z-axis angular rate data # validity flag -uint32 BITFIELD0_X_ACCEL_VALID = 2048 # Compensated x-axis acceleration data +uint32 BITFIELD0_X_ACCEL_VALID = 2048 # Compensated x-axis acceleration data # validity flag -uint32 BITFIELD0_Y_ACCEL_VALID = 4096 # Compensated y-axis acceleration data +uint32 BITFIELD0_Y_ACCEL_VALID = 4096 # Compensated y-axis acceleration data # validity flag -uint32 BITFIELD0_Z_ACCEL_VALID = 8192 # Compensated z-axis acceleration data +uint32 BITFIELD0_Z_ACCEL_VALID = 8192 # Compensated z-axis acceleration data # validity flag uint8[4] reserved1 # Reserved -uint32 iTOW # GPS time of week of the navigation epoch [ms] -int32 xAngRate # Compensated x-axis angular rate [deg/s / 1e-3] -int32 yAngRate # Compensated y-axis angular rate [deg/s / 1e-3] -int32 zAngRate # Compensated z-axis angular rate [deg/s / 1e-3] -int32 xAccel # Compensated x-axis acceleration (gravity-free) [mg] -int32 yAccel # Compensated y-axis acceleration (gravity-free) [mg] -int32 zAccel # Compensated z-axis acceleration (gravity-free) [mg] \ No newline at end of file +uint32 i_tow # GPS time of week of the navigation epoch [ms] +int32 x_ang_rate # Compensated x-axis angular rate [deg/s / 1e-3] +int32 y_ang_rate # Compensated y-axis angular rate [deg/s / 1e-3] +int32 z_ang_rate # Compensated z-axis angular rate [deg/s / 1e-3] +int32 x_accel # Compensated x-axis acceleration (gravity-free) [mg] +int32 y_accel # Compensated y-axis acceleration (gravity-free) [mg] +int32 z_accel # Compensated z-axis acceleration (gravity-free) [mg] diff --git a/ublox_msgs/msg/EsfMEAS.msg b/ublox_msgs/msg/EsfMEAS.msg index e0ab0ffd..24017c76 100644 --- a/ublox_msgs/msg/EsfMEAS.msg +++ b/ublox_msgs/msg/EsfMEAS.msg @@ -1,25 +1,25 @@ # ESF-MEAS (0x10 0x02) # External Sensor Fusion Measurements -# -# Possible data types for the data field are described in the ESF Measurement +# +# Possible data types for the data field are described in the ESF Measurement # Data section # uint8 CLASS_ID = 16 uint8 MESSAGE_ID = 2 -uint32 timeTag # Time tag of measurement generated by external +uint32 time_tag # Time tag of measurement generated by external # sensor uint16 flags # Flags. Set all unused bits to zero: -uint16 FLAGS_TIME_MARK_SENT_MASK = 3 # Time mark signal was supplied just +uint16 FLAGS_TIME_MARK_SENT_MASK = 3 # Time mark signal was supplied just # prior to sending this message: 0 = uint16 TIME_MARK_NONE = 0 uint16 TIME_MARK_EXT0 = 1 uint16 TIME_MARK_EXT = 2 -uint16 FLAGS_TIME_MARK_EDGE = 4 # Trigger on rising (0) or falling +uint16 FLAGS_TIME_MARK_EDGE = 4 # Trigger on rising (0) or falling # (1) edge of time mark signal -uint16 FLAGS_CALIB_T_TAG_VALID = 8 # Calibration time tag available. +uint16 FLAGS_CALIB_T_TAG_VALID = 8 # Calibration time tag available. # Always set to zero. uint16 id # Identification number of data provider @@ -30,40 +30,40 @@ uint32 DATA_FIELD_MASK = 16777215 # data uint32 DATA_TYPE_MASK = 1056964608 # type of data (1..63) uint32 DATA_TYPE_SHIFT = 24 uint32 DATA_TYPE_NONE = 0 # data field contains no data -uint32 DATA_TYPE_Z_AXIS_GYRO = 5 # z-axis gyroscope angular rate +uint32 DATA_TYPE_Z_AXIS_GYRO = 5 # z-axis gyroscope angular rate # [deg/s *2^-12 signed] -uint32 DATA_TYPE_WHEEL_TICKS_FRONT_LEFT = 6 # front-left wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_FRONT_LEFT = 6 # front-left wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT = 7 # front-right wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT = 7 # front-right wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_WHEEL_TICKS_REAR_LEFT = 8 # rear-left wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_REAR_LEFT = 8 # rear-left wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_WHEEL_TICKS_REAR_RIGHT = 9 # rear-right wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_REAR_RIGHT = 9 # rear-right wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_SINGLE_TICK = 10 # single tick (speed tick) - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_SINGLE_TICK = 10 # single tick (speed tick) + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) uint32 DATA_TYPE_SPEED = 11 # speed m/s * 1e-3 signed -uint32 DATA_TYPE_GYRO_TEMPERATURE = 12 # gyroscope temperature +uint32 DATA_TYPE_GYRO_TEMPERATURE = 12 # gyroscope temperature # [deg Celsius * 1e-2 signed] -uint32 DATA_TYPE_GYRO_ANG_RATE_Y = 13 # y-axis gyroscope angular rate +uint32 DATA_TYPE_GYRO_ANG_RATE_Y = 13 # y-axis gyroscope angular rate # [deg/s *2^-12 signed] -uint32 DATA_TYPE_GYRO_ANG_RATE_X = 14 # x-axis gyroscope angular rate +uint32 DATA_TYPE_GYRO_ANG_RATE_X = 14 # x-axis gyroscope angular rate # [deg/s *2^-12 signed] -uint32 DATA_TYPE_ACCELEROMETER_X = 16 # x-axis accelerometer specific +uint32 DATA_TYPE_ACCELEROMETER_X = 16 # x-axis accelerometer specific # [force m/s^2 *2^-10 signed] -uint32 DATA_TYPE_ACCELEROMETER_Y = 17 # y-axis accelerometer specific +uint32 DATA_TYPE_ACCELEROMETER_Y = 17 # y-axis accelerometer specific # [force m/s^2 *2^-10 signed] -uint32 DATA_TYPE_ACCELEROMETER_Z = 18 # z-axis accelerometer specific +uint32 DATA_TYPE_ACCELEROMETER_Z = 18 # z-axis accelerometer specific # [force m/s^2 *2^-10 signed] @@ -71,7 +71,7 @@ uint32 DATA_TYPE_ACCELEROMETER_Z = 18 # z-axis accelerometer specific # End of repeated block # Start of optional block (size is either 0 or 1) -uint32[] calibTtag # Receiver local time calibrated. +uint32[] calib_t_tag # Receiver local time calibrated. # This field must not be supplied when # calibTtagValid is set to 0 [ms] -# End of optional block \ No newline at end of file +# End of optional block diff --git a/ublox_msgs/msg/EsfRAW.msg b/ublox_msgs/msg/EsfRAW.msg index 52158d76..0f0a47f4 100644 --- a/ublox_msgs/msg/EsfRAW.msg +++ b/ublox_msgs/msg/EsfRAW.msg @@ -5,7 +5,7 @@ # to the GNSS chip. Possible data types for the data field are accelerometer, # gyroscope and temperature readings as described in the ESF Measurement Data # section. Note that the rate selected in CFG-MSG is not respected. If a -# positive rate is selected then all raw measurements will be output. +# positive rate is selected then all raw measurements will be output. # # Supported on ADR/UDR products. # @@ -15,4 +15,4 @@ uint8 MESSAGE_ID = 3 uint8[4] reserved0 # Reserved -EsfRAW_Block[] blocks \ No newline at end of file +EsfRAWBlock[] blocks diff --git a/ublox_msgs/msg/EsfRAW_Block.msg b/ublox_msgs/msg/EsfRAWBlock.msg similarity index 72% rename from ublox_msgs/msg/EsfRAW_Block.msg rename to ublox_msgs/msg/EsfRAWBlock.msg index c8375729..647dd3c2 100644 --- a/ublox_msgs/msg/EsfRAW_Block.msg +++ b/ublox_msgs/msg/EsfRAWBlock.msg @@ -3,6 +3,6 @@ uint32 data # Its scaling and unit depends on the type and is # the same as in ESF-MEAS uint32 DATA_FIELD_MASK = 16777215 -uint32 DATA_TYPE_MASK = 4278190080 # type of data +uint32 DATA_TYPE_MASK = 4278190080 # type of data # (0 = no data; 1..255 = data type) -uint32 sTtag # sensor time tag \ No newline at end of file +uint32 s_t_tag # sensor time tag diff --git a/ublox_msgs/msg/EsfSTATUS.msg b/ublox_msgs/msg/EsfSTATUS.msg index 18716da7..b3eb0d1d 100644 --- a/ublox_msgs/msg/EsfSTATUS.msg +++ b/ublox_msgs/msg/EsfSTATUS.msg @@ -7,28 +7,28 @@ uint8 CLASS_ID = 16 uint8 MESSAGE_ID = 16 -uint32 iTOW # GPS time of week of the navigation epoch [ms] +uint32 i_tow # GPS time of week of the navigation epoch [ms] uint8 version # Message version (2 for this version) uint8[7] reserved1 # Reserved -uint8 fusionMode # Fusion mode: +uint8 fusion_mode # Fusion mode: uint8 FUSION_MODE_INIT = 0 # receiver is initializing some unknown values # required for doing sensor fusion -uint8 FUSION_MODE_FUSION = 1 # GNSS and sensor data are +uint8 FUSION_MODE_FUSION = 1 # GNSS and sensor data are # used for navigation solution computation -uint8 FUSION_MODE_SUSPENDED = 2 # sensor fusion is temporarily disabled - # due to e.g. invalid sensor data or detected +uint8 FUSION_MODE_SUSPENDED = 2 # sensor fusion is temporarily disabled + # due to e.g. invalid sensor data or detected # ferry -uint8 FUSION_MODE_DISABLED = 3 # sensor fusion is permanently disabled - # until receiver reset due e.g. to sensor +uint8 FUSION_MODE_DISABLED = 3 # sensor fusion is permanently disabled + # until receiver reset due e.g. to sensor # error uint8[2] reserved2 # Reserved -uint8 numSens # Number of sensors +uint8 num_sens # Number of sensors # Start of repeated block (numSens times) -EsfSTATUS_Sens[] sens -# End of repeated block \ No newline at end of file +EsfSTATUSSens[] sens +# End of repeated block diff --git a/ublox_msgs/msg/EsfSTATUSSens.msg b/ublox_msgs/msg/EsfSTATUSSens.msg new file mode 100644 index 00000000..59d39641 --- /dev/null +++ b/ublox_msgs/msg/EsfSTATUSSens.msg @@ -0,0 +1,7 @@ +# See Esf-STATUS +# + +uint8 sens_status1 # Sensor status, part 1 (see graphic below) +uint8 sens_status2 # Sensor status, part 2 (see graphic below) +uint8 freq # Observation frequency [Hz] +uint8 faults # Sensor faults (see graphic below) diff --git a/ublox_msgs/msg/HnrPVT.msg b/ublox_msgs/msg/HnrPVT.msg index 5a926f9a..632b74ed 100644 --- a/ublox_msgs/msg/HnrPVT.msg +++ b/ublox_msgs/msg/HnrPVT.msg @@ -4,7 +4,7 @@ # Note that during a leap second there may be more (or less) than 60 seconds in # a minute; see the description of leap seconds for details. # -# This message provides the position, velocity and time solution with high +# This message provides the position, velocity and time solution with high # output rate. # # Supported on ADR and UDR products. @@ -12,7 +12,7 @@ uint8 CLASS_ID = 40 uint8 MESSAGE_ID = 0 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -22,21 +22,21 @@ uint8 sec # Seconds of minute, range 0..60 (UTC) uint8 valid # Validity flags uint8 VALID_DATE = 1 # Valid UTC Date -uint8 VALID_TIME = 2 # Valid -uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved +uint8 VALID_TIME = 2 # Valid +uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) -uint8 gpsFix # GPS fix Type, range 0..5 +uint8 gps_fix # GPS fix Type, range 0..5 uint8 FIX_TYPE_NO_FIX = 0 uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 -uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, +uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, # constant altitude assumed uint8 FIX_TYPE_3D = 3 uint8 FIX_TYPE_GPS_DEAD_RECKONING_COMBINED = 4 # GPS + Dead reckoning -uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix +uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix uint8 flags # Fix Status Flags uint8 FLAGS_GNSS_FIX_OK = 1 # i.e. within DOP & accuracy masks @@ -50,18 +50,18 @@ uint8[2] reserved0 # Reserved int32 lon # Longitude [deg / 1e-7] int32 lat # Latitude [deg / 1e-7] int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] +int32 h_msl # Height above mean sea level [mm] -int32 gSpeed # Ground Speed (2-D) [mm/s] +int32 g_speed # Ground Speed (2-D) [mm/s] int32 speed # Speed (3-D) [mm/s] -int32 headMot # Heading of motion (2-D) [deg / 1e-5] -int32 headVeh # Heading of vehicle (2-D) [deg / 1e-5] +int32 head_mot # Heading of motion (2-D) [deg / 1e-5] +int32 head_veh # Heading of vehicle (2-D) [deg / 1e-5] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] -uint32 sAcc # Speed Accuracy Estimate [mm/s] -uint32 headAcc # Heading Accuracy Estimate (both motion & vehicle) +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] +uint32 s_acc # Speed Accuracy Estimate [mm/s] +uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) # [deg / 1e-5] -uint8[4] reserved1 # Reserved \ No newline at end of file +uint8[4] reserved1 # Reserved diff --git a/ublox_msgs/msg/MgaGAL.msg b/ublox_msgs/msg/MgaGAL.msg index 55a6c985..76140930 100644 --- a/ublox_msgs/msg/MgaGAL.msg +++ b/ublox_msgs/msg/MgaGAL.msg @@ -1,9 +1,9 @@ # MGA-GAL (0x13 0x02) # Galileo Ephemeris Assistance # -# This message allows the delivery of Galileo ephemeris assistance to a +# This message allows the delivery of Galileo ephemeris assistance to a # receiver. See the description of AssistNow Online for details. -# +# uint8 CLASS_ID = 19 uint8 MESSAGE_ID = 2 @@ -14,48 +14,48 @@ uint8 svid # Galileo Satellite identifier uint8 reserved0 # Reserved -uint16 iodNav # Ephemeris and clock correction issue of Data -int16 deltaN # Mean motion difference from computed value +uint16 iod_nav # Ephemeris and clock correction issue of Data +int16 delta_n # Mean motion difference from computed value # [semi-cir cles/s * 2^-43] int32 m0 # Mean anomaly at reference time [semi-cir cles 2^-31] uint32 e # Eccentricity [2^-33] -uint32 sqrtA # Square root of the semi-major axis [m^0.5 * 2^-19] +uint32 sqrt_a # Square root of the semi-major axis [m^0.5 * 2^-19] int32 omega0 # Longitude of ascending node of orbital plane at weekly # epoch [semi-cir cles 2^-31] -int32 i0 # inclination angle at reference time +int32 i0 # inclination angle at reference time # [semi-cir cles 2^-31] int32 omega # Argument of perigee [semi-cir cles 2^-31] -int32 omegaDot # Rate of change of right ascension +int32 omega_dot # Rate of change of right ascension # [semi-cir cles/s 2^-43] -int16 iDot # Rate of change of inclination angle +int16 i_dot # Rate of change of inclination angle # [semi-cir cles/s 2^-43] -int16 cuc # Amplitude of the cosine harmonic correction term to +int16 cuc # Amplitude of the cosine harmonic correction term to # the argument of latitude [radians * 2^-29] -int16 cus # Amplitude of the sine harmonic correction term to +int16 cus # Amplitude of the sine harmonic correction term to # the argument of latitude [radians * 2^-29] -int16 crc # Amplitude of the cosine harmonic correction term +int16 crc # Amplitude of the cosine harmonic correction term # to the orbit radius [radians * 2^-5] -int16 crs # Amplitude of the sine harmonic correction term to the +int16 crs # Amplitude of the sine harmonic correction term to the # orbit radius [radians * 2^-5] -int16 cic # Amplitude of the cosine harmonic correction term to +int16 cic # Amplitude of the cosine harmonic correction term to # the angle of inclination [radians * 2^-29] -int16 cis # Amplitude of the sine harmonic correction term to the +int16 cis # Amplitude of the sine harmonic correction term to the # angle of inclination [radians * 2^-29] uint16 toe # Ephemeris reference time [60 * s] int32 af0 # clock bias correction coefficient [s * 2^-34] int32 af1 # SV clock drift correction coefficient [s/s * 2^-46] -int8 af2 # SV clock drift rate correction coefficient +int8 af2 # SV clock drift rate correction coefficient # [s/s^2 * 2^-59] -uint8 sisaindexE1E5b # Signal-in-Space Accuracy index for dual frequency +uint8 sisaindex_e1_e5b # Signal-in-Space Accuracy index for dual frequency # E1-E5b uint16 toc # Clock correction data reference Time of Week [60 * s] -int16 bgdE1E5b # E1-E5b Broadcast Group Delay +int16 bgd_e1_e5b # E1-E5b Broadcast Group Delay uint8[2] reserved1 # Reserved -uint8 healthE1B # E1-B Signal Health Status -uint8 dataValidityE1B # E1-B Data Validity Status -uint8 healthE5b # E5b Signal Health Status -uint8 dataValidityE5b # E5b Data Validity Status +uint8 health_e1b # E1-B Signal Health Status +uint8 data_validity_e1b # E1-B Data Validity Status +uint8 health_e5b # E5b Signal Health Status +uint8 data_validity_e5b # E5b Data Validity Status -uint8[4] reserved2 # Reserved \ No newline at end of file +uint8[4] reserved2 # Reserved diff --git a/ublox_msgs/msg/MonGNSS.msg b/ublox_msgs/msg/MonGNSS.msg index 431a48dc..9d379f2b 100644 --- a/ublox_msgs/msg/MonGNSS.msg +++ b/ublox_msgs/msg/MonGNSS.msg @@ -3,7 +3,7 @@ # # This message reports major GNSS selection. Augmentation systems are not # reported. -# +# uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 40 @@ -16,16 +16,16 @@ uint8 BIT_MASK_BEIDOU = 4 uint8 BIT_MASK_GALILEO = 8 uint8 supported # The major GNSS that can be supported by this receiver -uint8 defaultGnss # Default major GNSS selection. If the default major GNSS - # selection is currently configured in the efuse for this - # receiver, it takes precedence over the default major - # GNSS selection configured in the executing firmware of +uint8 default_gnss # Default major GNSS selection. If the default major GNSS + # selection is currently configured in the efuse for this + # receiver, it takes precedence over the default major + # GNSS selection configured in the executing firmware of # this receiver. # see bit mask constants uint8 enabled # Current major GNSS selection enabled for this receiver # see bit mask constants -uint8 simultaneous # Maximum number of concurrent major GNSS that can be +uint8 simultaneous # Maximum number of concurrent major GNSS that can be # supported by this receiver uint8[3] reserved1 # Reserved diff --git a/ublox_msgs/msg/MonHW.msg b/ublox_msgs/msg/MonHW.msg index 3203269e..e4e5291b 100644 --- a/ublox_msgs/msg/MonHW.msg +++ b/ublox_msgs/msg/MonHW.msg @@ -1,7 +1,7 @@ # MON-HW (0x0A 0x09) # Hardware Status # -# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral +# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral # Pins, Noise Level, Automatic Gain Control (AGC) # # WARNING: this message is a different length than the MonHW message for @@ -10,29 +10,29 @@ uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 9 -uint32 pinSel # Mask of Pins Set as Peripheral/PIO -uint32 pinBank # Mask of Pins Set as Bank A/B -uint32 pinDir # Mask of Pins Set as Input/Output -uint32 pinVal # Mask of Pins Value Low/High -uint16 noisePerMS # Noise Level as measured by the GPS Core -uint16 agcCnt # AGC Monitor (counts SIGHI xor SIGLO, +uint32 pin_sel # Mask of Pins Set as Peripheral/PIO +uint32 pin_bank # Mask of Pins Set as Bank A/B +uint32 pin_dir # Mask of Pins Set as Input/Output +uint32 pin_val # Mask of Pins Value Low/High +uint16 noise_per_ms # Noise Level as measured by the GPS Core +uint16 agc_cnt # AGC Monitor (counts SIGHI xor SIGLO, # range 0 to 8191) -uint8 aStatus # Status of the Antenna Supervisor State Machine +uint8 a_status # Status of the Antenna Supervisor State Machine uint8 A_STATUS_INIT = 0 uint8 A_STATUS_UNKNOWN = 1 uint8 A_STATUS_OK = 2 uint8 A_STATUS_SHORT = 3 uint8 A_STATUS_OPEN = 4 -uint8 aPower # Current PowerStatus of Antenna -uint8 A_POWER_OFF = 0 +uint8 a_power # Current PowerStatus of Antenna +uint8 A_POWER_OFF = 0 uint8 A_POWER_ON = 1 uint8 A_POWER_UNKNOWN = 2 uint8 flags # Flags: uint8 FLAGS_RTC_CALIB = 1 # RTC is calibrated uint8 FLAGS_SAFE_BOOT = 2 # Safe boot mode (0 = inactive, 1 = active) -uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: +uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: uint8 JAMMING_STATE_UNKNOWN_OR_DISABLED = 0 # unknown or feature disabled uint8 JAMMING_STATE_OK = 4 # ok - no significant jamming uint8 JAMMING_STATE_WARNING = 8 # interference visible but fix OK @@ -40,18 +40,18 @@ uint8 JAMMING_STATE_CRITICAL = 12 # interference visible and no fix uint8 FLAGS_XTAL_ABSENT = 16 # RTC XTAL is absent # (not supported in protocol versions < 18) uint8 reserved0 # Reserved -uint32 usedMask # Mask of Pins that are used by the Virtual Pin +uint32 used_mask # Mask of Pins that are used by the Virtual Pin # Manager -uint8[17] VP # Array of Pin Mappings for each of the 17 +uint8[17] vp # Array of Pin Mappings for each of the 17 # Physical Pins -uint8 jamInd # CW Jamming indicator, scaled: +uint8 jam_ind # CW Jamming indicator, scaled: uint8 JAM_IND_NONE = 0 # No CW Jamming -uint8 JAM_IND_STRONG = 255 # Strong CW Jamming +uint8 JAM_IND_STRONG = 255 # Strong CW Jamming uint8[2] reserved1 # Reserved -uint32 pinIrq # Mask of Pins Value using the PIO Irq -uint32 pullH # Mask of Pins Value using the PIO Pull High +uint32 pin_irq # Mask of Pins Value using the PIO Irq +uint32 pull_h # Mask of Pins Value using the PIO Pull High # Resistor -uint32 pullL # Mask of Pins Value using the PIO Pull Low +uint32 pull_l # Mask of Pins Value using the PIO Pull Low # Resistor diff --git a/ublox_msgs/msg/MonHW6.msg b/ublox_msgs/msg/MonHW6.msg index b6e1ba91..6de85912 100644 --- a/ublox_msgs/msg/MonHW6.msg +++ b/ublox_msgs/msg/MonHW6.msg @@ -2,7 +2,7 @@ # Hardware Status # Firmware 6 # -# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral +# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral # Pins, Noise Level, Automatic Gain Control (AGC) # # WARNING: this message is a different length than the MonHW message for @@ -11,29 +11,29 @@ uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 9 -uint32 pinSel # Mask of Pins Set as Peripheral/PIO -uint32 pinBank # Mask of Pins Set as Bank A/B -uint32 pinDir # Mask of Pins Set as Input/Output -uint32 pinVal # Mask of Pins Value Low/High -uint16 noisePerMS # Noise Level as measured by the GPS Core -uint16 agcCnt # AGC Monitor (counts SIGHI xor SIGLO, +uint32 pin_sel # Mask of Pins Set as Peripheral/PIO +uint32 pin_bank # Mask of Pins Set as Bank A/B +uint32 pin_dir # Mask of Pins Set as Input/Output +uint32 pin_val # Mask of Pins Value Low/High +uint16 noise_per_ms # Noise Level as measured by the GPS Core +uint16 agc_cnt # AGC Monitor (counts SIGHI xor SIGLO, # range 0 to 8191) -uint8 aStatus # Status of the Antenna Supervisor State Machine +uint8 a_status # Status of the Antenna Supervisor State Machine uint8 A_STATUS_INIT = 0 uint8 A_STATUS_UNKNOWN = 1 uint8 A_STATUS_OK = 2 uint8 A_STATUS_SHORT = 3 uint8 A_STATUS_OPEN = 4 -uint8 aPower # Current PowerStatus of Antenna -uint8 A_POWER_OFF = 0 +uint8 a_power # Current PowerStatus of Antenna +uint8 A_POWER_OFF = 0 uint8 A_POWER_ON = 1 uint8 A_POWER_UNKNOWN = 2 uint8 flags # Flags: uint8 FLAGS_RTC_CALIB = 1 # RTC is calibrated uint8 FLAGS_SAFE_BOOT = 2 # Safe boot mode (0 = inactive, 1 = active) -uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: +uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: uint8 JAMMING_STATE_UNKNOWN_OR_DISABLED = 0 # unknown or feature disabled uint8 JAMMING_STATE_OK = 4 # ok - no significant jamming uint8 JAMMING_STATE_WARNING = 8 # interference visible but fix OK @@ -41,18 +41,18 @@ uint8 JAMMING_STATE_CRITICAL = 12 # interference visible and no fix uint8 FLAGS_XTAL_ABSENT = 16 # RTC XTAL is absent # (not supported in protocol versions < 18) uint8 reserved0 # Reserved -uint32 usedMask # Mask of Pins that are used by the Virtual Pin +uint32 used_mask # Mask of Pins that are used by the Virtual Pin # Manager -uint8[25] VP # Array of Pin Mappings for each of the 25 +uint8[25] vp # Array of Pin Mappings for each of the 25 # Physical Pins -uint8 jamInd # CW Jamming indicator, scaled: +uint8 jam_ind # CW Jamming indicator, scaled: uint8 JAM_IND_NONE = 0 # No CW Jamming -uint8 JAM_IND_STRONG = 255 # Strong CW Jamming +uint8 JAM_IND_STRONG = 255 # Strong CW Jamming uint8[2] reserved1 # Reserved -uint32 pinIrq # Mask of Pins Value using the PIO Irq -uint32 pullH # Mask of Pins Value using the PIO Pull High +uint32 pin_irq # Mask of Pins Value using the PIO Irq +uint32 pull_h # Mask of Pins Value using the PIO Pull High + # Resistor +uint32 pull_l # Mask of Pins Value using the PIO Pull Low # Resistor -uint32 pullL # Mask of Pins Value using the PIO Pull Low - # Resistor \ No newline at end of file diff --git a/ublox_msgs/msg/MonVER.msg b/ublox_msgs/msg/MonVER.msg index 87fbb912..2ce3c6d3 100644 --- a/ublox_msgs/msg/MonVER.msg +++ b/ublox_msgs/msg/MonVER.msg @@ -6,9 +6,9 @@ uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 4 -char[30] swVersion # Zero-terminated software version string. -char[10] hwVersion # Zero-terminated hardware version string. +char[30] sw_version # Zero-terminated software version string. +char[10] hw_version # Zero-terminated hardware version string. # Start of repeated block (N times) -MonVER_Extension[] extension +MonVERExtension[] extension # End of repeated block diff --git a/ublox_msgs/msg/MonVER_Extension.msg b/ublox_msgs/msg/MonVERExtension.msg similarity index 100% rename from ublox_msgs/msg/MonVER_Extension.msg rename to ublox_msgs/msg/MonVERExtension.msg diff --git a/ublox_msgs/msg/NavATT.msg b/ublox_msgs/msg/NavATT.msg index 61371fd9..d6f2774b 100644 --- a/ublox_msgs/msg/NavATT.msg +++ b/ublox_msgs/msg/NavATT.msg @@ -8,7 +8,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 5 -uint32 iTOW # GPS time of week of the navigation epoch [ms] +uint32 i_tow # GPS time of week of the navigation epoch [ms] uint8 version # Message version (0 for this version) uint8[3] reserved0 # Reserved @@ -16,8 +16,8 @@ uint8[3] reserved0 # Reserved int32 roll # Vehicle roll. [deg / 1e-5] int32 pitch # Vehicle pitch. [deg / 1e-5] int32 heading # Vehicle heading. [deg / 1e-5] -uint32 accRoll # Vehicle roll accuracy (if null, roll angle is not +uint32 acc_roll # Vehicle roll accuracy (if null, roll angle is not # available). [deg / 1e-5] -uint32 accPitch # Vehicle pitch accuracy (if null, pitch angle is not +uint32 acc_pitch # Vehicle pitch accuracy (if null, pitch angle is not # available). [deg / 1e-5] -uint32 accHeading # Vehicle heading accuracy [deg / 1e-5] \ No newline at end of file +uint32 acc_heading # Vehicle heading accuracy [deg / 1e-5] diff --git a/ublox_msgs/msg/NavCLOCK.msg b/ublox_msgs/msg/NavCLOCK.msg index 7edfd188..1d8b6472 100644 --- a/ublox_msgs/msg/NavCLOCK.msg +++ b/ublox_msgs/msg/NavCLOCK.msg @@ -5,9 +5,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 34 -uint32 iTOW # GPS Millisecond Time of week [ms] +uint32 i_tow # GPS Millisecond Time of week [ms] -int32 clkB # Clock bias in nanoseconds [ns] -int32 clkD # Clock drift in nanoseconds per second [ns/s] -uint32 tAcc # Time Accuracy Estimate [ns] -uint32 fAcc # Frequency Accuracy Estimate [ps/s] +int32 clk_b # Clock bias in nanoseconds [ns] +int32 clk_d # Clock drift in nanoseconds per second [ns/s] +uint32 t_acc # Time Accuracy Estimate [ns] +uint32 f_acc # Frequency Accuracy Estimate [ps/s] diff --git a/ublox_msgs/msg/NavCOV.msg b/ublox_msgs/msg/NavCOV.msg new file mode 100644 index 00000000..d70d7be2 --- /dev/null +++ b/ublox_msgs/msg/NavCOV.msg @@ -0,0 +1,28 @@ +# NAV-COV (0x01 0x36) +# Covariance matrices for position and velocity solutions +# + +uint8 CLASS_ID = 1 +uint8 MESSAGE_ID = 54 + +uint32 i_tow # GPS Millisecond time of week [ms] + +uint8 version # Message version (0x00 for this version) +uint8 pos_cov_valid # Position covariance matrix validity flag +uint8 vel_cov_valid # Velocity covariance matrix validity flag +uint8[9] reserved_0 # Reserved + +float32 pos_cov_nn # Position covariance matrix value p_NN [m^2] +float32 pos_cov_ne # Position covariance matrix value p_NE [m^2] +float32 pos_cov_nd # Position covariance matrix value p_ND [m^2] +float32 pos_cov_ee # Position covariance matrix value p_EE [m^2] +float32 pos_cov_ed # Position covariance matrix value p_ED [m^2] +float32 pos_cov_dd # Position covariance matrix value p_DD [m^2] + + +float32 vel_cov_nn # Velocity covariance matrix value v_NN [m^2/s^2] +float32 vel_cov_ne # Velocity covariance matrix value v_NE [m^2/s^2] +float32 vel_cov_nd # Velocity covariance matrix value v_ND [m^2/s^2] +float32 vel_cov_ee # Velocity covariance matrix value v_EE [m^2/s^2] +float32 vel_cov_ed # Velocity covariance matrix value v_ED [m^2/s^2] +float32 vel_cov_dd # Velocity covariance matrix value v_DD [m^2/s^2] diff --git a/ublox_msgs/msg/NavDGPS.msg b/ublox_msgs/msg/NavDGPS.msg index 04b8f296..47360edc 100644 --- a/ublox_msgs/msg/NavDGPS.msg +++ b/ublox_msgs/msg/NavDGPS.msg @@ -8,12 +8,12 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 49 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] int32 age # Age of newest correction data [ms] -int16 baseId # DGPS Base Station ID -int16 baseHealth # DGPS Base Station Health Status -int8 numCh # Number of channels for which correction data is +int16 base_id # DGPS Base Station ID +int16 base_health # DGPS Base Station Health Status +int8 num_ch # Number of channels for which correction data is # following uint8 status # DGPS Correction Type Status @@ -22,4 +22,4 @@ uint8 DGPS_CORRECTION_PR_PRR = 1 uint16 reserved1 # Reserved -NavDGPS_SV[] sv +NavDGPSSV[] sv diff --git a/ublox_msgs/msg/NavDGPS_SV.msg b/ublox_msgs/msg/NavDGPSSV.msg similarity index 87% rename from ublox_msgs/msg/NavDGPS_SV.msg rename to ublox_msgs/msg/NavDGPSSV.msg index a501c0a8..5dc00d46 100644 --- a/ublox_msgs/msg/NavDGPS_SV.msg +++ b/ublox_msgs/msg/NavDGPSSV.msg @@ -7,7 +7,7 @@ uint8 FLAGS_CHANNEL_MASK = 15 # Bitmask for channel number, range 0..15 # Channel numbers > 15 marked as 15 uint8 FLAGS_DGPS = 16 # DGPS Used for this SV -uint16 ageC # Age of latest correction data [ms] +uint16 age_c # Age of latest correction data [ms] float32 prc # Pseudo Range Correction [m] float32 prrc # Pseudo Range Rate Correction [m/s] diff --git a/ublox_msgs/msg/NavDOP.msg b/ublox_msgs/msg/NavDOP.msg index cb350c0c..ed837c2e 100644 --- a/ublox_msgs/msg/NavDOP.msg +++ b/ublox_msgs/msg/NavDOP.msg @@ -2,19 +2,19 @@ # Dilution of precision # # - DOP values are dimensionless. -# - All DOP values are scaled by a factor of 100. If the unit transmits a value +# - All DOP values are scaled by a factor of 100. If the unit transmits a value # of e.g. 156, the DOP value is 1.56. # uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 4 -uint32 iTOW # GPS Millisecond Time of Week [ms] +uint32 i_tow # GPS Millisecond Time of Week [ms] -uint16 gDOP # Geometric DOP [1 / 0.01] -uint16 pDOP # Position DOP [1 / 0.01] -uint16 tDOP # Time DOP [1 / 0.01] -uint16 vDOP # Vertical DOP [1 / 0.01] -uint16 hDOP # Horizontal DOP [1 / 0.01] -uint16 nDOP # Northing DOP [1 / 0.01] -uint16 eDOP # Easting DOP [1 / 0.01] +uint16 g_dop # Geometric DOP [1 / 0.01] +uint16 p_dop # Position DOP [1 / 0.01] +uint16 t_dop # Time DOP [1 / 0.01] +uint16 v_dop # Vertical DOP [1 / 0.01] +uint16 h_dop # Horizontal DOP [1 / 0.01] +uint16 n_dop # Northing DOP [1 / 0.01] +uint16 e_dop # Easting DOP [1 / 0.01] diff --git a/ublox_msgs/msg/NavPOSECEF.msg b/ublox_msgs/msg/NavPOSECEF.msg index f5aba86a..b1f4560c 100644 --- a/ublox_msgs/msg/NavPOSECEF.msg +++ b/ublox_msgs/msg/NavPOSECEF.msg @@ -8,9 +8,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 1 -uint32 iTOW # GPS Millisecond Time of Week [ms] +uint32 i_tow # GPS Millisecond Time of Week [ms] -int32 ecefX # ECEF X coordinate [cm] -int32 ecefY # ECEF Y coordinate [cm] -int32 ecefZ # ECEF Z coordinate [cm] -uint32 pAcc # Position Accuracy Estimate [cm] +int32 ecef_x # ECEF X coordinate [cm] +int32 ecef_y # ECEF Y coordinate [cm] +int32 ecef_z # ECEF Z coordinate [cm] +uint32 p_acc # Position Accuracy Estimate [cm] diff --git a/ublox_msgs/msg/NavPOSLLH.msg b/ublox_msgs/msg/NavPOSLLH.msg index 8a3f2552..6f6e5464 100644 --- a/ublox_msgs/msg/NavPOSLLH.msg +++ b/ublox_msgs/msg/NavPOSLLH.msg @@ -3,7 +3,7 @@ # # See important comments concerning validity of position given in section # Navigation Output Filters. -# This message outputs the Geodetic position in the currently selected +# This message outputs the Geodetic position in the currently selected # Ellipsoid. The default is the WGS84 Ellipsoid, but can be changed with the # message CFG-DAT. # @@ -11,11 +11,11 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 2 -uint32 iTOW # GPS Millisecond Time of Week [ms] +uint32 i_tow # GPS Millisecond Time of Week [ms] int32 lon # Longitude [deg / 1e-7] int32 lat # Latitude [deg / 1e-7] int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] +int32 h_msl # Height above mean sea level [mm] +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] diff --git a/ublox_msgs/msg/NavPVT.msg b/ublox_msgs/msg/NavPVT.msg index d60778ef..eed12ad1 100644 --- a/ublox_msgs/msg/NavPVT.msg +++ b/ublox_msgs/msg/NavPVT.msg @@ -4,7 +4,7 @@ # Note that during a leap second there may be more (or less) than 60 seconds in # a minute; see the description of leap seconds for details. # -# This message combines Position, velocity and time solution in LLH, +# This message combines Position, velocity and time solution in LLH, # including accuracy figures # # WARNING: For firmware version 7, this message is a different length. @@ -13,7 +13,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 7 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -23,22 +23,22 @@ uint8 sec # Seconds of minute, range 0..60 (UTC) uint8 valid # Validity flags uint8 VALID_DATE = 1 # Valid UTC Date -uint8 VALID_TIME = 2 # Valid -uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved +uint8 VALID_TIME = 2 # Valid +uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination -uint32 tAcc # time accuracy estimate [ns] (UTC) +uint32 t_acc # time accuracy estimate [ns] (UTC) int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) -uint8 fixType # GNSS fix Type, range 0..5 +uint8 fix_type # GNSS fix Type, range 0..5 uint8 FIX_TYPE_NO_FIX = 0 uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 -uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, +uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, # constant altitude assumed uint8 FIX_TYPE_3D = 3 uint8 FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED = 4 # GNSS + Dead reckoning -uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision +uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision # devices) uint8 flags # Fix Status Flags @@ -52,41 +52,41 @@ uint8 PSM_TRACKING = 12 # Tracking uint8 PSM_POWER_OPTIMIZED_TRACKING = 16 # Power Optimized Tracking uint8 PSM_INACTIVE = 20 # Inactive uint8 FLAGS_HEAD_VEH_VALID = 32 # heading of vehicle is valid -uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status +uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status uint8 CARRIER_PHASE_NO_SOLUTION = 0 # no carrier phase range solution -uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed - # integer measurements have been used to +uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed + # integer measurements have been used to # calculate the solution) -uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer - # carrier phase range measurements have +uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer + # carrier phase range measurements have # been used to calculate the solution) uint8 flags2 # Additional Flags -uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of +uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of # Day validity confirmation is available uint8 FLAGS2_CONFIRMED_DATE = 64 # UTC Date validity could be confirmed uint8 FLAGS2_CONFIRMED_TIME = 128 # UTC Time of Day could be confirmed -uint8 numSV # Number of SVs used in Nav Solution -int32 lon # Longitude [deg / 1e-7] -int32 lat # Latitude [deg / 1e-7] -int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] +uint8 num_sv # Number of SVs used in Nav Solution +int32 lon # Longitude [deg / 1e-7] +int32 lat # Latitude [deg / 1e-7] +int32 height # Height above Ellipsoid [mm] +int32 h_msl # Height above mean sea level [mm] +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] -int32 velN # NED north velocity [mm/s] -int32 velE # NED east velocity [mm/s] -int32 velD # NED down velocity [mm/s] -int32 gSpeed # Ground Speed (2-D) [mm/s] -int32 heading # Heading of motion 2-D [deg / 1e-5] -uint32 sAcc # Speed Accuracy Estimate [mm/s] -uint32 headAcc # Heading Accuracy Estimate (both motion & vehicle) - # [deg / 1e-5] +int32 vel_n # NED north velocity [mm/s] +int32 vel_e # NED east velocity [mm/s] +int32 vel_d # NED down velocity [mm/s] +int32 g_speed # Ground Speed (2-D) [mm/s] +int32 heading # Heading of motion 2-D [deg / 1e-5] +uint32 s_acc # Speed Accuracy Estimate [mm/s] +uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) + # [deg / 1e-5] -uint16 pDOP # Position DOP [1 / 0.01] -uint8[6] reserved1 # Reserved +uint16 p_dop # Position DOP [1 / 0.01] +uint8[6] reserved1 # Reserved -int32 headVeh # Heading of vehicle (2-D) [deg / 1e-5] -int16 magDec # Magnetic declination [deg / 1e-2] -uint16 magAcc # Magnetic declination accuracy [deg / 1e-2] +int32 head_veh # Heading of vehicle (2-D) [deg / 1e-5] +int16 mag_dec # Magnetic declination [deg / 1e-2] +uint16 mag_acc # Magnetic declination accuracy [deg / 1e-2] diff --git a/ublox_msgs/msg/NavPVT7.msg b/ublox_msgs/msg/NavPVT7.msg index b3b37957..97f7a09c 100644 --- a/ublox_msgs/msg/NavPVT7.msg +++ b/ublox_msgs/msg/NavPVT7.msg @@ -4,7 +4,7 @@ # Note that during a leap second there may be more (or less) than 60 seconds in # a minute; see the description of leap seconds for details. # -# This message combines Position, velocity and time solution in LLH, +# This message combines Position, velocity and time solution in LLH, # including accuracy figures # # WARNING: For firmware version 7, this message is a different length. @@ -13,7 +13,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 7 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -23,22 +23,22 @@ uint8 sec # Seconds of minute, range 0..60 (UTC) uint8 valid # Validity flags uint8 VALID_DATE = 1 # Valid UTC Date -uint8 VALID_TIME = 2 # Valid -uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved +uint8 VALID_TIME = 2 # Valid +uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination -uint32 tAcc # time accuracy estimate [ns] (UTC) +uint32 t_acc # time accuracy estimate [ns] (UTC) int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) -uint8 fixType # GNSS fix Type, range 0..5 +uint8 fix_type # GNSS fix Type, range 0..5 uint8 FIX_TYPE_NO_FIX = 0 uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 -uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, +uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, # constant altitude assumed uint8 FIX_TYPE_3D = 3 uint8 FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED = 4 # GNSS + Dead reckoning -uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision +uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision # devices) uint8 flags # Fix Status Flags @@ -52,37 +52,37 @@ uint8 PSM_TRACKING = 12 # Tracking uint8 PSM_POWER_OPTIMIZED_TRACKING = 16 # Power Optimized Tracking uint8 PSM_INACTIVE = 20 # Inactive uint8 FLAGS_HEAD_VEH_VALID = 32 # heading of vehicle is valid -uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status +uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status uint8 CARRIER_PHASE_NO_SOLUTION = 0 # no carrier phase range solution -uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed - # integer measurements have been used to +uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed + # integer measurements have been used to # calculate the solution) -uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer - # carrier phase range measurements have +uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer + # carrier phase range measurements have # been used to calculate the solution) uint8 flags2 # Additional Flags -uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of +uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of # Day validity confirmation is available uint8 FLAGS2_CONFIRMED_DATE = 64 # UTC Date validity could be confirmed uint8 FLAGS2_CONFIRMED_TIME = 128 # UTC Time of Day could be confirmed -uint8 numSV # Number of SVs used in Nav Solution -int32 lon # Longitude [deg / 1e-7] -int32 lat # Latitude [deg / 1e-7] -int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] +uint8 num_sv # Number of SVs used in Nav Solution +int32 lon # Longitude [deg / 1e-7] +int32 lat # Latitude [deg / 1e-7] +int32 height # Height above Ellipsoid [mm] +int32 h_msl # Height above mean sea level [mm] +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] -int32 velN # NED north velocity [mm/s] -int32 velE # NED east velocity [mm/s] -int32 velD # NED down velocity [mm/s] -int32 gSpeed # Ground Speed (2-D) [mm/s] -int32 heading # Heading of motion 2-D [deg / 1e-5] -uint32 sAcc # Speed Accuracy Estimate [mm/s] -uint32 headAcc # Heading Accuracy Estimate (both motion & vehicle) - # [deg / 1e-5] +int32 vel_n # NED north velocity [mm/s] +int32 vel_e # NED east velocity [mm/s] +int32 vel_d # NED down velocity [mm/s] +int32 g_speed # Ground Speed (2-D) [mm/s] +int32 heading # Heading of motion 2-D [deg / 1e-5] +uint32 s_acc # Speed Accuracy Estimate [mm/s] +uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) + # [deg / 1e-5] -uint16 pDOP # Position DOP [1 / 0.01] -uint8[6] reserved1 # Reserved \ No newline at end of file +uint16 p_dop # Position DOP [1 / 0.01] +uint8[6] reserved1 # Reserved diff --git a/ublox_msgs/msg/NavRELPOSNED.msg b/ublox_msgs/msg/NavRELPOSNED.msg index e6bd26b3..6f443bda 100644 --- a/ublox_msgs/msg/NavRELPOSNED.msg +++ b/ublox_msgs/msg/NavRELPOSNED.msg @@ -1,6 +1,6 @@ # NAV-RELPOSNED (0x01 0x3C) # Relative Positioning Information in NED frame -# +# # The NED frame is defined as the local topological system at the reference # station. The relative position vector components in this message, along with # their associated accuracies, are given in that local topological system @@ -18,31 +18,31 @@ uint8 MESSAGE_ID = 60 uint8 version # Message version (0x00 for this version) uint8 reserved0 # Reserved -uint16 refStationId # Reference Station ID. Must be in the range +uint16 ref_station_id # Reference Station ID. Must be in the range # 0..4095 -uint32 iTow # GPS time of week of the navigation epoch +uint32 i_tow # GPS time of week of the navigation epoch # [ms] -int32 relPosN # North component of relative position vector +int32 rel_pos_n # North component of relative position vector # [cm] -int32 relPosE # East component of relative position vector +int32 rel_pos_e # East component of relative position vector # [cm] -int32 relPosD # Down component of relative position vector +int32 rel_pos_d # Down component of relative position vector # [cm] -int8 relPosHPN # High-precision North component of relative +int8 rel_pos_hpn # High-precision North component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full North component of the relative # position vector, in units of cm, is given by # relPosN + (relPosHPN * 1e-2) -int8 relPosHPE # High-precision East component of relative +int8 rel_pos_hpe # High-precision East component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. - # The full East component of the relative + # The full East component of the relative # position vector, in units of cm, is given by # relPosE + (relPosHPE * 1e-2) -int8 relPosHPD # High-precision Down component of relative +int8 rel_pos_hpd # High-precision Down component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full Down component of the relative @@ -51,35 +51,35 @@ int8 relPosHPD # High-precision Down component of relative uint8 reserved1 # Reserved -uint32 accN # Accuracy of relative position North +uint32 acc_n # Accuracy of relative position North # component [0.1 mm] -uint32 accE # Accuracy of relative position East component +uint32 acc_e # Accuracy of relative position East component # [0.1 mm] -uint32 accD # Accuracy of relative position Down component +uint32 acc_d # Accuracy of relative position Down component # [0.1 mm] uint32 flags -uint32 FLAGS_GNSS_FIX_OK = 1 # A valid fix (i.e within DOP & accuracy +uint32 FLAGS_GNSS_FIX_OK = 1 # A valid fix (i.e within DOP & accuracy # masks) uint32 FLAGS_DIFF_SOLN = 2 # Set if differential corrections were applied uint32 FLAGS_REL_POS_VALID = 4 # Set if relative position components and # accuracies are valid uint32 FLAGS_CARR_SOLN_MASK = 24 # Carrier phase range solution status: uint32 FLAGS_CARR_SOLN_NONE = 0 # No carrier phase range solution -uint32 FLAGS_CARR_SOLN_FLOAT = 8 # Float solution. No fixed integer carrier +uint32 FLAGS_CARR_SOLN_FLOAT = 8 # Float solution. No fixed integer carrier # phase measurements have been used to # calculate the solution uint32 FLAGS_CARR_SOLN_FIXED = 16 # Fixed solution. One or more fixed # integer carrier phase range measurements # have been used to calculate the solution -uint32 FLAGS_IS_MOVING = 32 # if the receiver is operating in moving +uint32 FLAGS_IS_MOVING = 32 # if the receiver is operating in moving # baseline mode (not supported in protocol # versions less than 20.3) uint32 FLAGS_REF_POS_MISS = 64 # Set if extrapolated reference position was - # used to compute moving baseline solution + # used to compute moving baseline solution # this epoch (not supported in protocol - # versions less than 20.3) + # versions less than 20.3) uint32 FLAGS_REF_OBS_MISS = 128 # Set if extrapolated reference observations # were used to compute moving baseline # solution this epoch (not supported in - # protocol versions less than 20.3) \ No newline at end of file + # protocol versions less than 20.3) diff --git a/ublox_msgs/msg/NavRELPOSNED9.msg b/ublox_msgs/msg/NavRELPOSNED9.msg index 679cca2f..1a7e6394 100644 --- a/ublox_msgs/msg/NavRELPOSNED9.msg +++ b/ublox_msgs/msg/NavRELPOSNED9.msg @@ -17,57 +17,57 @@ uint8 MESSAGE_ID = 60 uint8 version # Message version (0x00 for this version) uint8 reserved1 # Reserved -uint16 refStationId # Reference Station ID. Must be in the range +uint16 ref_station_id # Reference Station ID. Must be in the range # 0..4095 -uint32 iTow # GPS time of week of the navigation epoch +uint32 i_tow # GPS time of week of the navigation epoch # [ms] -int32 relPosN # North component of relative position vector +int32 rel_pos_n # North component of relative position vector # [cm] -int32 relPosE # East component of relative position vector +int32 rel_pos_e # East component of relative position vector # [cm] -int32 relPosD # Down component of relative position vector +int32 rel_pos_d # Down component of relative position vector # [cm] -int32 relPosLength # Length of the relative position vector +int32 rel_pos_length # Length of the relative position vector # [cm] -int32 relPosHeading # Heading of the relative position vector +int32 rel_pos_heading # Heading of the relative position vector # [1e-5 deg] uint8[4] reserved2 # Reserved -int8 relPosHPN # High-precision North component of relative +int8 rel_pos_hpn # High-precision North component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full North component of the relative # position vector, in units of cm, is given by # relPosN + (relPosHPN * 1e-2) -int8 relPosHPE # High-precision East component of relative +int8 rel_pos_hpe # High-precision East component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full East component of the relative # position vector, in units of cm, is given by # relPosE + (relPosHPE * 1e-2) -int8 relPosHPD # High-precision Down component of relative +int8 rel_pos_hpd # High-precision Down component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full Down component of the relative # position vector, in units of cm, is given by # relPosD + (relPosHPD * 1e-2) -int8 relPosHPLength # High-precision component of the length of +int8 rel_pos_hp_length # High-precision component of the length of # the relative position vector. # Must be in the range -99 to +99. # The full length of the relative position # vector, in units of cm, is given by # relPosLength + (relPosHPLength * 1e-2) -uint32 accN # Accuracy of relative position North +uint32 acc_n # Accuracy of relative position North # component [0.1 mm] -uint32 accE # Accuracy of relative position East component +uint32 acc_e # Accuracy of relative position East component # [0.1 mm] -uint32 accD # Accuracy of relative position Down component +uint32 acc_d # Accuracy of relative position Down component # [0.1 mm] -uint32 accLength # Accuracy of length of the relative position - # vector [0.1 mm] -uint32 accHeading # Accuracy of heading of the relative position +uint32 acc_length # Accuracy of length of the relative position # vector [0.1 mm] +uint32 acc_heading # Accuracy of heading of the relative position + # vector [1e-5 deg] uint8[4] reserved3 # Reserved @@ -103,4 +103,4 @@ uint32 FLAGS_REL_POS_HEAD_VALID = 256 # Set if extrapolated reference observat uint32 FLAGS_REL_POS_NORM = 512 # Set if extrapolated reference observations # were used to compute moving baseline # solution this epoch (not supported in - # protocol versions less than 20.3) \ No newline at end of file + # protocol versions less than 20.3) diff --git a/ublox_msgs/msg/NavSAT.msg b/ublox_msgs/msg/NavSAT.msg index 348782d3..a31689e7 100644 --- a/ublox_msgs/msg/NavSAT.msg +++ b/ublox_msgs/msg/NavSAT.msg @@ -1,18 +1,18 @@ # NAV-SAT (0x01 0x35) # Satellite Information # -# This message displays information about SVs which are either known to be +# This message displays information about SVs which are either known to be # visible or currently tracked by the receiver. # uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 53 -uint32 iTOW # GPS time of week of the navigation epoch. [ms] +uint32 i_tow # GPS time of week of the navigation epoch. [ms] uint8 version # Message version (1 for this version) -uint8 numSvs # Number of satellites -uint8[2] reserved0 # Reserved +uint8 num_svs # Number of satellites +uint8[2] reserved0 # Reserved # start of repeated block (numSvs times) -NavSAT_SV[] sv -# end of repeated block \ No newline at end of file +NavSATSV[] sv +# end of repeated block diff --git a/ublox_msgs/msg/NavSAT_SV.msg b/ublox_msgs/msg/NavSATSV.msg similarity index 87% rename from ublox_msgs/msg/NavSAT_SV.msg rename to ublox_msgs/msg/NavSATSV.msg index 476dab37..1d97290a 100644 --- a/ublox_msgs/msg/NavSAT_SV.msg +++ b/ublox_msgs/msg/NavSATSV.msg @@ -1,80 +1,80 @@ # see NAV-SAT message # -uint8 gnssId # GNSS identifier -uint8 svId # Satellite identifier +uint8 gnss_id # GNSS identifier +uint8 sv_id # Satellite identifier uint8 cno # Carrier to noise ratio (signal strength) ]dBHz int8 elev # Elevation (range: +/-90), unknown if out of range [deg] -int16 azim # Azimuth (range 0-360), unknown if elevation is out of range +int16 azim # Azimuth (range 0-360), unknown if elevation is out of range # [deg] -int16 prRes # Pseudo range residual [0.1 m] +int16 pr_res # Pseudo range residual [0.1 m] uint32 flags # Bitmask uint32 FLAGS_QUALITY_IND_MASK = 7 # Signal quality indicator: uint8 QUALITY_IND_NO_SIGNAL = 0 # no signal uint8 QUALITY_IND_SEARCHING_SIGNAL = 1 # searching signal uint8 QUALITY_IND_SIGNAL_ACQUIRED = 2 # signal acquired -uint8 QUALITY_IND_SIGNAL_DETECTED_BUT_UNUSABLE = 3 # signal detected but +uint8 QUALITY_IND_SIGNAL_DETECTED_BUT_UNUSABLE = 3 # signal detected but # unusable -uint8 QUALITY_IND_CODE_LOCKED_AND_TIME_SYNC = 4 # code locked and time +uint8 QUALITY_IND_CODE_LOCKED_AND_TIME_SYNC = 4 # code locked and time # synchronized -uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC1 = 5 # code and carrier - # locked and time - # synchronized, +uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC1 = 5 # code and carrier + # locked and time + # synchronized, # quality = 1 -uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC2 = 6 # code and carrier - # locked and time - # synchronized, +uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC2 = 6 # code and carrier + # locked and time + # synchronized, # quality = 2 -uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC3 = 7 # code and carrier - # locked and time - # synchronized, +uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC3 = 7 # code and carrier + # locked and time + # synchronized, # quality = 3 -# Note: Since IMES signals are not time synchronized, a channel tracking an IMES +# Note: Since IMES signals are not time synchronized, a channel tracking an IMES # signal can never reach a quality indicator value of higher than 3. -uint32 FLAGS_SV_USED = 8 # whether SV is currently being +uint32 FLAGS_SV_USED = 8 # whether SV is currently being # used for navigation uint32 FLAGS_HEALTH_MASK = 48 # SV health flag: uint32 HEALTH_UNKNOWN = 0 # unknown uint32 HEALTH_HEALTHY = 1 # healthy uint32 HEALTH_UNHEALTHY = 2 # unhealthy -uint32 FLAGS_DIFF_CORR = 64 # whether differential correction +uint32 FLAGS_DIFF_CORR = 64 # whether differential correction # data is available for this SV -uint32 FLAGS_SMOOTHED = 128 # whether carrier smoothed +uint32 FLAGS_SMOOTHED = 128 # whether carrier smoothed # pseudorange used uint32 FLAGS_ORBIT_SOURCE_MASK = 1792 # Orbit source: -uint32 ORBIT_SOURCE_UNAVAILABLE = 0 # no orbit information is +uint32 ORBIT_SOURCE_UNAVAILABLE = 0 # no orbit information is # available for this SV uint32 ORBIT_SOURCE_EPH = 256 # ephemeris is used uint32 ORBIT_SOURCE_ALM = 512 # almanac is used -uint32 ORBIT_SOURCE_ASSIST_OFFLINE = 768 # AssistNow Offline orbit is +uint32 ORBIT_SOURCE_ASSIST_OFFLINE = 768 # AssistNow Offline orbit is # used -uint32 ORBIT_SOURCE_ASSIST_AUTONOMOUS = 1024 # AssistNow Autonomous orbit is +uint32 ORBIT_SOURCE_ASSIST_AUTONOMOUS = 1024 # AssistNow Autonomous orbit is # used -uint32 ORBIT_SOURCE_OTHER1 = 1280 # other orbit information is +uint32 ORBIT_SOURCE_OTHER1 = 1280 # other orbit information is # used -uint32 ORBIT_SOURCE_OTHER2 = 1536 # other orbit information is +uint32 ORBIT_SOURCE_OTHER2 = 1536 # other orbit information is # used -uint32 ORBIT_SOURCE_OTHER3 = 1792 # other orbit information is +uint32 ORBIT_SOURCE_OTHER3 = 1792 # other orbit information is # used -uint32 FLAGS_EPH_AVAIL = 2048 # whether ephemeris is available +uint32 FLAGS_EPH_AVAIL = 2048 # whether ephemeris is available # for this SV -uint32 FLAGS_ALM_AVAIL = 4096 # whether almanac is available for +uint32 FLAGS_ALM_AVAIL = 4096 # whether almanac is available for # this SV -uint32 FLAGS_ANO_AVAIL = 8192 # whether AssistNow Offline data +uint32 FLAGS_ANO_AVAIL = 8192 # whether AssistNow Offline data # is available for this SV -uint32 FLAGS_AOP_AVAIL = 16384 # whether AssistNow Autonomous +uint32 FLAGS_AOP_AVAIL = 16384 # whether AssistNow Autonomous # data is available for this SV -uint32 FLAGS_SBAS_CORR_USED = 65536 # whether SBAS corrections have +uint32 FLAGS_SBAS_CORR_USED = 65536 # whether SBAS corrections have # been used for this SV -uint32 FLAGS_RTCM_CORR_USED = 131072 # whether RTCM corrections have +uint32 FLAGS_RTCM_CORR_USED = 131072 # whether RTCM corrections have # been used for this SV -uint32 FLAGS_PR_CORR_USED = 1048576 # whether Pseudorange corrections +uint32 FLAGS_PR_CORR_USED = 1048576 # whether Pseudorange corrections # have been used for this SV -uint32 FLAGS_CR_CORR_USED = 2097152 # whether Carrier range - # corrections have been used for +uint32 FLAGS_CR_CORR_USED = 2097152 # whether Carrier range + # corrections have been used for + # this SV +uint32 FLAGS_DO_CORR_USED = 4194304 # whether Range rate (Doppler) + # corrections have been used for # this SV -uint32 FLAGS_DO_CORR_USED = 4194304 # whether Range rate (Doppler) - # corrections have been used for - # this SV \ No newline at end of file diff --git a/ublox_msgs/msg/NavSBAS.msg b/ublox_msgs/msg/NavSBAS.msg index b41409d6..311c9b40 100644 --- a/ublox_msgs/msg/NavSBAS.msg +++ b/ublox_msgs/msg/NavSBAS.msg @@ -7,9 +7,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 50 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint8 geo # PRN Number of the GEO where correction and integrity +uint8 geo # PRN Number of the GEO where correction and integrity # data is used from uint8 mode # SBAS Mode @@ -34,4 +34,4 @@ uint8 SERVICE_TESTMODE = 8 uint8 cnt # Number of SV data following uint8[3] reserved0 # Reserved -NavSBAS_SV[] sv +NavSBASSV[] sv diff --git a/ublox_msgs/msg/NavSBAS_SV.msg b/ublox_msgs/msg/NavSBASSV.msg similarity index 72% rename from ublox_msgs/msg/NavSBAS_SV.msg rename to ublox_msgs/msg/NavSBASSV.msg index 1ae4066a..c3be214f 100644 --- a/ublox_msgs/msg/NavSBAS_SV.msg +++ b/ublox_msgs/msg/NavSBASSV.msg @@ -4,8 +4,8 @@ uint8 svid # SV Id uint8 flags # Flags for this SV uint8 udre # Monitoring status -uint8 svSys # System (WAAS/EGNOS/...), same as SYS -uint8 svService # Services available, same as SERVICE +uint8 sv_sys # System (WAAS/EGNOS/...), same as SYS +uint8 sv_service # Services available, same as SERVICE uint8 reserved1 # Reserved int16 prc # Pseudo Range correction in [cm] uint16 reserved2 # Reserved diff --git a/ublox_msgs/msg/NavSOL.msg b/ublox_msgs/msg/NavSOL.msg index 4181605c..e36d85de 100644 --- a/ublox_msgs/msg/NavSOL.msg +++ b/ublox_msgs/msg/NavSOL.msg @@ -1,21 +1,21 @@ # NAV-SOL (0x01 0x06) # Navigation Solution Information # -# This message combines Position, velocity and time solution in ECEF, including +# This message combines Position, velocity and time solution in ECEF, including # accuracy figures -# This message has only been retained for backwards compatibility; users are +# This message has only been retained for backwards compatibility; users are # recommended to use the UBX-NAV-PVT message in preference. # uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 6 -uint32 iTOW # GPS Millisecond time of week [ms] -int32 fTOW # Fractional Nanoseconds remainder of rounded +uint32 i_tow # GPS Millisecond time of week [ms] +int32 f_tow # Fractional Nanoseconds remainder of rounded # ms above, range -500000 .. 500000 [ns] int16 week # GPS week (GPS time) -uint8 gpsFix # GPSfix Type, range 0..5 +uint8 gps_fix # GPSfix Type, range 0..5 uint8 GPS_NO_FIX = 0 uint8 GPS_DEAD_RECKONING_ONLY = 1 uint8 GPS_2D_FIX = 2 @@ -29,15 +29,15 @@ uint8 FLAGS_DIFF_SOLN = 2 # DGPS used uint8 FLAGS_WKNSET = 4 # Week Number valid uint8 FLAGS_TOWSET = 8 # Time of Week valid -int32 ecefX # ECEF X coordinate [cm] -int32 ecefY # ECEF Y coordinate [cm] -int32 ecefZ # ECEF Z coordinate [cm] -uint32 pAcc # 3D Position Accuracy Estimate [cm] -int32 ecefVX # ECEF X velocity [cm/s] -int32 ecefVY # ECEF Y velocity [cm/s] -int32 ecefVZ # ECEF Z velocity [cm/s] -uint32 sAcc # Speed Accuracy Estimate [cm/s] -uint16 pDOP # Position DOP [1 / 0.01] +int32 ecef_x # ECEF X coordinate [cm] +int32 ecef_y # ECEF Y coordinate [cm] +int32 ecef_z # ECEF Z coordinate [cm] +uint32 p_acc # 3D Position Accuracy Estimate [cm] +int32 ecef_vx # ECEF X velocity [cm/s] +int32 ecef_vy # ECEF Y velocity [cm/s] +int32 ecef_vz # ECEF Z velocity [cm/s] +uint32 s_acc # Speed Accuracy Estimate [cm/s] +uint16 p_dop # Position DOP [1 / 0.01] uint8 reserved1 # Reserved -uint8 numSV # Number of SVs used in Nav Solution +uint8 num_sv # Number of SVs used in Nav Solution uint32 reserved2 # Reserved diff --git a/ublox_msgs/msg/NavSTATUS.msg b/ublox_msgs/msg/NavSTATUS.msg index 8738e8ee..d9d07eea 100644 --- a/ublox_msgs/msg/NavSTATUS.msg +++ b/ublox_msgs/msg/NavSTATUS.msg @@ -8,9 +8,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 3 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint8 gpsFix # GPSfix Type, this value does not qualify a fix as +uint8 gps_fix # GPSfix Type, this value does not qualify a fix as # valid and within the limits. See note on flag gpsFixOk # below uint8 GPS_NO_FIX = 0 @@ -21,41 +21,41 @@ uint8 GPS_GPS_DEAD_RECKONING_COMBINED = 4 uint8 GPS_TIME_ONLY_FIX = 5 uint8 flags # Navigation Status Flags -uint8 FLAGS_GPS_FIX_OK = 1 # position & velocity valid & within DOP & ACC +uint8 FLAGS_GPS_FIX_OK = 1 # position & velocity valid & within DOP & ACC # Masks uint8 FLAGS_DIFF_SOLN = 2 # Differential corrections were applied uint8 FLAGS_WKNSET = 4 # Week Number valid uint8 FLAGS_TOWSET = 8 # Time of Week valid -uint8 fixStat # Fix Status Information +uint8 fix_stat # Fix Status Information uint8 FIX_STAT_DIFF_CORR_MASK = 1 # 1 = differential corrections available # map matching status: uint8 FIX_STAT_MAP_MATCHING_MASK = 192 uint8 MAP_MATCHING_NONE = 0 # none -uint8 MAP_MATCHING_VALID = 64 # valid but not used, i.e. map matching data +uint8 MAP_MATCHING_VALID = 64 # valid but not used, i.e. map matching data # was received, but was too old uint8 MAP_MATCHING_USED = 128 # valid and used, map matching data was applied -uint8 MAP_MATCHING_DR = 192 # valid and used, map matching data was +uint8 MAP_MATCHING_DR = 192 # valid and used, map matching data was # applied. In case of sensor unavailability map - # matching data enables dead reckoning. - # This requires map matched latitude/longitude + # matching data enables dead reckoning. + # This requires map matched latitude/longitude # or heading data. uint8 flags2 # further information about navigation output # power safe mode state (Only for FW version >= 7.01; undefined otherwise) uint8 FLAGS2_PSM_STATE_MASK = 3 -uint8 PSM_STATE_ACQUISITION = 0 # ACQUISITION +uint8 PSM_STATE_ACQUISITION = 0 # ACQUISITION # [or when psm disabled] uint8 PSM_STATE_TRACKING = 1 # TRACKING uint8 PSM_STATE_POWER_OPTIMIZED_TRACKING = 2 # POWER OPTIMIZED TRACKING uint8 PSM_STATE_INACTIVE = 3 # INACTIVE -# Note that the spoofing state value only reflects the detector state for the -# current navigation epoch. As spoofing can be detected most easily at the -# transition from real signal to spoofing signal, this is also where the +# Note that the spoofing state value only reflects the detector state for the +# current navigation epoch. As spoofing can be detected most easily at the +# transition from real signal to spoofing signal, this is also where the # detector is triggered the most. I.e. a value of 1 - No spoofing indicated does # not mean that the receiver is not spoofed, it #simply states that the detector # was not triggered in this epoch. -uint8 FLAGS2_SPOOF_DET_STATE_MASK = 24 +uint8 FLAGS2_SPOOF_DET_STATE_MASK = 24 uint8 SPOOF_DET_STATE_UNKNOWN = 0 # Unknown or deactivated uint8 SPOOF_DET_STATE_NONE = 8 # No spoofing indicated uint8 SPOOF_DET_STATE_SPOOFING = 16 # Spoofing indicated diff --git a/ublox_msgs/msg/NavSVIN.msg b/ublox_msgs/msg/NavSVIN.msg index 93e6205c..1237fea2 100644 --- a/ublox_msgs/msg/NavSVIN.msg +++ b/ublox_msgs/msg/NavSVIN.msg @@ -1,6 +1,6 @@ # NAV-SVIN (0x01 0x3B) # Survey-in data -# +# # This message contains information about survey-in parameters. # Supported on: # - u-blox 8 / u-blox M8 with protocol version 20 (only with High Precision @@ -12,28 +12,28 @@ uint8 MESSAGE_ID = 59 uint8 version # Message version (0x00 for this version) uint8[3] reserved0 # Reserved -uint32 iTOW # GPS time of week of the navigation epoch [ms] +uint32 i_tow # GPS time of week of the navigation epoch [ms] uint32 dur # Passed survey-in observation time [s] -int32 meanX # Current survey-in mean position ECEF X coordinate [cm] -int32 meanY # Current survey-in mean position ECEF Y coordinate [cm] -int32 meanZ # Current survey-in mean position ECEF Z coordinate [cm] +int32 mean_x # Current survey-in mean position ECEF X coordinate [cm] +int32 mean_y # Current survey-in mean position ECEF Y coordinate [cm] +int32 mean_z # Current survey-in mean position ECEF Z coordinate [cm] -int8 meanXHP # Current high-precision survey-in mean position - # ECEF X coordinate. 0.1_mm +int8 mean_xhp # Current high-precision survey-in mean position + # ECEF X coordinate. 0.1_mm # Must be in the range -99..+99. # The current survey-in mean position ECEF X # coordinate, in units of cm, is given by # meanX + (0.01 * meanXHP) -int8 meanYHP # Current high-precision survey-in mean position - # ECEF Y coordinate. [0.1 mm] +int8 mean_yhp # Current high-precision survey-in mean position + # ECEF Y coordinate. [0.1 mm] # Must be in the range -99..+99. # The current survey-in mean position ECEF Y # coordinate, in units of cm, is given by # meanY + (0.01 * meanYHP) -int8 meanZHP # Current high-precision survey-in mean position - # ECEF Z coordinate. [0.1 mm] +int8 mean_zhp # Current high-precision survey-in mean position + # ECEF Z coordinate. [0.1 mm] # Must be in the range -99..+99. # The current survey-in mean position ECEF Z # coordinate, in units of cm, is given by @@ -41,11 +41,11 @@ int8 meanZHP # Current high-precision survey-in mean position uint8 reserved1 # Reserved -uint32 meanAcc # Current survey-in mean position accuracy [0.1 mm] +uint32 mean_acc # Current survey-in mean position accuracy [0.1 mm] uint32 obs # Number of position observations used during survey-in uint8 valid # Survey-in position validity flag, 1 = valid # otherwise 0 uint8 active # Survey-in in progress flag, 1 = in-progress # otherwise 0 -uint8[2] reserved3 # Reserved \ No newline at end of file +uint8[2] reserved3 # Reserved diff --git a/ublox_msgs/msg/NavSVINFO.msg b/ublox_msgs/msg/NavSVINFO.msg index b7d3bb52..8e4ebd30 100644 --- a/ublox_msgs/msg/NavSVINFO.msg +++ b/ublox_msgs/msg/NavSVINFO.msg @@ -5,11 +5,11 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 48 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint8 numCh # Number of channels +uint8 num_ch # Number of channels -uint8 globalFlags # Bitmask +uint8 global_flags # Bitmask # Chip Hardware generation flags uint8 CHIPGEN_ANTARIS = 0 # Antaris, Antaris 4 uint8 CHIPGEN_UBLOX5 = 1 # u-blox 5 @@ -19,4 +19,4 @@ uint8 CHIPGEN_UBLOX8 = 4 # u-blox 8 / u-blox M8 uint16 reserved2 # Reserved -NavSVINFO_SV[] sv +NavSVINFOSV[] sv diff --git a/ublox_msgs/msg/NavSVINFO_SV.msg b/ublox_msgs/msg/NavSVINFOSV.msg similarity index 93% rename from ublox_msgs/msg/NavSVINFO_SV.msg rename to ublox_msgs/msg/NavSVINFOSV.msg index befaed6e..d6675aa0 100644 --- a/ublox_msgs/msg/NavSVINFO_SV.msg +++ b/ublox_msgs/msg/NavSVINFOSV.msg @@ -6,20 +6,20 @@ uint8 svid # Satellite ID uint8 flags # Bitmask uint8 FLAGS_SV_USED = 1 # SV is used for navigation -uint8 FLAGS_DIFF_CORR = 2 # Differential correction data +uint8 FLAGS_DIFF_CORR = 2 # Differential correction data # is available for this SV -uint8 FLAGS_ORBIT_AVAIL = 4 # Orbit information is available for +uint8 FLAGS_ORBIT_AVAIL = 4 # Orbit information is available for # this SV (Ephemeris or Almanach) uint8 FLAGS_ORBIT_EPH = 8 # Orbit information is Ephemeris -uint8 FLAGS_UNHEALTHY = 16 # SV is unhealthy / shall not be +uint8 FLAGS_UNHEALTHY = 16 # SV is unhealthy / shall not be # used uint8 FLAGS_ORBIT_ALM = 32 # Orbit information is Almanac Plus -uint8 FLAGS_ORBIT_AOP = 64 # Orbit information is AssistNow +uint8 FLAGS_ORBIT_AOP = 64 # Orbit information is AssistNow # Autonomous uint8 FLAGS_SMOOTHED = 128 # Carrier smoothed pseudorange used uint8 quality # Bitfield -# qualityInd: Signal Quality indicator (range 0..7). The following list shows +# qualityInd: Signal Quality indicator (range 0..7). The following list shows # the meaning of the different QI values: # Note: Since IMES signals are not time synchronized, a channel tracking an IMES # signal can never reach a quality indicator value of higher than 3. @@ -28,14 +28,14 @@ uint8 QUALITY_SEARCHING = 1 # Channel is searching uint8 QUALITY_ACQUIRED = 2 # Signal acquired uint8 QUALITY_DETECTED = 3 # Signal detected but unusable uint8 QUALITY_CODE_LOCK = 4 # Code Lock on Signal -uint8 QUALITY_CODE_AND_CARRIER_LOCKED1 = 5 # Code and Carrier locked +uint8 QUALITY_CODE_AND_CARRIER_LOCKED1 = 5 # Code and Carrier locked # and time synchronized -uint8 QUALITY_CODE_AND_CARRIER_LOCKED2 = 6 # Code and Carrier locked +uint8 QUALITY_CODE_AND_CARRIER_LOCKED2 = 6 # Code and Carrier locked # and time synchronized -uint8 QUALITY_CODE_AND_CARRIER_LOCKED3 = 7 # Code and Carrier locked +uint8 QUALITY_CODE_AND_CARRIER_LOCKED3 = 7 # Code and Carrier locked # and time synchronized uint8 cno # Carrier to Noise Ratio (Signal Strength) [dBHz] int8 elev # Elevation in integer degrees [deg] int16 azim # Azimuth in integer degrees [deg] -int32 prRes # Pseudo range residual in centimetres [cm] +int32 pr_res # Pseudo range residual in centimetres [cm] diff --git a/ublox_msgs/msg/NavTIMEGPS.msg b/ublox_msgs/msg/NavTIMEGPS.msg index ec69e993..9d2136d7 100644 --- a/ublox_msgs/msg/NavTIMEGPS.msg +++ b/ublox_msgs/msg/NavTIMEGPS.msg @@ -5,16 +5,16 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 32 -uint32 iTOW # GPS Millisecond time of week [ms] -int32 fTOW # Fractional Nanoseconds remainder of rounded +uint32 i_tow # GPS Millisecond time of week [ms] +int32 f_tow # Fractional Nanoseconds remainder of rounded # ms above, range -500000 .. 500000 [ns] int16 week # GPS week (GPS time) -int8 leapS # Leap Seconds (GPS-UTC) [s] +int8 leap_s # Leap Seconds (GPS-UTC) [s] uint8 valid # Validity Flags uint8 VALID_TOW = 1 # Valid Time of Week uint8 VALID_WEEK = 2 # Valid Week Number uint8 VALID_LEAP_S = 4 # Valid Leap Seconds -uint32 tAcc # Time Accuracy Estimate [ns] +uint32 t_acc # Time Accuracy Estimate [ns] diff --git a/ublox_msgs/msg/NavTIMEUTC.msg b/ublox_msgs/msg/NavTIMEUTC.msg index f9d57171..3ab44017 100644 --- a/ublox_msgs/msg/NavTIMEUTC.msg +++ b/ublox_msgs/msg/NavTIMEUTC.msg @@ -5,16 +5,16 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 33 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint32 tAcc # Time Accuracy Estimate [ns] +uint32 t_acc # Time Accuracy Estimate [ns] int32 nano # Fraction of second, range -1e9 .. 1e9 (UTC) [ns] uint16 year # Year, range 1999..2099 (UTC) [y] uint8 month # Month, range 1..12 (UTC) [month] uint8 day # Day of Month, range 1..31 (UTC) [d] uint8 hour # Hour of Day, range 0..23 (UTC) [h] uint8 min # Minute of Hour, range 0..59 (UTC) [min] -uint8 sec # Seconds of Minute, range 0..60 (UTC) [s] (60 for +uint8 sec # Seconds of Minute, range 0..60 (UTC) [s] (60 for # leap second) uint8 valid # Validity Flags @@ -24,12 +24,12 @@ uint8 VALID_UTC = 4 # Valid Leap Seconds, i.e. Leap Seconds already know uint8 VALID_UTC_STANDARD_MASK = 240 # UTC standard Identifier Bit mask: uint8 UTC_STANDARD_NOT_AVAILABLE = 0 # Information not available uint8 UTC_STANDARD_CRL = 16 # Communications Research Labratory -uint8 UTC_STANDARD_NIST = 32 # National Institute of Standards and +uint8 UTC_STANDARD_NIST = 32 # National Institute of Standards and # Technology (NIST) uint8 UTC_STANDARD_USNO = 48 # U.S. Naval Observatory (USNO) -uint8 UTC_STANDARD_BIPM = 64 # International Bureau of Weights and +uint8 UTC_STANDARD_BIPM = 64 # International Bureau of Weights and # Measures (BIPM) uint8 UTC_STANDARD_EL = 80 # European Laboratory (tbd) uint8 UTC_STANDARD_SU = 96 # Former Soviet Union (SU) uint8 UTC_STANDARD_NTSC = 112 # National Time Service Center, China -uint8 UTC_STANDARD_UNKNOWN = 240 \ No newline at end of file +uint8 UTC_STANDARD_UNKNOWN = 240 diff --git a/ublox_msgs/msg/NavVELECEF.msg b/ublox_msgs/msg/NavVELECEF.msg index 52f5ceb6..bf11daa1 100644 --- a/ublox_msgs/msg/NavVELECEF.msg +++ b/ublox_msgs/msg/NavVELECEF.msg @@ -8,9 +8,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 17 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -int32 ecefVX # ECEF X velocity [cm/s] -int32 ecefVY # ECEF Y velocity [cm/s] -int32 ecefVZ # ECEF Z velocity [cm/s] -uint32 sAcc # Speed Accuracy Estimate [cm/s] +int32 ecef_vx # ECEF X velocity [cm/s] +int32 ecef_vy # ECEF Y velocity [cm/s] +int32 ecef_vz # ECEF Z velocity [cm/s] +uint32 s_acc # Speed Accuracy Estimate [cm/s] diff --git a/ublox_msgs/msg/NavVELNED.msg b/ublox_msgs/msg/NavVELNED.msg index 8796e8f6..e63325ea 100644 --- a/ublox_msgs/msg/NavVELNED.msg +++ b/ublox_msgs/msg/NavVELNED.msg @@ -8,13 +8,13 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 18 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -int32 velN # NED north velocity [cm/s] -int32 velE # NED east velocity [cm/s] -int32 velD # NED down velocity [cm/s] +int32 vel_n # NED north velocity [cm/s] +int32 vel_e # NED east velocity [cm/s] +int32 vel_d # NED down velocity [cm/s] uint32 speed # Speed (3-D) [cm/s] -uint32 gSpeed # Ground Speed (2-D) [cm/s] +uint32 g_speed # Ground Speed (2-D) [cm/s] int32 heading # Heading of motion 2-D [deg / 1e-5] -uint32 sAcc # Speed Accuracy Estimate [cm/s] -uint32 cAcc # Course / Heading Accuracy Estimate [deg / 1e-5] +uint32 s_acc # Speed Accuracy Estimate [cm/s] +uint32 c_acc # Course / Heading Accuracy Estimate [deg / 1e-5] diff --git a/ublox_msgs/msg/RxmRAW.msg b/ublox_msgs/msg/RxmRAW.msg index b8e7581c..c966f598 100644 --- a/ublox_msgs/msg/RxmRAW.msg +++ b/ublox_msgs/msg/RxmRAW.msg @@ -2,20 +2,20 @@ # Raw Measurement Data # # Supported up to ublox 7 firmware. See RxmRAWX for ublox 8 -# This message contains all information needed to be able to generate a RINEX +# This message contains all information needed to be able to generate a RINEX # observation file. -# This message outputs pseudorange, doppler and carrier phase measurements for -# GPS satellites once signals have been synchronised. No other GNSS types are +# This message outputs pseudorange, doppler and carrier phase measurements for +# GPS satellites once signals have been synchronised. No other GNSS types are # currently supported. -# +# uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 16 -int32 rcvTOW # Measurement time of week in receiver local time [s] +int32 rcv_tow # Measurement time of week in receiver local time [s] int16 week # Measurement week number in receiver local time [weeks] -uint8 numSV # # of satellites following +uint8 num_sv # # of satellites following uint8 reserved1 # Reserved -RxmRAW_SV[] sv # numSV times +RxmRAWSV[] sv # numSV times diff --git a/ublox_msgs/msg/RxmRAW_SV.msg b/ublox_msgs/msg/RxmRAWSV.msg similarity index 61% rename from ublox_msgs/msg/RxmRAW_SV.msg rename to ublox_msgs/msg/RxmRAWSV.msg index 70c3e604..954af99a 100644 --- a/ublox_msgs/msg/RxmRAW_SV.msg +++ b/ublox_msgs/msg/RxmRAWSV.msg @@ -1,15 +1,15 @@ # see message RxmRAW # -float64 cpMes # Carrier phase measurement [L1 cycles] -float64 prMes # Pseudorange measurement [m] -float32 doMes # Doppler measurement [Hz] +float64 cp_mes # Carrier phase measurement [L1 cycles] +float64 pr_mes # Pseudorange measurement [m] +float32 do_mes # Doppler measurement [Hz] uint8 sv # Space Vehicle Number -int8 mesQI # Nav Measurements Quality Indicator +int8 mes_qi # Nav Measurements Quality Indicator # >=4 : PR+DO OK # >=5 : PR+DO+CP OK - # <6 : likely loss of carrier lock in previous + # <6 : likely loss of carrier lock in previous # interval int8 cno # Signal strength C/No. [dbHz] uint8 lli # Loss of lock indicator (RINEX definition) diff --git a/ublox_msgs/msg/RxmRAWX.msg b/ublox_msgs/msg/RxmRAWX.msg index 7fe3c2bd..a985913c 100644 --- a/ublox_msgs/msg/RxmRAWX.msg +++ b/ublox_msgs/msg/RxmRAWX.msg @@ -6,34 +6,34 @@ # This message contains pseudorange, Doppler, carrier phase, phase lock and # signal quality information for GNSS satellites once signals have been # synchronized. This message supports all active GNSS. -# +# uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 21 -float64 rcvTOW # Measurement time of week in receiver local time [s] +float64 rcv_tow # Measurement time of week in receiver local time [s] # approximately aligned to the GPS time system. The # receiver local time of week number and leap second - # information can be used to translate the time to other + # information can be used to translate the time to other # time systems. More information about the difference in - # time systems can be found in RINEX 3 documentation. + # time systems can be found in RINEX 3 documentation. # For a receiver operating in GLONASS only mode, UTC - # time can be determined by subtracting the leapS field + # time can be determined by subtracting the leapS field # from GPS time regardless of whether the GPS leap - # seconds are valid. + # seconds are valid. uint16 week # GPS week number in receiver local time. [weeks] -int8 leapS # GPS leap seconds (GPS-UTC). [s] +int8 leap_s # GPS leap seconds (GPS-UTC). [s] # This field represents the receiver's best knowledge of # the leap seconds offset. A flag is given in the - # recStat bitfield to indicate if the leap seconds + # recStat bitfield to indicate if the leap seconds # are known. -uint8 numMeas # # of measurements to follow -uint8 recStat # Receiver tracking status bitfield +uint8 num_meas # # of measurements to follow +uint8 rec_stat # Receiver tracking status bitfield uint8 REC_STAT_LEAP_SEC = 1 # Leap seconds have been determined -uint8 REC_STAT_CLK_RESET = 2 # Clock reset applied. Typically the receiver +uint8 REC_STAT_CLK_RESET = 2 # Clock reset applied. Typically the receiver # clock is changed in increments of integer # milliseconds. uint8 version # Message version (0x01 for this version). uint8[2] reserved1 # Reserved -RxmRAWX_Meas[] meas +RxmRAWXMeas[] meas diff --git a/ublox_msgs/msg/RxmRAWX_Meas.msg b/ublox_msgs/msg/RxmRAWXMeas.msg similarity index 63% rename from ublox_msgs/msg/RxmRAWX_Meas.msg rename to ublox_msgs/msg/RxmRAWXMeas.msg index 37ddac1d..80bc7060 100644 --- a/ublox_msgs/msg/RxmRAWX_Meas.msg +++ b/ublox_msgs/msg/RxmRAWXMeas.msg @@ -1,39 +1,39 @@ # see message RxmRAWX # -float64 prMes # Pseudorange measurement [m]. GLONASS inter frequency +float64 pr_mes # Pseudorange measurement [m]. GLONASS inter frequency # channel delays are compensated with an internal # calibration table. -float64 cpMes # Carrier phase measurement [L1 cycles]. The carrier +float64 cp_mes # Carrier phase measurement [L1 cycles]. The carrier # phase initial ambiguity is initialized using an # approximate value to make the magnitude of # the phase close to the pseudorange # measurement. Clock resets are applied to both # phase and code measurements in accordance # with the RINEX specification. -float32 doMes # Doppler measurement [Hz] (positive sign for +float32 do_mes # Doppler measurement [Hz] (positive sign for # approaching satellites) -uint8 gnssId # GNSS identifier (see CfgGNSS for constants) +uint8 gnss_id # GNSS identifier (see CfgGNSS for constants) -uint8 svId # Satellite identifier (see Satellite Numbering) +uint8 sv_id # Satellite identifier (see Satellite Numbering) uint8 reserved0 # Reserved -uint8 freqId # Only used for GLONASS: This is the frequency +uint8 freq_id # Only used for GLONASS: This is the frequency # slot + 7 (range from 0 to 13) -uint16 locktime # Carrier phase locktime counter [ms] +uint16 locktime # Carrier phase locktime counter [ms] # (maximum 64500 ms) -int8 cno # Carrier-to-noise density ratio (signal strength) +int8 cno # Carrier-to-noise density ratio (signal strength) # [dB-Hz] -uint8 prStdev # Estimated pseudorange measurement standard +uint8 pr_stdev # Estimated pseudorange measurement standard # deviation [m / 0.01*2^n] -uint8 cpStdev # Estimated carrier phase measurement standard +uint8 cp_stdev # Estimated carrier phase measurement standard # deviation (note a raw value of 0x0F indicates the # value is invalid) [cycles / 0.004] -uint8 doStdev # Estimated Doppler measurement standard deviation +uint8 do_stdev # Estimated Doppler measurement standard deviation # [Hz / 0.002*2^n] -uint8 trkStat # Tracking status bitfield +uint8 trk_stat # Tracking status bitfield uint8 TRK_STAT_PR_VALID = 1 # Pseudorange valid uint8 TRK_STAT_CP_VALID = 2 # Carrier phase valid uint8 TRK_STAT_HALF_CYC = 4 # Half cycle valid diff --git a/ublox_msgs/msg/RxmRTCM.msg b/ublox_msgs/msg/RxmRTCM.msg index 4201d798..b0e97515 100644 --- a/ublox_msgs/msg/RxmRTCM.msg +++ b/ublox_msgs/msg/RxmRTCM.msg @@ -4,18 +4,18 @@ # Output upon processing of an RTCM input message # Supported on: # - u-blox 8 / u-blox M8 from protocol version 20.01 up to version 23.01 -# +# uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 50 uint8 version # Message version (0x02 for this version) uint8 flags # RTCM input status flags -uint8 FLAGS_CRC_FAILED = 1 # 0 when RTCM message received and passed CRC +uint8 FLAGS_CRC_FAILED = 1 # 0 when RTCM message received and passed CRC # check, 1 when failed in which case refStation # and msgType might be corrupted and misleading uint8[2] reserved0 # Reserved -uint16 refStation # Reference station ID -uint16 msgType # Message type +uint16 ref_station # Reference station ID +uint16 msg_type # Message type diff --git a/ublox_msgs/msg/RxmSFRBX.msg b/ublox_msgs/msg/RxmSFRBX.msg index 9cb2bef1..bf9351c2 100644 --- a/ublox_msgs/msg/RxmSFRBX.msg +++ b/ublox_msgs/msg/RxmSFRBX.msg @@ -1,7 +1,7 @@ # RXM-SFRB (0x02 0x13) # Subframe Buffer # -# This message reports a complete subframe of broadcast navigation data decoded +# This message reports a complete subframe of broadcast navigation data decoded # from a single signal. The number of data words reported in each message # depends on the nature of the signal. See the section on Broadcast Navigation # Data for further details. @@ -10,13 +10,13 @@ uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 19 -uint8 gnssId # GNSS identifier (see Cfg GNSS for constants) +uint8 gnss_id # GNSS identifier (see Cfg GNSS for constants) -uint8 svId # Satellite identifier within corresponding GNSS system +uint8 sv_id # Satellite identifier within corresponding GNSS system uint8 reserved0 # Reserved -uint8 freqId # Only used for GLONASS: This is the frequency +uint8 freq_id # Only used for GLONASS: This is the frequency # slot + 7 (range from 0 to 13) -uint8 numWords # The number of data words contained in this message (up +uint8 num_words # The number of data words contained in this message (up # to 10, for currently supported signals) uint8 chn # The tracking channel number the message was received # on @@ -25,4 +25,4 @@ uint8 reserved1 # Reserved # Start of repeated block (numWords times) uint32[] dwrd # The data words -# End of repeated block \ No newline at end of file +# End of repeated block diff --git a/ublox_msgs/msg/RxmSVSI.msg b/ublox_msgs/msg/RxmSVSI.msg index b163c7f8..f686def0 100644 --- a/ublox_msgs/msg/RxmSVSI.msg +++ b/ublox_msgs/msg/RxmSVSI.msg @@ -9,10 +9,10 @@ uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 32 -int32 iTOW # GPS time of week of the navigation epoch [ms] +int32 i_tow # GPS time of week of the navigation epoch [ms] int16 week # GPS week number of the navigation epoch [weeks] -uint8 numVis # Number of visible satellites -uint8 numSV # Number of per-SV data blocks following +uint8 num_vis # Number of visible satellites +uint8 num_sv # Number of per-SV data blocks following -RxmSVSI_SV[] sv +RxmSVSISV[] sv diff --git a/ublox_msgs/msg/RxmSVSISV.msg b/ublox_msgs/msg/RxmSVSISV.msg new file mode 100644 index 00000000..aa37b479 --- /dev/null +++ b/ublox_msgs/msg/RxmSVSISV.msg @@ -0,0 +1,22 @@ +# see message RxmSVSI +# + +uint8 svid # Satellite ID + +uint8 sv_flag # Information Flags +uint8 FLAG_URA_MASK = 15 # Figure of Merit (URA) range 0..15 +uint8 FLAG_HEALTHY = 16 # SV healthy flag +uint8 FLAG_EPH_VAL = 32 # Ephemeris valid +uint8 FLAG_ALM_VAL = 64 # Almanac valid +uint8 FLAG_NOT_AVAIL = 128 # SV not available + +int16 azim # Azimuth +int8 elev # Elevation + +uint8 age # Age of Almanac and Ephemeris +uint8 AGE_ALM_MASK = 15 # Age of ALM in days offset by 4 + # i.e. the reference time may be in the future: + # ageOfAlm = (age & 0x0f) - 4 +uint8 AGE_EPH_MASK = 240 # Age of EPH in hours offset by 4. + # i.e. the reference time may be in the future: + # ageOfEph = ((age & 0xf0) >> 4) - 4 diff --git a/ublox_msgs/msg/TimTM2.msg b/ublox_msgs/msg/TimTM2.msg index 2ff4a065..83582704 100644 --- a/ublox_msgs/msg/TimTM2.msg +++ b/ublox_msgs/msg/TimTM2.msg @@ -1,13 +1,13 @@ # TIM-TM2 (0x0D, 0x03) -# Time mark data +# Time mark data # # Description for details. -# +# # Supported on: # - u-blox 8 / u-blox M8 with protocol version 22 (only with Timing Products) # -uint8 CLASS_ID = 13 +uint8 CLASS_ID = 13 uint8 MESSAGE_ID = 3 uint8 ch # Channel (i.e. EXTINT) upon which the pulse was measured @@ -22,11 +22,11 @@ uint8 FLAGS_UTC_AVAIL = 32 # 0 = utc not available, 1 = utc available uint8 FLAGS_TIME_VALID = 64 # 0 = time is not valid, 1 time is valid uint8 FLAGS_NEWRISINGEDGE = 128 # new rising edge detected -uint16 risingEdgeCount # rising edge count -uint16 wnR # week number of last rising edge -uint16 wnF # week number of last falling edge -uint32 towMsR # Tow of rising edge -uint32 towSubMsR # Millisecond Fraction of Tow of rising edge in nanoseconds -uint32 towMsF # tow of falling edge -uint32 towSubMsF # millisecond fraction of tow of falling edge in nanoseconds -uint32 accEst # Accuracy estimate +uint16 rising_edge_count # rising edge count +uint16 wn_r # week number of last rising edge +uint16 wn_f # week number of last falling edge +uint32 tow_ms_r # Tow of rising edge +uint32 tow_sub_ms_r # Millisecond Fraction of Tow of rising edge in nanoseconds +uint32 tow_ms_f # tow of falling edge +uint32 tow_sub_ms_f # millisecond fraction of tow of falling edge in nanoseconds +uint32 acc_est # Accuracy estimate diff --git a/ublox_msgs/msg/UpdSOS.msg b/ublox_msgs/msg/UpdSOS.msg index 139b3a2f..236746e4 100644 --- a/ublox_msgs/msg/UpdSOS.msg +++ b/ublox_msgs/msg/UpdSOS.msg @@ -8,18 +8,18 @@ uint8 CLASS_ID = 9 uint8 MESSAGE_ID = 20 uint8 cmd # Command -# The host can send this message in order to save part of the BBR memory in a -# file in flash file system. The feature is designed in order to emulate the -# presence of the backup battery even if it is not present; the host can issue -# the save on shutdown command before switching off the device supply. It is -# recommended to issue a GNSS stop command before, in order to keep the BBR +# The host can send this message in order to save part of the BBR memory in a +# file in flash file system. The feature is designed in order to emulate the +# presence of the backup battery even if it is not present; the host can issue +# the save on shutdown command before switching off the device supply. It is +# recommended to issue a GNSS stop command before, in order to keep the BBR # memory content consistent. uint8 CMD_FLASH_BACKUP_CREATE = 0 # Create Backup File in Flash -# The host can send this message in order to erase the backup file present in -# flash. It is recommended that the clear operation is issued after the host has -# received the notification that the memory has been restored after a reset. -# Alternatively the host can parse the startup string 'Restored data saved on +# The host can send this message in order to erase the backup file present in +# flash. It is recommended that the clear operation is issued after the host has +# received the notification that the memory has been restored after a reset. +# Alternatively the host can parse the startup string 'Restored data saved on # shutdown' or poll the UBX-UPD-SOS message for getting the status. uint8 CMD_FLASH_BACKUP_CLEAR = 1 # Clear Backup File in Flash -uint8[3] reserved1 # Reserved \ No newline at end of file +uint8[3] reserved1 # Reserved diff --git a/ublox_msgs/msg/UpdSOS_Ack.msg b/ublox_msgs/msg/UpdSOSAck.msg similarity index 90% rename from ublox_msgs/msg/UpdSOS_Ack.msg rename to ublox_msgs/msg/UpdSOSAck.msg index 82584431..680cf091 100644 --- a/ublox_msgs/msg/UpdSOS_Ack.msg +++ b/ublox_msgs/msg/UpdSOSAck.msg @@ -1,7 +1,7 @@ # UPD-SOS (0x09 0x14) # # Backup File Creation Acknowledge / System Restored from Backup -# +# # Firmware Supported on: # u-blox 8 / u-blox M8 from protocol version 15 up to version 23.01 # @@ -11,17 +11,17 @@ uint8 MESSAGE_ID = 20 uint8 cmd # Command uint8 CMD_BACKUP_CREATE_ACK = 2 # Backup File Creation Acknowledge - # The message is sent from the device as - # confirmation of creation of a backup file - # in flash. The host can safely shut down the + # The message is sent from the device as + # confirmation of creation of a backup file + # in flash. The host can safely shut down the # device after received this message. uint8 CMD_SYSTEM_RESTORED = 3 # System Restored from Backup - # The message is sent from the device to - # notify the host the BBR has been restored - # from a backup file in flash. The host - # should clear the backup file after - # receiving this message. If the UBX-UPD-SOS - # message is polled, this message will be + # The message is sent from the device to + # notify the host the BBR has been restored + # from a backup file in flash. The host + # should clear the backup file after + # receiving this message. If the UBX-UPD-SOS + # message is polled, this message will be # present. uint8[3] reserved0 # Reserved @@ -30,9 +30,9 @@ uint8 response # Response: uint8 BACKUP_CREATE_NACK = 0 # Not acknowledged uint8 BACKUP_CREATE_ACK = 1 # Acknowledged uint8 SYSTEM_RESTORED_RESPONSE_UNKNOWN = 0 # Unknown -uint8 SYSTEM_RESTORED_RESPONSE_FAILED = 1 # Failed restoring from backup +uint8 SYSTEM_RESTORED_RESPONSE_FAILED = 1 # Failed restoring from backup # file uint8 SYSTEM_RESTORED_RESPONSE_RESTORED = 2 # Restored from backup file uint8 SYSTEM_RESTORED_RESPONSE_NOT_RESTORED = 3 # Not restored (no backup) -uint8[3] reserved1 # Reserved \ No newline at end of file +uint8[3] reserved1 # Reserved diff --git a/ublox_msgs/package.xml b/ublox_msgs/package.xml index 124cd2c2..35da63ff 100644 --- a/ublox_msgs/package.xml +++ b/ublox_msgs/package.xml @@ -1,6 +1,8 @@ - + + + ublox_msgs - 1.2.0 + 2.3.0 ublox_msgs contains raw messages for u-blox GNSS devices. @@ -11,22 +13,18 @@ BSD http://ros.org/wiki/ublox - catkin + ament_cmake_ros - message_generation - message_runtime + rosidl_default_generators - ublox_serialization - ublox_serialization + std_msgs + sensor_msgs + ublox_serialization - std_msgs - std_msgs - - sensor_msgs - sensor_msgs + rosidl_interface_packages - + ament_cmake diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 25511954..ac905307 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -26,175 +26,186 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include +#include +#include + +#include template std::vector > ublox::Message::keys_; -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, - ublox_msgs, NavATT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, - ublox_msgs, NavCLOCK); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, - ublox_msgs, NavDGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, - ublox_msgs, NavDOP); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, - ublox_msgs, NavPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, - ublox_msgs, NavPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED, - ublox_msgs, - NavRELPOSNED); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, + ublox_msgs, NavATT) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, + ublox_msgs, NavCLOCK) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::COV, + ublox_msgs, NavCOV) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, + ublox_msgs, NavDGPS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, + ublox_msgs, NavDOP) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, + ublox_msgs, NavPOSECEF) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, + ublox_msgs, NavPOSLLH) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, + ublox_msgs::Message::NAV::RELPOSNED, + ublox_msgs, + NavRELPOSNED) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED9, ublox_msgs, - NavRELPOSNED9); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, - ublox_msgs, NavSBAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, - ublox_msgs, NavSOL); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, - ublox_msgs, NavSAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, - ublox_msgs, NavSTATUS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, - ublox_msgs, NavSVIN); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, - ublox_msgs, NavSVINFO); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, - ublox_msgs, NavTIMEGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, - ublox_msgs, NavTIMEUTC); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, - ublox_msgs, NavVELECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, - ublox_msgs, NavVELNED); - -// ACK messages are declared differently because they both have the same + NavRELPOSNED9) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, + ublox_msgs, NavSBAS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, + ublox_msgs, NavSOL) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, + ublox_msgs, NavPVT) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, + ublox_msgs, NavPVT7) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, + ublox_msgs, NavSAT) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, + ublox_msgs, NavSTATUS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, + ublox_msgs, NavSVIN) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, + ublox_msgs, NavSVINFO) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, + ublox_msgs, NavTIMEGPS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, + ublox_msgs, NavTIMEUTC) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, + ublox_msgs, NavVELECEF) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, + ublox_msgs, NavVELNED) + +// ACK messages are declared differently because they both have the same // protocol, so only 1 ROS message is used -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, - ublox_msgs, Ack); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, - ublox_msgs, Ack, ACK); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, + ublox_msgs, Ack) +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, + ublox_msgs, Ack, ACK) -// INF messages are declared differently because they all have the same +// INF messages are declared differently because they all have the same // protocol, so only 1 ROS message is used. DECLARE_UBLOX_MESSAGE can only // be called once, and DECLARE_UBLOX_MESSAGE_ID is called for the following // messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, - ublox_msgs, Inf); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::WARNING, - ublox_msgs, Inf, WARNING); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::NOTICE, - ublox_msgs, Inf, NOTICE); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::TEST, - ublox_msgs, Inf, TEST); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::DEBUG, - ublox_msgs, Inf, DEBUG); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, - ublox_msgs, RxmALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, - ublox_msgs, RxmEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, - ublox_msgs, RxmRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, - ublox_msgs, RxmRAWX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, - ublox_msgs, RxmRTCM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, - ublox_msgs, RxmSFRB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, - ublox_msgs, RxmSFRBX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, - ublox_msgs, RxmSVSI); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, - ublox_msgs, CfgANT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, - ublox_msgs, CfgCFG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, - ublox_msgs, CfgDAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, - ublox_msgs, CfgDGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, - ublox_msgs, CfgGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, + ublox_msgs, Inf) +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::WARNING, + ublox_msgs, Inf, WARNING) +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::NOTICE, + ublox_msgs, Inf, NOTICE) +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::TEST, + ublox_msgs, Inf, TEST) +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::DEBUG, + ublox_msgs, Inf, DEBUG) + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, + ublox_msgs, RxmALM) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, + ublox_msgs, RxmEPH) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, + ublox_msgs, RxmRAW) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, + ublox_msgs, RxmRAWX) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, + ublox_msgs, RxmRTCM) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, + ublox_msgs, RxmSFRB) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, + ublox_msgs, RxmSFRBX) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, + ublox_msgs, RxmSVSI) + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, + ublox_msgs, CfgANT) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, + ublox_msgs, CfgCFG) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, + ublox_msgs, CfgDAT) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, + ublox_msgs, CfgDGNSS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, + ublox_msgs, CfgGNSS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, - ublox_msgs, CfgHNR); + ublox_msgs, CfgHNR) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, - ublox_msgs, CfgINF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, - ublox_msgs, CfgMSG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, - ublox_msgs, CfgNAV5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, - ublox_msgs, CfgNAVX5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, - ublox_msgs, CfgPRT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, - ublox_msgs, CfgRATE); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, - ublox_msgs, CfgRST); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, - ublox_msgs, CfgTMODE3); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, - ublox_msgs, CfgUSB); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS); + ublox_msgs, CfgINF) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, + ublox_msgs, CfgMSG) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, + ublox_msgs, CfgNAV5) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, + ublox_msgs, CfgNAVX5) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, + ublox_msgs, CfgNMEA) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, + ublox_msgs, CfgNMEA6) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, + ublox_msgs, CfgNMEA7) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, + ublox_msgs, CfgPRT) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, + ublox_msgs, CfgRATE) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, + ublox_msgs, CfgRST) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, + ublox_msgs, CfgTMODE3) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, + ublox_msgs, CfgUSB) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::VALDEL, + ublox_msgs, CfgVALDEL); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::VALGET, + ublox_msgs, CfgVALGET); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::VALSET, + ublox_msgs, CfgVALSET); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, + ublox_msgs, UpdSOS) // SOS and SOS_Ack have the same message ID, but different lengths -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS_Ack); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, - ublox_msgs, MonGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, - ublox_msgs, MonVER); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, - ublox_msgs, AidALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, - ublox_msgs, AidEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, - ublox_msgs, AidHUI); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, + ublox_msgs, UpdSOSAck) + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, + ublox_msgs, MonGNSS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, + ublox_msgs, MonHW) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, + ublox_msgs, MonHW6) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, + ublox_msgs, MonVER) + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, + ublox_msgs, AidALM) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, + ublox_msgs, AidEPH) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, + ublox_msgs, AidHUI) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, - ublox_msgs, EsfINS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, - ublox_msgs, EsfMEAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, - ublox_msgs, EsfRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, - ublox_msgs, EsfSTATUS); + ublox_msgs, EsfINS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, + ublox_msgs, EsfMEAS) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, + ublox_msgs, EsfRAW) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, + ublox_msgs, EsfSTATUS) -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, - ublox_msgs, MgaGAL); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, + ublox_msgs, MgaGAL) -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, - ublox_msgs, HnrPVT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, + ublox_msgs, HnrPVT) // TIM messages DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::TIM, ublox_msgs::Message::TIM::TM2, - ublox_msgs, TimTM2); + ublox_msgs, TimTM2) diff --git a/ublox_serialization/CHANGELOG.rst b/ublox_serialization/CHANGELOG.rst index ff4e0431..b399ea27 100644 --- a/ublox_serialization/CHANGELOG.rst +++ b/ublox_serialization/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package ublox_serialization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.0 (2022-04-13) +------------------ +* Revamp the building of the driver for modern ROS 2 practices. +* Contributors: Chao Qu, Chris Lalancette + +2.0.0 (2020-10-13) +------------------ +* Initial ROS 2 port to Dashing +* Fixes pointed out by clang-tidy. +* Make sure to initialize checksum properly. +* Port of ublox_serialization to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Remove unused vector serialization. +* Move serialization into the ublox_serialization module. +* Move the directory structure around just a bit. +* Remove boost from serialization. +* Remove trailing whitespace in all files. +* Rename header files to have .hpp extension. +* Contributors: Chao Qu, Chris Lalancette + 1.2.0 (2019-11-19) ------------------ diff --git a/ublox_serialization/CMakeLists.txt b/ublox_serialization/CMakeLists.txt index e8543e75..15bb03a4 100644 --- a/ublox_serialization/CMakeLists.txt +++ b/ublox_serialization/CMakeLists.txt @@ -1,14 +1,21 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox_serialization) -find_package(catkin REQUIRED COMPONENTS roscpp roscpp_serialization) +find_package(ament_cmake REQUIRED) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp roscpp_serialization) +add_library(${PROJECT_NAME} INTERFACE) +target_include_directories(${PROJECT_NAME} INTERFACE + "$" + "$") install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE + DESTINATION include/${PROJECT_NAME} ) +install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") + +ament_export_targets(export_${PROJECT_NAME}) + +ament_package() diff --git a/ublox_serialization/include/ublox/serialization.h b/ublox_serialization/include/ublox/serialization.h deleted file mode 100644 index 5e5cfa72..00000000 --- a/ublox_serialization/include/ublox/serialization.h +++ /dev/null @@ -1,443 +0,0 @@ -//============================================================================== -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * 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. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, 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 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 UBLOX_SERIALIZATION_H -#define UBLOX_SERIALIZATION_H - -#include -#include -#include -#include -#include - -#include "checksum.h" - -/// -/// This file defines the Serializer template class which encodes and decodes -/// specific message types. -/// The Reader class decodes messages and from a buffer and the Writer class -/// encodes messages and writes them to a buffer. -/// It also declares macros for declaring Messages. The Message class -/// maps ROS messages types to class and message ID(s). -/// - - -/** - * @namespace ublox - * This namespace is for u-blox message serialization. - */ -namespace ublox { - -//! u-blox message Sync A char -static const uint8_t DEFAULT_SYNC_A = 0xB5; -//! u-blox message Sync B char -static const uint8_t DEFAULT_SYNC_B = 0x62; -//! Number of bytes in a message header (Sync chars + class ID + message ID) -static const uint8_t kHeaderLength = 6; -//! Number of checksum bytes in the u-blox message -static const uint8_t kChecksumLength = 2; - -/** - * @brief Encodes and decodes messages. - */ -template -struct Serializer { - /** - * @brief Decode the message payload from the data buffer. - * @param data a pointer to the start of the message payload - * @param count the number of bytes in the message payload - * @param message the output message - */ - static void read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message); - /** - * @brief Get the length of the message payload in bytes. - * - * @details The payload does not include the header or checksum. - * @param message the message to get the length of - * @return the length of the message in bytes. - */ - static uint32_t serializedLength( - typename boost::call_traits::param_type message); - - /** - * @brief Encode the message payload as a byte array. - * @param data a buffer to fill with the message payload bytes - * @param size the length of the buffer - * @param message the output message - */ - static void write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message); -}; - -/** - * @brief Keeps track of which class and message IDs can be decoded by a given - * message type. - */ -template -class Message { - public: - /** - * @brief Can this message type decode a u-blox message with the given ID? - * @param class_id the class ID of the u-blox message - * @param message_id the message ID of the u-blox message - * @return whether or not this message type decode the u-blox message - */ - static bool canDecode(uint8_t class_id, uint8_t message_id) { - return std::find(keys_.begin(), keys_.end(), - std::make_pair(class_id, message_id)) != keys_.end(); - } - - /** - * @brief Indicate that this message type can decode u-blox messages with the - * given ID - * @param class_id the class ID of the u-blox message - * @param message_id the message ID of the u-blox message - */ - static void addKey(uint8_t class_id, uint8_t message_id) { - keys_.push_back(std::make_pair(class_id, message_id)); - } - - struct StaticKeyInitializer - { - StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { - Message::addKey(class_id, message_id); - } - }; - - private: - static std::vector > keys_; -}; - -/** - * @brief Options for the Reader and Writer for encoding and decoding messages. - */ -struct Options { - /** - * The default options for a u-blox message. - */ - Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), - header_length(kHeaderLength), checksum_length(kChecksumLength) {} - //! The sync_a byte value identifying the start of a message - uint8_t sync_a; - //! The sync_b byte value identifying the start of a message - uint8_t sync_b; - //! The length of the message header in bytes (everything before the payload) - uint8_t header_length; - //! The length of the checksum in bytes - uint8_t checksum_length; - - /** - * @brief Get the number of bytes in the header and footer. - * @return the number of bytes in the header and footer - */ - int wrapper_length() { - return header_length + checksum_length; - } -}; - -/** - * @brief Decodes byte messages into u-blox ROS messages. - */ -class Reader { - public: - /** - * @param data a buffer containing u-blox messages - * @param count the size of the buffer - * @param options A struct containing the parameters sync_a and sync_b - * which represent the sync bytes indicating the beginning of the message - */ - Reader(const uint8_t *data, uint32_t count, - const Options &options = Options()) : - data_(data), count_(count), found_(false), options_(options) {} - - typedef const uint8_t *iterator; - - /** - * @brief Search the buffer for the beginning of the next u-blox message - * @return a pointer to the start of the next u-blox message - */ - iterator search() - { - if (found_) next(); - - // Search for a message header - for( ; count_ > 0; --count_, ++data_) { - if (data_[0] == options_.sync_a && - (count_ == 1 || data_[1] == options_.sync_b)) - break; - } - - return data_; - } - - /** - * @brief Has a u-blox message been found in the buffer? - * @returns true if A message with the correct header & length has been found - */ - bool found() - { - if (found_) return true; - // Verify message is long enough to have sync chars, id, length & checksum - if (count_ < options_.wrapper_length()) return false; - // Verify the header bits - if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) - return false; - // Verify that the buffer length is long enough based on the received - // message length - if (count_ < length() + options_.wrapper_length()) return false; - - found_ = true; - return true; - } - - /** - * @brief Go to the start of the next message based on the received message - * length. - * - * @details Warning: Does not go to the correct byte location if the received - * message length is incorrect. If this is the case, search must be called. - */ - iterator next() { - if (found()) { - uint32_t size = length() + options_.wrapper_length(); - data_ += size; count_ -= size; - } - found_ = false; - return data_; - } - - /** - * @brief Get the current position in the read buffer. - * @return the current position of the read buffer - */ - iterator pos() { - return data_; - } - - iterator end() { - return data_ + count_; - } - - uint8_t classId() { return data_[2]; } - uint8_t messageId() { return data_[3]; } - - /** - * @brief Get the length of the u-blox message payload. - * - * @details Payload length does not include the header or checksum length. - * Determines the length from the header of the u-blox message. - * @return the length of the message payload - */ - uint32_t length() { return (data_[5] << 8) + data_[4]; } - const uint8_t *data() { return data_ + options_.header_length; } - - /** - * @brief Get the checksum of the u-blox message. - * - * @return the checksum of the u-blox message - */ - uint16_t checksum() { - return *reinterpret_cast(data_ + options_.header_length + - length()); - } - - /** - * @brief Decode the given message. - * @param message the output message - * @param search whether or not to skip to the next message in the buffer - */ - template - bool read(typename boost::call_traits::reference message, - bool search = false) { - if (search) this->search(); - if (!found()) return false; - if (!Message::canDecode(classId(), messageId())) return false; - - uint16_t chk; - if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { - // checksum error - ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), - messageId()); - return false; - } - - Serializer::read(data_ + options_.header_length, length(), message); - return true; - } - - /** - * @brief Can the given message type decode the current message in the buffer? - * @return whether the given message type can decode the current message in - * the buffer - */ - template - bool hasType() { - if (!found()) return false; - return Message::canDecode(classId(), messageId()); - } - - /** - * @brief Does the u-blox message have the given class and message ID? - * @return Whether or not the u-blox message has the given class and message - * ID - */ - bool isMessage(uint8_t class_id, uint8_t message_id) { - if (!found()) return false; - return (classId() == class_id && messageId() == message_id); - } - - private: - //! The buffer of message bytes - const uint8_t *data_; - //! the number of bytes in the buffer, //! decrement as the buffer is read - uint32_t count_; - //! Whether or not a message has been found - bool found_; - //! Options representing the sync char values, etc. - Options options_; -}; - -/** - * @brief Encodes a u-blox ROS message as a byte array. - */ -class Writer { - public: - typedef uint8_t *iterator; - - /** - * @brief Construct a Writer with the given buffer. - * @param data a buffer for messages - * @param size the size of the buffer - * @param options options representing the message sync chars, etc. - */ - Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : - data_(data), size_(size), options_(options) {} - - /** - * @brief Encode the u-blox message. - * @param message the message to encode - * @param class_id the u-blox class ID, defaults to the message CLASS_ID - * @param message_id the u-blox message ID, defaults to the message MESSAGE_ID - * @return true if the message was encoded correctly, false otherwise - */ - template bool write(const T& message, - uint8_t class_id = T::CLASS_ID, - uint8_t message_id = T::MESSAGE_ID) { - // Check for buffer overflow - uint32_t length = Serializer::serializedLength(message); - if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); - return false; - } - // Encode the message and add it to the buffer - Serializer::write(data_ + options_.header_length, - size_ - options_.header_length, message); - return write(0, length, class_id, message_id); - } - - /** - * @brief Wrap the encoded message payload with a header and checksum and - * add it to the buffer. - * @param message the encoded message payload (no header or checksum) - * @param length the length of the message payload - * @param class_id the u-blox class ID - * @param message_id the u-blox message ID - * @return true if the message was encoded correctly, false otherwise - */ - bool write(const uint8_t* message, uint32_t length, uint8_t class_id, - uint8_t message_id) { - if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); - return false; - } - iterator start = data_; - - // write header - *data_++ = options_.sync_a; - *data_++ = options_.sync_b; - *data_++ = class_id; - *data_++ = message_id; - *data_++ = length & 0xFF; - *data_++ = (length >> 8) & 0xFF; - size_ -= options_.header_length; - - // write message - if (message) std::copy(message, message + length, data_); - data_ += length; - size_ -= length; - - // write checksum - uint8_t ck_a, ck_b; - calculateChecksum(start + 2, length + 4, ck_a, ck_b); - *data_++ = ck_a; - *data_++ = ck_b; - size_ -= options_.checksum_length; - - return true; - } - - iterator end() { - return data_; - } - - private: - //! The buffer of message bytes - iterator data_; - //! The number of remaining bytes in the buffer - /*! Decrements as messages are written to the buffer */ - uint32_t size_; - //! Options representing the sync char values, etc. - Options options_; -}; - -} // namespace ublox - -// Use to declare u-blox messages and message serializers -#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ - template class ublox::Serializer; \ - template class ublox::Message; \ - namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ - } } \ - -// Use for messages which have the same structure but different IDs, e.g. INF -// Call DECLARE_UBLOX_MESSAGE for the first message and DECLARE_UBLOX_MESSAGE_ID -// for following declarations -#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ - namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ - } } \ - - -// use implementation of class Serializer in "serialization_ros.h" -#include "serialization_ros.h" - -#endif // UBLOX_SERIALIZATION_H diff --git a/ublox_serialization/include/ublox/checksum.h b/ublox_serialization/include/ublox_serialization/checksum.hpp similarity index 81% rename from ublox_serialization/include/ublox/checksum.h rename to ublox_serialization/include/ublox_serialization/checksum.hpp index 38041390..471882cb 100644 --- a/ublox_serialization/include/ublox/checksum.h +++ b/ublox_serialization/include/ublox_serialization/checksum.hpp @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -26,23 +26,23 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_MSGS_CHECKSUM_H -#define UBLOX_MSGS_CHECKSUM_H +#ifndef UBLOX_SERIALIZATION_CHECKSUM_HPP +#define UBLOX_SERIALIZATION_CHECKSUM_HPP -#include +#include namespace ublox { /** * @brief calculate the checksum of a u-blox_message - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param ck_a the checksum a output * @param ck_b the checksum b output */ -static inline void calculateChecksum(const uint8_t *data, - uint32_t size, - uint8_t &ck_a, +static inline void calculateChecksum(const uint8_t *data, + uint32_t size, + uint8_t &ck_a, uint8_t &ck_b) { ck_a = 0; ck_b = 0; for(uint32_t i = 0; i < size; ++i) @@ -54,19 +54,19 @@ static inline void calculateChecksum(const uint8_t *data, /** * @brief calculate the checksum of a u-blox_message. - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param checksum the checksum output * @return the checksum */ -static inline uint16_t calculateChecksum(const uint8_t *data, - uint32_t size, +static inline uint16_t calculateChecksum(const uint8_t *data, + uint32_t size, uint16_t &checksum) { uint8_t *byte = reinterpret_cast(&checksum); calculateChecksum(data, size, byte[0], byte[1]); return checksum; } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_MSGS_CHECKSUM_H +#endif // UBLOX_SERIALIZATION_CHECKSUM_HPP diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp new file mode 100644 index 00000000..f86baddd --- /dev/null +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -0,0 +1,673 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 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 UBLOX_SERIALIZATION_SERIALIZATION_HPP +#define UBLOX_SERIALIZATION_SERIALIZATION_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "checksum.hpp" + +/// +/// This file defines the UbloxSerializer template class which encodes and decodes +/// specific message types. +/// The Reader class decodes messages and from a buffer and the Writer class +/// encodes messages and writes them to a buffer. +/// It also declares macros for declaring Messages. The Message class +/// maps ROS messages types to class and message ID(s). +/// + + +/** + * @namespace ublox + * This namespace is for u-blox message serialization. + */ +namespace ublox { + +//! u-blox message Sync A char +static const uint8_t DEFAULT_SYNC_A = 0xB5; +//! u-blox message Sync B char +static const uint8_t DEFAULT_SYNC_B = 0x62; +//! Number of bytes in a message header (Sync chars + class ID + message ID) +static const uint8_t kHeaderLength = 6; +//! Number of checksum bytes in the u-blox message +static const uint8_t kChecksumLength = 2; + +/** + * \brief Templated serialization class. Default implementation provides backwards compatibility with + * old message types. + * + * Specializing the UbloxSerializer class is the only thing you need to do to get the ROS serialization system + * to work with a type. + */ +template +struct UbloxSerializer +{ + /** + * \brief Write an object to the stream. Normally the stream passed in here will be a UbloxOStream + */ + template + inline static void write(Stream& stream, const T & t); + + /** + * \brief Read an object from the stream. Normally the stream passed in here will be a UbloxIStream + */ + template + inline static void read(Stream& stream, T & t); + + /** + * \brief Determine the serialized length of an object. + */ + inline static uint32_t serializedLength(const T & t); +}; + +template +struct UbloxSerializer::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 || + std::is_same::value || + std::is_same::value>::type> +{ + template + inline static void write(Stream& stream, const T v) { + *reinterpret_cast(stream.advance(sizeof(v))) = v; + } + + template + inline static void read(Stream& stream, T& v) { + v = *reinterpret_cast(stream.advance(sizeof(v))); + } + + inline static uint32_t serializedLength(const T& v) { + (void)v; + return sizeof(T); + } +}; + +/** + * \brief Serialize an object. Stream here should normally be a UbloxOStream + */ +template +inline void serialize(Stream& stream, const T& t) { + UbloxSerializer::write(stream, t); +} + +/** + * \brief Deserialize an object. Stream here should normally be a UbloxIStream + */ +template +inline void deserialize(Stream& stream, T& t) { + UbloxSerializer::read(stream, t); +} + +/** + * \brief Determine the serialized length of an object + */ +template +inline uint32_t serializationLength(const T& t) { + return UbloxSerializer::serializedLength(t); +} + +/** + * \brief Array serializer, default implementation does nothing + */ +template +struct StdArrayUbloxSerializer +{}; + +/** + * \brief Array serializer, specialized for fixed-size, simple types + */ +template +struct StdArrayUbloxSerializer::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 || + std::is_same::value || + std::is_same::value>::type> +{ + template + inline static void write(Stream& stream, const std::array& v) { + const uint32_t data_len = N * sizeof(T); + std::memcpy(stream.advance(data_len), &v.front(), data_len); + } + + template + inline static void read(Stream& stream, std::array& v) { + const uint32_t data_len = N * sizeof(T); + std::memcpy(&v.front(), stream.advance(data_len), data_len); + } + + inline static uint32_t serializedLength(const std::array& v) { + (void)v; + return N * sizeof(T); + } +}; + +/** + * \brief serialize version for std::array + */ +template +inline void serialize(Stream& stream, const std::array& t) +{ + StdArrayUbloxSerializer::write(stream, t); +} + +/** + * \brief deserialize version for std::array + */ +template +inline void deserialize(Stream& stream, std::array& t) { + StdArrayUbloxSerializer::read(stream, t); +} + +/** + * \brief serializationLength version for std::array + */ +template +inline uint32_t serializationLength(const std::array& t) +{ + return StdArrayUbloxSerializer::serializedLength(t); +} + +/** + * \brief UbloxStream base-class, provides common functionality for UbloxIStream and UbloxOStream + */ +struct UbloxStream +{ + /* + * \brief Returns a pointer to the current position of the stream + */ + inline uint8_t* getData() { + return data_; + } + /** + * \brief Advances the stream, checking bounds, and returns a pointer to the position before it + * was advanced. + * \throws StreamOverrunException if len would take this stream past the end of its buffer + */ + uint8_t* advance(uint32_t len) { + uint8_t* old_data = data_; + data_ += len; + if (data_ > end_) { + // Throwing directly here causes a significant speed hit due to the extra code generated + // for the throw statement + //throwStreamOverrun(); + } + return old_data; + } + + /** + * \brief Returns the amount of space left in the stream + */ + inline uint32_t getLength() { return static_cast(end_ - data_); } + +protected: + UbloxStream(uint8_t* _data, uint32_t _count) + : data_(_data) + , end_(_data + _count) + {} + +private: + uint8_t* data_; + uint8_t* end_; +}; + +/** + * \brief Input stream + */ +struct UbloxIStream : public UbloxStream +{ + UbloxIStream(uint8_t* data, uint32_t count) + : UbloxStream(data, count) + {} + + /** + * \brief Deserialize an item from this input stream + */ + template + void next(T& t) { + deserialize(*this, t); + } +}; + +/** + * \brief Output stream + */ +struct UbloxOStream : public UbloxStream +{ + UbloxOStream(uint8_t* data, uint32_t count) + : UbloxStream(data, count) + {} + + /** + * \brief Serialize an item to this output stream + */ + template + void next(const T& t) { + serialize(*this, t); + } +}; + +/** + * @brief Keeps track of which class and message IDs can be decoded by a given + * message type. + */ +template +class Message { + public: + /** + * @brief Can this message type decode a u-blox message with the given ID? + * @param class_id the class ID of the u-blox message + * @param message_id the message ID of the u-blox message + * @return whether or not this message type decode the u-blox message + */ + static bool canDecode(uint8_t class_id, uint8_t message_id) { + return std::find(keys_.begin(), keys_.end(), + std::make_pair(class_id, message_id)) != keys_.end(); + } + + /** + * @brief Indicate that this message type can decode u-blox messages with the + * given ID + * @param class_id the class ID of the u-blox message + * @param message_id the message ID of the u-blox message + */ + static void addKey(uint8_t class_id, uint8_t message_id) { + keys_.emplace_back(std::make_pair(class_id, message_id)); + } + + struct StaticKeyInitializer + { + StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { + Message::addKey(class_id, message_id); + } + }; + + private: + static std::vector > keys_; +}; + +/** + * @brief Options for the Reader and Writer for encoding and decoding messages. + */ +struct Options { + /** + * The default options for a u-blox message. + */ + Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), + header_length(kHeaderLength), checksum_length(kChecksumLength) {} + //! The sync_a byte value identifying the start of a message + uint8_t sync_a; + //! The sync_b byte value identifying the start of a message + uint8_t sync_b; + //! The length of the message header in bytes (everything before the payload) + uint8_t header_length; + //! The length of the checksum in bytes + uint8_t checksum_length; + + /** + * @brief Get the number of bytes in the header and footer. + * @return the number of bytes in the header and footer + */ + uint32_t wrapper_length() { + return header_length + checksum_length; + } +}; + +/** + * @brief Decodes byte messages into u-blox ROS messages. + */ +class Reader { + public: + /** + * @param data a buffer containing u-blox messages + * @param count the size of the buffer + * @param options A struct containing the parameters sync_a and sync_b + * which represent the sync bytes indicating the beginning of the message + */ + Reader(const uint8_t *data, uint32_t count, + const Options &options = Options()) : + data_(data), count_(count), found_(false), options_(options) + { + extra_data_.reserve(1024); + } + + using iterator = const uint8_t *; + + /** + * @brief Search the buffer for the beginning of the next u-blox message + * @return a pointer to the start of the next u-blox message + */ + iterator search() + { + if (found_) { + next(); + } + + // Search for a message header + for( ; count_ > 0; --count_, ++data_) { + if (data_[0] == options_.sync_a && + (count_ == 1 || data_[1] == options_.sync_b)) { + break; + } else { + extra_data_.push_back(data_[0]); + } + } + + return data_; + } + + /** + * @brief Has a u-blox message been found in the buffer? + * @returns true if A message with the correct header & length has been found + */ + bool found() + { + if (found_) { + return true; + } + // Verify message is long enough to have sync chars, id, length & checksum + if (count_ < options_.wrapper_length()) { + return false; + } + // Verify the header bits + if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) { + return false; + } + // Verify that the buffer length is long enough based on the received + // message length + if (count_ < length() + options_.wrapper_length()) { + return false; + } + + found_ = true; + return true; + } + + /** + * @brief Go to the start of the next message based on the received message + * length. + * + * @details Warning: Does not go to the correct byte location if the received + * message length is incorrect. If this is the case, search must be called. + */ + iterator next() { + if (found()) { + uint32_t size = length() + options_.wrapper_length(); + data_ += size; count_ -= size; + } + found_ = false; + return data_; + } + + /** + * @brief Get the current position in the read buffer. + * @return the current position of the read buffer + */ + iterator pos() { + return data_; + } + + iterator end() { + return data_ + count_; + } + + uint8_t classId() { return data_[2]; } + uint8_t messageId() { return data_[3]; } + + /** + * @brief Get the length of the u-blox message payload. + * + * @details Payload length does not include the header or checksum length. + * Determines the length from the header of the u-blox message. + * @return the length of the message payload + */ + uint32_t length() { return (data_[5] << 8) + data_[4]; } + const uint8_t *data() { return data_ + options_.header_length; } + + /** + * @brief Get the checksum of the u-blox message. + * + * @return the checksum of the u-blox message + */ + uint16_t checksum() { + return *reinterpret_cast(data_ + options_.header_length + + length()); + } + + /** + * @brief Decode the given message. + * @param message the output message + * @param search whether or not to skip to the next message in the buffer + */ + template + bool read(T &message, + bool search = false) { + if (search) { + this->search(); + } + if (!found()) { + return false; + } + if (!Message::canDecode(classId(), messageId())) { + return false; + } + + uint16_t chk{0}; + if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { + // checksum error + // Note that it is possible (and even likely) that we get here without + // having the entire packet available. This happens when there are both + // NMEA and UBlox messages configured on the serial wire, and the packet is + // laid out like: + // + // + // + // Therefore, we do not print errors in this case and instead just don't + // do any additional work. + return false; + } + + UbloxSerializer::read(data_ + options_.header_length, length(), message); + return true; + } + + /** + * @brief Can the given message type decode the current message in the buffer? + * @return whether the given message type can decode the current message in + * the buffer + */ + template + bool hasType() { + if (!found()) { + return false; + } + return Message::canDecode(classId(), messageId()); + } + + /** + * @brief Does the u-blox message have the given class and message ID? + * @return Whether or not the u-blox message has the given class and message + * ID + */ + bool isMessage(uint8_t class_id, uint8_t message_id) { + if (!found()) { + return false; + } + return (classId() == class_id && messageId() == message_id); + } + + const std::string &getExtraData() const { + return extra_data_; + } + +private: + //! The buffer of message bytes + const uint8_t *data_; + //! Unused data from the read buffer, contains nmea messages. + std::string extra_data_; + //! the number of bytes in the buffer, //! decrement as the buffer is read + uint32_t count_; + //! Whether or not a message has been found + bool found_; + //! Options representing the sync char values, etc. + Options options_; +}; + +/** + * @brief Encodes a u-blox ROS message as a byte array. + */ +class Writer { + public: + using iterator = uint8_t *; + + /** + * @brief Construct a Writer with the given buffer. + * @param data a buffer for messages + * @param size the size of the buffer + * @param options options representing the message sync chars, etc. + */ + Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : + data_(data), size_(size), options_(options) {} + + /** + * @brief Encode the u-blox message. + * @param message the message to encode + * @param class_id the u-blox class ID, defaults to the message CLASS_ID + * @param message_id the u-blox message ID, defaults to the message MESSAGE_ID + * @return true if the message was encoded correctly, false otherwise + */ + template bool write(const T& message, + uint8_t class_id = T::CLASS_ID, + uint8_t message_id = T::MESSAGE_ID) { + // Check for buffer overflow + uint32_t length = UbloxSerializer::serializedLength(message); + if (size_ < length + options_.wrapper_length()) { + // ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", + // class_id, message_id); + return false; + } + // Encode the message and add it to the buffer + UbloxSerializer::write(data_ + options_.header_length, + size_ - options_.header_length, message); + return write(nullptr, length, class_id, message_id); + } + + /** + * @brief Wrap the encoded message payload with a header and checksum and + * add it to the buffer. + * @param message the encoded message payload (no header or checksum) + * @param length the length of the message payload + * @param class_id the u-blox class ID + * @param message_id the u-blox message ID + * @return true if the message was encoded correctly, false otherwise + */ + bool write(const uint8_t* message, uint32_t length, uint8_t class_id, + uint8_t message_id) { + if (size_ < length + options_.wrapper_length()) { + // ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", + // class_id, message_id); + return false; + } + iterator start = data_; + + // write header + *data_++ = options_.sync_a; + *data_++ = options_.sync_b; + *data_++ = class_id; + *data_++ = message_id; + *data_++ = length & 0xFF; + *data_++ = (length >> 8) & 0xFF; + size_ -= options_.header_length; + + // write message + if (message) { + std::copy(message, message + length, data_); + } + data_ += length; + size_ -= length; + + // write checksum + uint8_t ck_a, ck_b; + calculateChecksum(start + 2, length + 4, ck_a, ck_b); + *data_++ = ck_a; + *data_++ = ck_b; + size_ -= options_.checksum_length; + + return true; + } + + iterator end() { + return data_; + } + + private: + //! The buffer of message bytes + iterator data_; + //! The number of remaining bytes in the buffer + /*! Decrements as messages are written to the buffer */ + uint32_t size_; + //! Options representing the sync char values, etc. + Options options_; +}; + +} // namespace ublox + +// Use to declare u-blox messages and message serializers +#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ + template class ublox::UbloxSerializer; \ + template class ublox::Message; \ + namespace package { namespace { \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ + } } \ + +// Use for messages which have the same structure but different IDs, e.g. INF +// Call DECLARE_UBLOX_MESSAGE for the first message and DECLARE_UBLOX_MESSAGE_ID +// for following declarations +#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ + namespace package { namespace { \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ + } } \ + +#endif // UBLOX_SERIALIZATION_SERIALIZATION_HPP diff --git a/ublox_serialization/package.xml b/ublox_serialization/package.xml index 867ccd14..6492dcf4 100644 --- a/ublox_serialization/package.xml +++ b/ublox_serialization/package.xml @@ -1,7 +1,7 @@ - + ublox_serialization - 1.2.0 + 2.3.0 ublox_serialization provides header files for serialization of ROS messages to and from u-blox message format. @@ -12,16 +12,10 @@ BSD http://ros.org/wiki/ublox - catkin - - roscpp - roscpp - - roscpp_serialization - roscpp_serialization + ament_cmake - + ament_cmake