├── .gitignore ├── ROSaicLogo.png ├── msg ├── RFBand.msg ├── RFStatus.msg ├── BaseVectorCart.msg ├── BaseVectorGeod.msg ├── VelSensorSetup.msg ├── MeasEpoch.msg ├── GALAuthStatus.msg ├── IMUSetup.msg ├── ReceiverTime.msg ├── MeasEpochChannelType2.msg ├── MeasEpochChannelType1.msg ├── AttCovEuler.msg ├── BlockHeader.msg ├── VectorInfoCart.msg ├── VectorInfoGeod.msg ├── AttEuler.msg ├── PosCovCartesian.msg ├── PosCovGeodetic.msg ├── VelCovCartesian.msg ├── VelCovGeodetic.msg ├── AIMPlusStatus.msg ├── ExtSensorMeas.msg ├── PVTGeodetic.msg ├── PVTCartesian.msg ├── INSNavCart.msg └── INSNavGeod.msg ├── rosdoc.yaml ├── test ├── CMakeLists.txt ├── test_string_utilities.cpp └── test_parsing_utilities.cpp ├── msg_ros2 └── msg │ └── BlockHeader.msg ├── include └── septentrio_gnss_driver │ ├── plugin │ └── BlockHeader.h │ ├── parsers │ ├── sbf_utilities.hpp │ ├── nmea_sentence.hpp │ ├── parse_exception.hpp │ ├── nmea_parsers │ │ ├── gpgsv.hpp │ │ ├── gpgsa.hpp │ │ ├── gpgga.hpp │ │ └── gprmc.hpp │ ├── parser_base_class.hpp │ └── string_utilities.hpp │ ├── crc │ └── crc.hpp │ ├── node │ ├── rosaic_node.hpp │ └── rosaic_node_ros1.hpp │ └── communication │ ├── telegram_handler.hpp │ ├── communication_core.hpp │ ├── telegram.hpp │ └── settings_helpers.hpp ├── launch ├── rover.launch ├── rover_node.launch.py └── rover.launch.py ├── LICENSE ├── src └── septentrio_gnss_driver │ ├── node │ └── main.cpp │ ├── crc │ └── crc.cpp │ ├── parsers │ ├── nmea_parsers │ │ ├── gpgsa.cpp │ │ ├── gpgga.cpp │ │ ├── gprmc.cpp │ │ └── gpgsv.cpp │ ├── string_utilities.cpp │ └── parsing_utilities.cpp │ └── communication │ └── telegram_handler.cpp ├── package.xml ├── config ├── gnss.yaml ├── ins.yaml ├── rover.yaml └── rover_node.yaml ├── .clang-format └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | #config/rover.yaml 2 | .vscode 3 | build 4 | -------------------------------------------------------------------------------- /ROSaicLogo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/septentrio-gnss/septentrio_gnss_driver/HEAD/ROSaicLogo.png -------------------------------------------------------------------------------- /msg/RFBand.msg: -------------------------------------------------------------------------------- 1 | # RFband block 2 | 3 | uint32 frequency # Hz 4 | uint16 bandwidth # kHz 5 | uint8 info -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | file_patterns: '*.c *.cpp *.h *.hpp *.msg *.md' 3 | use_mdfile_as_mainpage: README.md 4 | -------------------------------------------------------------------------------- /msg/RFStatus.msg: -------------------------------------------------------------------------------- 1 | # RFStatus block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 n 9 | uint8 sb_length 10 | uint8 flags 11 | RFBand[] rfband 12 | 13 | -------------------------------------------------------------------------------- /msg/BaseVectorCart.msg: -------------------------------------------------------------------------------- 1 | # BaseVectorCart block 2 | # Block_Number 4043 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 n 10 | uint8 sb_length 11 | 12 | VectorInfoCart[] vector_info_cart -------------------------------------------------------------------------------- /msg/BaseVectorGeod.msg: -------------------------------------------------------------------------------- 1 | # BaseVectorGeod block 2 | # Block_Number 4028 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 n 10 | uint8 sb_length 11 | 12 | VectorInfoGeod[] vector_info_geod -------------------------------------------------------------------------------- /msg/VelSensorSetup.msg: -------------------------------------------------------------------------------- 1 | # VelSensorSetup block 2 | # Block_Number 4244 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 port 10 | float32 lever_arm_x # m 11 | float32 lever_arm_y # m 12 | float32 lever_arm_z # m -------------------------------------------------------------------------------- /msg/MeasEpoch.msg: -------------------------------------------------------------------------------- 1 | # MeasEpoch block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 n 9 | uint8 sb1_length 10 | uint8 sb2_length 11 | uint8 common_flags 12 | uint8 cum_clk_jumps # 0.001 s 13 | 14 | MeasEpochChannelType1[] type1 -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_string_utilities 2 | test_string_utilities.cpp 3 | ) 4 | 5 | target_link_libraries(test_string_utilities 6 | ${library_name} 7 | ) 8 | 9 | ament_add_gtest(test_parsing_utilities 10 | test_parsing_utilities.cpp 11 | ) 12 | 13 | target_link_libraries(test_parsing_utilities 14 | ${library_name} 15 | ) 16 | -------------------------------------------------------------------------------- /msg/GALAuthStatus.msg: -------------------------------------------------------------------------------- 1 | # GALAuthStatus block 2 | # Block_Number 4245 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint16 osnma_status 10 | float32 trusted_time_delta # s 11 | uint64 gal_active_mask 12 | uint64 gal_authentic_mask 13 | uint64 gps_active_mask 14 | uint64 gps_authentic_mask -------------------------------------------------------------------------------- /msg/IMUSetup.msg: -------------------------------------------------------------------------------- 1 | # IMUSetup block 2 | # Block_Number 4224 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 serial_port 10 | float32 ant_lever_arm_x # m 11 | float32 ant_lever_arm_y # m 12 | float32 ant_lever_arm_z # m 13 | float32 theta_x # deg 14 | float32 theta_y # deg 15 | float32 theta_z # deg -------------------------------------------------------------------------------- /msg/ReceiverTime.msg: -------------------------------------------------------------------------------- 1 | # ReceiverTime block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | int8 utc_year # year 9 | int8 utc_month # month 10 | int8 utc_day # day 11 | int8 utc_hour # hour 12 | int8 utc_min # minute 13 | int8 utc_second # s 14 | int8 delta_ls # s 15 | uint8 sync_level -------------------------------------------------------------------------------- /msg/MeasEpochChannelType2.msg: -------------------------------------------------------------------------------- 1 | # MeasEpochChannelType2 block 2 | 3 | uint8 type 4 | uint8 lock_time # s 5 | uint8 cn0 # 0.25 dB-Hz 6 | uint8 offsets_msb # 65.536 m or 65.536 Hz 7 | int8 carrier_msb # 65.536 cycles 8 | uint8 obs_info 9 | uint16 code_offset_lsb # 0.001 m 10 | uint16 carrier_lsb # 0.001 cycles 11 | uint16 doppler_offset_lsb # 0.0001 Hz -------------------------------------------------------------------------------- /msg/MeasEpochChannelType1.msg: -------------------------------------------------------------------------------- 1 | # MeasEpochChannelType1 block 2 | 3 | uint8 rx_channel 4 | uint8 type 5 | uint8 sv_id 6 | uint8 misc # 4294967.296 m 7 | uint32 code_lsb # 0.001 m 8 | int32 doppler # 0.0001 Hz 9 | uint16 carrier_lsb # 0.001 cycles 10 | int8 carrier_msb # 65.536 cycles 11 | uint8 cn0 # 0.25 dB-Hz 12 | uint16 lock_time 13 | uint8 obs_info 14 | uint8 n2 15 | 16 | MeasEpochChannelType2[] type2 -------------------------------------------------------------------------------- /msg/AttCovEuler.msg: -------------------------------------------------------------------------------- 1 | # AttCovEuler block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 error 9 | float32 cov_headhead # deg^2 10 | float32 cov_pitchpitch # deg^2 11 | float32 cov_rollroll # deg^2 12 | float32 cov_headpitch # deg^2 13 | float32 cov_headroll # deg^2 14 | float32 cov_pitchroll # deg^2 15 | -------------------------------------------------------------------------------- /msg/BlockHeader.msg: -------------------------------------------------------------------------------- 1 | # Blockheader including time header for all ROS messages that publish SBF blocks 2 | # This is the ROS 1 version of the message file. The ROS 2 version is 3 | # msg_ros2/msg/BlockHeader.msg . 4 | 5 | uint8 sync_1 6 | uint8 sync_2 7 | uint16 crc 8 | uint16 id 9 | uint8 revision 10 | uint16 length 11 | 12 | uint32 tow # ms since week start 13 | uint16 wnc # weeks since Jan 06, 1980 at 00:00:00 UTC 14 | -------------------------------------------------------------------------------- /msg/VectorInfoCart.msg: -------------------------------------------------------------------------------- 1 | # VectorInfoCart block 2 | 3 | uint8 nr_sv 4 | uint8 error 5 | uint8 mode 6 | uint8 misc 7 | float64 delta_x # m 8 | float64 delta_y # m 9 | float64 delta_z # m 10 | float32 delta_vx # m 11 | float32 delta_vy # m 12 | float32 delta_vz # m 13 | uint16 azimuth # 0.01 deg 14 | int16 elevation # 0.01 deg 15 | uint16 reference_id 16 | uint16 corr_age # 0.01 s 17 | uint32 signal_info -------------------------------------------------------------------------------- /msg/VectorInfoGeod.msg: -------------------------------------------------------------------------------- 1 | # VectorInfoGeod block 2 | 3 | uint8 nr_sv 4 | uint8 error 5 | uint8 mode 6 | uint8 misc 7 | float64 delta_east # m 8 | float64 delta_north # m 9 | float64 delta_up # m 10 | float32 delta_ve # m 11 | float32 delta_vn # m 12 | float32 delta_vu # m 13 | uint16 azimuth # 0.01 deg 14 | int16 elevation # 0.01 deg 15 | uint16 reference_id 16 | uint16 corr_age # 0.01 s 17 | uint32 signal_info -------------------------------------------------------------------------------- /msg/AttEuler.msg: -------------------------------------------------------------------------------- 1 | # AttEuler block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 nr_sv 9 | uint8 error 10 | uint16 mode 11 | float32 heading # deg 12 | float32 pitch # deg 13 | float32 roll # deg 14 | float32 pitch_dot # deg/s 15 | float32 roll_dot # deg/s 16 | float32 heading_dot # deg/s 17 | -------------------------------------------------------------------------------- /msg_ros2/msg/BlockHeader.msg: -------------------------------------------------------------------------------- 1 | # Blockheader including time header for all ROS messages that publish SBF blocks 2 | # This is the ROS 2 version of the message. The ROS 1 version is at 3 | # msg/BlockHeader.msg . 4 | 5 | uint8 sync_1 36 # 0x24 6 | uint8 sync_2 64 # 0x40 7 | uint16 crc 8 | uint16 id 9 | uint8 revision 10 | uint16 length 11 | 12 | uint32 tow 4294967295 # ms since week start 13 | uint16 wnc 65535 # weeks since Jan 06, 1980 at 00:00:00 UTC 14 | -------------------------------------------------------------------------------- /msg/PosCovCartesian.msg: -------------------------------------------------------------------------------- 1 | # PVTGeodetic block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 mode 9 | uint8 error 10 | float32 cov_xx # m^2 11 | float32 cov_yy # m^2 12 | float32 cov_zz # m^2 13 | float32 cov_bb # m^2 14 | float32 cov_xy # m^2 15 | float32 cov_xz # m^2 16 | float32 cov_xb # m^2 17 | float32 cov_yz # m^2 18 | float32 cov_yb # m^2 19 | float32 cov_zb # m^2 -------------------------------------------------------------------------------- /msg/PosCovGeodetic.msg: -------------------------------------------------------------------------------- 1 | # PVTGeodetic block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 mode 9 | uint8 error 10 | float32 cov_latlat # m^2 11 | float32 cov_lonlon # m^2 12 | float32 cov_hgthgt # m^2 13 | float32 cov_bb # m^2 14 | float32 cov_latlon # m^2 15 | float32 cov_lathgt # m^2 16 | float32 cov_latb # m^2 17 | float32 cov_lonhgt # m^2 18 | float32 cov_lonb # m^2 19 | float32 cov_hb # m^2 -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/plugin/BlockHeader.h: -------------------------------------------------------------------------------- 1 | // Custom constructor macro to initalize SBF msgs with do_not_use values in block 2 | // header 3 | #define SEPTENTRIO_GNSS_DRIVER_MESSAGE_BLOCKHEADER_PLUGIN_CONSTRUCTOR \ 4 | BlockHeader_() : \ 5 | sync_1(0x24), sync_2(0x40), crc(0), id(0), revision(0), length(0), \ 6 | tow(4294967295UL), wnc(65535) \ 7 | { \ 8 | } 9 | -------------------------------------------------------------------------------- /msg/VelCovCartesian.msg: -------------------------------------------------------------------------------- 1 | # VelCovCartesian block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 mode 9 | uint8 error 10 | float32 cov_vxvx # m^2/s^2 11 | float32 cov_vyvy # m^2/s^2 12 | float32 cov_vzvz # m^2/s^2 13 | float32 cov_dtdt # m^2/s^2 14 | float32 cov_vxvy # m^2/s^2 15 | float32 cov_vxvz # m^2/s^2 16 | float32 cov_vxdt # m^2/s^2 17 | float32 cov_vyvz # m^2/s^2 18 | float32 cov_vydt # m^2/s^2 19 | float32 cov_vzdt # m^2/s^2 -------------------------------------------------------------------------------- /msg/VelCovGeodetic.msg: -------------------------------------------------------------------------------- 1 | # VelCovGeodetic block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 mode 9 | uint8 error 10 | float32 cov_vnvn # m^2/s^2 11 | float32 cov_veve # m^2/s^2 12 | float32 cov_vuvu # m^2/s^2 13 | float32 cov_dtdt # m^2/s^2 14 | float32 cov_vnve # m^2/s^2 15 | float32 cov_vnvu # m^2/s^2 16 | float32 cov_vndt # m^2/s^2 17 | float32 cov_vevu # m^2/s^2 18 | float32 cov_vedt # m^2/s^2 19 | float32 cov_vudt # m^2/s^2 -------------------------------------------------------------------------------- /msg/AIMPlusStatus.msg: -------------------------------------------------------------------------------- 1 | # AIMPlusStatus message 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | uint32 tow # ms since week start 6 | uint16 wnc # weeks since Jan 06, 1980 at 00:00:00 UTC 7 | 8 | uint8 interference 9 | #------------------------------- 10 | uint8 SPECTRUM_CLEAN = 0 11 | uint8 INTERFERENCE_MITIGATED = 1 12 | uint8 INTERFERENCE_PRESENT = 2 13 | #------------------------------- 14 | 15 | uint8 spoofing 16 | #-------------------------------------------------------- 17 | uint8 NONE_DETECTED = 0 18 | uint8 SPOOFING_DETECTED_BY_OSNMA = 1 19 | uint8 SPOOFING_DETECTED_BY_AUTHENTCITY_TEST = 2 20 | uint8 SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST = 3 21 | #-------------------------------------------------------- 22 | 23 | bool osnma_authenticating 24 | uint8 galileo_authentic 25 | uint8 galileo_spoofed 26 | uint8 gps_authentic 27 | uint8 gps_spoofed -------------------------------------------------------------------------------- /msg/ExtSensorMeas.msg: -------------------------------------------------------------------------------- 1 | # ExtSensorMeas block 2 | # Block_Number 4050 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 n 10 | uint8 sb_length 11 | 12 | # ExtSensorMeasSet 13 | uint8[] source 14 | uint8[] sensor_model 15 | uint8[] type 16 | uint8[] obs_info 17 | 18 | #ExtSensorMeasAcceleration 19 | float64 acceleration_x # m/s^2 20 | float64 acceleration_y # m/s^2 21 | float64 acceleration_z # m/s^2 22 | 23 | #ExtSensorMeasAngularRate 24 | float64 angular_rate_x # deg/s 25 | float64 angular_rate_y # deg/s 26 | float64 angular_rate_z # deg/s 27 | 28 | #ExtSensorMeasVelocity 29 | float32 velocity_x # m/s 30 | float32 velocity_y # m/s 31 | float32 velocity_z # m/s 32 | float32 std_dev_x # m/s 33 | float32 std_dev_y # m/s 34 | float32 std_dev_z # m/s 35 | 36 | #ExtSensorMeasInfo 37 | float32 sensor_temperature # deg C 38 | 39 | #ExtSensorMeasZeroVelocityFlag 40 | float64 zero_velocity_flag -------------------------------------------------------------------------------- /msg/PVTGeodetic.msg: -------------------------------------------------------------------------------- 1 | # PVTGeodetic block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 mode 9 | uint8 error 10 | float64 latitude # rad 11 | float64 longitude # rad 12 | float64 height # m (ellipsoidal) 13 | float32 undulation # m 14 | float32 vn # m/s 15 | float32 ve # m/s 16 | float32 vu # m/s 17 | float32 cog # deg 18 | float64 rx_clk_bias # ms 19 | float32 rx_clk_drift # ppm 20 | uint8 time_system 21 | uint8 datum 22 | uint8 nr_sv 23 | uint8 wa_corr_info 24 | uint16 reference_id 25 | uint16 mean_corr_age # 0.01s 26 | uint32 signal_info 27 | uint8 alert_flag 28 | uint8 nr_bases 29 | uint16 ppp_info # s 30 | uint16 latency # 0.0001 s 31 | uint16 h_accuracy # 0.01 m 32 | uint16 v_accuracy # 0.01 m 33 | uint8 misc -------------------------------------------------------------------------------- /msg/PVTCartesian.msg: -------------------------------------------------------------------------------- 1 | # PVTCartesian block 2 | # ROS message header 3 | std_msgs/Header header 4 | 5 | # SBF block header including time header 6 | BlockHeader block_header 7 | 8 | uint8 mode 9 | uint8 error 10 | float64 x # m 11 | float64 y # m 12 | float64 z # m 13 | float32 undulation # m 14 | float32 vx # m/s 15 | float32 vy # m/s 16 | float32 vz # m/s 17 | float32 cog # deg 18 | float64 rx_clk_bias # ms 19 | float32 rx_clk_drift # ppm 20 | uint8 time_system 21 | uint8 datum 22 | uint8 nr_sv 23 | uint8 wa_corr_info 24 | uint16 reference_id 25 | uint16 mean_corr_age # 0.01s 26 | uint32 signal_info 27 | uint8 alert_flag 28 | uint8 nr_bases 29 | uint16 ppp_info # s 30 | uint16 latency # 0.0001 s 31 | uint16 h_accuracy # 0.01 m 32 | uint16 v_accuracy # 0.01 m 33 | uint8 misc 34 | -------------------------------------------------------------------------------- /launch/rover.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 15 | 16 | 18 | 19 | 21 | 22 | 26 | 28 | 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Septentrio NV/SA. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /msg/INSNavCart.msg: -------------------------------------------------------------------------------- 1 | # INSNavCart block 2 | # Block_Number 4225 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 gnss_mode 10 | uint8 error 11 | uint16 info 12 | uint16 gnss_age # 0.01 s 13 | float64 x # m 14 | float64 y # m 15 | float64 z # m 16 | uint16 accuracy # 0.01 m 17 | uint16 latency # 0.1 ms 18 | uint8 datum 19 | #uint8 reserved 20 | uint16 sb_list 21 | 22 | # INSNavCartPosStdDev sub-block definition: 23 | # If the Position StdDev field is 1 then this sub block is available: 24 | float32 x_std_dev # m 25 | float32 y_std_dev # m 26 | float32 z_std_dev # m 27 | 28 | # INSNavCartPosCov sub-block definition: 29 | # If the Position Cov field is 1 then this sub block is available: 30 | float32 xy_cov # m^2 31 | float32 xz_cov # m^2 32 | float32 yz_cov # m^2 33 | 34 | # INSNavCartAtt sub-block definition: 35 | # If the Attitude field is 1 then this sub block is available: 36 | float32 heading # deg 37 | float32 pitch # deg 38 | float32 roll # deg 39 | 40 | # INSNavCartAttStdDev sub-block definition: 41 | # If the Attitude StdDev field is 1 then this sub block is available: 42 | float32 heading_std_dev # deg 43 | float32 pitch_std_dev # deg 44 | float32 roll_std_dev # deg 45 | 46 | # INSNavCartAttCov sub-block definition: 47 | # If the Attitude Cov field is 1 then this sub block is available: 48 | float32 heading_pitch_cov # deg^2 49 | float32 heading_roll_cov # deg^2 50 | float32 pitch_roll_cov # deg^2 51 | 52 | # INSNavCartVel sub-block definition: 53 | # If the Velocity field is 1 then this sub block is available: 54 | float32 vx # m/s 55 | float32 vy # m/S 56 | float32 vz # m/s 57 | 58 | # INSNavCartVelStdDev sub-block definition: 59 | # If the Velocity StdDev field is 1 then this sub block is available: 60 | float32 vx_std_dev # m/s 61 | float32 vy_std_dev # m/s 62 | float32 vz_std_dev # m/s 63 | 64 | # INSNavCartVelCov sub-block definition: 65 | # If the Velocity Cov field is 1 then this sub block is available: 66 | float32 vx_vy_cov # m^2/s^2 67 | float32 vx_vz_cov # m^2/s^2 68 | float32 vy_vz_cov # m^2/s^2 -------------------------------------------------------------------------------- /msg/INSNavGeod.msg: -------------------------------------------------------------------------------- 1 | # INSNavGeod block 2 | # Block_Number 4226 3 | 4 | std_msgs/Header header 5 | 6 | # SBF block header including time header 7 | BlockHeader block_header 8 | 9 | uint8 gnss_mode 10 | uint8 error 11 | uint16 info 12 | uint16 gnss_age # 0.01 s 13 | float64 latitude # rad 14 | float64 longitude # rad 15 | float64 height # m (ellipsoidal) 16 | float32 undulation # m 17 | uint16 accuracy # 0.01 m 18 | uint16 latency # 0.0001 s 19 | uint8 datum 20 | #uint8 reserved 21 | uint16 sb_list 22 | 23 | # INSNavGeodPosStdDev sub-block definition: 24 | # If the Position StdDev field is 1 then this sub block is available: 25 | float32 latitude_std_dev # m 26 | float32 longitude_std_dev # m 27 | float32 height_std_dev # m 28 | 29 | # INSNavGeodPosCov sub-block definition: 30 | # If the Position Cov field is 1 then this sub block is available: 31 | float32 latitude_longitude_cov # m^2 32 | float32 latitude_height_cov # m^2 33 | float32 longitude_height_cov # m^2 34 | 35 | # INSNavGeodAtt sub-block definition: 36 | # If the Attitude field is 1 then this sub block is available: 37 | float32 heading # deg 38 | float32 pitch # deg 39 | float32 roll # deg 40 | 41 | # INSNavGeodAttStdDev sub-block definition: 42 | # If the Attitude StdDev field is 1 then this sub block is available: 43 | float32 heading_std_dev # deg 44 | float32 pitch_std_dev # deg 45 | float32 roll_std_dev # deg 46 | 47 | # INSNavGeodAttCov sub-block definition: 48 | # If the Attitude Cov field is 1 then this sub block is available: 49 | float32 heading_pitch_cov # deg^2 50 | float32 heading_roll_cov # deg^2 51 | float32 pitch_roll_cov # deg^2 52 | 53 | # INSNavGeodVel sub-block definition: 54 | # If the Velocity field is 1 then this sub block is available: 55 | float32 ve # m/s 56 | float32 vn # m/s 57 | float32 vu # m/s 58 | 59 | # INSNavGeodVelStdDev sub-block definition: 60 | # If the Velocity StdDev field is 1 then this sub block is available: 61 | float32 ve_std_dev # m/s 62 | float32 vn_std_dev # m/s 63 | float32 vu_std_dev # m/s 64 | 65 | # INSNavGeodVelCov sub-block definition: 66 | # If the Velocity Cov field is 1 then this sub block is available: 67 | float32 ve_vn_cov # m^2/s^2 68 | float32 ve_vu_cov # m^2/s^2 69 | float32 vn_vu_cov # m^2/s^2 70 | -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/node/main.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // **************************************************************************** 30 | 31 | #include 32 | 33 | /** 34 | * @file main.cpp 35 | * @date 01/12/21 36 | * @brief Main function of the ROSaic driver: 37 | */ 38 | 39 | int main(int argc, char** argv) 40 | { 41 | rclcpp::init(argc, argv); 42 | 43 | auto options = rclcpp::NodeOptions().use_intra_process_comms(false); 44 | auto rx_node = std::make_shared(options); 45 | 46 | rclcpp::spin(rx_node->get_node_base_interface()); 47 | 48 | rclcpp::shutdown(); 49 | return 0; 50 | } -------------------------------------------------------------------------------- /launch/rover_node.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | from launch_ros.actions import Node 4 | from launch_ros.actions import ComposableNodeContainer 5 | from launch_ros.descriptions import ComposableNode 6 | from launch.actions import DeclareLaunchArgument 7 | from launch.substitutions import LaunchConfiguration, Command, TextSubstitution 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}' 11 | # Verbose log: 12 | #os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message} ({function_name}() at {file_name}:{line_number})' 13 | 14 | # Start as component: 15 | 16 | def generate_launch_description(): 17 | 18 | tf_imu = Node( 19 | package="tf2_ros", 20 | executable="static_transform_publisher", 21 | arguments = "0 0 0 0 0 0 base_link imu".split(' ') 22 | ) 23 | 24 | tf_gnss = Node( 25 | package="tf2_ros", 26 | executable="static_transform_publisher", 27 | arguments = "0 0 0 0 0 0 imu gnss".split(' ') 28 | ) 29 | 30 | tf_vsm = Node( 31 | package="tf2_ros", 32 | executable="static_transform_publisher", 33 | arguments = "0 0 0 0 0 0 imu vsm".split(' ') 34 | ) 35 | 36 | tf_aux1 = Node( 37 | package="tf2_ros", 38 | executable="static_transform_publisher", 39 | arguments = "0 0 0 0 0 0 imu aux1".split(' ') 40 | ) 41 | 42 | default_file_name = 'rover_node.yaml' 43 | name_arg_file_name = "file_name" 44 | arg_file_name = DeclareLaunchArgument(name_arg_file_name, 45 | default_value=TextSubstitution(text=str(default_file_name))) 46 | name_arg_file_path = 'path_to_config' 47 | arg_file_path = DeclareLaunchArgument(name_arg_file_path, 48 | default_value=[get_package_share_directory('septentrio_gnss_driver'), '/config/', LaunchConfiguration(name_arg_file_name)]) 49 | 50 | node = Node( 51 | package='septentrio_gnss_driver', 52 | executable='septentrio_gnss_driver_node', 53 | name='septentrio_gnss_driver', 54 | emulate_tty=True, 55 | sigterm_timeout = '20', 56 | parameters=[LaunchConfiguration(name_arg_file_path)]) 57 | 58 | return launch.LaunchDescription([arg_file_name, arg_file_path, node, tf_imu, tf_gnss, tf_vsm, tf_aux1]) 59 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | septentrio_gnss_driver 4 | 1.4.5 5 | ROSaic: C++ driver for Septentrio's GNSS and INS receivers 6 | 7 | 8 | Tibor Dome 9 | Thomas Emter 10 | Septentrio 11 | 12 | 13 | 14 | BSD 3-Clause License 15 | 16 | 17 | 18 | https://github.com/septentrio-gnss/septentrio_gnss_driver 19 | https://www.ros.org/wiki/septentrio_gnss_driver 20 | 21 | 22 | Tibor Dome 23 | Thomas Emter 24 | 25 | 26 | ament_cmake 27 | rosidl_default_generators 28 | rosidl_default_runtime 29 | rosidl_interface_packages 30 | rclcpp 31 | rclcpp_components 32 | ament_cmake_ros 33 | gtest_vendor 34 | 35 | catkin 36 | message_generation 37 | roscpp 38 | message_runtime 39 | 40 | diagnostic_msgs 41 | nmea_msgs 42 | sensor_msgs 43 | geometry_msgs 44 | gps_msgs 45 | gps_common 46 | nav_msgs 47 | boost 48 | libpcap 49 | geographiclib 50 | tf2 51 | tf2_eigen 52 | tf2_geometry_msgs 53 | tf2_ros 54 | 55 | catkin 56 | ament_cmake 57 | 58 | 59 | -------------------------------------------------------------------------------- /config/gnss.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for the Rover Rx 2 | 3 | # GNSS/INS Parameters 4 | 5 | device: tcp://192.168.3.1:28784 6 | 7 | serial: 8 | baudrate: 921600 9 | hw_flow_control: "off" 10 | 11 | stream_device: 12 | tcp: 13 | ip_server: "" 14 | port: 0 15 | udp: 16 | ip_server: "" 17 | port: 0 18 | unicast_ip: "" 19 | 20 | configure_rx: true 21 | 22 | login: 23 | user: "" 24 | password: "" 25 | 26 | custom_commands_file: "" 27 | 28 | osnma: 29 | mode: "loose" 30 | ntp_server: "" 31 | keep_open: true 32 | 33 | frame_id: gnss 34 | 35 | aux1_frame_id: aux1 36 | 37 | get_spatial_config_from_tf: true 38 | 39 | use_ros_axis_orientation: true 40 | 41 | receiver_type: gnss 42 | 43 | multi_antenna: true 44 | 45 | datum: Default 46 | 47 | att_offset: 48 | heading: 0.0 49 | pitch: 0.0 50 | 51 | ant_type: "Unknown" 52 | ant_serial_nr: "Unknown" 53 | ant_aux1_type: "Unknown" 54 | ant_aux1_serial_nr: "Unknown" 55 | 56 | polling_period: 57 | pvt: 100 58 | rest: 500 59 | 60 | # time 61 | use_gnss_time: false 62 | ntp_server: true 63 | ptp_server_clock: false 64 | latency_compensation: true 65 | 66 | # RTK 67 | rtk_settings: 68 | ntrip_1: 69 | id: "" 70 | caster: "" 71 | caster_port: 2101 72 | username: "" 73 | password: "" 74 | mountpoint: "" 75 | version: "v2" 76 | tls: false 77 | fingerprint: "" 78 | rtk_standard: "auto" 79 | send_gga: "auto" 80 | keep_open: true 81 | ip_server_1: 82 | id: "" 83 | port: 0 84 | rtk_standard: "auto" 85 | send_gga: "auto" 86 | keep_open: true 87 | serial_1: 88 | port: "" 89 | baud_rate: 115200 90 | rtk_standard: "auto" 91 | send_gga: "auto" 92 | keep_open: true 93 | 94 | publish: 95 | # For both GNSS and INS Rxs 96 | auto_publish: false 97 | publish_only_valid: false 98 | navsatfix: true 99 | gpsfix: true 100 | gpgga: true 101 | gprmc: true 102 | gpst: true 103 | measepoch: true 104 | pvtcartesian: true 105 | pvtgeodetic: true 106 | basevectorcart: false 107 | basevectorgeod: false 108 | poscovcartesian: true 109 | poscovgeodetic: true 110 | velcovcartesian: false 111 | velcovgeodetic: true 112 | atteuler: true 113 | attcoveuler: true 114 | pose: true 115 | twist: false 116 | diagnostics: true 117 | aimplusstatus: true 118 | galauthstatus: false 119 | # For GNSS Rx only 120 | gpgsa: false 121 | gpgsv: false 122 | 123 | # logger 124 | 125 | activate_debug_log: false -------------------------------------------------------------------------------- /test/test_string_utilities.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | #include 33 | 34 | TEST(TrimTest, string_trimming) 35 | { 36 | { 37 | double val = 0.333333333; 38 | auto str = string_utilities::trimDecimalPlaces(val); 39 | 40 | EXPECT_EQ(str.size(), 5); 41 | } 42 | { 43 | 44 | double val = 0.0; 45 | auto str = string_utilities::trimDecimalPlaces(val); 46 | 47 | EXPECT_EQ(str.size(), 5); 48 | } 49 | { 50 | 51 | double val = 100.333333333; 52 | auto str = string_utilities::trimDecimalPlaces(val); 53 | 54 | EXPECT_EQ(str.size(), 7); 55 | } 56 | { 57 | double val = 100.0; 58 | auto str = string_utilities::trimDecimalPlaces(val); 59 | 60 | EXPECT_EQ(str.size(), 7); 61 | } 62 | } -------------------------------------------------------------------------------- /launch/rover.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | from launch_ros.actions import Node 4 | from launch_ros.actions import ComposableNodeContainer 5 | from launch_ros.descriptions import ComposableNode 6 | from launch.actions import DeclareLaunchArgument 7 | from launch.substitutions import LaunchConfiguration, Command, TextSubstitution 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}' 11 | # Verbose log: 12 | #os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message} ({function_name}() at {file_name}:{line_number})' 13 | 14 | # Start as component: 15 | 16 | def generate_launch_description(): 17 | 18 | tf_imu = Node( 19 | package="tf2_ros", 20 | executable="static_transform_publisher", 21 | arguments = "0 0 0 0 0 0 base_link imu".split(' ') 22 | ) 23 | 24 | tf_gnss = Node( 25 | package="tf2_ros", 26 | executable="static_transform_publisher", 27 | arguments = "0 0 0 0 0 0 imu gnss".split(' ') 28 | ) 29 | 30 | tf_vsm = Node( 31 | package="tf2_ros", 32 | executable="static_transform_publisher", 33 | arguments = "0 0 0 0 0 0 imu vsm".split(' ') 34 | ) 35 | 36 | tf_aux1 = Node( 37 | package="tf2_ros", 38 | executable="static_transform_publisher", 39 | arguments = "0 0 0 0 0 0 imu aux1".split(' ') 40 | ) 41 | 42 | default_file_name = 'rover.yaml' 43 | name_arg_file_name = "file_name" 44 | arg_file_name = DeclareLaunchArgument(name_arg_file_name, 45 | default_value=TextSubstitution(text=str(default_file_name))) 46 | name_arg_file_path = 'path_to_config' 47 | arg_file_path = DeclareLaunchArgument(name_arg_file_path, 48 | default_value=[get_package_share_directory('septentrio_gnss_driver'), '/config/', LaunchConfiguration(name_arg_file_name)]) 49 | 50 | composable_node = ComposableNode( 51 | name='septentrio_gnss_driver', 52 | package='septentrio_gnss_driver', 53 | plugin='rosaic_node::ROSaicNode', 54 | #emulate_tty=True, 55 | parameters=[LaunchConfiguration(name_arg_file_path)]) 56 | 57 | container = ComposableNodeContainer( 58 | name='septentrio_gnss_driver_container', 59 | namespace='septentrio_gnss_driver', 60 | package='rclcpp_components', 61 | executable='component_container_isolated', 62 | emulate_tty=True, 63 | sigterm_timeout = '20', 64 | composable_node_descriptions=[composable_node], 65 | output='screen' 66 | ) 67 | 68 | return launch.LaunchDescription([arg_file_name, arg_file_path, container, tf_imu, tf_gnss, tf_vsm, tf_aux1]) 69 | -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/sbf_utilities.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | 36 | template 37 | struct has_block_header : std::false_type 38 | { 39 | }; 40 | 41 | template 42 | struct has_block_header> : std::true_type 43 | { 44 | }; 45 | 46 | /** 47 | * validValue 48 | * @brief Check if value is not set to Do-Not-Use 49 | */ 50 | template 51 | [[nodiscard]] bool validValue(T s) 52 | { 53 | static_assert(std::is_same::value || 54 | std::is_same::value || 55 | std::is_same::value || std::is_same::value); 56 | if (std::is_same::value) 57 | { 58 | return (s != static_cast(65535)); 59 | } else if (std::is_same::value) 60 | { 61 | return (s != 4294967295u); 62 | } else if (std::is_same::value) 63 | { 64 | return (!std::isnan(s) && (s != -2e10f)); 65 | } else if (std::is_same::value) 66 | { 67 | return (!std::isnan(s) && (s != -2e10)); 68 | } 69 | } -------------------------------------------------------------------------------- /test/test_parsing_utilities.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | #include 33 | 34 | TEST(WrapTest, angle180) 35 | { 36 | { 37 | double val = 270.0; 38 | auto wrapped_val = parsing_utilities::wrapAngle180to180(val); 39 | 40 | EXPECT_EQ(wrapped_val, -90.0); 41 | } 42 | { 43 | double val = -270.0; 44 | auto wrapped_val = parsing_utilities::wrapAngle180to180(val); 45 | 46 | EXPECT_EQ(wrapped_val, 90.0); 47 | } 48 | { 49 | double val = -90.0; 50 | auto wrapped_val = parsing_utilities::wrapAngle180to180(val); 51 | 52 | EXPECT_EQ(wrapped_val, -90.0); 53 | } 54 | { 55 | double val = 90.0; 56 | auto wrapped_val = parsing_utilities::wrapAngle180to180(val); 57 | 58 | EXPECT_EQ(wrapped_val, 90.0); 59 | } 60 | { 61 | double val = 630.0; 62 | auto wrapped_val = parsing_utilities::wrapAngle180to180(val); 63 | 64 | EXPECT_EQ(wrapped_val, -90.0); 65 | } 66 | { 67 | double val = -630.0; 68 | auto wrapped_val = parsing_utilities::wrapAngle180to180(val); 69 | 70 | EXPECT_EQ(wrapped_val, 90.0); 71 | } 72 | } -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/nmea_sentence.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | // C++ library includes 34 | #include 35 | #include 36 | 37 | /** 38 | * @file nmea_sentence.hpp 39 | * @brief Defines a struct NMEASentence, into which NMEA sentences - both 40 | * standardized and proprietary ones - should be mapped 41 | * @date 13/08/20 42 | */ 43 | 44 | /** 45 | * @brief Struct to split an NMEA sentence into its ID and its body, the latter 46 | * tokenized into a vector of strings. 47 | * 48 | * By ID, we mean either a standardized ID, e.g. "$GPGGA", or proprietary ID such as 49 | * "$PSSN,HRP". The STL Container Vector can be used to dynamically allocate arrays 50 | * (C++ feature). Also note that the ID of !all! (not just those defined by 51 | * Septentrio) proprietary NMEA messages starts with "$P". The body_ member variable 52 | * shall exclude the NMEA checksum (also hinted at in files that implement the 53 | * parsing). 54 | */ 55 | class NMEASentence 56 | { 57 | public: 58 | NMEASentence(std::string id, std::vector body) : 59 | id_(id), body_(body) 60 | { 61 | } 62 | std::vector get_body() const { return body_; } 63 | 64 | protected: 65 | std::string id_; 66 | std::vector body_; 67 | }; -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/crc/crc.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | // ROSaic includes 34 | // The following imports structs into which SBF blocks can be unpacked then shipped 35 | // to handler functions 36 | #include 37 | // C++ libary includes 38 | #include 39 | #include 40 | #include 41 | 42 | namespace crc { 43 | /** 44 | * @file crc.hpp 45 | * @brief Declares the functions to compute and validate the CRC of a buffer 46 | * @date 17/08/20 47 | */ 48 | 49 | /** 50 | * @brief This function computes the CRC-8-CCITT (Cyclic Redundancy Check) of a 51 | * buffer "buf" of "buf_length" bytes 52 | * @param[in] buf The buffer at hand 53 | * @param[in] buf_length Number of bytes in "buf" 54 | * @return The calculated CRC 55 | */ 56 | uint16_t compute16CCITT(const uint8_t* buf, size_t buf_length); 57 | 58 | /** 59 | * @brief Validates whether the calculated CRC of the SBF block at hand matches 60 | * the CRC field of the streamed SBF block 61 | * @param block The SBF block that we are interested in 62 | * @return True if the CRC check of the SBFBlock has passed, false otherwise 63 | */ 64 | bool isValid(const std::vector& message); 65 | 66 | } // namespace crc 67 | -------------------------------------------------------------------------------- /config/ins.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for the Rover Rx 2 | 3 | # INS Parameters 4 | 5 | device: tcp://192.168.3.1:28784 6 | 7 | serial: 8 | baudrate: 921600 9 | hw_flow_control: "off" 10 | 11 | stream_device: 12 | tcp: 13 | ip_server: "" 14 | port: 0 15 | udp: 16 | ip_server: "" 17 | port: 0 18 | unicast_ip: "" 19 | 20 | configure_rx: true 21 | 22 | login: 23 | user: "" 24 | password: "" 25 | 26 | custom_commands_file: "" 27 | 28 | osnma: 29 | mode: "loose" 30 | ntp_server: "" 31 | keep_open: true 32 | 33 | frame_id: gnss 34 | 35 | imu_frame_id: imu 36 | 37 | poi_frame_id: base_link 38 | 39 | vsm_frame_id: vsm 40 | 41 | aux1_frame_id: aux1 42 | 43 | vehicle_frame_id: base_link 44 | 45 | local_frame_id: odom 46 | 47 | insert_local_frame: false 48 | 49 | get_spatial_config_from_tf: true 50 | 51 | lock_utm_zone: true 52 | 53 | use_ros_axis_orientation: true 54 | 55 | receiver_type: ins 56 | 57 | multi_antenna: true 58 | 59 | datum: Default 60 | 61 | att_offset: 62 | heading: 0.0 63 | pitch: 0.0 64 | 65 | ant_type: "Unknown" 66 | ant_serial_nr: "Unknown" 67 | ant_aux1_type: "Unknown" 68 | ant_aux1_serial_nr: "Unknown" 69 | 70 | polling_period: 71 | pvt: 0 72 | rest: 500 73 | 74 | # time 75 | use_gnss_time: false 76 | ntp_server: true 77 | latency_compensation: true 78 | 79 | # RTK 80 | rtk_settings: 81 | keep_open: true 82 | ntrip_1: 83 | id: "" 84 | caster: "" 85 | caster_port: 2101 86 | username: "" 87 | password: "" 88 | mountpoint: "" 89 | version: "v2" 90 | tls: false 91 | fingerprint: "" 92 | rtk_standard: "auto" 93 | send_gga: "auto" 94 | ip_server_1: 95 | id: "" 96 | port: 0 97 | rtk_standard: "auto" 98 | send_gga: "auto" 99 | serial_1: 100 | port: "" 101 | baud_rate: 115200 102 | rtk_standard: "auto" 103 | send_gga: "auto" 104 | 105 | publish: 106 | # For both GNSS and INS Rxs 107 | auto_publish: false 108 | publish_only_valid: false 109 | navsatfix: false 110 | gpsfix: false 111 | gpgga: false 112 | gprmc: false 113 | gpst: false 114 | measepoch: false 115 | pvtcartesian: false 116 | pvtgeodetic: false 117 | basevectorcart: false 118 | basevectorgeod: false 119 | poscovcartesian: false 120 | poscovgeodetic: false 121 | velcovcartesian: false 122 | velcovgeodetic: false 123 | atteuler: false 124 | attcoveuler: false 125 | pose: false 126 | twist: true 127 | diagnostics: true 128 | aimplusstatus: true 129 | galauthstatus: false 130 | # For INS Rx only 131 | insnavcart: true 132 | insnavgeod: true 133 | extsensormeas: true 134 | imusetup: false 135 | velsensorsetup: false 136 | exteventinsnavcart: false 137 | exteventinsnavgeod: false 138 | imu: true 139 | localization: true 140 | tf: true 141 | localization_ecef: false 142 | tf_ecef: false 143 | 144 | 145 | # INS-Specific Parameters 146 | 147 | ins_spatial_config: 148 | imu_orientation: 149 | theta_x: 0.0 150 | theta_y: 0.0 151 | theta_z: 0.0 152 | poi_lever_arm: 153 | delta_x: 0.0 154 | delta_y: 0.0 155 | delta_z: 0.0 156 | ant_lever_arm: 157 | x: 0.0 158 | y: 0.0 159 | z: 0.0 160 | vsm_lever_arm: 161 | vsm_x: 0.0 162 | vsm_y: 0.0 163 | vsm_z: 0.0 164 | 165 | ins_initial_heading: auto 166 | 167 | ins_std_dev_mask: 168 | att_std_dev: 5.0 169 | pos_std_dev: 10.0 170 | 171 | ins_use_poi: true 172 | 173 | ins_vsm: 174 | ros: 175 | source: "" 176 | config: [false, false, false] 177 | variances_by_parameter: false 178 | variances: [0.0, 0.0, 0.0] 179 | ip_server: 180 | id: "IP5" 181 | port: 24786 182 | keep_open: true 183 | serial: 184 | port: "" 185 | baud_rate: 115200 186 | keep_open: true 187 | 188 | # logger 189 | 190 | activate_debug_log: false -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/parse_exception.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | // C++ library includes 34 | #include 35 | // The C++ Standard library provides a base class specifically designed to declare 36 | // objects to be thrown as exceptions. It is called std::exception and is defined in 37 | // the header. This class has a virtual member function called what that 38 | // returns a null-terminated character sequence (of type char *) and that can be 39 | // overwritten in derived classes to contain some sort of description of the 40 | // exception. Example Code for using base class exception: 41 | /* 42 | class myexception: public exception 43 | { 44 | virtual const char* what() const throw() 45 | { 46 | return "My exception happened"; 47 | } 48 | } myex; 49 | 50 | int main () { 51 | try 52 | { 53 | throw myex; 54 | } 55 | catch (exception& e) 56 | { 57 | cout << e.what() << '\n'; 58 | } 59 | return 0; 60 | } 61 | */ 62 | 63 | /** 64 | * @file parse_exception.hpp 65 | * @brief Declares a derived class of the class "std::runtime_error" for throwing 66 | * error messages when parsing NMEA/SBF 67 | * @date 17/08/20 68 | */ 69 | 70 | /** 71 | * @class ParseException 72 | * @date 17/08/20 73 | * @brief Class to declare error message format when parsing, derived from the public 74 | * class "std::runtime_error" 75 | * 76 | * Such error messages shall be thrown whenever a parser class has an unrecoverable 77 | * issue parsing a message.. Note that "std::runtime_error" is already a class 78 | * derived from the base class "exception". Note on "explicit" keyword: When a class 79 | * has a constructor which can be called with a single argument (since arguments 80 | * might be set to some values by default), then this constructor becomes a 81 | * conversion constructor, since it allows the !implicit! conversion of the single 82 | * argument to the full class. We can avoid such implicit conversions as these may 83 | * lead to unexpected results by making the constructor explicit with the help of the 84 | * "explicit" keyword. 85 | */ 86 | class ParseException : public std::runtime_error 87 | { 88 | public: 89 | explicit ParseException(const std::string& error) : std::runtime_error(error) {} 90 | }; -------------------------------------------------------------------------------- /config/rover.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for the Rover Rx 2 | 3 | # GNSS/INS Parameters 4 | 5 | device: tcp://192.168.3.1:28784 6 | 7 | serial: 8 | baudrate: 921600 9 | hw_flow_control: "off" 10 | 11 | stream_device: 12 | tcp: 13 | ip_server: "" 14 | port: 0 15 | udp: 16 | ip_server: "" 17 | port: 0 18 | unicast_ip: "" 19 | 20 | configure_rx: true 21 | 22 | custom_commands_file: "" 23 | 24 | login: 25 | user: "" 26 | password: "" 27 | 28 | osnma: 29 | mode: "off" 30 | ntp_server: "" 31 | keep_open: true 32 | 33 | frame_id: gnss 34 | 35 | imu_frame_id: imu 36 | 37 | poi_frame_id: base_link 38 | 39 | vsm_frame_id: vsm 40 | 41 | aux1_frame_id: aux1 42 | 43 | vehicle_frame_id: base_link 44 | 45 | local_frame_id: odom 46 | 47 | insert_local_frame: false 48 | 49 | get_spatial_config_from_tf: false 50 | 51 | lock_utm_zone: true 52 | 53 | use_ros_axis_orientation: true 54 | 55 | receiver_type: gnss 56 | 57 | multi_antenna: true 58 | 59 | datum: Default 60 | 61 | poi_to_arp: 62 | delta_e: 0.0 63 | delta_n: 0.0 64 | delta_u: 0.0 65 | 66 | att_offset: 67 | heading: 0.0 68 | pitch: 0.0 69 | 70 | ant_type: "Unknown" 71 | ant_serial_nr: "Unknown" 72 | ant_aux1_type: "Unknown" 73 | ant_aux1_serial_nr: "Unknown" 74 | 75 | polling_period: 76 | pvt: 100 77 | rest: 500 78 | 79 | # time 80 | use_gnss_time: false 81 | ntp_server: true 82 | ptp_server_clock: false 83 | latency_compensation: true 84 | 85 | # RTK 86 | rtk_settings: 87 | ntrip_1: 88 | id: "" 89 | caster: "" 90 | caster_port: 2101 91 | username: "" 92 | password: "" 93 | mountpoint: "" 94 | version: "v2" 95 | tls: false 96 | fingerprint: "" 97 | rtk_standard: "auto" 98 | send_gga: "auto" 99 | keep_open: true 100 | ip_server_1: 101 | id: "" 102 | port: 0 103 | rtk_standard: "auto" 104 | send_gga: "auto" 105 | keep_open: true 106 | serial_1: 107 | port: "" 108 | baud_rate: 115200 109 | rtk_standard: "auto" 110 | send_gga: "auto" 111 | keep_open: true 112 | 113 | publish: 114 | # For both GNSS and INS Rxs 115 | auto_publish: false 116 | publish_only_valid: false 117 | navsatfix: false 118 | gpsfix: true 119 | gpgga: false 120 | gprmc: false 121 | gpst: false 122 | measepoch: false 123 | pvtcartesian: false 124 | pvtgeodetic: true 125 | basevectorcart: false 126 | basevectorgeod: false 127 | poscovcartesian: false 128 | poscovgeodetic: true 129 | velcovcartesian: false 130 | velcovgeodetic: true 131 | atteuler: true 132 | attcoveuler: true 133 | pose: false 134 | twist: false 135 | diagnostics: false 136 | aimplusstatus: true 137 | galauthstatus: false 138 | # For GNSS Rx only 139 | gpgsa: false 140 | gpgsv: false 141 | # For INS Rx only 142 | insnavcart: false 143 | insnavgeod: false 144 | extsensormeas: false 145 | imusetup: false 146 | velsensorsetup: false 147 | exteventinsnavcart: false 148 | exteventinsnavgeod: false 149 | imu: false 150 | localization: false 151 | tf: false 152 | localization_ecef: false 153 | tf_ecef: false 154 | 155 | 156 | # INS-Specific Parameters 157 | 158 | ins_spatial_config: 159 | imu_orientation: 160 | theta_x: 0.0 161 | theta_y: 0.0 162 | theta_z: 0.0 163 | poi_lever_arm: 164 | delta_x: 0.0 165 | delta_y: 0.0 166 | delta_z: 0.0 167 | ant_lever_arm: 168 | x: 0.0 169 | y: 0.0 170 | z: 0.0 171 | vsm_lever_arm: 172 | vsm_x: 0.0 173 | vsm_y: 0.0 174 | vsm_z: 0.0 175 | 176 | ins_initial_heading: auto 177 | 178 | ins_std_dev_mask: 179 | att_std_dev: 5.0 180 | pos_std_dev: 10.0 181 | 182 | ins_use_poi: false 183 | 184 | ins_vsm: 185 | ros: 186 | source: "" 187 | config: [false, false, false] 188 | variances_by_parameter: false 189 | variances: [0.0, 0.0, 0.0] 190 | ip_server: 191 | id: "" 192 | port: 0 193 | keep_open: true 194 | serial: 195 | port: "" 196 | baud_rate: 115200 197 | keep_open: true 198 | 199 | 200 | # logger 201 | 202 | activate_debug_log: false -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -4 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: true 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Right 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Always 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: Never 20 | AllowShortLoopsOnASingleLine: false 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: true 24 | AlwaysBreakTemplateDeclarations: Yes 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: true 29 | AfterClass: true 30 | AfterControlStatement: true 31 | AfterEnum: true 32 | AfterFunction: true 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: true 36 | AfterUnion: true 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Custom 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: AfterColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: AfterColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: false 53 | ColumnLimit: 85 54 | CommentPragmas: "^ IWYU pragma:" 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Regroup 70 | IncludeCategories: 71 | - Regex: '^"(llvm|llvm-c|clang|clang-c)/' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^(<|"(gtest|gmock|isl|json)/)' 75 | Priority: 3 76 | SortPriority: 0 77 | - Regex: ".*" 78 | Priority: 1 79 | SortPriority: 0 80 | IncludeIsMainRegex: "(Test)?$" 81 | IncludeIsMainSourceRegex: "" 82 | IndentCaseLabels: false 83 | IndentGotoLabels: true 84 | IndentPPDirectives: None 85 | IndentWidth: 4 86 | IndentWrappedFunctionNames: false 87 | JavaScriptQuotes: Leave 88 | JavaScriptWrapImports: true 89 | KeepEmptyLinesAtTheStartOfBlocks: true 90 | MacroBlockBegin: "" 91 | MacroBlockEnd: "" 92 | MaxEmptyLinesToKeep: 1 93 | NamespaceIndentation: All 94 | ObjCBinPackProtocolList: Auto 95 | ObjCBlockIndentWidth: 2 96 | ObjCSpaceAfterProperty: false 97 | ObjCSpaceBeforeProtocolList: true 98 | PenaltyBreakAssignment: 2 99 | PenaltyBreakBeforeFirstCallParameter: 19 100 | PenaltyBreakComment: 300 101 | PenaltyBreakFirstLessLess: 120 102 | PenaltyBreakString: 1000 103 | PenaltyBreakTemplateDeclaration: 10 104 | PenaltyExcessCharacter: 1000000 105 | PenaltyReturnTypeOnItsOwnLine: 60 106 | PointerAlignment: Left 107 | ReflowComments: true 108 | SortIncludes: true 109 | SortUsingDeclarations: true 110 | SpaceAfterCStyleCast: false 111 | SpaceAfterLogicalNot: false 112 | SpaceAfterTemplateKeyword: true 113 | SpaceBeforeAssignmentOperators: true 114 | SpaceBeforeCpp11BracedList: false 115 | SpaceBeforeCtorInitializerColon: true 116 | SpaceBeforeInheritanceColon: true 117 | SpaceBeforeParens: ControlStatements 118 | SpaceBeforeRangeBasedForLoopColon: true 119 | SpaceInEmptyBlock: false 120 | SpaceInEmptyParentheses: false 121 | SpacesBeforeTrailingComments: 1 122 | SpacesInAngles: false 123 | SpacesInConditionalStatement: false 124 | SpacesInContainerLiterals: true 125 | SpacesInCStyleCastParentheses: false 126 | SpacesInParentheses: false 127 | SpacesInSquareBrackets: false 128 | SpaceBeforeSquareBrackets: false 129 | Standard: Latest 130 | StatementMacros: 131 | - Q_UNUSED 132 | - QT_REQUIRE_VERSION 133 | TabWidth: 4 134 | UseCRLF: false 135 | UseTab: Never 136 | --- 137 | -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/crc/crc.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | #include 33 | 34 | namespace crc { 35 | /** 36 | * @file crc.cpp 37 | * @brief Defines the CRC table and the functions to compute and validate the CRC 38 | * of an SBF block 39 | * @date 17/08/20 40 | */ 41 | 42 | uint16_t compute16CCITT(const uint8_t* buf, 43 | size_t buf_length) // The CRC we choose is 2 bytes, 44 | // remember, hence uint16_t.. 45 | { 46 | uint16_t crc = 0; // Seed is 0, as suggested by the firmware, will compute 47 | // CRC in the forward direction.. 48 | 49 | for (size_t i = 0; i < buf_length; i++) 50 | { 51 | crc = (crc << 8) ^ CRC_LOOK_UP[uint8_t((crc >> 8) ^ buf[i])]; 52 | // The ^ (bitwise XOR) in C or C++ takes two numbers as operands and does 53 | // XOR on every bit of two numbers. The result of XOR is 1 if the two 54 | // bits are different. The << (left shift) in C or C++ takes two numbers, 55 | // left shifts the bits of the first operand, the second operand decides 56 | // the number of places to shift. The >> (right shift) in C or C++ takes 57 | // two numbers, right shifts the bits of the first operand, the second 58 | // operand decides the number of places to shift; you can just loose the 59 | // smallest values if big-endian. The left shift and right shift 60 | // operators should not be used for negative numbers. The left-shift and 61 | // right-shift operators are equivalent to multiplication and division by 62 | // 2 respectively, hence only rightshift is non-exact (remainder is not 63 | // retained). CRC_LOOK_UP is constructed from truncated polynomial 64 | // (divisor). The above implements a kind of CRC 32 algorithm: efficient, 65 | // fast. 66 | } 67 | 68 | return crc; 69 | } 70 | 71 | bool isValid(const std::vector& message) 72 | { 73 | // We need all of the message except for the first 4 bytes (Sync and CRC), 74 | // i.e. we start at the address of ID. 75 | uint16_t length = parsing_utilities::getLength(message); 76 | if (length > 4) 77 | { 78 | uint16_t crc = compute16CCITT(message.data() + 4, length - 4); 79 | return (crc == parsing_utilities::getCrc(message)); 80 | } else 81 | { 82 | return false; 83 | } 84 | } 85 | } // namespace crc 86 | -------------------------------------------------------------------------------- /config/rover_node.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for the Rover Rx 2 | 3 | # GNSS/INS Parameters 4 | septentrio_gnss_driver: 5 | ros__parameters: 6 | device: tcp://192.168.3.1:28784 7 | 8 | serial: 9 | baudrate: 921600 10 | hw_flow_control: "off" 11 | 12 | stream_device: 13 | tcp: 14 | ip_server: "" 15 | port: 0 16 | udp: 17 | ip_server: "" 18 | port: 0 19 | unicast_ip: "" 20 | 21 | configure_rx: true 22 | 23 | custom_commands_file: "" 24 | 25 | osnma: 26 | mode: "off" 27 | ntp_server: "" 28 | keep_open: true 29 | 30 | frame_id: gnss 31 | 32 | imu_frame_id: imu 33 | 34 | poi_frame_id: base_link 35 | 36 | vsm_frame_id: vsm 37 | 38 | aux1_frame_id: aux1 39 | 40 | vehicle_frame_id: base_link 41 | 42 | local_frame_id: odom 43 | 44 | insert_local_frame: false 45 | 46 | get_spatial_config_from_tf: false 47 | 48 | lock_utm_zone: true 49 | 50 | use_ros_axis_orientation: true 51 | 52 | receiver_type: gnss 53 | 54 | multi_antenna: true 55 | 56 | datum: Default 57 | 58 | poi_to_arp: 59 | delta_e: 0.0 60 | delta_n: 0.0 61 | delta_u: 0.0 62 | 63 | att_offset: 64 | heading: 0.0 65 | pitch: 0.0 66 | 67 | ant_type: "Unknown" 68 | ant_serial_nr: "Unknown" 69 | ant_aux1_type: "Unknown" 70 | ant_aux1_serial_nr: "Unknown" 71 | 72 | polling_period: 73 | pvt: 100 74 | rest: 500 75 | 76 | # time 77 | use_gnss_time: false 78 | ntp_server: true 79 | ptp_server_clock: false 80 | latency_compensation: true 81 | 82 | # RTK 83 | rtk_settings: 84 | ntrip_1: 85 | id: "" 86 | caster: "" 87 | caster_port: 2101 88 | username: "" 89 | password: "" 90 | mountpoint: "" 91 | version: "v2" 92 | tls: false 93 | fingerprint: "" 94 | rtk_standard: "auto" 95 | send_gga: "auto" 96 | keep_open: true 97 | ip_server_1: 98 | id: "" 99 | port: 0 100 | rtk_standard: "auto" 101 | send_gga: "auto" 102 | keep_open: true 103 | serial_1: 104 | port: "" 105 | baud_rate: 115200 106 | rtk_standard: "auto" 107 | send_gga: "auto" 108 | keep_open: true 109 | 110 | publish: 111 | # For both GNSS and INS Rxs 112 | auto_publish: false 113 | publish_only_valid: false 114 | navsatfix: false 115 | gpsfix: true 116 | gpgga: false 117 | gprmc: false 118 | gpst: false 119 | measepoch: false 120 | pvtcartesian: false 121 | pvtgeodetic: true 122 | basevectorcart: false 123 | basevectorgeod: false 124 | poscovcartesian: false 125 | poscovgeodetic: true 126 | velcovcartesian: false 127 | velcovgeodetic: true 128 | atteuler: true 129 | attcoveuler: true 130 | pose: false 131 | twist: false 132 | diagnostics: false 133 | aimplusstatus: true 134 | galauthstatus: false 135 | # For GNSS Rx only 136 | gpgsa: false 137 | gpgsv: false 138 | # For INS Rx only 139 | insnavcart: false 140 | insnavgeod: false 141 | extsensormeas: false 142 | imusetup: false 143 | velsensorsetup: false 144 | exteventinsnavcart: false 145 | exteventinsnavgeod: false 146 | imu: false 147 | localization: false 148 | tf: false 149 | 150 | # INS-Specific Parameters 151 | 152 | ins_spatial_config: 153 | imu_orientation: 154 | theta_x: 0.0 155 | theta_y: 0.0 156 | theta_z: 0.0 157 | poi_to_imu: 158 | delta_x: 0.0 159 | delta_y: 0.0 160 | delta_z: 0.0 161 | ant_lever_arm: 162 | x: 0.0 163 | y: 0.0 164 | z: 0.0 165 | vsm_lever_arm: 166 | vsm_x: 0.0 167 | vsm_y: 0.0 168 | vsm_z: 0.0 169 | 170 | ins_initial_heading: auto 171 | 172 | ins_std_dev_mask: 173 | att_std_dev: 5.0 174 | pos_std_dev: 10.0 175 | 176 | ins_use_poi: false 177 | 178 | ins_vsm: 179 | ros: 180 | source: "" 181 | config: [false, false, false] 182 | variances_by_parameter: false 183 | variances: [0.0, 0.0, 0.0] 184 | ip_server: 185 | id: "" 186 | port: 0 187 | keep_open: true 188 | serial: 189 | port: "" 190 | baud_rate: 115200 191 | keep_open: true 192 | 193 | # logger 194 | 195 | activate_debug_log: false -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | // ROSaic includes 62 | #include 63 | #include 64 | 65 | /** 66 | * @file gpgsv.hpp 67 | * @brief Derived class for parsing GSV messages 68 | * @date 29/09/20 69 | */ 70 | 71 | /** 72 | * @class GpgsvParser 73 | * @brief Derived class for parsing GSV messages 74 | * @date 29/09/20 75 | */ 76 | class GpgsvParser : public BaseParser 77 | { 78 | public: 79 | /** 80 | * @brief Constructor of the class GpgsvParser 81 | */ 82 | GpgsvParser() : BaseParser() {} 83 | 84 | /** 85 | * @brief Returns the ASCII message ID, here "$GPGSV" 86 | * @return The message ID 87 | */ 88 | const std::string getMessageID() const override; 89 | 90 | /** 91 | * @brief Parses one GSV message 92 | * @param[in] sentence The GSV message to be parsed 93 | * @return A ROS message pointer of ROS type nmea_msgs::GpgsvPtr 94 | */ 95 | GpgsvMsg parseASCII(const NMEASentence& sentence, const std::string& frame_id, 96 | bool use_gnss_time, 97 | Timestamp time_obj) noexcept(false) override; 98 | 99 | /** 100 | * @brief Declares the string MESSAGE_ID 101 | */ 102 | static const std::string MESSAGE_ID; 103 | }; -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | // ROSaic includes 62 | #include 63 | #include 64 | // Boost and ROS includes 65 | #include 66 | 67 | /** 68 | * @file gpgsa.hpp 69 | * @brief Derived class for parsing GSA messages 70 | * @date 29/09/20 71 | */ 72 | 73 | /** 74 | * @class GpgsaParser 75 | * @brief Derived class for parsing GSA messages 76 | * @date 29/09/20 77 | */ 78 | class GpgsaParser : public BaseParser 79 | { 80 | public: 81 | /** 82 | * @brief Constructor of the class GpgsaParser 83 | */ 84 | GpgsaParser() : BaseParser() {} 85 | 86 | /** 87 | * @brief Returns the ASCII message ID, here "$GPGSA" 88 | * @return The message ID 89 | */ 90 | const std::string getMessageID() const override; 91 | 92 | /** 93 | * @brief Parses one GSA message 94 | * @param[in] sentence The GSA message to be parsed 95 | * @return A ROS message pointer of ROS type GpgsaMsg 96 | */ 97 | GpgsaMsg parseASCII(const NMEASentence& sentence, const std::string& frame_id, 98 | bool use_gnss_time, 99 | Timestamp time_obj) noexcept(false) override; 100 | 101 | /** 102 | * @brief Declares the string MESSAGE_ID 103 | */ 104 | static const std::string MESSAGE_ID; 105 | }; -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/parser_base_class.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | // ROSaic includes 34 | #include "nmea_sentence.hpp" 35 | #include "parse_exception.hpp" 36 | #include "parsing_utilities.hpp" 37 | 38 | /** 39 | * @file parser_base_class.hpp 40 | * @brief Declares a base class for parsing NMEA messages and SBF blocks 41 | * @date 13/08/20 42 | */ 43 | 44 | /** 45 | * @class BaseParser 46 | * @brief Base class for parsing NMEA messages and SBF blocks 47 | * 48 | * Subclasses that parse NMEA messages should implement 49 | * ParseASCII(const NMEASentence&); The base class is implemented 50 | * as a template, which is a simple and yet very powerful tool in C++. The 51 | * simple idea is to pass data type as a parameter so that we don’t need to 52 | * write the same code for different data types. Like function templates, class 53 | * templates are useful when a class defines something that is independent of 54 | * the data type, as here the notion of parsing. 55 | * 56 | * @tparam T The ROS message pointer type that the parser should produce, e.g. 57 | * nmea_msgs::GpggaPtr. 58 | */ 59 | template 60 | class BaseParser 61 | { 62 | public: 63 | /** 64 | * @brief Default constructor of the class BaseParser 65 | * 66 | * Adding the "default" keyword to the constructor declaration is a new feature 67 | * since C++11 for creating a default constructor (a constructor which can be 68 | * called with no arguments). Strictly speaking, it is not the same as when the 69 | * keyword is omitted, but the differences are miniscule. 70 | * 71 | * Also note that in C++, the constructor cannot be virtual, because when a 72 | * constructor of a class is executed, there is no virtual table in the memory, 73 | * i.e. no virtual pointer defined yet. 74 | * 75 | */ 76 | BaseParser() = default; 77 | 78 | /** 79 | * @brief Default destructor of the class BaseParser 80 | * 81 | * As opposed to the constructor, a destructor can be virtual, as here. 82 | */ 83 | virtual ~BaseParser() = default; 84 | 85 | /** 86 | * @brief Returns the ASCII message name 87 | * 88 | * GetMessageID() is a pure virtual function, i.e. a function 89 | * for which writing a function declaration suffices. It is declared by 90 | * assigning the value 0 in the declaration. Since we now have at least 91 | * 1 pure virtual function, our class BaseParser thus becomes an 92 | * "abstract" one. 93 | * 94 | * @return The ASCII message name. 95 | */ 96 | virtual const std::string getMessageID() const = 0; 97 | 98 | /** 99 | * @brief Converts an NMEA sentence - both standardized and proprietary ones - 100 | * into a ROS message pointer (e.g. nmea_msgs::GpggaPtr) and returns it 101 | * 102 | * The returned value should not be NULL. ParseException will be thrown 103 | * if there are any issues parsing the message. 104 | * @param[in] sentence The standardized NMEA sentence to convert, of type 105 | * NMEASentence 106 | * @return A valid ROS message pointer 107 | */ 108 | virtual T parseASCII(const NMEASentence& sentence, const std::string& frame_id, 109 | bool use_gnss_time, Timestamp time_obj) noexcept(false) 110 | { 111 | throw ParseException("ParseASCII not implemented."); 112 | }; 113 | }; -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | 33 | /** 34 | * @file gpgsa.cpp 35 | * @brief Derived class for parsing GSA messages 36 | * @date 29/09/20 37 | */ 38 | 39 | const std::string GpgsaParser::MESSAGE_ID = "$GPGSA"; 40 | 41 | const std::string GpgsaParser::getMessageID() const 42 | { 43 | return GpgsaParser::MESSAGE_ID; 44 | } 45 | 46 | /** 47 | * Caution: Due to the occurrence of the throw keyword, this method ParseASCII should 48 | * be called within a try / catch framework... Note: This method is called from 49 | * within the read() method of the RxMessage class by including the checksum part in 50 | * the argument "sentence" here, though the checksum is never parsed: It would be 51 | * sentence.get_body()[18] if anybody ever needs it. 52 | */ 53 | GpgsaMsg GpgsaParser::parseASCII(const NMEASentence& sentence, 54 | const std::string& frame_id, bool /*use_gnss_time*/, 55 | Timestamp /*time_obj*/) noexcept(false) 56 | { 57 | 58 | // Checking the length first, it should be 19 elements 59 | const size_t LENGTH = 19; 60 | if (sentence.get_body().size() != LENGTH) 61 | { 62 | std::stringstream error; 63 | error << "Expected GPGSA length is " << LENGTH << ". The actual length is " 64 | << sentence.get_body().size(); 65 | throw ParseException(error.str()); 66 | } 67 | 68 | GpgsaMsg msg; 69 | msg.header.frame_id = frame_id; 70 | msg.message_id = sentence.get_body()[0]; 71 | msg.auto_manual_mode = sentence.get_body()[1]; 72 | if (!parsing_utilities::parseUInt8(sentence.get_body()[2], msg.fix_mode)) 73 | { 74 | std::stringstream error; 75 | error << "GPGSA fix_mode parsing error."; 76 | throw ParseException(error.str()); 77 | } 78 | // Words 3-14 of the sentence are SV PRNs. Copying only the non-null strings.. 79 | // 0 is the character needed to fill the new character space, in case 12 (first 80 | // argument) is larger than sv_ids. 81 | msg.sv_ids.resize(12, 0); 82 | size_t n_svs = 0; 83 | for (std::vector::const_iterator id = 84 | sentence.get_body().begin() + 3; 85 | id < sentence.get_body().begin() + 15; ++id) 86 | { 87 | if (!id->empty()) 88 | { 89 | if (!parsing_utilities::parseUInt8(*id, msg.sv_ids[n_svs])) 90 | { 91 | std::stringstream error; 92 | error << "GPGSA sv_ids parsing error."; 93 | throw ParseException(error.str()); 94 | } 95 | ++n_svs; 96 | } 97 | } 98 | msg.sv_ids.resize(n_svs); 99 | 100 | if (!parsing_utilities::parseFloat(sentence.get_body()[15], msg.pdop)) 101 | { 102 | std::stringstream error; 103 | error << "GPGSA pdop parsing error."; 104 | throw ParseException(error.str()); 105 | } 106 | if (!parsing_utilities::parseFloat(sentence.get_body()[16], msg.hdop)) 107 | { 108 | std::stringstream error; 109 | error << "GPGSA hdop parsing error."; 110 | throw ParseException(error.str()); 111 | } 112 | if (!parsing_utilities::parseFloat(sentence.get_body()[17], msg.vdop)) 113 | { 114 | std::stringstream error; 115 | error << "GPGSA vdop parsing error."; 116 | throw ParseException(error.str()); 117 | } 118 | return msg; 119 | } -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | // ROSaic includes 62 | #include 63 | #include 64 | 65 | /** 66 | * @file gpgga.hpp 67 | * @brief Derived class for parsing GGA messages 68 | * @date 17/08/20 69 | */ 70 | 71 | /** 72 | * @class GpggaParser 73 | * @brief Derived class for parsing GGA messages 74 | * @date 13/08/20 75 | */ 76 | class GpggaParser : public BaseParser 77 | { 78 | public: 79 | /** 80 | * @brief Constructor of the class GpggaParser 81 | */ 82 | GpggaParser() : BaseParser(), was_last_gpgga_valid_(false) {} 83 | 84 | /** 85 | * @brief Returns the ASCII message ID, here "$GPGGA" 86 | * @return The message ID 87 | */ 88 | const std::string getMessageID() const override; 89 | 90 | /** 91 | * @brief Parses one GGA message 92 | * @param[in] sentence The GGA message to be parsed 93 | * @return A ROS message pointer of ROS type GpggaMsg 94 | */ 95 | GpggaMsg parseASCII(const NMEASentence& sentence, const std::string& frame_id, 96 | bool use_gnss_time, 97 | Timestamp time_obj) noexcept(false) override; 98 | 99 | /** 100 | * @brief Tells us whether the last GGA message was valid or not 101 | * @return True if last GGA message was valid, false if not 102 | */ 103 | bool wasLastGPGGAValid() const; 104 | 105 | /** 106 | * @brief Declares the string MESSAGE_ID 107 | */ 108 | static const std::string MESSAGE_ID; 109 | 110 | private: 111 | /** 112 | * @brief Declares a boolean representing whether or not the last GPGGA message 113 | * was valid 114 | */ 115 | bool was_last_gpgga_valid_; 116 | }; -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | // ROSaic includes 60 | #include 61 | #include 62 | // Boost and ROS includes 63 | #include 64 | 65 | /** 66 | * @file gprmc.hpp 67 | * @brief Derived class for parsing RMC messages 68 | * @date 28/09/20 69 | */ 70 | 71 | /** 72 | * @class GprmcParser 73 | * @brief Derived class for parsing RMC messages 74 | * @date 28/09/20 75 | */ 76 | class GprmcParser : public BaseParser 77 | { 78 | public: 79 | /** 80 | * @brief Constructor of the class GprmcParser 81 | */ 82 | GprmcParser() : BaseParser(), was_last_gprmc_valid_(false) {} 83 | 84 | /** 85 | * @brief Returns the ASCII message ID, here "$GPRMC" 86 | * @return The message ID 87 | */ 88 | const std::string getMessageID() const override; 89 | 90 | /** 91 | * @brief Parses one RMC message 92 | * @param[in] sentence The RMC message to be parsed 93 | * @return A ROS message pointer of ROS type GprmcMsg 94 | */ 95 | GprmcMsg parseASCII(const NMEASentence& sentence, const std::string& frame_id, 96 | bool use_gnss_time, 97 | Timestamp time_obj) noexcept(false) override; 98 | 99 | /** 100 | * @brief Tells us whether the last RMC message was valid/usable or not 101 | * @return True if last RMC message was valid, false if not 102 | * 103 | * E.g. if the status is V=Void, then this function will also return false. 104 | */ 105 | bool wasLastGPRMCValid() const; 106 | 107 | /** 108 | * @brief Declares the string MESSAGE_ID 109 | */ 110 | static const std::string MESSAGE_ID; 111 | 112 | //! 1 kt = 0.51444444444 mps (meters per second) 113 | //! Note that constexpr is needed for in-class initialization of static data 114 | //! members. 115 | static constexpr double KNOTS_TO_MPS = 0.5144444; 116 | 117 | private: 118 | /** 119 | * @brief Declares a boolean representing whether or not the last GPRMC message 120 | * was valid 121 | */ 122 | bool was_last_gprmc_valid_; 123 | }; -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/node/rosaic_node.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | /** 62 | * @file rosaic_node.hpp 63 | * @date 21/08/20 64 | * @brief The heart of the ROSaic driver: The ROS node that represents it 65 | */ 66 | 67 | // tf2 includes 68 | #include 69 | #include 70 | // ROSaic includes 71 | #include 72 | 73 | /** 74 | * @namespace rosaic_node 75 | * This namespace is for the ROSaic node, handling all aspects regarding 76 | * ROS parameters, ROS message publishing etc. 77 | */ 78 | namespace rosaic_node { 79 | /** 80 | * @class ROSaicNode 81 | * @brief This class represents the ROsaic node, to be extended.. 82 | */ 83 | class ROSaicNode : public ROSaicNodeBase 84 | { 85 | public: 86 | //! The constructor initializes and runs the ROSaic node, if everything works 87 | //! fine. It loads the user-defined ROS parameters, subscribes to Rx 88 | //! messages, and publishes requested ROS messages... 89 | ROSaicNode(const rclcpp::NodeOptions& options); 90 | 91 | ~ROSaicNode(); 92 | 93 | private: 94 | void setup(); 95 | /** 96 | * @brief Gets the node parameters from the ROS Parameter Server, parts of 97 | * which are specified in a YAML file 98 | * 99 | * The other ROSaic parameters are specified via the command line. 100 | */ 101 | [[nodiscard]] bool getROSParams(); 102 | /** 103 | * @brief Checks if the period has a valid value 104 | * @param[in] period period [ms] 105 | * @param[in] isIns wether recevier is an INS 106 | * @return wether the period is valid 107 | */ 108 | [[nodiscard]] bool validPeriod(uint32_t period, bool isIns) const; 109 | /** 110 | * @brief Gets transforms from tf2 111 | * @param[in] targetFrame traget frame id 112 | * @param[in] sourceFrame source frame id 113 | * @param[out] T_s_t transfrom from source to target 114 | */ 115 | void getTransform(const std::string& targetFrame, 116 | const std::string& sourceFrame, 117 | TransformStampedMsg& T_s_t) const; 118 | /** 119 | * @brief Gets Euler angles from quaternion message 120 | * @param[in] qm quaternion message 121 | * @param[out] roll roll angle 122 | * @param[out] pitch pitch angle 123 | * @param[out] yaw yaw angle 124 | */ 125 | void getRPY(const QuaternionMsg& qm, double& roll, double& pitch, 126 | double& yaw) const; 127 | 128 | void sendVelocity(const std::string& velNmea); 129 | 130 | //! Handles communication with the Rx 131 | io::CommunicationCore IO_; 132 | //! tf2 buffer and listener 133 | tf2_ros::Buffer tfBuffer_; 134 | std::unique_ptr tfListener_; 135 | 136 | std::thread setupThread_; 137 | }; 138 | } // namespace rosaic_node -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/node/rosaic_node_ros1.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | /** 62 | * @file rosaic_node.hpp 63 | * @date 21/08/20 64 | * @brief The heart of the ROSaic driver: The ROS node that represents it 65 | */ 66 | 67 | // ROS includes 68 | #include 69 | #include 70 | // tf2 includes 71 | #include 72 | // ROSaic includes 73 | #include 74 | 75 | /** 76 | * @namespace rosaic_node 77 | * This namespace is for the ROSaic node, handling all aspects regarding 78 | * ROS parameters, ROS message publishing etc. 79 | */ 80 | namespace rosaic_node { 81 | /** 82 | * @class ROSaicNode 83 | * @brief This class represents the ROsaic node, to be extended.. 84 | */ 85 | class ROSaicNode : ROSaicNodeBase 86 | { 87 | public: 88 | //! The constructor initializes and runs the ROSaic node, if everything works 89 | //! fine. It loads the user-defined ROS parameters, subscribes to Rx 90 | //! messages, and publishes requested ROS messages... 91 | ROSaicNode(); 92 | 93 | ~ROSaicNode(); 94 | 95 | private: 96 | void setup(); 97 | /** 98 | * @brief Gets the node parameters from the ROS Parameter Server, parts of 99 | * which are specified in a YAML file 100 | * 101 | * The other ROSaic parameters are specified via the command line. 102 | */ 103 | [[nodiscard]] bool getROSParams(); 104 | /** 105 | * @brief Checks if the period has a valid value 106 | * @param[in] period period [ms] 107 | * @param[in] isIns wether recevier is an INS 108 | * @return wether the period is valid 109 | */ 110 | [[nodiscard]] bool validPeriod(uint32_t period, bool isIns) const; 111 | /** 112 | * @brief Gets transforms from tf2 113 | * @param[in] targetFrame traget frame id 114 | * @param[in] sourceFrame source frame id 115 | * @param[out] T_s_t transfrom from source to target 116 | */ 117 | void getTransform(const std::string& targetFrame, 118 | const std::string& sourceFrame, 119 | TransformStampedMsg& T_s_t) const; 120 | /** 121 | * @brief Gets Euler angles from quaternion message 122 | * @param[in] qm quaternion message 123 | * @param[out] roll roll angle 124 | * @param[out] pitch pitch angle 125 | * @param[out] yaw yaw angle 126 | */ 127 | void getRPY(const QuaternionMsg& qm, double& roll, double& pitch, 128 | double& yaw) const; 129 | 130 | void sendVelocity(const std::string& velNmea); 131 | 132 | //! Handles communication with the Rx 133 | io::CommunicationCore IO_; 134 | //! tf2 buffer and listener 135 | tf2_ros::Buffer tfBuffer_; 136 | std::unique_ptr tfListener_; 137 | 138 | std::thread setupThread_; 139 | }; 140 | } // namespace rosaic_node -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/communication/telegram_handler.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | 33 | /** 34 | * @file telegram_handler.cpp 35 | * @date 22/08/20 36 | * @brief Handles callbacks when reading NMEA/SBF messages 37 | */ 38 | 39 | namespace io { 40 | 41 | void TelegramHandler::handleTelegram(const std::shared_ptr& telegram) 42 | { 43 | switch (telegram->type) 44 | { 45 | case telegram_type::SBF: 46 | { 47 | handleSbf(telegram); 48 | break; 49 | } 50 | case telegram_type::NMEA: 51 | { 52 | handleNmea(telegram); 53 | break; 54 | } 55 | case telegram_type::NMEA_INS: 56 | { 57 | handleNmea(telegram); 58 | break; 59 | } 60 | case telegram_type::RESPONSE: 61 | { 62 | handleResponse(telegram); 63 | break; 64 | } 65 | case telegram_type::ERROR_RESPONSE: 66 | { 67 | handleResponse(telegram); 68 | break; 69 | } 70 | case telegram_type::CONNECTION_DESCRIPTOR: 71 | { 72 | handleCd(telegram); 73 | break; 74 | } 75 | case telegram_type::UNKNOWN: 76 | { 77 | std::string block_in_string(telegram->message.begin(), 78 | telegram->message.end()); 79 | 80 | node_->log(log_level::DEBUG, "A message received: " + block_in_string); 81 | if (block_in_string.find("ReceiverCapabilities") != std::string::npos) 82 | { 83 | if (block_in_string.find("INS") != std::string::npos) 84 | { 85 | node_->setIsIns(); 86 | } 87 | 88 | if (block_in_string.find("Heading") != std::string::npos) 89 | { 90 | node_->setHasHeading(); 91 | } 92 | capabilitiesSemaphore_.notify(); 93 | } 94 | break; 95 | } 96 | default: 97 | { 98 | node_->log(log_level::DEBUG, 99 | "TelegramHandler received an invalid message to handle"); 100 | break; 101 | } 102 | } 103 | } 104 | 105 | void TelegramHandler::handleSbf(const std::shared_ptr& telegram) 106 | { 107 | messageHandler_.parseSbf(telegram); 108 | } 109 | 110 | void TelegramHandler::handleNmea(const std::shared_ptr& telegram) 111 | { 112 | messageHandler_.parseNmea(telegram); 113 | } 114 | 115 | void TelegramHandler::handleResponse(const std::shared_ptr& telegram) 116 | { 117 | std::string block_in_string(telegram->message.begin(), 118 | telegram->message.end()); 119 | 120 | if (telegram->type == telegram_type::ERROR_RESPONSE) 121 | { 122 | if (block_in_string == 123 | std::string( 124 | "$R? setGNSSAttitude: Argument 'Source' is invalid!\r\n")) 125 | { 126 | node_->log( 127 | log_level::WARN, 128 | "Rx does not support dual antenna mode, set parameter multi_antenna to false and/or disable publishing of atteuler."); 129 | } else if (block_in_string == 130 | std::string("$R? sptp, on : Invalid command!\r\n")) 131 | { 132 | node_->log( 133 | log_level::WARN, 134 | "Rx does not support PTP server clock. GNSS needs firmare >= 4.14., INS does not support it yet."); 135 | } else 136 | { 137 | node_->log( 138 | log_level::ERROR, 139 | "Invalid command just sent to the Rx! The Rx's response contains " + 140 | std::to_string(block_in_string.size()) + 141 | " bytes and reads:\n " + block_in_string); 142 | } 143 | } else 144 | { 145 | node_->log(log_level::DEBUG, "The Rx's response contains " + 146 | std::to_string(block_in_string.size()) + 147 | " bytes and reads:\n " + 148 | block_in_string); 149 | } 150 | responseSemaphore_.notify(); 151 | } 152 | 153 | void TelegramHandler::handleCd(const std::shared_ptr& telegram) 154 | { 155 | node_->log(log_level::DEBUG, 156 | "handleCd: " + std::string(telegram->message.begin(), 157 | telegram->message.end())); 158 | if (telegram->message.back() == CONNECTION_DESCRIPTOR_FOOTER) 159 | { 160 | mainConnectionDescriptor_ = 161 | std::string(telegram->message.begin(), telegram->message.end() - 1); 162 | 163 | cdSemaphore_.notify(); 164 | } 165 | } 166 | } // namespace io -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/communication/telegram_handler.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | // C++ includes 62 | #include 63 | 64 | // ROSaic includes 65 | #ifdef ROS2 66 | #include 67 | #endif 68 | #ifdef ROS1 69 | #include 70 | #endif 71 | #include 72 | #include 73 | 74 | /** 75 | * @file telegram_handler.hpp 76 | * @brief Handles messages when reading NMEA/SBF/response/error/connection descriptor 77 | * messages 78 | */ 79 | 80 | namespace io { 81 | class Semaphore 82 | { 83 | public: 84 | Semaphore() : block_(true) {} 85 | 86 | void notify() 87 | { 88 | std::unique_lock lock(mtx_); 89 | block_ = false; 90 | cv_.notify_one(); 91 | } 92 | 93 | void wait() 94 | { 95 | std::unique_lock lock(mtx_); 96 | while (block_) 97 | { 98 | cv_.wait(lock); 99 | } 100 | block_ = true; 101 | } 102 | 103 | private: 104 | std::mutex mtx_; 105 | std::condition_variable cv_; 106 | bool block_; 107 | }; 108 | 109 | /** 110 | * @class TelegramHandler 111 | * @brief Represents ensemble of (to be constructed) ROS messages, to be handled 112 | * at once by this class 113 | */ 114 | class TelegramHandler 115 | { 116 | 117 | public: 118 | TelegramHandler(ROSaicNodeBase* node) : node_(node), messageHandler_(node) {} 119 | 120 | ~TelegramHandler() 121 | { 122 | cdSemaphore_.notify(); 123 | responseSemaphore_.notify(); 124 | } 125 | 126 | void clearSemaphores() 127 | { 128 | cdSemaphore_.notify(); 129 | responseSemaphore_.notify(); 130 | } 131 | 132 | /** 133 | * @brief Called every time a telegram is received 134 | */ 135 | void handleTelegram(const std::shared_ptr& telegram); 136 | 137 | //! Returns the connection descriptor 138 | void resetWaitforMainCd() { mainConnectionDescriptor_ = std::string(); } 139 | 140 | //! Returns the connection descriptor 141 | [[nodiscard]] std::string getMainCd() 142 | { 143 | cdSemaphore_.wait(); 144 | return mainConnectionDescriptor_; 145 | } 146 | 147 | //! Waits for response 148 | void waitForResponse() { responseSemaphore_.wait(); } 149 | 150 | //! Waits for capabilities 151 | void waitForCapabilities() { capabilitiesSemaphore_.wait(); } 152 | 153 | private: 154 | void handleSbf(const std::shared_ptr& telegram); 155 | void handleNmea(const std::shared_ptr& telegram); 156 | void handleResponse(const std::shared_ptr& telegram); 157 | void handleError(const std::shared_ptr& telegram); 158 | void handleCd(const std::shared_ptr& telegram); 159 | //! Pointer to Node 160 | ROSaicNodeBase* node_; 161 | 162 | //! MessageHandler parser 163 | MessageHandler messageHandler_; 164 | 165 | Semaphore cdSemaphore_; 166 | Semaphore responseSemaphore_; 167 | Semaphore capabilitiesSemaphore_; 168 | std::string mainConnectionDescriptor_ = std::string(); 169 | }; 170 | 171 | } // namespace io -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/parsers/string_utilities.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | // C and C++ library includes 34 | #include 35 | #include // Merely for "isdigit()" function, also available in C header.. 36 | #include 37 | 38 | /** 39 | * @file string_utilities.hpp 40 | * @brief Declares lower-level string utility functions used when parsing messages 41 | * @date 13/08/20 42 | */ 43 | 44 | /** 45 | * @namespace string_utilities 46 | * This namespace is for the functions that encapsulate basic string manipulation and 47 | * conversion techniques. 48 | */ 49 | namespace string_utilities { 50 | /** 51 | * @brief Interprets the contents of "string" as a floating point number of type 52 | * double It stores the "string"'s value in "value" and returns whether or not 53 | * all went well. 54 | * @param[in] string The string whose content should be interpreted as a floating 55 | * point number 56 | * @param[out] value The double variable that should be overwritten by the 57 | * floating point number found in "string" 58 | * @return True if all went fine, false if not 59 | */ 60 | [[nodiscard]] bool toDouble(const std::string& string, double& value); 61 | 62 | /** 63 | * @brief Interprets the contents of "string" as a floating point number of type 64 | * float. 65 | * 66 | * It stores the "string"'s value in "value" and returns whether or not all went 67 | * well. 68 | * @param[in] string The string whose content should be interpreted as a floating 69 | * point number 70 | * @param[out] value The float variable that should be overwritten by the 71 | * floating point number found in "string" 72 | * @return True if all went fine, false if not 73 | */ 74 | [[nodiscard]] bool toFloat(const std::string& string, float& value); 75 | 76 | /** 77 | * @brief Interprets the contents of "string" as a floating point number of 78 | * whatever integer type your system has that is exactly 32 bits. 79 | * 80 | * It stores the "string"'s value in "value" and returns whether or not all went 81 | * well. 82 | * @param[in] string The string whose content should be interpreted as a floating 83 | * point number 84 | * @param[out] value The int32_t variable that should be overwritten by the 85 | * floating point number found in "string" 86 | * @param[in] base The conversion assumes this base, here: decimal 87 | * @return True if all went fine, false if not 88 | */ 89 | [[nodiscard]] bool toInt32(const std::string& string, int32_t& value, int32_t base = 10); 90 | 91 | /** 92 | * @brief Interprets the contents of "string" as a floating point number of 93 | * whatever unsigned integer type your system has that is exactly 32 bits. 94 | * 95 | * It stores the "string"'s value in "value" and returns whether or not all went 96 | * well. 97 | * @param[in] string The string whose content should be interpreted as a floating 98 | * point number 99 | * @param[out] value The uint32_t variable that should be overwritten by the 100 | * floating point number found in "string" 101 | * @param[in] base The conversion assumes this base, here: decimal 102 | * @return True if all went fine, false if not 103 | */ 104 | [[nodiscard]] bool toUInt32(const std::string& string, uint32_t& value, int32_t base = 10); 105 | 106 | /** 107 | * @brief Interprets the contents of "string" as a floating point number of 108 | * whatever integer type your system has that is exactly 8 bits. 109 | * 110 | * It stores the "string"'s value in "value". 111 | * @param[in] string The string whose content should be interpreted as a floating 112 | * point number 113 | * @param[out] value The int8_t variable that should be overwritten by the 114 | * floating point number found in "string" 115 | * @param[in] base The conversion assumes this base, here: decimal 116 | * @return The value found in "string" 117 | */ 118 | [[nodiscard]] int8_t toInt8(const std::string& string, int8_t& value, int32_t base = 10); 119 | 120 | /** 121 | * @brief Interprets the contents of "string" as a floating point number of 122 | * whatever unsigned integer type your system has that is exactly 8 bits. 123 | * 124 | * It stores the "string"'s value in "value". 125 | * @param[in] string The string whose content should be interpreted as a floating 126 | * point number 127 | * @param[out] value The uint8_t variable that should be overwritten by the 128 | * floating point number found in "string" 129 | * @param[in] base The conversion assumes this base, here: decimal 130 | * @return The value found in "string" 131 | */ 132 | [[nodiscard]] uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base = 10); 133 | 134 | /** 135 | * @brief Trims decimal places to three 136 | * @param[in] num The double who shall be trimmed 137 | * @return The string 138 | */ 139 | [[nodiscard]] std::string trimDecimalPlaces(double num); 140 | 141 | /** 142 | * @brief Checks if a string contains spaces 143 | * @param[in] str the string to be analyzed 144 | * @return true if string contains space 145 | */ 146 | [[nodiscard]] bool containsSpace(const std::string str); 147 | } // namespace string_utilities -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | 33 | /** 34 | * @file gpgga.cpp 35 | * @brief Derived class for parsing GGA messages 36 | * @date 17/08/20 37 | */ 38 | 39 | const std::string GpggaParser::MESSAGE_ID = "$GPGGA"; 40 | 41 | const std::string GpggaParser::getMessageID() const 42 | { 43 | return GpggaParser::MESSAGE_ID; 44 | } 45 | 46 | /** 47 | * Caution: Due to the occurrence of the throw keyword, this method parseASCII should 48 | * be called within a try / catch framework... Note: This method is called from 49 | * within the read() method of the RxMessage class by including the checksum part in 50 | * the argument "sentence" here, though the checksum is never parsed: It would be 51 | * sentence.get_body()[15] if anybody ever needs it. 52 | */ 53 | GpggaMsg GpggaParser::parseASCII(const NMEASentence& sentence, 54 | const std::string& frame_id, bool use_gnss_time, 55 | Timestamp time_obj) noexcept(false) 56 | { 57 | // ROS_DEBUG("Just testing that first entry is indeed what we expect it to be: 58 | // %s", sentence.get_body()[0].c_str()); 59 | // Check the length first, which should be 16 elements. 60 | const size_t LEN = 16; 61 | if (sentence.get_body().size() > LEN || sentence.get_body().size() < LEN) 62 | { 63 | std::stringstream error; 64 | error << "GGA parsing failed: Expected GPGGA length is " << LEN 65 | << ", but actual length is " << sentence.get_body().size(); 66 | throw ParseException(error.str()); 67 | } 68 | 69 | GpggaMsg msg; 70 | msg.header.frame_id = frame_id; 71 | 72 | msg.message_id = sentence.get_body()[0]; 73 | 74 | if (sentence.get_body()[1].empty() || sentence.get_body()[1] == "0") 75 | { 76 | msg.utc_seconds = 0; 77 | } else 78 | { 79 | double utc_double; 80 | if (string_utilities::toDouble(sentence.get_body()[1], utc_double)) 81 | { 82 | if (use_gnss_time) 83 | { 84 | // ROS_DEBUG("utc_double is %f", (float) utc_double); 85 | msg.utc_seconds = 86 | parsing_utilities::convertUTCDoubleToSeconds(utc_double); 87 | 88 | // The Header's Unix Epoch time stamp 89 | time_t unix_time_seconds = 90 | parsing_utilities::convertUTCtoUnix(utc_double); 91 | // The following assumes that there are two digits after the decimal 92 | // point in utc_double, i.e. in the NMEA UTC time. 93 | Timestamp unix_time_nanoseconds = 94 | unix_time_seconds * 1000000000 + 95 | (static_cast(utc_double * 100) % 100) * 10000; 96 | msg.header.stamp = timestampToRos(unix_time_nanoseconds); 97 | } else 98 | { 99 | msg.header.stamp = timestampToRos(time_obj); 100 | } 101 | } else 102 | { 103 | throw ParseException( 104 | "Error parsing UTC seconds in GPGGA"); // E.g. if one of the fields 105 | // of the NMEA UTC string is 106 | // empty 107 | } 108 | } 109 | 110 | bool valid = true; 111 | 112 | double latitude = 0.0; 113 | valid = 114 | valid && parsing_utilities::parseDouble(sentence.get_body()[2], latitude); 115 | msg.lat = parsing_utilities::convertDMSToDegrees(latitude); 116 | 117 | double longitude = 0.0; 118 | valid = 119 | valid && parsing_utilities::parseDouble(sentence.get_body()[4], longitude); 120 | msg.lon = parsing_utilities::convertDMSToDegrees(longitude); 121 | 122 | msg.lat_dir = sentence.get_body()[3]; 123 | msg.lon_dir = sentence.get_body()[5]; 124 | valid = valid && 125 | parsing_utilities::parseUInt32(sentence.get_body()[6], msg.gps_qual); 126 | valid = valid && 127 | parsing_utilities::parseUInt32(sentence.get_body()[7], msg.num_sats); 128 | // ROS_INFO("Valid is %s so far with number of satellites in use being %s", valid 129 | // ? "true" : "false", sentence.get_body()[7].c_str()); 130 | 131 | valid = valid && parsing_utilities::parseFloat(sentence.get_body()[8], msg.hdop); 132 | valid = valid && parsing_utilities::parseFloat(sentence.get_body()[9], msg.alt); 133 | msg.altitude_units = sentence.get_body()[10]; 134 | valid = valid && 135 | parsing_utilities::parseFloat(sentence.get_body()[11], msg.undulation); 136 | msg.undulation_units = sentence.get_body()[12]; 137 | double diff_age_temp; 138 | valid = valid && 139 | parsing_utilities::parseDouble(sentence.get_body()[13], diff_age_temp); 140 | msg.diff_age = static_cast(round(diff_age_temp)); 141 | msg.station_id = sentence.get_body()[14]; 142 | 143 | if (!valid) 144 | { 145 | was_last_gpgga_valid_ = false; 146 | throw ParseException("GPGGA message was invalid."); 147 | } 148 | 149 | // If we made it this far, we successfully parsed the message and will consider 150 | // it to be valid. 151 | was_last_gpgga_valid_ = true; 152 | 153 | return msg; 154 | } 155 | 156 | bool GpggaParser::wasLastGPGGAValid() const { return was_last_gpgga_valid_; } 157 | -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/parsers/string_utilities.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ROSaic includes 32 | #include 33 | // C++ library includes 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | /** 42 | * @file string_utilities.cpp 43 | * @brief Defines lower-level string utility functions used when parsing messages 44 | * @date 13/08/20 45 | */ 46 | 47 | namespace string_utilities { 48 | /** 49 | * It checks whether an error occurred (via errno) and whether junk characters 50 | * exist within "string", and returns true if the latter two tests are negative 51 | * and the string is non-empty, false otherwise. 52 | */ 53 | [[nodiscard]] bool toDouble(const std::string& string, double& value) 54 | { 55 | if (string.empty()) 56 | { 57 | return false; 58 | } 59 | 60 | char* end; 61 | errno = 0; 62 | 63 | double value_new = std::strtod(string.c_str(), &end); 64 | 65 | if (errno != 0 || end != string.c_str() + string.length()) 66 | { 67 | return false; 68 | } 69 | 70 | value = value_new; 71 | return true; 72 | } 73 | 74 | /** 75 | * It checks whether an error occurred (via errno) and whether junk characters 76 | * exist within "string", and returns true if the latter two tests are negative 77 | * and the string is non-empty, false otherwise. 78 | */ 79 | [[nodiscard]] bool toFloat(const std::string& string, float& value) 80 | { 81 | if (string.empty()) 82 | { 83 | return false; 84 | } 85 | 86 | char* end; 87 | errno = 0; 88 | float value_new = std::strtof(string.c_str(), &end); 89 | 90 | if (errno != 0 || end != string.c_str() + string.length()) 91 | { 92 | return false; 93 | } 94 | 95 | value = value_new; 96 | return true; 97 | } 98 | 99 | /** 100 | * It checks whether an error occurred (via errno) and whether junk characters 101 | * exist within "string", and returns true if the latter two tests are negative 102 | * and the string is non-empty, false otherwise. 103 | */ 104 | [[nodiscard]] bool toInt32(const std::string& string, int32_t& value, 105 | int32_t base) 106 | { 107 | if (string.empty()) 108 | { 109 | return false; 110 | } 111 | 112 | char* end; 113 | errno = 0; 114 | int64_t value_new = std::strtol(string.c_str(), &end, base); 115 | 116 | if (errno != 0 || end != string.c_str() + string.length()) 117 | { 118 | return false; 119 | } 120 | 121 | if (value_new > std::numeric_limits::max() || 122 | value_new < std::numeric_limits::min()) 123 | { 124 | return false; 125 | } 126 | 127 | value = (int32_t)value_new; 128 | return true; 129 | } 130 | 131 | /** 132 | * It checks whether an error occurred (via errno) and whether junk characters 133 | * exist within "string", and returns true if the latter two tests are negative 134 | * and the string is non-empty, false otherwise. 135 | */ 136 | [[nodiscard]] bool toUInt32(const std::string& string, uint32_t& value, 137 | int32_t base) 138 | { 139 | if (string.empty()) 140 | { 141 | return false; 142 | } 143 | 144 | char* end; 145 | errno = 0; 146 | int64_t value_new = std::strtol(string.c_str(), &end, base); 147 | 148 | if (errno != 0 || end != string.c_str() + string.length()) 149 | { 150 | return false; 151 | } 152 | 153 | if (value_new > std::numeric_limits::max() || value_new < 0) 154 | { 155 | return false; 156 | } 157 | 158 | value = (uint32_t)value_new; 159 | return true; 160 | } 161 | 162 | /** 163 | * Not used as of now.. 164 | */ 165 | [[nodiscard]] int8_t toInt8(const std::string& string, int8_t& value, 166 | int32_t base) 167 | { 168 | char* end; 169 | errno = 0; 170 | int64_t value_new = std::strtol(string.c_str(), &end, base); 171 | 172 | value = (int8_t)value_new; 173 | return value; 174 | } 175 | 176 | /** 177 | * Not used as of now.. 178 | */ 179 | [[nodiscard]] uint8_t toUInt8(const std::string& string, uint8_t& value, 180 | int32_t base) 181 | { 182 | char* end; 183 | errno = 0; 184 | int64_t value_new = std::strtol(string.c_str(), &end, base); 185 | 186 | value = (uint8_t)value_new; 187 | return true; 188 | } 189 | 190 | [[nodiscard]] std::string trimDecimalPlaces(double num) 191 | { 192 | num = std::round(num * 1000); 193 | num = num / 1000; 194 | std::stringstream ss; 195 | ss << std::fixed; 196 | ss << std::setprecision(3); 197 | ss << num; 198 | return ss.str(); 199 | } 200 | 201 | [[nodiscard]] bool containsSpace(const std::string str) 202 | { 203 | for (size_t i = 0; i < str.size(); ++i) 204 | { 205 | if (std::isspace(str[i])) 206 | return true; 207 | } 208 | return false; 209 | } 210 | } // namespace string_utilities 211 | -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | 33 | /** 34 | * @file gprmc.cpp 35 | * @brief Derived class for parsing RMC messages 36 | * @date 28/09/20 37 | */ 38 | 39 | const std::string GprmcParser::MESSAGE_ID = "$GPRMC"; 40 | 41 | const std::string GprmcParser::getMessageID() const 42 | { 43 | return GprmcParser::MESSAGE_ID; 44 | } 45 | 46 | /** 47 | * Caution: Due to the occurrence of the throw keyword, this method ParseASCII should 48 | * be called within a try / catch framework... Note: This method is called from 49 | * within the read() method of the RxMessage class by including the checksum part in 50 | * the argument "sentence" here, though the checksum is never parsed: It would be 51 | * sentence.get_body()[13] if anybody ever needs it. The status character can be 'A' 52 | * (for Active) or 'V' (for Void), signaling whether the GPS was active when the 53 | * positioning was made. If it is void, the GPS could not make a good positioning and 54 | * you should thus ignore it. This usually occurs when the GPS is still searching for 55 | * satellites. WasLastGPRMCValid() will return false in this case. 56 | */ 57 | GprmcMsg GprmcParser::parseASCII(const NMEASentence& sentence, 58 | const std::string& frame_id, bool use_gnss_time, 59 | Timestamp time_obj) noexcept(false) 60 | { 61 | 62 | // Checking the length first, it should be between 13 and 14 elements 63 | const size_t LEN_MIN = 13; 64 | const size_t LEN_MAX = 14; 65 | 66 | if (sentence.get_body().size() > LEN_MAX || sentence.get_body().size() < LEN_MIN) 67 | { 68 | std::stringstream error; 69 | error << "Expected GPRMC length is between " << LEN_MIN << " and " << LEN_MAX 70 | << ". The actual length is " << sentence.get_body().size(); 71 | throw ParseException(error.str()); 72 | } 73 | 74 | GprmcMsg msg; 75 | 76 | msg.header.frame_id = frame_id; 77 | 78 | msg.message_id = sentence.get_body()[0]; 79 | 80 | if (sentence.get_body()[1].empty() || sentence.get_body()[1] == "0") 81 | { 82 | msg.utc_seconds = 0; 83 | } else 84 | { 85 | double utc_double; 86 | if (string_utilities::toDouble(sentence.get_body()[1], utc_double)) 87 | { 88 | msg.utc_seconds = 89 | parsing_utilities::convertUTCDoubleToSeconds(utc_double); 90 | if (use_gnss_time) 91 | { 92 | // The Header's Unix Epoch time stamp 93 | time_t unix_time_seconds = 94 | parsing_utilities::convertUTCtoUnix(utc_double); 95 | // The following assumes that there are two digits after the decimal 96 | // point in utc_double, i.e. in the NMEA UTC time. 97 | Timestamp unix_time_nanoseconds = 98 | unix_time_seconds * 1000000000 + 99 | (static_cast(utc_double * 100) % 100) * 10000; 100 | msg.header.stamp = timestampToRos(unix_time_nanoseconds); 101 | } else 102 | { 103 | msg.header.stamp = timestampToRos(time_obj); 104 | } 105 | } else 106 | { 107 | throw ParseException( 108 | "Error parsing UTC seconds in GPRMC"); // E.g. if one of the fields 109 | // of the NMEA UTC string is 110 | // empty 111 | } 112 | } 113 | bool valid = true; 114 | bool to_be_ignored = false; 115 | 116 | msg.position_status = sentence.get_body()[2]; 117 | // Check to see whether this message should be ignored 118 | to_be_ignored &= !(sentence.get_body()[2].compare("A") == 119 | 0); // 0 : if both strings are equal. 120 | to_be_ignored &= 121 | (sentence.get_body()[3].empty() || sentence.get_body()[5].empty()); 122 | 123 | double latitude = 0.0; 124 | valid = 125 | valid && parsing_utilities::parseDouble(sentence.get_body()[3], latitude); 126 | msg.lat = parsing_utilities::convertDMSToDegrees(latitude); 127 | 128 | double longitude = 0.0; 129 | valid = 130 | valid && parsing_utilities::parseDouble(sentence.get_body()[5], longitude); 131 | msg.lon = parsing_utilities::convertDMSToDegrees(longitude); 132 | 133 | msg.lat_dir = sentence.get_body()[4]; 134 | msg.lon_dir = sentence.get_body()[6]; 135 | 136 | valid = 137 | valid && parsing_utilities::parseFloat(sentence.get_body()[7], msg.speed); 138 | msg.speed *= KNOTS_TO_MPS; 139 | 140 | valid = 141 | valid && parsing_utilities::parseFloat(sentence.get_body()[8], msg.track); 142 | 143 | std::string date_str = sentence.get_body()[9]; 144 | if (!date_str.empty()) 145 | { 146 | msg.date = std::string("20") + date_str.substr(4, 2) + std::string("-") + 147 | date_str.substr(2, 2) + std::string("-") + date_str.substr(0, 2); 148 | } 149 | valid = 150 | valid && parsing_utilities::parseFloat(sentence.get_body()[10], msg.mag_var); 151 | msg.mag_var_direction = sentence.get_body()[11]; 152 | if (sentence.get_body().size() == LEN_MAX) 153 | { 154 | msg.mode_indicator = sentence.get_body()[12]; 155 | } 156 | 157 | if (!valid) 158 | { 159 | was_last_gprmc_valid_ = false; 160 | throw ParseException("Error parsing GPRMC message."); 161 | } 162 | 163 | was_last_gprmc_valid_ = !to_be_ignored; 164 | 165 | return msg; 166 | } 167 | 168 | bool GprmcParser::wasLastGPRMCValid() const { return was_last_gprmc_valid_; } 169 | -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/communication/communication_core.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ***************************************************************************** 32 | // 33 | // Boost Software License - Version 1.0 - August 17th, 2003 34 | // 35 | // Permission is hereby granted, free of charge, to any person or organization 36 | // obtaining a copy of the software and accompanying documentation covered by 37 | // this license (the "Software") to use, reproduce, display, distribute, 38 | // execute, and transmit the Software, and to prepare derivative works of the 39 | // Software, and to permit third-parties to whom the Software is furnished to 40 | // do so, all subject to the following: 41 | 42 | // The copyright notices in the Software and this entire statement, including 43 | // the above license grant, this restriction and the following disclaimer, 44 | // must be included in all copies of the Software, in whole or in part, and 45 | // all derivative works of the Software, unless such copies or derivative 46 | // works are solely in the form of machine-executable object code generated by 47 | // a source language processor. 48 | // 49 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 50 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 51 | // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 52 | // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 53 | // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 54 | // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 55 | // DEALINGS IN THE SOFTWARE. 56 | // 57 | // ***************************************************************************** 58 | 59 | #pragma once 60 | 61 | // Boost includes 62 | #include 63 | #include 64 | // C++ library includes 65 | #include 66 | #include 67 | #include 68 | // ROSaic includes 69 | #include 70 | #include 71 | 72 | /** 73 | * @file communication_core.hpp 74 | * @date 22/08/20 75 | * @brief Highest-Level view on communication services 76 | */ 77 | 78 | /** 79 | * @namespace io 80 | * This namespace is for the communication interface, handling all aspects related to 81 | * serial and TCP/IP communication.. 82 | */ 83 | namespace io { 84 | 85 | //! Possible baudrates for the Rx 86 | const static uint32_t BAUDRATES[] = { 87 | 1200, 2400, 4800, 9600, 19200, 38400, 57600, 88 | 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 89 | 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; 90 | 91 | /** 92 | * @class CommunicationCore 93 | * @brief Handles communication with and configuration of the mosaic (and beyond) 94 | * receiver(s) 95 | */ 96 | class CommunicationCore 97 | { 98 | public: 99 | /** 100 | * @brief Constructor of the class CommunicationCore 101 | * @param[in] node Pointer to node 102 | */ 103 | CommunicationCore(ROSaicNodeBase* node); 104 | /** 105 | * @brief Default destructor of the class CommunicationCore 106 | */ 107 | ~CommunicationCore(); 108 | 109 | void close(); 110 | 111 | /** 112 | * @brief Connects the data stream 113 | */ 114 | void connect(); 115 | 116 | /** 117 | * @brief Configures Rx: Which SBF/NMEA messages it should output and later 118 | * correction settings 119 | * */ 120 | void configureRx(); 121 | 122 | /** 123 | * @brief Hands over NMEA velocity message over to the send() method of 124 | * manager_ 125 | * @param cmd The command to hand over 126 | */ 127 | void sendVelocity(const std::string& velNmea); 128 | 129 | private: 130 | /** 131 | * @brief Resets Rx settings 132 | */ 133 | void resetSettings(); 134 | 135 | /** 136 | * @brief Initializes the I/O handling 137 | * * @return Wether connection was successful 138 | */ 139 | [[nodiscard]] bool initializeIo(); 140 | 141 | /** 142 | * @brief Reset main connection so it can receive commands 143 | * @return Main connection descriptor 144 | */ 145 | std::string resetMainConnection(); 146 | 147 | void processTelegrams(); 148 | 149 | /** 150 | * @brief Hands over to the send() method of manager_ 151 | * @param cmd The command to hand over 152 | */ 153 | void send(const std::string& cmd); 154 | 155 | //! Pointer to Node 156 | ROSaicNodeBase* node_; 157 | //! Settings 158 | const Settings* settings_; 159 | //! TelegramQueue 160 | TelegramQueue telegramQueue_; 161 | //! TelegramHandler 162 | TelegramHandler telegramHandler_; 163 | //! Processing thread 164 | std::thread processingThread_; 165 | //! Whether connecting was successful 166 | bool initializedIo_ = false; 167 | //! Processes I/O stream data 168 | //! This declaration is deliberately stream-independent (Serial or TCP). 169 | std::unique_ptr manager_; 170 | 171 | std::unique_ptr> tcpClient_; 172 | std::unique_ptr udpClient_; 173 | 174 | std::unique_ptr> tcpVsm_; 175 | 176 | bool nmeaActivated_ = false; 177 | 178 | //! Indicator for threads to run 179 | std::atomic running_; 180 | 181 | //! Main communication port 182 | std::string mainConnectionPort_; 183 | // Port for receiving data streams 184 | std::string streamPort_; 185 | }; 186 | } // namespace io 187 | -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/communication/telegram.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | // C++ 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | // ROSaic 41 | #ifdef ROS2 42 | #include 43 | #endif 44 | #ifdef ROS1 45 | #include 46 | #endif 47 | 48 | //! 0x24 is ASCII for $ - 1st byte in each message 49 | static const uint8_t SYNC_BYTE_1 = 0x24; 50 | //! 0x40 is ASCII for @ - 2nd byte to indicate SBF block 51 | static const uint8_t SBF_SYNC_BYTE_2 = 0x40; 52 | //! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message 53 | static const uint8_t NMEA_SYNC_BYTE_2 = 0x47; 54 | //! 0x50 is ASCII for P - 3rd byte to indicate NMEA-type ASCII message 55 | static const uint8_t NMEA_SYNC_BYTE_3 = 0x50; 56 | //! 0x49 is ASCII for I - 2nd byte to indicate INS NMEA-type ASCII message 57 | static const uint8_t NMEA_INS_SYNC_BYTE_2 = 0x49; 58 | //! 0x4E is ASCII for N - 3rd byte to indicate NMEA-type ASCII message 59 | static const uint8_t NMEA_INS_SYNC_BYTE_3 = 0x4E; 60 | //! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx 61 | static const uint8_t RESPONSE_SYNC_BYTE_2 = 0x52; 62 | //! 0x3A is ASCII for : - 3rd byte in the response message from the Rx 63 | static const uint8_t RESPONSE_SYNC_BYTE_3 = 0x3A; 64 | //! 0x21 is ASCII for ! 3rd byte in the response message from the Rx 65 | static const uint8_t RESPONSE_SYNC_BYTE_3a = 0x21; 66 | //! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the 67 | //! command was invalid 68 | static const uint8_t ERROR_SYNC_BYTE_3 = 0x3F; 69 | //! 0x0D is ASCII for "Carriage Return", i.e. "Enter" 70 | static const uint8_t CR = 0x0D; 71 | //! 0x0A is ASCII for "Line Feed", i.e. "New Line" 72 | static const uint8_t LF = 0x0A; 73 | //! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after 74 | //! initiating IP connection 75 | static const uint8_t CONNECTION_DESCRIPTOR_BYTE_I = 0x49; 76 | //! 0x43 is ASCII for C - 1st character of connection descriptor sent by the Rx after 77 | //! initiating COM connection 78 | static const uint8_t CONNECTION_DESCRIPTOR_BYTE_C = 0x43; 79 | //! 0x55 is ASCII for U - 1st character of connection descriptor sent by the Rx after 80 | //! initiating USB connection 81 | static const uint8_t CONNECTION_DESCRIPTOR_BYTE_U = 0x55; 82 | //! 0x4E is ASCII for N - 1st character of connection descriptor sent by the Rx after 83 | //! initiating NTRIP connection 84 | static const uint8_t CONNECTION_DESCRIPTOR_BYTE_N = 0x4E; 85 | //! 0x44 is ASCII for D - 1st character of connection descriptor sent by the Rx after 86 | //! initiating DSK connection 87 | static const uint8_t CONNECTION_DESCRIPTOR_BYTE_D = 0x44; 88 | //! 0x3E is ASCII for > - end character of connection descriptor 89 | static const uint8_t CONNECTION_DESCRIPTOR_FOOTER = 0x3E; 90 | 91 | static const uint16_t SBF_HEADER_SIZE = 8; 92 | static const uint16_t MAX_SBF_SIZE = 65535; 93 | static const uint16_t MAX_UDP_PACKET_SIZE = 65535; 94 | 95 | namespace telegram_type { 96 | enum TelegramType 97 | { 98 | EMPTY, 99 | SBF, 100 | NMEA, 101 | NMEA_INS, 102 | RESPONSE, 103 | ERROR_RESPONSE, 104 | CONNECTION_DESCRIPTOR, 105 | UNKNOWN 106 | }; 107 | } 108 | 109 | struct Telegram 110 | { 111 | Timestamp stamp; 112 | telegram_type::TelegramType type; 113 | std::vector message; 114 | 115 | Telegram(size_t preallocate = 3) noexcept : 116 | stamp(0), type(telegram_type::EMPTY), 117 | message(std::vector(preallocate)) 118 | { 119 | } 120 | 121 | ~Telegram() {} 122 | 123 | Telegram(const Telegram& other) noexcept : 124 | stamp(other.stamp), type(other.type), message(other.message) 125 | { 126 | } 127 | 128 | Telegram(Telegram&& other) noexcept : 129 | stamp(other.stamp), type(other.type), message(other.message) 130 | { 131 | } 132 | 133 | Telegram& operator=(const Telegram& other) noexcept 134 | { 135 | if (this != &other) 136 | { 137 | this->stamp = other.stamp; 138 | this->type = other.type; 139 | this->message = other.message; 140 | } 141 | return *this; 142 | } 143 | 144 | Telegram& operator=(Telegram&& other) noexcept 145 | { 146 | if (this != &other) 147 | { 148 | this->stamp = other.stamp; 149 | this->type = other.type; 150 | this->message = other.message; 151 | } 152 | return *this; 153 | } 154 | }; 155 | 156 | template 157 | class ConcurrentQueue 158 | { 159 | public: 160 | [[nodiscard]] bool empty() const noexcept; 161 | [[nodiscard]] size_t size() const noexcept; 162 | void push(const T& input) noexcept; 163 | void pop(T& output) noexcept; 164 | 165 | private: 166 | std::queue queue_; 167 | std::condition_variable cond_; 168 | mutable std::mutex mtx_; 169 | }; 170 | 171 | template 172 | [[nodiscard]] bool ConcurrentQueue::empty() const noexcept 173 | { 174 | std::lock_guard lck(mtx_); 175 | return queue_.empty(); 176 | } 177 | 178 | template 179 | [[nodiscard]] size_t ConcurrentQueue::size() const noexcept 180 | { 181 | std::lock_guard lck(mtx_); 182 | return queue_.size(); 183 | } 184 | 185 | template 186 | void ConcurrentQueue::push(const T& input) noexcept 187 | { 188 | { 189 | std::lock_guard lck(mtx_); 190 | queue_.push(input); 191 | } 192 | cond_.notify_one(); 193 | } 194 | 195 | template 196 | void ConcurrentQueue::pop(T& output) noexcept 197 | { 198 | std::unique_lock lck(mtx_); 199 | cond_.wait(lck, [this] { return !queue_.empty(); }); 200 | output = queue_.front(); 201 | queue_.pop(); 202 | } 203 | 204 | typedef ConcurrentQueue> TelegramQueue; -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #include 32 | 33 | /** 34 | * @file gpgsv.cpp 35 | * @brief Derived class for parsing GSV messages 36 | * @date 29/09/20 37 | */ 38 | 39 | const std::string GpgsvParser::MESSAGE_ID = "$GPGSV"; 40 | 41 | const std::string GpgsvParser::getMessageID() const 42 | { 43 | return GpgsvParser::MESSAGE_ID; 44 | } 45 | 46 | /** 47 | * Caution: Due to the occurrence of the throw keyword, this method parseASCII should 48 | * be called within a try / catch framework... Note: This method is called from 49 | * within the read() method of the RxMessage class by including the checksum part in 50 | * the argument "sentence" here, though the checksum is never parsed: E.g. for 51 | * message with 4 Svs it would be sentence.get_body()[20] if anybody ever needs it. 52 | */ 53 | GpgsvMsg GpgsvParser::parseASCII(const NMEASentence& sentence, 54 | const std::string& frame_id, bool /*use_gnss_time*/, 55 | Timestamp /*time_obj*/) noexcept(false) 56 | { 57 | 58 | const size_t MIN_LENGTH = 4; 59 | // Checking that the message is at least as long as a GPGSV with no satellites 60 | if (sentence.get_body().size() < MIN_LENGTH) 61 | { 62 | std::stringstream error; 63 | error << "Expected GSV length is at least " << MIN_LENGTH 64 | << ". The actual length is " << sentence.get_body().size(); 65 | throw ParseException(error.str()); 66 | } 67 | GpgsvMsg msg; 68 | msg.header.frame_id = frame_id; 69 | msg.message_id = sentence.get_body()[0]; 70 | if (!parsing_utilities::parseUInt8(sentence.get_body()[1], msg.n_msgs)) 71 | { 72 | throw ParseException("Error parsing n_msgs in GSV."); 73 | } 74 | if (msg.n_msgs > 75 | 9) // Checking that the number of messages is smaller or equal to 9 76 | { 77 | std::stringstream error; 78 | error << "n_msgs in GSV is too large: " << msg.n_msgs << "."; 79 | throw ParseException(error.str()); 80 | } 81 | 82 | if (!parsing_utilities::parseUInt8(sentence.get_body()[2], msg.msg_number)) 83 | { 84 | throw ParseException("Error parsing msg_number in GSV."); 85 | } 86 | if (msg.msg_number > 87 | msg.n_msgs) // Checking that this message is within the sequence range 88 | { 89 | std::stringstream error; 90 | error << "msg_number in GSV is larger than n_msgs: " << msg.msg_number 91 | << " > " << msg.n_msgs << "."; 92 | throw ParseException(error.str()); 93 | } 94 | if (!parsing_utilities::parseUInt8(sentence.get_body()[3], msg.n_satellites)) 95 | { 96 | throw ParseException("Error parsing n_satellites in GSV."); 97 | } 98 | // Figuring out how many satellites should be described in this sentence 99 | size_t n_sats_in_sentence = 4; 100 | if (msg.msg_number == msg.n_msgs) 101 | { 102 | n_sats_in_sentence = msg.n_satellites % static_cast(4); 103 | if (msg.n_satellites % static_cast(4) == 0) 104 | { 105 | n_sats_in_sentence = 4; 106 | } 107 | if (msg.n_satellites == 0) 108 | { 109 | n_sats_in_sentence = 0; 110 | } 111 | if (msg.msg_number == 1) 112 | { 113 | n_sats_in_sentence = msg.n_satellites; 114 | } 115 | } 116 | // Checking that the sentence is the right length for the number of satellites 117 | size_t expected_length = MIN_LENGTH + 4 * n_sats_in_sentence + 1; 118 | // Note that we add +1 due to the checksum data being part of the argument 119 | // "sentence". 120 | if (n_sats_in_sentence == 0) 121 | { 122 | // Even if the number of sats is 0, the message will still have enough 123 | // blank fields for 1 satellite. 124 | expected_length += 4; 125 | } 126 | // ROS_DEBUG("number of sats is %u but nsats in sentence if msg_number = max is 127 | // %u and msg.msg_number == msg.n_msgs is %s and nsats in sentence is %li", 128 | // msg.n_satellites, msg.n_satellites % static_cast(4), 129 | // msg.msg_number 130 | // == msg.n_msgs ? "true" : "false", n_sats_in_sentence); 131 | if (sentence.get_body().size() != expected_length && 132 | sentence.get_body().size() != expected_length - 1) 133 | { 134 | std::stringstream ss; 135 | for (size_t i = 0; i < sentence.get_body().size(); ++i) 136 | { 137 | ss << sentence.get_body()[i]; 138 | if ((i + 1) < sentence.get_body().size()) 139 | { 140 | ss << ","; 141 | } 142 | } 143 | std::stringstream error; 144 | error << "Expected GSV length is " << expected_length << " for message with " 145 | << n_sats_in_sentence << " satellites. The actual length is " 146 | << sentence.get_body().size() << ".\n" 147 | << ss.str().c_str(); 148 | throw ParseException(error.str()); 149 | } 150 | 151 | // Parsing information about n_sats_in_sentence SVs.. 152 | msg.satellites.resize(n_sats_in_sentence); 153 | for (size_t sat = 0, index = MIN_LENGTH; sat < n_sats_in_sentence; 154 | ++sat, index += 4) 155 | { 156 | if (!parsing_utilities::parseUInt8(sentence.get_body()[index], 157 | msg.satellites[sat].prn)) 158 | { 159 | std::stringstream error; 160 | error << "Error parsing PRN for satellite " << sat << " in GSV."; 161 | throw ParseException(error.str()); 162 | } 163 | float elevation; 164 | if (!parsing_utilities::parseFloat(sentence.get_body()[index + 1], 165 | elevation)) 166 | { 167 | std::stringstream error; 168 | error << "Error parsing elevation for satellite " << sat << " in GSV."; 169 | throw ParseException(error.str()); 170 | } 171 | msg.satellites[sat].elevation = static_cast(elevation); 172 | 173 | float azimuth; 174 | if (!parsing_utilities::parseFloat(sentence.get_body()[index + 2], azimuth)) 175 | { 176 | std::stringstream error; 177 | error << "Error parsing azimuth for satellite " << sat << " in GSV."; 178 | throw ParseException(error.str()); 179 | } 180 | msg.satellites[sat].azimuth = static_cast(azimuth); 181 | 182 | if ((index + 3) >= sentence.get_body().size() || 183 | sentence.get_body()[index + 3].empty()) 184 | { 185 | msg.satellites[sat].snr = -1; 186 | } else 187 | { 188 | uint8_t snr; 189 | if (!parsing_utilities::parseUInt8(sentence.get_body()[index + 3], snr)) 190 | { 191 | std::stringstream error; 192 | error << "Error parsing snr for satellite " << sat << " in GSV."; 193 | throw ParseException(error.str()); 194 | } 195 | msg.satellites[sat].snr = static_cast(snr); 196 | } 197 | } 198 | return msg; 199 | } -------------------------------------------------------------------------------- /include/septentrio_gnss_driver/communication/settings_helpers.hpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, node list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, node list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from node software without specific prior written permission. 16 | // 17 | // node SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF node SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | #pragma once 32 | 33 | #include "settings.hpp" 34 | #ifdef ROS1 35 | #include 36 | #endif 37 | #ifdef ROS2 38 | #include 39 | #endif 40 | 41 | namespace settings { 42 | 43 | // Check uniqueness of IPS ids 44 | void checkUniquenssOfIps(ROSaicNodeBase* node, const Settings& settings) 45 | { 46 | if (!settings.tcp_ip_server.empty()) 47 | { 48 | if (settings.tcp_ip_server == settings.udp_ip_server) 49 | node->log( 50 | log_level::ERROR, 51 | "stream_device.tcp.ip_server and stream_device.udp.ip_server cannot use the same IP server"); 52 | for (size_t i = 0; i < settings.rtk.ip_server.size(); ++i) 53 | { 54 | if (settings.tcp_ip_server == settings.rtk.ip_server[i].id) 55 | node->log( 56 | log_level::ERROR, 57 | "stream_device.tcp.ip_server and rtk_settings.ip_server_" + 58 | std::to_string(i + 1) + 59 | ".id cannot use the same IP server"); 60 | } 61 | } 62 | if (!settings.udp_ip_server.empty()) 63 | { 64 | for (size_t i = 0; i < settings.rtk.ip_server.size(); ++i) 65 | { 66 | if (settings.udp_ip_server == settings.rtk.ip_server[i].id) 67 | node->log( 68 | log_level::ERROR, 69 | "stream_device.udp.ip_server and rtk_settings.ip_server_" + 70 | std::to_string(i + 1) + 71 | ".id cannot use the same IP server"); 72 | } 73 | } 74 | if (settings.rtk.ip_server.size() == 2) 75 | { 76 | if (!settings.rtk.ip_server[0].id.empty() && 77 | (settings.rtk.ip_server[0].id == settings.rtk.ip_server[1].id)) 78 | node->log( 79 | log_level::ERROR, 80 | "rtk_settings.ip_server_1.id and rtk_settings.ip_server_2.id cannot use the same IP server"); 81 | } 82 | } 83 | 84 | // Check uniqueness of IPS ports 85 | void checkUniquenssOfIpsPorts(ROSaicNodeBase* node, const Settings& settings) 86 | { 87 | if (settings.tcp_port != 0) 88 | { 89 | if (std::to_string(settings.tcp_port) == settings.device_tcp_port) 90 | node->log( 91 | log_level::ERROR, 92 | "stream_device.tcp.port and device port cannot be the same"); 93 | for (size_t i = 0; i < settings.rtk.ip_server.size(); ++i) 94 | { 95 | if (settings.tcp_port == settings.rtk.ip_server[i].port) 96 | node->log(log_level::ERROR, 97 | "stream_device.tcp.port and rtk_settings.ip_server_" + 98 | std::to_string(i + 1) + 99 | ".port cannot be the same!"); 100 | } 101 | } 102 | if (settings.rtk.ip_server.size() == 2) 103 | { 104 | if ((settings.rtk.ip_server[0].port != 0) && 105 | (settings.rtk.ip_server[0].port == settings.rtk.ip_server[1].port)) 106 | node->log( 107 | log_level::ERROR, 108 | "rtk_settings.ip_server_1.port and rtk_settings.ip_server_2.port cannot be the same"); 109 | } 110 | } 111 | 112 | // Check uniqueness of IPS id for VSM 113 | void checkUniquenssOfIpsVsm(ROSaicNodeBase* node, const Settings& settings) 114 | { 115 | if (!settings.ins_vsm.ip_server.empty()) 116 | { 117 | if (!settings.tcp_ip_server.empty() && 118 | (settings.tcp_ip_server == settings.ins_vsm.ip_server)) 119 | node->log( 120 | log_level::ERROR, 121 | "stream_device.tcp.ip_server and ins_vsm.ip_server.id cannot use the same IP server"); 122 | if (!settings.udp_ip_server.empty() && 123 | (settings.udp_ip_server == settings.ins_vsm.ip_server)) 124 | node->log( 125 | log_level::ERROR, 126 | "stream_device.udp.ip_server and ins_vsm.ip_server.id cannot use the same IP server"); 127 | for (size_t i = 0; i < settings.rtk.ip_server.size(); ++i) 128 | { 129 | if (settings.ins_vsm.ip_server == settings.rtk.ip_server[i].id) 130 | node->log(log_level::ERROR, 131 | "ins_vsm.ip_server.id and rtk_settings.ip_server_" + 132 | std::to_string(i + 1) + 133 | ".id cannot use the same IP server"); 134 | } 135 | } 136 | } 137 | 138 | // Check uniqueness of IPS port for VSM 139 | void checkUniquenssOfIpsPortsVsm(ROSaicNodeBase* node, const Settings& settings) 140 | { 141 | if (settings.ins_vsm.ip_server_port != 0) 142 | { 143 | if (std::to_string(settings.ins_vsm.ip_server_port) == 144 | settings.device_tcp_port) 145 | node->log( 146 | log_level::ERROR, 147 | "device port and ins_vsm.ip_server.port cannot be the same"); 148 | if ((settings.tcp_port != 0) && 149 | (settings.tcp_port == settings.ins_vsm.ip_server_port)) 150 | node->log( 151 | log_level::ERROR, 152 | "stream_device.tcp.port and ins_vsm.ip_server.port cannot be the same"); 153 | if ((settings.udp_port != 0) && 154 | (settings.udp_port == settings.ins_vsm.ip_server_port)) 155 | node->log( 156 | log_level::ERROR, 157 | "stream_device.udp.port and ins_vsm.ip_server.port cannot be the same"); 158 | for (size_t i = 0; i < settings.rtk.ip_server.size(); ++i) 159 | { 160 | if (settings.ins_vsm.ip_server_port == 161 | settings.rtk.ip_server[i].port) 162 | node->log(log_level::ERROR, 163 | "ins_vsm.ip_server.port and rtk_settings.ip_server_" + 164 | std::to_string(i + 1) + 165 | ".port cannot use be same"); 166 | } 167 | } 168 | } 169 | 170 | void autoPublish(ROSaicNodeBase* node, Settings& settings) 171 | { 172 | if (settings.auto_publish && !settings.configure_rx) 173 | { 174 | settings.publish_gpst = true; 175 | settings.publish_navsatfix = true; 176 | settings.publish_gpsfix = true; 177 | settings.publish_pose = true; 178 | settings.publish_diagnostics = true; 179 | settings.publish_aimplusstatus = true; 180 | settings.publish_galauthstatus = true; 181 | settings.publish_gpgga = true; 182 | settings.publish_gprmc = true; 183 | settings.publish_gpgsa = true; 184 | settings.publish_gpgsv = true; 185 | settings.publish_measepoch = true; 186 | settings.publish_pvtcartesian = true; 187 | settings.publish_pvtgeodetic = true; 188 | settings.publish_basevectorcart = true; 189 | settings.publish_basevectorgeod = true; 190 | settings.publish_poscovcartesian = true; 191 | settings.publish_poscovgeodetic = true; 192 | settings.publish_velcovcartesian = true; 193 | settings.publish_velcovgeodetic = true; 194 | settings.publish_atteuler = true; 195 | settings.publish_attcoveuler = true; 196 | settings.publish_insnavcart = true; 197 | settings.publish_insnavgeod = true; 198 | settings.publish_imusetup = true; 199 | settings.publish_velsensorsetup = true; 200 | settings.publish_exteventinsnavgeod = true; 201 | settings.publish_exteventinsnavcart = true; 202 | settings.publish_extsensormeas = true; 203 | settings.publish_imu = true; 204 | settings.publish_localization = true; 205 | settings.publish_localization_ecef = true; 206 | settings.publish_twist = true; 207 | if (!settings.publish_tf_ecef) 208 | settings.publish_tf = true; 209 | } else if (settings.auto_publish && settings.configure_rx) 210 | { 211 | node->log(log_level::WARN, 212 | "auto_publish has no effect if configure_rx is true."); 213 | } 214 | } 215 | 216 | } // namespace settings -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(septentrio_gnss_driver) 3 | 4 | ## Compile as C++17 5 | add_compile_options(-std=c++17) 6 | 7 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 8 | message(STATUS "Setting build type to Release as none was specified.") 9 | set(CMAKE_BUILD_TYPE "Release" CACHE 10 | STRING "Choose the type of build." FORCE) 11 | # Set the possible values of build type for cmake-gui 12 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS 13 | "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 14 | endif() 15 | 16 | set(WARNFLAGS "-Wall -Wextra -Wpedantic -Wcast-align -Wnon-virtual-dtor -Woverloaded-virtual -Wcast-qual -Wctor-dtor-privacy") 17 | 18 | ## System dependencies are found with CMake's conventions 19 | find_package(Boost REQUIRED) 20 | LIST(APPEND CMAKE_MODULE_PATH "/usr/share/cmake/geographiclib") 21 | find_package(GeographicLib REQUIRED) 22 | 23 | ## For PCAP file handling 24 | find_library(libpcap_LIBRARIES pcap) 25 | if ("${libpcap_LIBRARIES}" STREQUAL "pcap-NOTFOUND") 26 | set(libpcap_FOUND FALSE) 27 | else () 28 | set(libpcap_FOUND TRUE) 29 | endif () 30 | 31 | find_package(ament_cmake QUIET) 32 | find_package(catkin QUIET) 33 | 34 | if(catkin_FOUND) 35 | ## ROS 1 ----------------------------------------------------------------------------------------------- 36 | set(ENV{ROS_VERSION} "1") 37 | 38 | ## Dependencies 39 | find_package(catkin REQUIRED COMPONENTS 40 | roscpp 41 | nmea_msgs 42 | sensor_msgs 43 | geometry_msgs 44 | nav_msgs 45 | diagnostic_msgs 46 | gps_common 47 | message_generation 48 | tf2 49 | tf2_eigen 50 | tf2_geometry_msgs 51 | tf2_ros 52 | ) 53 | 54 | ## Declare messages 55 | add_message_files( 56 | FILES 57 | AIMPlusStatus.msg 58 | BaseVectorCart.msg 59 | BaseVectorGeod.msg 60 | BlockHeader.msg 61 | GALAuthStatus.msg 62 | RFBand.msg 63 | RFStatus.msg 64 | MeasEpoch.msg 65 | MeasEpochChannelType1.msg 66 | MeasEpochChannelType2.msg 67 | PVTCartesian.msg 68 | PVTGeodetic.msg 69 | PosCovCartesian.msg 70 | PosCovGeodetic.msg 71 | ReceiverTime.msg 72 | VelCovCartesian.msg 73 | VelCovGeodetic.msg 74 | AttEuler.msg 75 | AttCovEuler.msg 76 | INSNavCart.msg 77 | INSNavGeod.msg 78 | IMUSetup.msg 79 | VectorInfoCart.msg 80 | VectorInfoGeod.msg 81 | VelSensorSetup.msg 82 | ExtSensorMeas.msg 83 | ) 84 | ## Generate messages 85 | generate_messages( 86 | DEPENDENCIES 87 | std_msgs 88 | sensor_msgs 89 | diagnostic_msgs 90 | gps_common 91 | ) 92 | 93 | # Catkin package 94 | catkin_package( 95 | INCLUDE_DIRS include 96 | LIBRARIES 97 | CATKIN_DEPENDS roscpp message_runtime 98 | DEPENDS Boost 99 | ) 100 | 101 | # Build 102 | include_directories( 103 | include 104 | ${catkin_INCLUDE_DIRS} 105 | ${Boost_INCLUDE_DIRS} 106 | ${GeographicLib_INCLUDE_DIRS} 107 | ) 108 | add_executable(${PROJECT_NAME}_node 109 | src/septentrio_gnss_driver/communication/communication_core.cpp 110 | src/septentrio_gnss_driver/communication/message_handler.cpp 111 | src/septentrio_gnss_driver/communication/telegram_handler.cpp 112 | src/septentrio_gnss_driver/crc/crc.cpp 113 | src/septentrio_gnss_driver/node/rosaic_node_ros1.cpp 114 | src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp 115 | src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp 116 | src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp 117 | src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp 118 | src/septentrio_gnss_driver/parsers/parsing_utilities.cpp 119 | src/septentrio_gnss_driver/parsers/string_utilities.cpp 120 | ) 121 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | target_link_libraries(${PROJECT_NAME}_node 123 | ${catkin_LIBRARIES} 124 | boost_regex # ordering is important for regex https://stackoverflow.com/questions/17588440/cannot-link-boost-regex 125 | ${Boost_LIBRARIES} 126 | ${libpcap_LIBRARIES} 127 | ${GeographicLib_LIBRARIES} 128 | ) 129 | target_compile_definitions(${PROJECT_NAME}_node PUBLIC ROS1) 130 | 131 | # Installation 132 | install(TARGETS ${PROJECT_NAME}_node 133 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 134 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 135 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 136 | ) 137 | install(DIRECTORY include/${PROJECT_NAME}/ 138 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 139 | ) 140 | install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 141 | #------------------------------------------------------------------------------------------------------- 142 | elseif(ament_cmake_FOUND) 143 | ## ROS 2 ----------------------------------------------------------------------------------------------- 144 | set(ENV{ROS_VERSION} "2") 145 | 146 | unset(CATKIN_INSTALL_INTO_PREFIX_ROOT) 147 | unset(CATKIN_SYMLINK_INSTALL) 148 | 149 | # Dependencies 150 | find_package(ament_cmake REQUIRED) 151 | find_package(rclcpp REQUIRED) 152 | find_package(rclcpp_components REQUIRED) 153 | find_package(rosidl_default_generators REQUIRED) 154 | find_package(diagnostic_msgs REQUIRED) 155 | find_package(nmea_msgs REQUIRED) 156 | find_package(sensor_msgs REQUIRED) 157 | find_package(geometry_msgs REQUIRED) 158 | find_package(gps_msgs REQUIRED) 159 | find_package(nav_msgs REQUIRED) 160 | find_package(tf2 REQUIRED) 161 | find_package(tf2_eigen REQUIRED) 162 | find_package(tf2_geometry_msgs REQUIRED) 163 | find_package(tf2_ros REQUIRED) 164 | find_package(gtest_vendor REQUIRED) 165 | find_package(ament_cmake_ros REQUIRED) 166 | 167 | ## Declare messages 168 | rosidl_generate_interfaces(${PROJECT_NAME} 169 | "msg/AIMPlusStatus.msg" 170 | "msg/BaseVectorCart.msg" 171 | "msg/BaseVectorGeod.msg" 172 | "${CMAKE_CURRENT_SOURCE_DIR}/msg_ros2:msg/BlockHeader.msg" 173 | "msg/GALAuthStatus.msg" 174 | "msg/RFBand.msg" 175 | "msg/RFStatus.msg" 176 | "msg/MeasEpoch.msg" 177 | "msg/MeasEpochChannelType1.msg" 178 | "msg/MeasEpochChannelType2.msg" 179 | "msg/PVTCartesian.msg" 180 | "msg/PVTGeodetic.msg" 181 | "msg/PosCovCartesian.msg" 182 | "msg/PosCovGeodetic.msg" 183 | "msg/ReceiverTime.msg" 184 | "msg/VelCovCartesian.msg" 185 | "msg/VelCovGeodetic.msg" 186 | "msg/AttEuler.msg" 187 | "msg/AttCovEuler.msg" 188 | "msg/INSNavCart.msg" 189 | "msg/INSNavGeod.msg" 190 | "msg/IMUSetup.msg" 191 | "msg/VectorInfoCart.msg" 192 | "msg/VectorInfoGeod.msg" 193 | "msg/VelSensorSetup.msg" 194 | "msg/ExtSensorMeas.msg" 195 | DEPENDENCIES std_msgs 196 | ) 197 | 198 | set(library_name septentrio_gnss_driver_core) 199 | set(executable_name septentrio_gnss_driver_node) 200 | 201 | set(dependencies 202 | rclcpp 203 | rclcpp_components 204 | diagnostic_msgs 205 | nmea_msgs 206 | sensor_msgs 207 | geometry_msgs 208 | gps_msgs 209 | nav_msgs 210 | tf2 211 | tf2_eigen 212 | tf2_geometry_msgs 213 | tf2_ros 214 | ) 215 | 216 | ## shared library 217 | add_library(${library_name} SHARED 218 | src/septentrio_gnss_driver/communication/communication_core.cpp 219 | src/septentrio_gnss_driver/communication/message_handler.cpp 220 | src/septentrio_gnss_driver/communication/telegram_handler.cpp 221 | src/septentrio_gnss_driver/crc/crc.cpp 222 | src/septentrio_gnss_driver/node/main.cpp 223 | src/septentrio_gnss_driver/node/rosaic_node.cpp 224 | src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp 225 | src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp 226 | src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp 227 | src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp 228 | src/septentrio_gnss_driver/parsers/parsing_utilities.cpp 229 | src/septentrio_gnss_driver/parsers/string_utilities.cpp 230 | ) 231 | target_compile_definitions(${library_name} PUBLIC ROS2) 232 | target_include_directories(${library_name} PUBLIC 233 | "$" 234 | "$" 235 | ) 236 | target_link_libraries(${library_name} 237 | boost_regex # ordering is important for regex https://stackoverflow.com/questions/17588440/cannot-link-boost-regex 238 | ${Boost_LIBRARIES} 239 | pcap 240 | ${GeographicLib_LIBRARIES} 241 | ${diagnostic_msgs_TARGETS} 242 | ${geometry_msgs_TARGETS} 243 | ${gps_msgs_TARGETS} 244 | ${nav_msgs_TARGETS} 245 | ${nmea_msgs_TARGETS} 246 | ${sensor_msgs_TARGETS} 247 | rclcpp::rclcpp 248 | rclcpp_components::component 249 | rclcpp_components::component_manager 250 | sensor_msgs::sensor_msgs_library 251 | tf2::tf2 252 | tf2_eigen::tf2_eigen 253 | tf2_geometry_msgs::tf2_geometry_msgs 254 | tf2_ros::static_transform_broadcaster_node 255 | tf2_ros::tf2_ros 256 | ) 257 | 258 | if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) 259 | rosidl_target_interfaces(${library_name} 260 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 261 | else() 262 | rosidl_get_typesupport_target( 263 | cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") 264 | target_link_libraries(${library_name} "${cpp_typesupport_target}") 265 | target_compile_definitions(${library_name} PUBLIC ROS2_VER_N250) 266 | target_compile_definitions(${library_name} INTERFACE ROS2_VER_N250) 267 | endif() 268 | 269 | ## executable 270 | add_executable(${executable_name} 271 | src/septentrio_gnss_driver/node/main.cpp 272 | ) 273 | target_link_libraries(${executable_name} 274 | ${library_name} 275 | ) 276 | 277 | rclcpp_components_register_nodes(${library_name} "rosaic_node::ROSaicNode") 278 | 279 | # Testing 280 | find_package(ament_cmake_gtest REQUIRED) 281 | add_subdirectory(test) 282 | 283 | # Export Targets 284 | ament_export_targets(${library_name} HAS_LIBRARY_TARGET) 285 | ament_export_dependencies(${dependencies} rosidl_default_runtime) 286 | 287 | # Installation 288 | install(TARGETS 289 | ${library_name} EXPORT ${library_name} 290 | ARCHIVE DESTINATION lib 291 | LIBRARY DESTINATION lib 292 | RUNTIME DESTINATION bin 293 | ) 294 | 295 | install(TARGETS ${executable_name} EXPORT ${executable_name} 296 | DESTINATION lib/${PROJECT_NAME} 297 | ) 298 | 299 | install(DIRECTORY include/ 300 | DESTINATION include/ 301 | ) 302 | 303 | install(DIRECTORY launch 304 | DESTINATION share/${PROJECT_NAME} 305 | ) 306 | install(DIRECTORY config 307 | DESTINATION share/${PROJECT_NAME} 308 | ) 309 | 310 | ament_package() 311 | ##------------------------------------------------------------------------- 312 | else() 313 | message(FATAL_ERROR "Build failed because neither catkin nor ament found.") 314 | endif() 315 | 316 | -------------------------------------------------------------------------------- /src/septentrio_gnss_driver/parsers/parsing_utilities.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // © Copyright 2020, Septentrio NV/SA. 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived 15 | // from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // ***************************************************************************** 30 | 31 | // ROSaic includes 32 | #include 33 | #include 34 | // C++ library includes 35 | #include 36 | // Boost 37 | #include 38 | 39 | /** 40 | * @file parsing_utilities.cpp 41 | * @brief Declares utility functions used when parsing messages 42 | * @date 17/08/20 43 | */ 44 | 45 | namespace parsing_utilities { 46 | 47 | namespace qi = boost::spirit::qi; 48 | 49 | [[nodiscard]] double parseDouble(const uint8_t* buffer) 50 | { 51 | double val; 52 | qi::parse(buffer, buffer + 8, qi::little_bin_double, val); 53 | return val; 54 | } 55 | 56 | /** 57 | * It checks whether an error occurred (via errno) and whether junk characters 58 | * exist within "string", and returns true if the latter two tests are negative 59 | * or when the string is empty, false otherwise. 60 | */ 61 | [[nodiscard]] bool parseDouble(const std::string& string, double& value) 62 | { 63 | return string_utilities::toDouble(string, value) || string.empty(); 64 | } 65 | 66 | [[nodiscard]] float parseFloat(const uint8_t* buffer) 67 | { 68 | float val; 69 | qi::parse(buffer, buffer + 4, qi::little_bin_float, val); 70 | return val; 71 | } 72 | 73 | /** 74 | * It checks whether an error occurred (via errno) and whether junk characters 75 | * exist within "string", and returns true if the latter two tests are negative 76 | * or when the string is empty, false otherwise. 77 | */ 78 | [[nodiscard]] bool parseFloat(const std::string& string, float& value) 79 | { 80 | return string_utilities::toFloat(string, value) || string.empty(); 81 | } 82 | 83 | /** 84 | * The function assumes that the bytes in the buffer are already arranged with 85 | * the same endianness as the local platform. It copies the elements in the range 86 | * [buffer,buffer + 2) into the range beginning at 87 | * reinterpret_cast(&x). Recall: data_type *var_name = reinterpret_cast 88 | * (pointer_variable) converts the pointer type, no return type 89 | */ 90 | [[nodiscard]] int16_t parseInt16(const uint8_t* buffer) 91 | { 92 | int16_t val; 93 | qi::parse(buffer, buffer + 2, qi::little_word, val); 94 | return val; 95 | } 96 | 97 | /** 98 | * It checks whether an error occurred (via errno) and whether junk characters 99 | * exist within "string", and returns true if the latter two tests are negative 100 | * or when the string is empty, false otherwise. 101 | */ 102 | [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, 103 | int32_t base) 104 | { 105 | value = 0; 106 | if (string.empty()) 107 | { 108 | return true; 109 | } 110 | 111 | int32_t intermd; 112 | if (string_utilities::toInt32(string, intermd, base) && 113 | intermd <= std::numeric_limits::max() && 114 | intermd >= std::numeric_limits::min()) 115 | { 116 | value = static_cast(intermd); 117 | return true; 118 | } 119 | 120 | return false; 121 | } 122 | 123 | [[nodiscard]] int32_t parseInt32(const uint8_t* buffer) 124 | { 125 | int32_t val; 126 | qi::parse(buffer, buffer + 4, qi::little_dword, val); 127 | return val; 128 | } 129 | 130 | /** 131 | * It checks whether an error occurred (via errno) and whether junk characters 132 | * exist within "string", and returns true if the latter two tests are negative 133 | * or when the string is empty, false otherwise. 134 | */ 135 | [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, 136 | int32_t base) 137 | { 138 | return string_utilities::toInt32(string, value, base) || string.empty(); 139 | } 140 | 141 | /** 142 | * It checks whether an error occurred (via errno) and whether junk characters 143 | * exist within "string", and returns true if the latter two tests are negative 144 | * or when the string is empty, false otherwise. 145 | */ 146 | [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, 147 | int32_t base) 148 | { 149 | value = 0; 150 | if (string.empty()) 151 | { 152 | return true; 153 | } 154 | 155 | uint32_t intermd; 156 | if (string_utilities::toUInt32(string, intermd, base) && 157 | intermd <= std::numeric_limits::max()) 158 | { 159 | value = static_cast(intermd); 160 | return true; 161 | } 162 | 163 | return false; 164 | } 165 | 166 | [[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer) 167 | { 168 | uint16_t val; 169 | qi::parse(buffer, buffer + 2, qi::little_word, val); 170 | return val; 171 | } 172 | 173 | /** 174 | * It checks whether an error occurred (via errno) and whether junk characters 175 | * exist within "string", and returns true if the latter two tests are negative 176 | * or when the string is empty, false otherwise. 177 | */ 178 | [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, 179 | int32_t base) 180 | { 181 | value = 0; 182 | if (string.empty()) 183 | { 184 | return true; 185 | } 186 | 187 | uint32_t intermd; 188 | if (string_utilities::toUInt32(string, intermd, base) && 189 | intermd <= std::numeric_limits::max()) 190 | { 191 | value = static_cast(intermd); 192 | return true; 193 | } 194 | 195 | return false; 196 | } 197 | 198 | [[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer) 199 | { 200 | uint32_t val; 201 | qi::parse(buffer, buffer + 4, qi::little_dword, val); 202 | return val; 203 | } 204 | 205 | /** 206 | * It checks whether an error occurred (via errno) and whether junk characters 207 | * exist within "string", and returns true if the latter two tests are negative 208 | * or when the string is empty, false otherwise. 209 | */ 210 | [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, 211 | int32_t base) 212 | { 213 | return string_utilities::toUInt32(string, value, base) || string.empty(); 214 | } 215 | 216 | /** 217 | * The UTC precision in NMEA messages is down to a tenth of a second, naturally 218 | * in both the without-colon-delimiter and the number-of-seconds-since-midnight 219 | * formats. 220 | */ 221 | [[nodiscard]] double convertUTCDoubleToSeconds(double utc_double) 222 | { 223 | uint32_t hours = static_cast(utc_double) / 10000; 224 | uint32_t minutes = (static_cast(utc_double) - hours * 10000) / 100; 225 | 226 | return utc_double - static_cast(hours * 10000 + minutes * 100) + 227 | static_cast(hours * 3600 + minutes * 60); 228 | } 229 | 230 | /** 231 | * Recall: One degree is divided into 60 minutes (of arc), and in turn one minute 232 | * into 60 seconds (of arc). Use of the degrees-minutes-seconds system is also 233 | * called DMS notation. 234 | */ 235 | [[nodiscard]] double convertDMSToDegrees(double dms) 236 | { 237 | uint32_t whole_degrees = static_cast(dms) / 100; 238 | double minutes = dms - static_cast(whole_degrees * 100); 239 | return static_cast(whole_degrees) + minutes / 60.0; 240 | } 241 | 242 | /** 243 | * Time information (hours, minutes, seconds) is extracted from the given double 244 | * and augmented with the date, which is taken from the current system time on 245 | * the host computer (i.e. current UTC+some_shift time via time(0)). The date 246 | * ambiguity is resolved by adding/subtracting a day to the current date if the 247 | * host time is more than 12 hours behind/ahead the NMEA time (i.e. UTC time). 248 | * Recall time(0), time(NULL): If argument is a null pointer, the parameter is 249 | * not used (the function still returns the current calendar time of type 250 | * time_t). Otherwise, the return value is the same as the one stored in the 251 | * location pointed by the argument. Note that the function assumes that 252 | * utc_double has two significant digits after the decimal point, i.e. hhmmss.ss, 253 | * yet it does not round the number of seconds to the nearest unsigned integer, 254 | * but instead disregards ss. This is since we use this function for the 255 | * "header.stamp.sec" field of ROS messages, while "header.stamp.nsec" is taken 256 | * care of separately. 257 | */ 258 | [[nodiscard]] time_t convertUTCtoUnix(double utc_double) 259 | { 260 | time_t time_now = time(0); 261 | struct tm* timeinfo; 262 | 263 | // The function gmtime uses the value at &time_now to fill a tm structure 264 | // with the values that represent the corresponding time, expressed as a UTC 265 | // time. 266 | timeinfo = gmtime(&time_now); 267 | 268 | uint32_t hours = static_cast(utc_double) / 10000; 269 | uint32_t minutes = (static_cast(utc_double) - hours * 10000) / 100; 270 | uint32_t seconds = 271 | (static_cast(utc_double) - hours * 10000 - minutes * 100); 272 | 273 | // Overwriting timeinfo with UTC time as extracted from utc_double.. 274 | timeinfo->tm_hour = hours; // hours since midnight - [0,23] 275 | timeinfo->tm_min = minutes; // minutes after the hour - [0,59] 276 | timeinfo->tm_sec = seconds; // seconds after the minute - [0,59] 277 | 278 | /* // If you are doing a simulation, add year, month and day here: 279 | uint32_t year; // year, starting from 1900 280 | uint32_t month; // months since January - [0,11] 281 | uint32_t day; //day of the month - [1,31] 282 | timeinfo->tm_year = year; 283 | timeinfo->tm_mon = month; 284 | timeinfo->tm_mday = day; 285 | */ 286 | 287 | // Inverse of gmtime, the latter converts time_t (Unix time) to tm (UTC time) 288 | return timegm(timeinfo); 289 | } 290 | 291 | [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user) 292 | { 293 | std::string cmd; 294 | 295 | if (period_user == 0) 296 | return "OnChange"; 297 | else if (period_user < 1000) 298 | return "msec" + std::to_string(period_user); 299 | else if (period_user <= 60000) 300 | return "sec" + std::to_string(period_user / 1000); 301 | else 302 | return "min" + std::to_string(period_user / 60000); 303 | } 304 | 305 | [[nodiscard]] uint16_t getCrc(const std::vector& message) 306 | { 307 | return parseUInt16(message.data() + 2); 308 | } 309 | 310 | [[nodiscard]] uint16_t getId(const std::vector& message) 311 | { 312 | // Defines bit mask.. 313 | // Highest three bits are for revision and rest for block number 314 | constexpr uint16_t mask = 8191; 315 | // Bitwise AND gives us all but highest 3 bits set to zero, rest unchanged 316 | 317 | return parseUInt16(message.data() + 4) & mask; 318 | } 319 | 320 | [[nodiscard]] uint16_t getLength(const std::vector& message) 321 | { 322 | return parseUInt16(message.data() + 6); 323 | } 324 | 325 | [[nodiscard]] uint32_t getTow(const std::vector& message) 326 | { 327 | return parseUInt32(message.data() + 8); 328 | } 329 | 330 | [[nodiscard]] uint16_t getWnc(const std::vector& message) 331 | { 332 | return parseUInt16(message.data() + 12); 333 | } 334 | 335 | } // namespace parsing_utilities 336 | --------------------------------------------------------------------------------