From ef16634ecde3980116404f777c7fd0eb4e7121f5 Mon Sep 17 00:00:00 2001 From: Michael Zemb Date: Tue, 19 Oct 2021 11:49:47 +0200 Subject: [PATCH 01/74] fix build on Windows --- CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 64d76cf..5c86bf5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,11 +106,19 @@ set (SBG_COMMON_RESOURCES ## Declare a C++ executable add_executable(sbg_device ${SBG_COMMON_RESOURCES} src/main.cpp) add_dependencies(sbg_device ${PROJECT_NAME}) +if(WIN32) +target_compile_options(sbg_device PRIVATE) +else() target_compile_options(sbg_device PRIVATE -Wall -Wextra) +endif() add_executable(sbg_device_mag ${SBG_COMMON_RESOURCES} src/main_mag.cpp) add_dependencies(sbg_device_mag ${PROJECT_NAME}) +if(WIN32) +target_compile_options(sbg_device_mag PRIVATE) +else() target_compile_options(sbg_device_mag PRIVATE -Wall -Wextra) +endif() ## Specify libraries to link a library or executable target against target_link_libraries(sbg_device ${catkin_LIBRARIES} sbgECom) From 4823326675d0b824bee58794da39716bc264f9ae Mon Sep 17 00:00:00 2001 From: Michael Zemb Date: Wed, 20 Oct 2021 11:43:39 +0200 Subject: [PATCH 02/74] remove build status --- README.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/README.md b/README.md index d303e20..f8a1edf 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,5 @@ # sbg_driver -TODO - add build status - ## Overview ROS package for SBG Systems IMU.
The driver allows the user to configure the IMU (if possible, according to the device), to receive messages from the Sbg message protocol, publish ROS standard messages , and to calibrate the magnetometers. From c65ed4c547fe377cdf652d15a67044add266aae6 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 30 May 2023 14:26:49 +0200 Subject: [PATCH 03/74] Added subscription to RTCM msg --- CMakeLists.txt | 4 ++ include/sbg_driver/message_subscriber.h | 82 +++++++++++++++++++++++++ include/sbg_driver/sbg_device.h | 7 +++ package.xml | 2 + src/main.cpp | 1 + src/message_subscriber.cpp | 56 +++++++++++++++++ src/sbg_device.cpp | 12 ++++ 7 files changed, 164 insertions(+) create mode 100644 include/sbg_driver/message_subscriber.h create mode 100644 src/message_subscriber.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c86bf5..d2714ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(nav_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -28,6 +29,7 @@ set (USED_LIBRARIES std_srvs geometry_msgs nav_msgs + mavros_msgs tf2_ros tf2_msgs tf2_geometry_msgs @@ -98,6 +100,7 @@ include_directories( set (SBG_COMMON_RESOURCES src/config_applier.cpp src/message_publisher.cpp + src/message_subscriber.cpp src/message_wrapper.cpp src/config_store.cpp src/sbg_device.cpp @@ -174,6 +177,7 @@ ament_export_dependencies( std_srvs geometry_msgs nav_msgs + mavros_msgs tf2_ros tf2_msgs tf2_geometry_msgs diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h new file mode 100644 index 0000000..0b68537 --- /dev/null +++ b/include/sbg_driver/message_subscriber.h @@ -0,0 +1,82 @@ +/*! +* \file message_subscriber.h +* \author SBG Systems +* \date 26/05/2023 +* +* \brief Manage subscription of messages. +* +* \section CodeCopyright Copyright Notice +* MIT License +* +* Copyright (c) 2023 SBG Systems +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ + +#ifndef SBG_ROS_MESSAGE_SUBSCRIBER_H +#define SBG_ROS_MESSAGE_SUBSCRIBER_H + +// Project headers +#include +#include + +namespace sbg +{ +/*! + * Class to publish all SBG-ROS messages to the corresponding publishers. + */ +class MessageSubscriber: public rclcpp::Node +{ +private: + + rclcpp::Subscription::SharedPtr m_rtcm_sub_; + + uint32_t m_max_messages_; + SbgInterface *m_sbg_interface_; + + //---------------------------------------------------------------------// + //- Private methods -// + //---------------------------------------------------------------------// + void readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedPtr msg) const; + +public: + + //---------------------------------------------------------------------// + //- Constructor -// + //---------------------------------------------------------------------// + + /*! + * Default constructor. + */ + MessageSubscriber(SbgInterface *sbg_interface); + + //---------------------------------------------------------------------// + //- Operations -// + //---------------------------------------------------------------------// + + /*! + * Initialize the publishers for the output configuration. + * + * \param[in] ref_config_store Store configuration for the publishers. + */ + void initTopicSubscriptions(const sbg::ConfigStore &ref_config_store); +}; +} + +#endif // SBG_ROS_MESSAGE_SUBSCRIBER_H diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index a1111d6..eddf4d3 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -14,6 +14,7 @@ #include #include #include +#include namespace sbg { @@ -41,6 +42,7 @@ class SbgDevice SbgInterface m_sbg_interface_; rclcpp::Node& m_ref_node_; MessagePublisher m_message_publisher_; + std::shared_ptr m_message_subscriber_; ConfigStore m_config_store_; uint32_t m_rate_frequency_; @@ -108,6 +110,11 @@ class SbgDevice */ void initPublishers(void); + /*! + * Initialize the subscribers according to the configuration. + */ + void initSubscribers(void); + /*! * Configure the connected SBG device. * This function will configure the device if the config file allows it. diff --git a/package.xml b/package.xml index a4e2643..24f6f00 100644 --- a/package.xml +++ b/package.xml @@ -33,6 +33,7 @@ std_srvs message_runtime nav_msgs + mavros_msgs tf2_ros tf2_msgs tf2_geometry_msgs @@ -47,6 +48,7 @@ std_msgs std_srvs nav_msgs + mavros_msgs tf2_ros tf2_msgs tf2_geometry_msgs diff --git a/src/main.cpp b/src/main.cpp index fb126e3..7ed804a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -27,6 +27,7 @@ int main(int argc, char **argv) loop_rate.sleep(); } + rclcpp::shutdown(); return 0; } catch (std::exception const& refE) diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp new file mode 100644 index 0000000..dbe2dda --- /dev/null +++ b/src/message_subscriber.cpp @@ -0,0 +1,56 @@ +#include "message_subscriber.h" + +// ROS headers +#include + +// SBG headers +#include + +using sbg::MessageSubscriber; + +/*! + * Class to publish all SBG-ROS messages to the corresponding publishers. + */ +//---------------------------------------------------------------------// +//- Constructor -// +//---------------------------------------------------------------------// + +MessageSubscriber::MessageSubscriber(SbgInterface *sbg_interface): Node("sbg_subscriber"), +m_max_messages_(10), m_sbg_interface_(sbg_interface) +{ +} + +//---------------------------------------------------------------------// +//- Private methods -// +//---------------------------------------------------------------------// + +void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedPtr msg) const +{ + assert(m_sbg_interface_); + + auto rtcm_data = msg->data; + auto error_code = sbgInterfaceWrite(m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); + if (error_code != SBG_NO_ERROR) { + char error_str[256]; + + sbgEComErrorToString(error_code, error_str); + SBG_LOG_ERROR(SBG_ERROR, "Failed to sent RTCM data to device: %s", error_str); + } + +} + + + +//---------------------------------------------------------------------// +//- Operations -// +//---------------------------------------------------------------------// + +void MessageSubscriber::initTopicSubscriptions(const sbg::ConfigStore &ref_config_store) { + SBG_UNUSED_PARAMETER(ref_config_store); + + auto rtcm_cb = [&](const mavros_msgs::msg::RTCM::SharedPtr msg) -> void { + this->readRosRtcmMessage(msg); + }; + m_rtcm_sub_ = create_subscription( + "ntrip_client/rtcm", m_max_messages_, rtcm_cb); +} diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 2f2ace3..09104c0 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -207,6 +207,16 @@ void SbgDevice::initPublishers(void) m_rate_frequency_ = m_config_store_.getReadingRateFrequency(); } +void SbgDevice::initSubscribers(void) +{ + m_message_subscriber_ = std::make_shared(&m_sbg_interface_); + + m_message_subscriber_->initTopicSubscriptions(m_config_store_); + std::thread([&]{ + rclcpp::spin(m_message_subscriber_); + }).detach(); +} + void SbgDevice::configure(void) { if (m_config_store_.checkConfigWithRos()) @@ -468,6 +478,7 @@ void SbgDevice::initDeviceForReceivingData(void) { SbgErrorCode error_code; initPublishers(); + initSubscribers(); configure(); error_code = sbgEComSetReceiveLogCallback(&m_com_handle_, onLogReceivedCallback, this); @@ -489,4 +500,5 @@ void SbgDevice::initDeviceForMagCalibration(void) void SbgDevice::periodicHandle(void) { sbgEComHandle(&m_com_handle_); + } From e308f8f8f11dd361291e4fd575fab9fcaae7da2b Mon Sep 17 00:00:00 2001 From: cledant Date: Wed, 31 May 2023 18:26:02 +0200 Subject: [PATCH 04/74] Added publisher for nmea msg --- CMakeLists.txt | 2 + include/sbg_driver/message_publisher.h | 2 + include/sbg_driver/message_wrapper.h | 26 +++++- package.xml | 1 + src/message_publisher.cpp | 10 ++- src/message_wrapper.cpp | 108 +++++++++++++++++++++++++ 6 files changed, 146 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d2714ec..d0793a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(mavros_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(nmea_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(Boost REQUIRED) @@ -33,6 +34,7 @@ set (USED_LIBRARIES tf2_ros tf2_msgs tf2_geometry_msgs + nmea_msgs ) set (msg_files diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index f83023f..a675dda 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -82,6 +82,8 @@ class MessagePublisher rclcpp::Publisher>::SharedPtr m_nav_sat_fix_pub_; rclcpp::Publisher>::SharedPtr m_odometry_pub_; + rclcpp::Publisher>::SharedPtr m_SbgGpsPos_gga_pub_; + MessageWrapper m_message_wrapper_; uint32_t m_max_messages_; std::string m_frame_id_; diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 51a215e..da73cc3 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -56,6 +56,7 @@ #include #include #include +#include // SbgRos message headers #include "sbg_driver/msg/sbg_status.hpp" @@ -86,6 +87,19 @@ typedef struct _UTM0 int zone; } UTM0; +typedef enum _SbgNmeaGpsQuality +{ + SBG_NMEA_GPS_QUALITY_NO_FIX = 0, + SBG_NMEA_GPS_QUALITY_GPS_FIX = 1, + SBG_NMEA_GPS_QUALITY_DIFFERENTIAL_GPS_FIX = 2, + SBG_NMEA_GPS_QUALITY_PPS_FIX = 3, + SBG_NMEA_GPS_QUALITY_RTK = 4, + SBG_NMEA_GPS_QUALITY_RTK_FLOAT = 5, + SBG_NMEA_GPS_QUALITY_ESTIMATED = 6, + SBG_NMEA_GPS_QUALITY_MANUAL_INPUT = 7, + SBG_NMEA_GPS_QUALITY_SIMULATED = 8, +} SbgNmeaGpsQuality; + /*! * Class to wrap the SBG logs into ROS messages. */ @@ -318,6 +332,8 @@ class MessageWrapper : public rclcpp::Node */ void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const; + static uint32_t convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType); + public: //---------------------------------------------------------------------// @@ -643,7 +659,15 @@ class MessageWrapper : public rclcpp::Node * \param[in] ref_sbg_air_msg SBG-ROS AirData message. * \return ROS standard fluid pressure message. */ - const sensor_msgs::msg::FluidPressure createRosFluidPressureMessage(const sbg_driver::msg::SbgAirData& ref_sbg_air_msg) const; + const sensor_msgs::msg::FluidPressure createRosFluidPressureMessage(const sbg_driver::msg::SbgAirData& ref_sbg_air_msg) const; + + /*! + * Create a SBG-ROS GPS-Position message. + * + * \param[in] ref_log_gps_pos SBG GPS Position log. + * \return GPS Position message in nmea format. + */ + const nmea_msgs::msg::Sentence createSbgGpsPosMessageGGA(const SbgLogGpsPos& ref_log_gps_pos) const; }; } diff --git a/package.xml b/package.xml index 24f6f00..13b8527 100644 --- a/package.xml +++ b/package.xml @@ -52,6 +52,7 @@ tf2_ros tf2_msgs tf2_geometry_msgs + nmea_msgs boost rosidl_default_runtime diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index ab6f174..cce2256 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -450,6 +450,10 @@ void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log) { m_nav_sat_fix_pub_->publish(m_message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); } + if (m_SbgGpsPos_gga_pub_) + { + m_SbgGpsPos_gga_pub_->publish(m_message_wrapper_.createSbgGpsPosMessageGGA(ref_sbg_log.gpsPosData)); + } } //---------------------------------------------------------------------// @@ -480,6 +484,8 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C initPublisher(ref_ros_node_handle, ref_output.message_id, ref_output.output_mode, getOutputTopicName(ref_output.message_id)); } + m_SbgGpsPos_gga_pub_ = ref_ros_node_handle.create_publisher("ntrip_client/nmea", m_max_messages_); + if (ref_config_store.checkRosStandardMessages()) { defineRosStandardPublishers(ref_ros_node_handle, ref_config_store.getOdomEnable()); @@ -659,14 +665,14 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ default: break; - } + } } else if (sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_1) { switch (sbg_msg_id) { default: - break; + break; } } } diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index edf7b42..ea971cd 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -510,6 +510,33 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT } } +uint32_t MessageWrapper::convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType) +{ + switch (sbgGpsType) { + case SBG_ECOM_POS_NO_SOLUTION: + case SBG_ECOM_POS_UNKNOWN_TYPE: + return SBG_NMEA_GPS_QUALITY_NO_FIX; + case SBG_ECOM_POS_SINGLE: + case SBG_ECOM_POS_FIXED: + return SBG_NMEA_GPS_QUALITY_GPS_FIX; + case SBG_ECOM_POS_PSRDIFF: + return SBG_NMEA_GPS_QUALITY_DIFFERENTIAL_GPS_FIX; + case SBG_ECOM_POS_SBAS: + case SBG_ECOM_POS_OMNISTAR: + return SBG_NMEA_GPS_QUALITY_PPS_FIX; + case SBG_ECOM_POS_RTK_FLOAT: + return SBG_NMEA_GPS_QUALITY_RTK_FLOAT; + case SBG_ECOM_POS_RTK_INT: + return SBG_NMEA_GPS_QUALITY_RTK; + case SBG_ECOM_POS_PPP_FLOAT: + case SBG_ECOM_POS_PPP_INT: + return SBG_NMEA_GPS_QUALITY_SIMULATED; + default: + return SBG_NMEA_GPS_QUALITY_NO_FIX; + } +} + + //---------------------------------------------------------------------// //- Parameters -// //---------------------------------------------------------------------// @@ -1310,3 +1337,84 @@ const sensor_msgs::msg::FluidPressure MessageWrapper::createRosFluidPressureMess return fluid_pressure_message; } + +const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(const SbgLogGpsPos &ref_log_gps_pos) const +{ + nmea_msgs::msg::Sentence gps_pos_nmea_msg; + + // Latitude conversion + char lat_dir; + int32_t lat_degs = ref_log_gps_pos.latitude; + float lat_mins = (ref_log_gps_pos.latitude - static_cast(lat_degs)) * 60.0f; + if(lat_degs < 0) + { + lat_degs *= -1; + lat_mins *= -1.0f; + lat_dir = 'S'; + } + else + { + lat_dir = 'N'; + } + + // Longitude conversion + char lon_dir; + int32_t lon_degs = ref_log_gps_pos.longitude; + float lon_mins = (ref_log_gps_pos.longitude - static_cast(lon_degs)) * 60.0f; + if(lon_degs < 0) + { + lon_degs *= -1; + lon_mins *= -1.0f; + lon_dir = 'W'; + } + else + { + lon_dir = 'E'; + } + + // Gps time of week To UTC + auto time_of_week_ms = ref_log_gps_pos.timeOfWeek; + auto nb_hours = time_of_week_ms / (3600 * 1000); + auto current_hour = nb_hours % 24; + auto nb_minutes = time_of_week_ms / (60 * 1000); + auto current_minute = nb_minutes % 60; + auto current_second = (time_of_week_ms / 1000) % 60; + auto current_ms = time_of_week_ms % (60 * 1000); + + // Writing NMEA sentence + constexpr uint32_t nmea_sentence_buffer_size = 128; + char nmeas_sentence_buff[nmea_sentence_buffer_size]{}; + auto len = snprintf(nmeas_sentence_buff, nmea_sentence_buffer_size, "$GPGGA,%02d%02d%02d.%03d,%02d%02.3f,%c,%03d%02.3f,%c,%d,%d,%.3f,%.3f,M,%.3f,M,%d,%d", + current_hour, + current_minute, + current_second, + current_ms - (current_second * 1000), + lat_degs, + lat_mins, + lat_dir, + lon_degs, + lon_mins, + lon_dir, + convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status)), + ref_log_gps_pos.numSvUsed, + ref_log_gps_pos.altitudeAccuracy, + ref_log_gps_pos.altitude, + ref_log_gps_pos.undulation, + ref_log_gps_pos.differentialAge, + ref_log_gps_pos.baseStationId + ); + + // Checksum computation + uint8_t checksum = 0; + for(int32_t i = 1; i < len; i++) + { + checksum ^= nmeas_sentence_buff[i]; + } + snprintf(&nmeas_sentence_buff[len], nmea_sentence_buffer_size - len, "*%02X\r\n", checksum); + + // Populating ROS message + gps_pos_nmea_msg.header = createRosHeader(ref_log_gps_pos.timeStamp); + gps_pos_nmea_msg.sentence = nmeas_sentence_buff; + + return gps_pos_nmea_msg; +} From a57302e4b8cb385b7384c093e5e1c3e78e821e82 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 2 Jun 2023 14:27:47 +0200 Subject: [PATCH 05/74] Fixed deprecated header warning --- src/message_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index ea971cd..5a6a9b9 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -3,7 +3,7 @@ // ROS headers #include -#include +#include #include #include From 9ce8d6c53d0b782c7b8a15d69fd38f7c8117fa55 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 2 Jun 2023 15:27:38 +0200 Subject: [PATCH 06/74] Added rtcm / nmea parameters in config files --- config/sbg_device_uart_default.yaml | 16 +++++++++++ config/sbg_device_udp_default.yaml | 16 +++++++++++ include/sbg_driver/config_store.h | 30 ++++++++++++++++++--- src/config_store.cpp | 42 ++++++++++++++++++++++++++++- src/message_publisher.cpp | 5 +++- src/message_subscriber.cpp | 18 +++++++------ src/sbg_device.cpp | 6 ++++- 7 files changed, 118 insertions(+), 15 deletions(-) diff --git a/config/sbg_device_uart_default.yaml b/config/sbg_device_uart_default.yaml index 101336d..a74751a 100644 --- a/config/sbg_device_uart_default.yaml +++ b/config/sbg_device_uart_default.yaml @@ -302,3 +302,19 @@ log_air_data: 0 # Short IMU data log_imu_short: 8 + + rtcm: + # Should ros driver listen to RTCM topic + listen_rtcm: false + # Topic on which RTCM is published + topic_name: rtcm + # Namespace where topic is published + namespace: ntrip_client + + nmea: + # Should ros driver publish NMEA string + publish_nmea: false + # Topic on which to publish nmea data + topic_name: nmea + # Namespace where to publish topic + namespace: ntrip_client \ No newline at end of file diff --git a/config/sbg_device_udp_default.yaml b/config/sbg_device_udp_default.yaml index d539831..542cfb1 100644 --- a/config/sbg_device_udp_default.yaml +++ b/config/sbg_device_udp_default.yaml @@ -292,3 +292,19 @@ log_air_data: 0 # Short IMU data log_imu_short: 0 + + rtcm: + # Should ros driver listen to RTCM topic + listen_rtcm: false + # Topic on which RTCM is published + topic_name: rtcm + # Namespace where topic is published + namespace: ntrip_client + + nmea: + # Should ros driver publish NMEA string + publish_nmea: false + # Topic on which to publish nmea data + topic_name: nmea + # Namespace where to publish topic + namespace: ntrip_client \ No newline at end of file diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index fb710c3..efb7bd5 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -121,6 +121,16 @@ class ConfigStore std::string m_odom_base_frame_id_; std::string m_odom_init_frame_id_; + bool m_listen_rtcm_; + std::string m_rtcm_topic_name_; + std::string m_rtcm_topic_namespace_; + std::string m_rtcm_full_topic_; + + bool m_publish_nmea_; + std::string m_nmea_topic_name_; + std::string m_nmea_topic_namespace_; + std::string m_nmea_full_topic_; + //---------------------------------------------------------------------// //- Private methods -// //---------------------------------------------------------------------// @@ -239,6 +249,10 @@ class ConfigStore */ void loadOutputTimeReference(const rclcpp::Node& ref_node_handle, const std::string& ref_key); + void loadRtcmParameters(const rclcpp::Node& ref_node_handle); + + void loadNmeaParameters(const rclcpp::Node& ref_node_handle); + public: //---------------------------------------------------------------------// @@ -456,21 +470,21 @@ class ConfigStore * * \return True if the frame convention to use is ENU. */ - bool getUseEnu(void) const; + bool getUseEnu(void) const; /*! * Get odom enable. * * \return True if the odometry is enabled. */ - bool getOdomEnable(void) const; + bool getOdomEnable(void) const; /*! * Get odom publish_tf. * * \return If true publish odometry transforms. */ - bool getOdomPublishTf(void) const; + bool getOdomPublishTf(void) const; /*! * Get the odometry frame ID. @@ -502,7 +516,15 @@ class ConfigStore */ TimeReference getTimeReference(void) const; - //---------------------------------------------------------------------// + bool shouldListenRtcm(void) const; + + const std::string &getRtcmFullTopic(void) const; + + bool shouldPublishNmea() const; + + const std::string &getNmeaFullTopic(void) const; + + //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// diff --git a/src/config_store.cpp b/src/config_store.cpp index 15a23f9..f926d11 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -14,7 +14,9 @@ ConfigStore::ConfigStore(void): m_serial_communication_(false), m_upd_communication_(false), m_configure_through_ros_(false), -m_ros_standard_output_(false) +m_ros_standard_output_(false), +m_listen_rtcm_(false), +m_publish_nmea_(false) { } @@ -191,6 +193,22 @@ void ConfigStore::loadOutputTimeReference(const rclcpp::Node& ref_node_handle, c } } +void sbg::ConfigStore::loadRtcmParameters(const rclcpp::Node &ref_node_handle) +{ + ref_node_handle.get_parameter_or("rtcm.listen_rtcm", m_listen_rtcm_, false); + ref_node_handle.get_parameter_or("rtcm.topic_name", m_rtcm_topic_name_, "rtcm"); + ref_node_handle.get_parameter_or("rtcm.namespace", m_rtcm_topic_namespace_, "ntrip_client"); + m_rtcm_full_topic_ = m_rtcm_topic_namespace_ + "/" + m_rtcm_topic_name_; +} + +void sbg::ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) +{ + ref_node_handle.get_parameter_or("nmea.publish_nmea", m_publish_nmea_, false); + ref_node_handle.get_parameter_or("nmea.topic_name", m_nmea_topic_name_, "nmea"); + ref_node_handle.get_parameter_or("nmea.namespace", m_nmea_topic_namespace_, "ntrip_client"); + m_nmea_full_topic_ = m_nmea_topic_namespace_ + "/" + m_nmea_topic_name_; +} + //---------------------------------------------------------------------// //- Parameters -// //---------------------------------------------------------------------// @@ -370,6 +388,26 @@ const std::string &ConfigStore::getOdomInitFrameId(void) const return m_odom_init_frame_id_; } +bool sbg::ConfigStore::shouldListenRtcm(void) const +{ + return m_listen_rtcm_; +} + +const std::string &sbg::ConfigStore::getRtcmFullTopic(void) const +{ + return m_rtcm_full_topic_; +} + +bool sbg::ConfigStore::shouldPublishNmea(void) const +{ + return m_publish_nmea_; +} + +const std::string &sbg::ConfigStore::getNmeaFullTopic(void) const +{ + return m_nmea_full_topic_; +} + //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// @@ -386,6 +424,8 @@ void ConfigStore::loadFromRosNodeHandle(const rclcpp::Node& ref_node_handle) loadGnssParameters(ref_node_handle); loadOdometerParameters(ref_node_handle); loadOutputFrameParameters(ref_node_handle); + loadRtcmParameters(ref_node_handle); + loadNmeaParameters(ref_node_handle); loadOutputTimeReference(ref_node_handle, "output/time_reference"); diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index cce2256..755182d 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -484,7 +484,10 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C initPublisher(ref_ros_node_handle, ref_output.message_id, ref_output.output_mode, getOutputTopicName(ref_output.message_id)); } - m_SbgGpsPos_gga_pub_ = ref_ros_node_handle.create_publisher("ntrip_client/nmea", m_max_messages_); + if (ref_config_store.shouldPublishNmea()) + { + m_SbgGpsPos_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(),m_max_messages_); + } if (ref_config_store.checkRosStandardMessages()) { diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp index dbe2dda..e0e3d11 100644 --- a/src/message_subscriber.cpp +++ b/src/message_subscriber.cpp @@ -30,7 +30,8 @@ void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedP auto rtcm_data = msg->data; auto error_code = sbgInterfaceWrite(m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); - if (error_code != SBG_NO_ERROR) { + if (error_code != SBG_NO_ERROR) + { char error_str[256]; sbgEComErrorToString(error_code, error_str); @@ -39,18 +40,19 @@ void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedP } - - //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// -void MessageSubscriber::initTopicSubscriptions(const sbg::ConfigStore &ref_config_store) { - SBG_UNUSED_PARAMETER(ref_config_store); - +void MessageSubscriber::initTopicSubscriptions(const sbg::ConfigStore &ref_config_store) +{ auto rtcm_cb = [&](const mavros_msgs::msg::RTCM::SharedPtr msg) -> void { this->readRosRtcmMessage(msg); }; - m_rtcm_sub_ = create_subscription( - "ntrip_client/rtcm", m_max_messages_, rtcm_cb); + + if (ref_config_store.shouldListenRtcm()) + { + m_rtcm_sub_ = create_subscription( + ref_config_store.getRtcmFullTopic(), m_max_messages_, rtcm_cb); + } } diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 09104c0..468d157 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -209,8 +209,12 @@ void SbgDevice::initPublishers(void) void SbgDevice::initSubscribers(void) { - m_message_subscriber_ = std::make_shared(&m_sbg_interface_); + if (!m_config_store_.shouldListenRtcm()) + { + return; + } + m_message_subscriber_ = std::make_shared(&m_sbg_interface_); m_message_subscriber_->initTopicSubscriptions(m_config_store_); std::thread([&]{ rclcpp::spin(m_message_subscriber_); From b8309a47cfa60c15b094bc9ebd2ece665d103ea0 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 5 Jun 2023 13:34:30 +0200 Subject: [PATCH 07/74] Improved NMEA GGA message --- include/sbg_driver/message_publisher.h | 2 +- include/sbg_driver/message_wrapper.h | 10 +- src/message_publisher.cpp | 6 +- src/message_wrapper.cpp | 130 ++++++++++++++++++------- 4 files changed, 106 insertions(+), 42 deletions(-) diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index a675dda..9d14ef0 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -173,7 +173,7 @@ class MessagePublisher * * \param[in] ref_sbg_log SBG log. */ - void publishGpsPosData(const SbgBinaryLogData &ref_sbg_log); + void publishGpsPosData(const SbgBinaryLogData &ref_sbg_log, SbgEComMsgId sbg_msg_id); public: diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index da73cc3..2e25d01 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -89,13 +89,13 @@ typedef struct _UTM0 typedef enum _SbgNmeaGpsQuality { - SBG_NMEA_GPS_QUALITY_NO_FIX = 0, - SBG_NMEA_GPS_QUALITY_GPS_FIX = 1, - SBG_NMEA_GPS_QUALITY_DIFFERENTIAL_GPS_FIX = 2, - SBG_NMEA_GPS_QUALITY_PPS_FIX = 3, + SBG_NMEA_GPS_QUALITY_INVALID = 0, + SBG_NMEA_GPS_QUALITY_SINGLE = 1, + SBG_NMEA_GPS_QUALITY_DGPS = 2, + SBG_NMEA_GPS_QUALITY_PPS = 3, SBG_NMEA_GPS_QUALITY_RTK = 4, SBG_NMEA_GPS_QUALITY_RTK_FLOAT = 5, - SBG_NMEA_GPS_QUALITY_ESTIMATED = 6, + SBG_NMEA_GPS_QUALITY_FIX_DEAD_RECKONING = 6, SBG_NMEA_GPS_QUALITY_MANUAL_INPUT = 7, SBG_NMEA_GPS_QUALITY_SIMULATED = 8, } SbgNmeaGpsQuality; diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 755182d..995f7fe 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -436,7 +436,7 @@ void MessagePublisher::publishUtcData(const SbgBinaryLogData &ref_sbg_log) } } -void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log) +void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log, SbgEComMsgId sbg_msg_id) { sbg_driver::msg::SbgGpsPos sbg_gps_pos_message; @@ -450,7 +450,7 @@ void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log) { m_nav_sat_fix_pub_->publish(m_message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); } - if (m_SbgGpsPos_gga_pub_) + if (m_SbgGpsPos_gga_pub_ && sbg_msg_id == SBG_ECOM_LOG_GPS1_POS) { m_SbgGpsPos_gga_pub_->publish(m_message_wrapper_.createSbgGpsPosMessageGGA(ref_sbg_log.gpsPosData)); } @@ -584,7 +584,7 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_GPS1_POS: case SBG_ECOM_LOG_GPS2_POS: - publishGpsPosData(ref_sbg_log); + publishGpsPosData(ref_sbg_log, sbg_msg_id); break; case SBG_ECOM_LOG_GPS1_HDT: diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 5a6a9b9..53e5291 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -514,25 +514,24 @@ uint32_t MessageWrapper::convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType) { switch (sbgGpsType) { case SBG_ECOM_POS_NO_SOLUTION: - case SBG_ECOM_POS_UNKNOWN_TYPE: - return SBG_NMEA_GPS_QUALITY_NO_FIX; + return SBG_NMEA_GPS_QUALITY_INVALID; case SBG_ECOM_POS_SINGLE: + case SBG_ECOM_POS_UNKNOWN_TYPE: case SBG_ECOM_POS_FIXED: - return SBG_NMEA_GPS_QUALITY_GPS_FIX; + return SBG_NMEA_GPS_QUALITY_SINGLE; case SBG_ECOM_POS_PSRDIFF: - return SBG_NMEA_GPS_QUALITY_DIFFERENTIAL_GPS_FIX; case SBG_ECOM_POS_SBAS: case SBG_ECOM_POS_OMNISTAR: - return SBG_NMEA_GPS_QUALITY_PPS_FIX; - case SBG_ECOM_POS_RTK_FLOAT: - return SBG_NMEA_GPS_QUALITY_RTK_FLOAT; - case SBG_ECOM_POS_RTK_INT: - return SBG_NMEA_GPS_QUALITY_RTK; + return SBG_NMEA_GPS_QUALITY_DGPS; case SBG_ECOM_POS_PPP_FLOAT: case SBG_ECOM_POS_PPP_INT: - return SBG_NMEA_GPS_QUALITY_SIMULATED; + return SBG_NMEA_GPS_QUALITY_PPS; + case SBG_ECOM_POS_RTK_INT: + return SBG_NMEA_GPS_QUALITY_RTK; + case SBG_ECOM_POS_RTK_FLOAT: + return SBG_NMEA_GPS_QUALITY_RTK_FLOAT; default: - return SBG_NMEA_GPS_QUALITY_NO_FIX; + return SBG_NMEA_GPS_QUALITY_INVALID; } } @@ -1342,11 +1341,29 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co { nmea_msgs::msg::Sentence gps_pos_nmea_msg; + // Gps time of week To UTC + uint32_t nb_hours = ref_log_gps_pos.timeOfWeek / (3600 * 1000); + uint32_t nb_minutes = ref_log_gps_pos.timeOfWeek / (60 * 1000); + uint32_t current_hour = nb_hours % 24; + uint32_t current_minute = nb_minutes % 60; + uint32_t current_second = (ref_log_gps_pos.timeOfWeek / 1000) % 60; + uint32_t current_ms = ref_log_gps_pos.timeOfWeek % (60 * 1000); + current_ms = (current_ms - (current_second * 1000)) / 10; + // Latitude conversion char lat_dir; - int32_t lat_degs = ref_log_gps_pos.latitude; - float lat_mins = (ref_log_gps_pos.latitude - static_cast(lat_degs)) * 60.0f; - if(lat_degs < 0) + float latitude = ref_log_gps_pos.latitude; + if (latitude >= 1000.0f) + { + latitude = 999.9999f; + } + if (latitude <= -100.0f) + { + latitude = -99.9999f; + } + int32_t lat_degs = latitude; + float lat_mins = (latitude - static_cast(lat_degs)) * 60.0f; + if(latitude < 0.0f) { lat_degs *= -1; lat_mins *= -1.0f; @@ -1359,9 +1376,18 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co // Longitude conversion char lon_dir; - int32_t lon_degs = ref_log_gps_pos.longitude; - float lon_mins = (ref_log_gps_pos.longitude - static_cast(lon_degs)) * 60.0f; - if(lon_degs < 0) + float longitude = ref_log_gps_pos.longitude; + if (longitude >= 10000.0f) + { + longitude = 9999.9999f; + } + if (longitude <= -1000.0f) + { + longitude = -999.9999f; + } + int32_t lon_degs = longitude; + float lon_mins = (longitude - static_cast(lon_degs)) * 60.0f; + if(longitude < 0.0f) { lon_degs *= -1; lon_mins *= -1.0f; @@ -1372,23 +1398,61 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co lon_dir = 'E'; } - // Gps time of week To UTC - auto time_of_week_ms = ref_log_gps_pos.timeOfWeek; - auto nb_hours = time_of_week_ms / (3600 * 1000); - auto current_hour = nb_hours % 24; - auto nb_minutes = time_of_week_ms / (60 * 1000); - auto current_minute = nb_minutes % 60; - auto current_second = (time_of_week_ms / 1000) % 60; - auto current_ms = time_of_week_ms % (60 * 1000); + // DOP computation + double h_dop = sqrt((ref_log_gps_pos.latitudeAccuracy * ref_log_gps_pos.latitudeAccuracy) + \ + (ref_log_gps_pos.longitudeAccuracy * ref_log_gps_pos.longitudeAccuracy)); + if (h_dop >= 10.0) + { + h_dop = 9.9; + } + + double diff_age = (ref_log_gps_pos.differentialAge / 100.0f); + if (diff_age >= 10.0) + { + diff_age = 9.9; + } + + uint8_t svUsed = ref_log_gps_pos.numSvUsed; + if (svUsed > 100) + { + svUsed = 99; + } + + double altitude = ref_log_gps_pos.altitude; + if (altitude <= -100000.0) + { + altitude = -99999.9; + } + if (altitude >= 1000000.0) + { + altitude = 999999.9; + } + + double undulation = ref_log_gps_pos.undulation; + if (undulation <= -100000.0) + { + undulation = -99999.9; + } + if (undulation >= 1000000.0) + { + undulation = 999999.9; + } + + uint16_t baseStationId = ref_log_gps_pos.baseStationId; + if (baseStationId >= 10000) + { + baseStationId = 9999; + } // Writing NMEA sentence constexpr uint32_t nmea_sentence_buffer_size = 128; char nmeas_sentence_buff[nmea_sentence_buffer_size]{}; - auto len = snprintf(nmeas_sentence_buff, nmea_sentence_buffer_size, "$GPGGA,%02d%02d%02d.%03d,%02d%02.3f,%c,%03d%02.3f,%c,%d,%d,%.3f,%.3f,M,%.3f,M,%d,%d", + auto len = snprintf(nmeas_sentence_buff, nmea_sentence_buffer_size, + "$GPGGA,%02d%02d%02d.%02d,%02d%02.4f,%c,%03d%02.4f,%c,%d,%d,%.1f,%.1f,M,%d,M,%.1f,%04d", current_hour, current_minute, current_second, - current_ms - (current_second * 1000), + current_ms, lat_degs, lat_mins, lat_dir, @@ -1396,12 +1460,12 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co lon_mins, lon_dir, convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status)), - ref_log_gps_pos.numSvUsed, - ref_log_gps_pos.altitudeAccuracy, - ref_log_gps_pos.altitude, - ref_log_gps_pos.undulation, - ref_log_gps_pos.differentialAge, - ref_log_gps_pos.baseStationId + svUsed, + h_dop, + altitude, + static_cast(undulation), + diff_age, + baseStationId ); // Checksum computation From b1664cda05d8c18b21b7a0f5b0231681143134b3 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 6 Jun 2023 15:17:30 +0200 Subject: [PATCH 08/74] Code documentation --- README.md | 25 +++++++++++-- include/sbg_driver/config_store.h | 30 ++++++++++++++++ include/sbg_driver/message_publisher.h | 1 + include/sbg_driver/message_subscriber.h | 13 +++++-- include/sbg_driver/message_wrapper.h | 22 +++++++----- src/message_wrapper.cpp | 47 +++++++++++++------------ 6 files changed, 102 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index f8a1edf..37404cb 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ User can install the sbg_ros2_driver through the standard ROS installation syste #### Building 1. Clone the repository (use a Release version) -2. Build using the normal ROS catkin build system +2. Build using the ROS colcon build system ``` cd colcon_ws/src @@ -93,7 +93,10 @@ Launch the sbg_device_mag node to calibrate the magnetometers, and load the `ell ## Nodes ### sbg_device -The sbg_device node handles the communication with the connected device, and publishes the SBG output to the Ros environment. +The `sbg_device` node handles the communication with the connected device, and publishes the SBG output to the Ros environment. + +### sbg_subscriber +The `sbg_subscriber` node handles the subscriptions to the Ros environment topics, and forward data the device. #### Published Topics ##### SBG specific topics @@ -211,6 +214,24 @@ For each ROS standard, you have to activate the needed SBG outputs. Requires `/sbg/imu_data` and `/sbg/ekv_nav` and either `/sbg/ekf_euler` or `/sbg/ekf_quat`. Disabled by default, set odometry.enable in configuration file. +##### NMEA topics +GPS data in NMEA format can be published when `publish_nmea` is set to `true` in .yaml config file. +Data published on that topic can be used by a third party ROS2 module for NTRIP purpose. + +* **`/ntrip_client/nmea`** [nmea_msgs/Sentence](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html) + + Data from `/sbg/gps_pos` serialized into NMEA GGA format. Requires `/sbg/gps_pos`. + Namespace `ntrip_client` and topic_name `nmea` can be customized in .yaml config files. + +#### Subscribed Topics +##### RTCM topics +SBG ROS Driver will listen to some topics published by third party ROS2 modules. + +* **`/ntrip_client/rtcm`** [mavros_msgs/RTCM](http://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/RTCM.html) + + RTCM data from `/ntrip_client/rtcm` will be forwarded to the IMU. + Namespace `ntrip_client` and topic_name `rtcm` can be customized in .yaml config files. + ### sbg_device_mag The sbg_device_mag node handles the magnetic calibration for suitable devices. diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index efb7bd5..f397167 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -249,8 +249,18 @@ class ConfigStore */ void loadOutputTimeReference(const rclcpp::Node& ref_node_handle, const std::string& ref_key); + /*! + * Load RTCM parameters. + * + * \param[in] ref_node_handle ROS nodeHandle. + */ void loadRtcmParameters(const rclcpp::Node& ref_node_handle); + /*! + * Load NMEA parameters. + * + * \param[in] ref_node_handle ROS nodeHandle. + */ void loadNmeaParameters(const rclcpp::Node& ref_node_handle); public: @@ -516,12 +526,32 @@ class ConfigStore */ TimeReference getTimeReference(void) const; + /*! + * Get RTCM enable. + * + * \return True if RTCM is enabled. + */ bool shouldListenRtcm(void) const; + /*! + * Get RTCM full topic. + * + * \return String with RTCM namespace + topic. + */ const std::string &getRtcmFullTopic(void) const; + /*! + * Get NMEA enable. + * + * \return True if NMEA is enabled. + */ bool shouldPublishNmea() const; + /*! + * Get NMEA full topic. + * + * \return String with NMEA namespace + topic. + */ const std::string &getNmeaFullTopic(void) const; //---------------------------------------------------------------------// diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index 9d14ef0..f0baf46 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -172,6 +172,7 @@ class MessagePublisher * Publish a received SBG GpsPos log. * * \param[in] ref_sbg_log SBG log. + * \param[in] sbg_msg_id Id of the SBG message. */ void publishGpsPosData(const SbgBinaryLogData &ref_sbg_log, SbgEComMsgId sbg_msg_id); diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h index 0b68537..68e02d6 100644 --- a/include/sbg_driver/message_subscriber.h +++ b/include/sbg_driver/message_subscriber.h @@ -39,7 +39,7 @@ namespace sbg { /*! - * Class to publish all SBG-ROS messages to the corresponding publishers. + * Class to subscribe to all corresponding topics. */ class MessageSubscriber: public rclcpp::Node { @@ -53,6 +53,11 @@ class MessageSubscriber: public rclcpp::Node //---------------------------------------------------------------------// //- Private methods -// //---------------------------------------------------------------------// + /*! + * Handler for subscription to RTCM topic. + * + * \param[in] msg ROS RTCM message. + */ void readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedPtr msg) const; public: @@ -63,6 +68,8 @@ class MessageSubscriber: public rclcpp::Node /*! * Default constructor. + * + * \param[in] sbg_interface SBG Interface handle. */ MessageSubscriber(SbgInterface *sbg_interface); @@ -71,9 +78,9 @@ class MessageSubscriber: public rclcpp::Node //---------------------------------------------------------------------// /*! - * Initialize the publishers for the output configuration. + * Initialize the subscribers with configuration. * - * \param[in] ref_config_store Store configuration for the publishers. + * \param[in] ref_config_store Store configuration for the subscribers. */ void initTopicSubscriptions(const sbg::ConfigStore &ref_config_store); }; diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 2e25d01..53910bd 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -302,26 +302,26 @@ class MessageWrapper : public rclcpp::Node * \param[in] ref_pose Pose. * \param[out] ref_transform_stamped Stamped transformation. */ - void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::msg::Pose &ref_pose, geometry_msgs::msg::TransformStamped &ref_transform_stamped); + void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::msg::Pose &ref_pose, geometry_msgs::msg::TransformStamped &ref_transform_stamped); - /*! + /*! * Get UTM letter designator for the given latitude. * * \param[in] Lat Latitude, in degrees. * \return UTM letter designator. */ - char UTMLetterDesignator(double Lat); + char UTMLetterDesignator(double Lat); - /*! + /*! * Set UTM initial position. * * \param[in] Lat Latitude, in degrees. * \param[in] Long Longitude, in degrees. * \param[in] altitude Altitude, in meters. */ - void initUTM(double Lat, double Long, double altitude); + void initUTM(double Lat, double Long, double altitude); - /*! + /*! * Convert latitude and longitude to a position relative to UTM initial position. * * \param[in] Lat Latitude, in degrees. @@ -330,9 +330,15 @@ class MessageWrapper : public rclcpp::Node * \param[out] UTMNorthing UTM northing, in meters. * \param[out] UTMEasting UTM easting, in meters. */ - void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const; + void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const; - static uint32_t convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType); + /*! + * Convert SbgEComGpsPosType enum to SbgNmeaGpsQuality enum + * + * \param[in] sbgGpsType SbgECom GPS type + * \return NMEA GPS type + */ + static uint32_t convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType); public: diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 53e5291..cb428a4 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -512,7 +512,8 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT uint32_t MessageWrapper::convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType) { - switch (sbgGpsType) { + switch (sbgGpsType) + { case SBG_ECOM_POS_NO_SOLUTION: return SBG_NMEA_GPS_QUALITY_INVALID; case SBG_ECOM_POS_SINGLE: @@ -1446,39 +1447,39 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co // Writing NMEA sentence constexpr uint32_t nmea_sentence_buffer_size = 128; - char nmeas_sentence_buff[nmea_sentence_buffer_size]{}; - auto len = snprintf(nmeas_sentence_buff, nmea_sentence_buffer_size, + char nmea_sentence_buff[nmea_sentence_buffer_size]{}; + auto len = snprintf(nmea_sentence_buff, nmea_sentence_buffer_size, "$GPGGA,%02d%02d%02d.%02d,%02d%02.4f,%c,%03d%02.4f,%c,%d,%d,%.1f,%.1f,M,%d,M,%.1f,%04d", - current_hour, - current_minute, - current_second, - current_ms, - lat_degs, - lat_mins, - lat_dir, - lon_degs, - lon_mins, - lon_dir, - convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status)), - svUsed, - h_dop, - altitude, - static_cast(undulation), - diff_age, - baseStationId + current_hour, + current_minute, + current_second, + current_ms, + lat_degs, + lat_mins, + lat_dir, + lon_degs, + lon_mins, + lon_dir, + convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status)), + svUsed, + h_dop, + altitude, + static_cast(undulation), + diff_age, + baseStationId ); // Checksum computation uint8_t checksum = 0; for(int32_t i = 1; i < len; i++) { - checksum ^= nmeas_sentence_buff[i]; + checksum ^= nmea_sentence_buff[i]; } - snprintf(&nmeas_sentence_buff[len], nmea_sentence_buffer_size - len, "*%02X\r\n", checksum); + snprintf(&nmea_sentence_buff[len], nmea_sentence_buffer_size - len, "*%02X\r\n", checksum); // Populating ROS message gps_pos_nmea_msg.header = createRosHeader(ref_log_gps_pos.timeStamp); - gps_pos_nmea_msg.sentence = nmeas_sentence_buff; + gps_pos_nmea_msg.sentence = nmea_sentence_buff; return gps_pos_nmea_msg; } From 2612c90bf408adab53d488189aeadf1527f88103 Mon Sep 17 00:00:00 2001 From: cledant Date: Wed, 7 Jun 2023 14:58:40 +0200 Subject: [PATCH 09/74] Realigned members. --- include/sbg_driver/sbg_device.h | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index eddf4d3..3a5bd44 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -38,19 +38,19 @@ class SbgDevice //- Private variables -// //---------------------------------------------------------------------// - SbgEComHandle m_com_handle_; - SbgInterface m_sbg_interface_; - rclcpp::Node& m_ref_node_; - MessagePublisher m_message_publisher_; - std::shared_ptr m_message_subscriber_; - ConfigStore m_config_store_; - - uint32_t m_rate_frequency_; - - bool m_mag_calibration_ongoing_; - bool m_mag_calibration_done_; - SbgEComMagCalibResults m_magCalibResults; - rclcpp::Service::SharedPtr m_calib_service_; + SbgEComHandle m_com_handle_; + SbgInterface m_sbg_interface_; + rclcpp::Node& m_ref_node_; + MessagePublisher m_message_publisher_; + std::shared_ptr m_message_subscriber_; + ConfigStore m_config_store_; + + uint32_t m_rate_frequency_; + + bool m_mag_calibration_ongoing_; + bool m_mag_calibration_done_; + SbgEComMagCalibResults m_magCalibResults; + rclcpp::Service::SharedPtr m_calib_service_; rclcpp::Service::SharedPtr m_calib_save_service_; //---------------------------------------------------------------------// From 07ef7aabb43e4f7d6605bc08d8f2ad588f9f0951 Mon Sep 17 00:00:00 2001 From: cledant Date: Wed, 7 Jun 2023 15:19:33 +0200 Subject: [PATCH 10/74] Fixes in GGA serialization --- include/sbg_driver/message_wrapper.h | 2 +- src/message_wrapper.cpp | 26 ++++++++++++-------------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 53910bd..067d7f4 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -338,7 +338,7 @@ class MessageWrapper : public rclcpp::Node * \param[in] sbgGpsType SbgECom GPS type * \return NMEA GPS type */ - static uint32_t convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType); + static SbgNmeaGpsQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType); public: diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index cb428a4..7fb0425 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -510,12 +510,10 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT } } -uint32_t MessageWrapper::convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType) +sbg::SbgNmeaGpsQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType) { switch (sbgGpsType) { - case SBG_ECOM_POS_NO_SOLUTION: - return SBG_NMEA_GPS_QUALITY_INVALID; case SBG_ECOM_POS_SINGLE: case SBG_ECOM_POS_UNKNOWN_TYPE: case SBG_ECOM_POS_FIXED: @@ -531,6 +529,7 @@ uint32_t MessageWrapper::convertSbgGpsTypeToNmeaGpsType(uint32_t sbgGpsType) return SBG_NMEA_GPS_QUALITY_RTK; case SBG_ECOM_POS_RTK_FLOAT: return SBG_NMEA_GPS_QUALITY_RTK_FLOAT; + case SBG_ECOM_POS_NO_SOLUTION: default: return SBG_NMEA_GPS_QUALITY_INVALID; } @@ -1354,13 +1353,13 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co // Latitude conversion char lat_dir; float latitude = ref_log_gps_pos.latitude; - if (latitude >= 1000.0f) + if (latitude >= 90.0f) { - latitude = 999.9999f; + latitude = 90.0; } - if (latitude <= -100.0f) + if (latitude <= -90.0f) { - latitude = -99.9999f; + latitude = -90.0f; } int32_t lat_degs = latitude; float lat_mins = (latitude - static_cast(lat_degs)) * 60.0f; @@ -1378,13 +1377,13 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co // Longitude conversion char lon_dir; float longitude = ref_log_gps_pos.longitude; - if (longitude >= 10000.0f) + if (longitude >= 180.0f) { - longitude = 9999.9999f; + longitude = 180.0f; } - if (longitude <= -1000.0f) + if (longitude <= -180.0f) { - longitude = -999.9999f; + longitude = -180.0f; } int32_t lon_degs = longitude; float lon_mins = (longitude - static_cast(lon_degs)) * 60.0f; @@ -1400,8 +1399,7 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co } // DOP computation - double h_dop = sqrt((ref_log_gps_pos.latitudeAccuracy * ref_log_gps_pos.latitudeAccuracy) + \ - (ref_log_gps_pos.longitudeAccuracy * ref_log_gps_pos.longitudeAccuracy)); + double h_dop = std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy); if (h_dop >= 10.0) { h_dop = 9.9; @@ -1478,7 +1476,7 @@ const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(co snprintf(&nmea_sentence_buff[len], nmea_sentence_buffer_size - len, "*%02X\r\n", checksum); // Populating ROS message - gps_pos_nmea_msg.header = createRosHeader(ref_log_gps_pos.timeStamp); + gps_pos_nmea_msg.header = createRosHeader(ref_log_gps_pos.timeStamp); gps_pos_nmea_msg.sentence = nmea_sentence_buff; return gps_pos_nmea_msg; From 2c8fbcf64d0c20c8e4ee0e5228a89dd2d57c1598 Mon Sep 17 00:00:00 2001 From: cledant Date: Wed, 7 Jun 2023 17:14:26 +0200 Subject: [PATCH 11/74] Namespace related coding style fix --- include/sbg_driver/message_subscriber.h | 2 +- src/config_store.cpp | 12 ++++++------ src/message_subscriber.cpp | 2 +- src/message_wrapper.cpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h index 68e02d6..a761cef 100644 --- a/include/sbg_driver/message_subscriber.h +++ b/include/sbg_driver/message_subscriber.h @@ -82,7 +82,7 @@ class MessageSubscriber: public rclcpp::Node * * \param[in] ref_config_store Store configuration for the subscribers. */ - void initTopicSubscriptions(const sbg::ConfigStore &ref_config_store); + void initTopicSubscriptions(const ConfigStore &ref_config_store); }; } diff --git a/src/config_store.cpp b/src/config_store.cpp index f926d11..519cdf1 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -193,7 +193,7 @@ void ConfigStore::loadOutputTimeReference(const rclcpp::Node& ref_node_handle, c } } -void sbg::ConfigStore::loadRtcmParameters(const rclcpp::Node &ref_node_handle) +void ConfigStore::loadRtcmParameters(const rclcpp::Node &ref_node_handle) { ref_node_handle.get_parameter_or("rtcm.listen_rtcm", m_listen_rtcm_, false); ref_node_handle.get_parameter_or("rtcm.topic_name", m_rtcm_topic_name_, "rtcm"); @@ -201,7 +201,7 @@ void sbg::ConfigStore::loadRtcmParameters(const rclcpp::Node &ref_node_handle) m_rtcm_full_topic_ = m_rtcm_topic_namespace_ + "/" + m_rtcm_topic_name_; } -void sbg::ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) +void ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) { ref_node_handle.get_parameter_or("nmea.publish_nmea", m_publish_nmea_, false); ref_node_handle.get_parameter_or("nmea.topic_name", m_nmea_topic_name_, "nmea"); @@ -388,22 +388,22 @@ const std::string &ConfigStore::getOdomInitFrameId(void) const return m_odom_init_frame_id_; } -bool sbg::ConfigStore::shouldListenRtcm(void) const +bool ConfigStore::shouldListenRtcm(void) const { return m_listen_rtcm_; } -const std::string &sbg::ConfigStore::getRtcmFullTopic(void) const +const std::string &ConfigStore::getRtcmFullTopic(void) const { return m_rtcm_full_topic_; } -bool sbg::ConfigStore::shouldPublishNmea(void) const +bool ConfigStore::shouldPublishNmea(void) const { return m_publish_nmea_; } -const std::string &sbg::ConfigStore::getNmeaFullTopic(void) const +const std::string &ConfigStore::getNmeaFullTopic(void) const { return m_nmea_full_topic_; } diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp index e0e3d11..2f3760b 100644 --- a/src/message_subscriber.cpp +++ b/src/message_subscriber.cpp @@ -44,7 +44,7 @@ void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedP //- Operations -// //---------------------------------------------------------------------// -void MessageSubscriber::initTopicSubscriptions(const sbg::ConfigStore &ref_config_store) +void MessageSubscriber::initTopicSubscriptions(const ConfigStore &ref_config_store) { auto rtcm_cb = [&](const mavros_msgs::msg::RTCM::SharedPtr msg) -> void { this->readRosRtcmMessage(msg); diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 7fb0425..56dd5eb 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1337,7 +1337,7 @@ const sensor_msgs::msg::FluidPressure MessageWrapper::createRosFluidPressureMess return fluid_pressure_message; } -const nmea_msgs::msg::Sentence sbg::MessageWrapper::createSbgGpsPosMessageGGA(const SbgLogGpsPos &ref_log_gps_pos) const +const nmea_msgs::msg::Sentence MessageWrapper::createSbgGpsPosMessageGGA(const SbgLogGpsPos &ref_log_gps_pos) const { nmea_msgs::msg::Sentence gps_pos_nmea_msg; From d30ba532c06449a7cde8e8adad925aac182f640b Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 12 Jun 2023 13:17:58 +0200 Subject: [PATCH 12/74] Removed threaded subscription. --- include/sbg_driver/message_subscriber.h | 9 ++++----- include/sbg_driver/sbg_device.h | 2 +- src/main.cpp | 17 +++++++++++------ src/message_subscriber.cpp | 15 ++++++++++----- src/sbg_device.cpp | 8 +++----- 5 files changed, 29 insertions(+), 22 deletions(-) diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h index a761cef..e6d86ef 100644 --- a/include/sbg_driver/message_subscriber.h +++ b/include/sbg_driver/message_subscriber.h @@ -41,7 +41,7 @@ namespace sbg /*! * Class to subscribe to all corresponding topics. */ -class MessageSubscriber: public rclcpp::Node +class MessageSubscriber { private: @@ -68,21 +68,20 @@ class MessageSubscriber: public rclcpp::Node /*! * Default constructor. - * - * \param[in] sbg_interface SBG Interface handle. */ - MessageSubscriber(SbgInterface *sbg_interface); + MessageSubscriber(); //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// + void setSbgInterface(SbgInterface *sbg_interface); /*! * Initialize the subscribers with configuration. * * \param[in] ref_config_store Store configuration for the subscribers. */ - void initTopicSubscriptions(const ConfigStore &ref_config_store); + void initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, const ConfigStore &ref_config_store); }; } diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index 3a5bd44..4df4d28 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -42,7 +42,7 @@ class SbgDevice SbgInterface m_sbg_interface_; rclcpp::Node& m_ref_node_; MessagePublisher m_message_publisher_; - std::shared_ptr m_message_subscriber_; + MessageSubscriber m_message_subscriber_; ConfigStore m_config_store_; uint32_t m_rate_frequency_; diff --git a/src/main.cpp b/src/main.cpp index 7ed804a..b617988 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,25 +5,30 @@ using sbg::SbgDevice; int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::Node node_handle("sbg_device"); + auto node_handle = std::make_shared("sbg_device"); try { uint32_t loopFrequency; - RCLCPP_INFO(node_handle.get_logger(), "SBG DRIVER - Init node, load params and connect to the device."); - SbgDevice sbg_device(node_handle); + RCLCPP_INFO(node_handle->get_logger(), "SBG DRIVER - Init node, load params and connect to the device."); + SbgDevice sbg_device(*node_handle); - RCLCPP_INFO(node_handle.get_logger(), "SBG DRIVER - Initialize device for receiving data"); + RCLCPP_INFO(node_handle->get_logger(), "SBG DRIVER - Initialize device for receiving data"); sbg_device.initDeviceForReceivingData(); loopFrequency = sbg_device.getUpdateFrequency(); - RCLCPP_INFO(node_handle.get_logger(), "SBG DRIVER - ROS Node frequency : %u Hz", loopFrequency); + RCLCPP_INFO(node_handle->get_logger(), "SBG DRIVER - ROS Node frequency : %u Hz", loopFrequency); rclcpp::Rate loop_rate(loopFrequency); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_handle); + + std::chrono::duration timeout(1000000); while (rclcpp::ok()) { sbg_device.periodicHandle(); + executor.spin_some(timeout); loop_rate.sleep(); } @@ -32,7 +37,7 @@ int main(int argc, char **argv) } catch (std::exception const& refE) { - RCLCPP_ERROR(node_handle.get_logger(), "SBG_DRIVER - %s", refE.what()); + RCLCPP_ERROR(node_handle->get_logger(), "SBG_DRIVER - %s", refE.what()); } return 0; diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp index 2f3760b..301e52d 100644 --- a/src/message_subscriber.cpp +++ b/src/message_subscriber.cpp @@ -9,14 +9,14 @@ using sbg::MessageSubscriber; /*! - * Class to publish all SBG-ROS messages to the corresponding publishers. + * Class to subscribe to all corresponding topics. */ //---------------------------------------------------------------------// //- Constructor -// //---------------------------------------------------------------------// -MessageSubscriber::MessageSubscriber(SbgInterface *sbg_interface): Node("sbg_subscriber"), -m_max_messages_(10), m_sbg_interface_(sbg_interface) +MessageSubscriber::MessageSubscriber(): +m_max_messages_(10), m_sbg_interface_(nullptr) { } @@ -44,7 +44,12 @@ void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedP //- Operations -// //---------------------------------------------------------------------// -void MessageSubscriber::initTopicSubscriptions(const ConfigStore &ref_config_store) +void MessageSubscriber::setSbgInterface(SbgInterface *sbg_interface) +{ + m_sbg_interface_ = sbg_interface; +} + +void MessageSubscriber::initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, const ConfigStore &ref_config_store) { auto rtcm_cb = [&](const mavros_msgs::msg::RTCM::SharedPtr msg) -> void { this->readRosRtcmMessage(msg); @@ -52,7 +57,7 @@ void MessageSubscriber::initTopicSubscriptions(const ConfigStore &ref_config_sto if (ref_config_store.shouldListenRtcm()) { - m_rtcm_sub_ = create_subscription( + m_rtcm_sub_ = ref_ros_node_handle.create_subscription( ref_config_store.getRtcmFullTopic(), m_max_messages_, rtcm_cb); } } diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 468d157..76d300c 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -214,11 +214,9 @@ void SbgDevice::initSubscribers(void) return; } - m_message_subscriber_ = std::make_shared(&m_sbg_interface_); - m_message_subscriber_->initTopicSubscriptions(m_config_store_); - std::thread([&]{ - rclcpp::spin(m_message_subscriber_); - }).detach(); + // TODO no pointer + m_message_subscriber_.setSbgInterface(&m_sbg_interface_); + m_message_subscriber_.initTopicSubscriptions(m_ref_node_,m_config_store_); } void SbgDevice::configure(void) From a6b07f326206041168791fd29f4c92796322cd82 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 12 Jun 2023 13:38:52 +0200 Subject: [PATCH 13/74] Removed SbgInterface as class member --- include/sbg_driver/message_subscriber.h | 8 ++++---- src/message_subscriber.cpp | 19 +++++++------------ src/sbg_device.cpp | 4 +--- 3 files changed, 12 insertions(+), 19 deletions(-) diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h index e6d86ef..c066f3d 100644 --- a/include/sbg_driver/message_subscriber.h +++ b/include/sbg_driver/message_subscriber.h @@ -48,7 +48,6 @@ class MessageSubscriber rclcpp::Subscription::SharedPtr m_rtcm_sub_; uint32_t m_max_messages_; - SbgInterface *m_sbg_interface_; //---------------------------------------------------------------------// //- Private methods -// @@ -58,7 +57,7 @@ class MessageSubscriber * * \param[in] msg ROS RTCM message. */ - void readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedPtr msg) const; + void readRosRtcmMessage(SbgInterface &sbg_interface, const mavros_msgs::msg::RTCM::SharedPtr msg) const; public: @@ -75,13 +74,14 @@ class MessageSubscriber //- Operations -// //---------------------------------------------------------------------// - void setSbgInterface(SbgInterface *sbg_interface); /*! * Initialize the subscribers with configuration. * * \param[in] ref_config_store Store configuration for the subscribers. */ - void initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, const ConfigStore &ref_config_store); + void initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, + SbgInterface &sbg_interface, + const ConfigStore &ref_config_store); }; } diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp index 301e52d..881a9f1 100644 --- a/src/message_subscriber.cpp +++ b/src/message_subscriber.cpp @@ -16,7 +16,7 @@ using sbg::MessageSubscriber; //---------------------------------------------------------------------// MessageSubscriber::MessageSubscriber(): -m_max_messages_(10), m_sbg_interface_(nullptr) +m_max_messages_(10) { } @@ -24,12 +24,10 @@ m_max_messages_(10), m_sbg_interface_(nullptr) //- Private methods -// //---------------------------------------------------------------------// -void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedPtr msg) const +void MessageSubscriber::readRosRtcmMessage(SbgInterface &sbg_interface, const mavros_msgs::msg::RTCM::SharedPtr msg) const { - assert(m_sbg_interface_); - auto rtcm_data = msg->data; - auto error_code = sbgInterfaceWrite(m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); + auto error_code = sbgInterfaceWrite(&sbg_interface, rtcm_data.data(), rtcm_data.size()); if (error_code != SBG_NO_ERROR) { char error_str[256]; @@ -44,15 +42,12 @@ void MessageSubscriber::readRosRtcmMessage(const mavros_msgs::msg::RTCM::SharedP //- Operations -// //---------------------------------------------------------------------// -void MessageSubscriber::setSbgInterface(SbgInterface *sbg_interface) -{ - m_sbg_interface_ = sbg_interface; -} - -void MessageSubscriber::initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, const ConfigStore &ref_config_store) +void MessageSubscriber::initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, + SbgInterface &sbg_interface, + const ConfigStore &ref_config_store) { auto rtcm_cb = [&](const mavros_msgs::msg::RTCM::SharedPtr msg) -> void { - this->readRosRtcmMessage(msg); + this->readRosRtcmMessage(sbg_interface, msg); }; if (ref_config_store.shouldListenRtcm()) diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 76d300c..c387919 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -214,9 +214,7 @@ void SbgDevice::initSubscribers(void) return; } - // TODO no pointer - m_message_subscriber_.setSbgInterface(&m_sbg_interface_); - m_message_subscriber_.initTopicSubscriptions(m_ref_node_,m_config_store_); + m_message_subscriber_.initTopicSubscriptions(m_ref_node_, m_sbg_interface_, m_config_store_); } void SbgDevice::configure(void) From b60629325d0af3d5a9ba6ae2ef4b9322e85faa47 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 12 Jun 2023 13:56:14 +0200 Subject: [PATCH 14/74] Updated documentation --- README.md | 5 +---- include/sbg_driver/message_subscriber.h | 5 ++++- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 37404cb..e3de5f3 100644 --- a/README.md +++ b/README.md @@ -93,10 +93,7 @@ Launch the sbg_device_mag node to calibrate the magnetometers, and load the `ell ## Nodes ### sbg_device -The `sbg_device` node handles the communication with the connected device, and publishes the SBG output to the Ros environment. - -### sbg_subscriber -The `sbg_subscriber` node handles the subscriptions to the Ros environment topics, and forward data the device. +The `sbg_device` node handles the communication with the connected device, publishes the SBG output to the Ros environment and subscribes to topics. #### Published Topics ##### SBG specific topics diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h index c066f3d..1c4c22a 100644 --- a/include/sbg_driver/message_subscriber.h +++ b/include/sbg_driver/message_subscriber.h @@ -55,7 +55,8 @@ class MessageSubscriber /*! * Handler for subscription to RTCM topic. * - * \param[in] msg ROS RTCM message. + * \param[in] sbg_interface Interface to use for read/write operations. + * \param[in] msg ROS RTCM message. */ void readRosRtcmMessage(SbgInterface &sbg_interface, const mavros_msgs::msg::RTCM::SharedPtr msg) const; @@ -77,6 +78,8 @@ class MessageSubscriber /*! * Initialize the subscribers with configuration. * + * \param[in] ref_ros_node_handle Ros Node to advertise the subscribers. + * \param[in] sbg_interface Interface to use for read/write operations. * \param[in] ref_config_store Store configuration for the subscribers. */ void initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, From 5979b241fcfe7486a92a0481c08432a4ddf1e0a0 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 12 Jun 2023 15:05:46 +0200 Subject: [PATCH 15/74] Switched dependency from mavros_msgs to rtcm_msgs --- CMakeLists.txt | 6 ++--- README.md | 2 +- include/sbg_driver/message_subscriber.h | 6 ++--- package.xml | 32 +------------------------ src/message_subscriber.cpp | 8 +++---- 5 files changed, 12 insertions(+), 42 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d0793a6..8f54f54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(nav_msgs REQUIRED) -find_package(mavros_msgs REQUIRED) +find_package(rtcm_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -30,7 +30,7 @@ set (USED_LIBRARIES std_srvs geometry_msgs nav_msgs - mavros_msgs + rtcm_msgs tf2_ros tf2_msgs tf2_geometry_msgs @@ -179,7 +179,7 @@ ament_export_dependencies( std_srvs geometry_msgs nav_msgs - mavros_msgs + rtcm_msgs tf2_ros tf2_msgs tf2_geometry_msgs diff --git a/README.md b/README.md index e3de5f3..c2cd646 100644 --- a/README.md +++ b/README.md @@ -224,7 +224,7 @@ Data published on that topic can be used by a third party ROS2 module for NTRIP ##### RTCM topics SBG ROS Driver will listen to some topics published by third party ROS2 modules. -* **`/ntrip_client/rtcm`** [mavros_msgs/RTCM](http://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/RTCM.html) +* **`/ntrip_client/rtcm`** [rtcm_msgs/Message](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg) RTCM data from `/ntrip_client/rtcm` will be forwarded to the IMU. Namespace `ntrip_client` and topic_name `rtcm` can be customized in .yaml config files. diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h index 1c4c22a..db6749e 100644 --- a/include/sbg_driver/message_subscriber.h +++ b/include/sbg_driver/message_subscriber.h @@ -34,7 +34,7 @@ // Project headers #include -#include +#include namespace sbg { @@ -45,7 +45,7 @@ class MessageSubscriber { private: - rclcpp::Subscription::SharedPtr m_rtcm_sub_; + rclcpp::Subscription::SharedPtr m_rtcm_sub_; uint32_t m_max_messages_; @@ -58,7 +58,7 @@ class MessageSubscriber * \param[in] sbg_interface Interface to use for read/write operations. * \param[in] msg ROS RTCM message. */ - void readRosRtcmMessage(SbgInterface &sbg_interface, const mavros_msgs::msg::RTCM::SharedPtr msg) const; + void readRosRtcmMessage(SbgInterface &sbg_interface, const rtcm_msgs::msg::Message::SharedPtr msg) const; public: diff --git a/package.xml b/package.xml index 13b8527..06e4bad 100644 --- a/package.xml +++ b/package.xml @@ -11,34 +11,6 @@ http://wiki.ros.org/sbg_driver - - - ament_cmake rosidl_default_generators @@ -48,7 +20,7 @@ std_msgs std_srvs nav_msgs - mavros_msgs + rtcm_msgs tf2_ros tf2_msgs tf2_geometry_msgs @@ -59,8 +31,6 @@ rosidl_interface_packages - - ament_cmake diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp index 881a9f1..ccc13f8 100644 --- a/src/message_subscriber.cpp +++ b/src/message_subscriber.cpp @@ -24,9 +24,9 @@ m_max_messages_(10) //- Private methods -// //---------------------------------------------------------------------// -void MessageSubscriber::readRosRtcmMessage(SbgInterface &sbg_interface, const mavros_msgs::msg::RTCM::SharedPtr msg) const +void MessageSubscriber::readRosRtcmMessage(SbgInterface &sbg_interface, const rtcm_msgs::msg::Message::SharedPtr msg) const { - auto rtcm_data = msg->data; + auto rtcm_data = msg->message; auto error_code = sbgInterfaceWrite(&sbg_interface, rtcm_data.data(), rtcm_data.size()); if (error_code != SBG_NO_ERROR) { @@ -46,13 +46,13 @@ void MessageSubscriber::initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle SbgInterface &sbg_interface, const ConfigStore &ref_config_store) { - auto rtcm_cb = [&](const mavros_msgs::msg::RTCM::SharedPtr msg) -> void { + auto rtcm_cb = [&](const rtcm_msgs::msg::Message::SharedPtr msg) -> void { this->readRosRtcmMessage(sbg_interface, msg); }; if (ref_config_store.shouldListenRtcm()) { - m_rtcm_sub_ = ref_ros_node_handle.create_subscription( + m_rtcm_sub_ = ref_ros_node_handle.create_subscription( ref_config_store.getRtcmFullTopic(), m_max_messages_, rtcm_cb); } } From 3dd22610b3d65451290bd8daa18afade43a78837 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 12 Jun 2023 16:05:49 +0200 Subject: [PATCH 16/74] Removed MessageSubscriber class --- CMakeLists.txt | 1 - include/sbg_driver/message_subscriber.h | 91 ------------------------- include/sbg_driver/sbg_device.h | 36 ++++++---- src/message_subscriber.cpp | 58 ---------------- src/sbg_device.cpp | 29 ++++++-- 5 files changed, 47 insertions(+), 168 deletions(-) delete mode 100644 include/sbg_driver/message_subscriber.h delete mode 100644 src/message_subscriber.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f54f54..a35d16b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,7 +102,6 @@ include_directories( set (SBG_COMMON_RESOURCES src/config_applier.cpp src/message_publisher.cpp - src/message_subscriber.cpp src/message_wrapper.cpp src/config_store.cpp src/sbg_device.cpp diff --git a/include/sbg_driver/message_subscriber.h b/include/sbg_driver/message_subscriber.h deleted file mode 100644 index db6749e..0000000 --- a/include/sbg_driver/message_subscriber.h +++ /dev/null @@ -1,91 +0,0 @@ -/*! -* \file message_subscriber.h -* \author SBG Systems -* \date 26/05/2023 -* -* \brief Manage subscription of messages. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2023 SBG Systems -* -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: -* -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. -* -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. -*/ - -#ifndef SBG_ROS_MESSAGE_SUBSCRIBER_H -#define SBG_ROS_MESSAGE_SUBSCRIBER_H - -// Project headers -#include -#include - -namespace sbg -{ -/*! - * Class to subscribe to all corresponding topics. - */ -class MessageSubscriber -{ -private: - - rclcpp::Subscription::SharedPtr m_rtcm_sub_; - - uint32_t m_max_messages_; - - //---------------------------------------------------------------------// - //- Private methods -// - //---------------------------------------------------------------------// - /*! - * Handler for subscription to RTCM topic. - * - * \param[in] sbg_interface Interface to use for read/write operations. - * \param[in] msg ROS RTCM message. - */ - void readRosRtcmMessage(SbgInterface &sbg_interface, const rtcm_msgs::msg::Message::SharedPtr msg) const; - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - */ - MessageSubscriber(); - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Initialize the subscribers with configuration. - * - * \param[in] ref_ros_node_handle Ros Node to advertise the subscribers. - * \param[in] sbg_interface Interface to use for read/write operations. - * \param[in] ref_config_store Store configuration for the subscribers. - */ - void initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, - SbgInterface &sbg_interface, - const ConfigStore &ref_config_store); -}; -} - -#endif // SBG_ROS_MESSAGE_SUBSCRIBER_H diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index 4df4d28..10cb4de 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -9,12 +9,12 @@ // ROS headers #include #include +#include // Project headers #include #include #include -#include namespace sbg { @@ -38,20 +38,21 @@ class SbgDevice //- Private variables -// //---------------------------------------------------------------------// - SbgEComHandle m_com_handle_; - SbgInterface m_sbg_interface_; - rclcpp::Node& m_ref_node_; - MessagePublisher m_message_publisher_; - MessageSubscriber m_message_subscriber_; - ConfigStore m_config_store_; + SbgEComHandle m_com_handle_; + SbgInterface m_sbg_interface_; + rclcpp::Node& m_ref_node_; + MessagePublisher m_message_publisher_; + ConfigStore m_config_store_; - uint32_t m_rate_frequency_; + uint32_t m_rate_frequency_; - bool m_mag_calibration_ongoing_; - bool m_mag_calibration_done_; - SbgEComMagCalibResults m_magCalibResults; - rclcpp::Service::SharedPtr m_calib_service_; - rclcpp::Service::SharedPtr m_calib_save_service_; + bool m_mag_calibration_ongoing_; + bool m_mag_calibration_done_; + SbgEComMagCalibResults m_magCalibResults; + rclcpp::Service::SharedPtr m_calib_service_; + rclcpp::Service::SharedPtr m_calib_save_service_; + + rclcpp::Subscription::SharedPtr rtcm_sub_; //---------------------------------------------------------------------// //- Private methods -// @@ -113,7 +114,7 @@ class SbgDevice /*! * Initialize the subscribers according to the configuration. */ - void initSubscribers(void); + void initSubscribers(); /*! * Configure the connected SBG device. @@ -173,6 +174,13 @@ class SbgDevice */ void exportMagCalibrationResults(void) const; + /*! + * Handler for subscription to RTCM topic. + * + * \param[in] msg ROS RTCM message. + */ + void writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPtr msg); + public: //---------------------------------------------------------------------// diff --git a/src/message_subscriber.cpp b/src/message_subscriber.cpp deleted file mode 100644 index ccc13f8..0000000 --- a/src/message_subscriber.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "message_subscriber.h" - -// ROS headers -#include - -// SBG headers -#include - -using sbg::MessageSubscriber; - -/*! - * Class to subscribe to all corresponding topics. - */ -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -MessageSubscriber::MessageSubscriber(): -m_max_messages_(10) -{ -} - -//---------------------------------------------------------------------// -//- Private methods -// -//---------------------------------------------------------------------// - -void MessageSubscriber::readRosRtcmMessage(SbgInterface &sbg_interface, const rtcm_msgs::msg::Message::SharedPtr msg) const -{ - auto rtcm_data = msg->message; - auto error_code = sbgInterfaceWrite(&sbg_interface, rtcm_data.data(), rtcm_data.size()); - if (error_code != SBG_NO_ERROR) - { - char error_str[256]; - - sbgEComErrorToString(error_code, error_str); - SBG_LOG_ERROR(SBG_ERROR, "Failed to sent RTCM data to device: %s", error_str); - } - -} - -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// - -void MessageSubscriber::initTopicSubscriptions(rclcpp::Node& ref_ros_node_handle, - SbgInterface &sbg_interface, - const ConfigStore &ref_config_store) -{ - auto rtcm_cb = [&](const rtcm_msgs::msg::Message::SharedPtr msg) -> void { - this->readRosRtcmMessage(sbg_interface, msg); - }; - - if (ref_config_store.shouldListenRtcm()) - { - m_rtcm_sub_ = ref_ros_node_handle.create_subscription( - ref_config_store.getRtcmFullTopic(), m_max_messages_, rtcm_cb); - } -} diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index c387919..a9b406c 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -207,14 +207,22 @@ void SbgDevice::initPublishers(void) m_rate_frequency_ = m_config_store_.getReadingRateFrequency(); } -void SbgDevice::initSubscribers(void) +void SbgDevice::initSubscribers() { if (!m_config_store_.shouldListenRtcm()) { return; } - m_message_subscriber_.initTopicSubscriptions(m_ref_node_, m_sbg_interface_, m_config_store_); + auto rtcm_cb = [&](const rtcm_msgs::msg::Message::SharedPtr msg) -> void { + this->writeRtcmMessageToDevice(msg); + }; + + if (m_config_store_.shouldListenRtcm()) + { + rtcm_sub_ = m_ref_node_.create_subscription( + m_config_store_.getRtcmFullTopic(), 10, rtcm_cb); + } } void SbgDevice::configure(void) @@ -461,6 +469,19 @@ void SbgDevice::exportMagCalibrationResults(void) const RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Magnetometers calibration results saved to file %s", output_filename.c_str()); } +void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPtr msg) +{ + auto rtcm_data = msg->message; + auto error_code = sbgInterfaceWrite(&m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); + if (error_code != SBG_NO_ERROR) + { + char error_str[256]; + + sbgEComErrorToString(error_code, error_str); + SBG_LOG_ERROR(SBG_ERROR, "Failed to sent RTCM data to device: %s", error_str); + } +} + //---------------------------------------------------------------------// //- Parameters -// //---------------------------------------------------------------------// @@ -478,7 +499,6 @@ void SbgDevice::initDeviceForReceivingData(void) { SbgErrorCode error_code; initPublishers(); - initSubscribers(); configure(); error_code = sbgEComSetReceiveLogCallback(&m_com_handle_, onLogReceivedCallback, this); @@ -487,6 +507,8 @@ void SbgDevice::initDeviceForReceivingData(void) { rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to set the callback function - " + std::string(sbgErrorCodeToString(error_code))); } + + initSubscribers(); } void SbgDevice::initDeviceForMagCalibration(void) @@ -500,5 +522,4 @@ void SbgDevice::initDeviceForMagCalibration(void) void SbgDevice::periodicHandle(void) { sbgEComHandle(&m_com_handle_); - } From 06b37974a45be9bdb8fc9c3088e6725daa9ab39b Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 27 Jun 2023 17:57:33 +0200 Subject: [PATCH 17/74] Added documentation about RTCM messages and device configuration. --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index c2cd646..c96ce0b 100644 --- a/README.md +++ b/README.md @@ -253,6 +253,11 @@ confWithRos: true Then, modify the desired parameters in the config file, using the SBG Firmware Manual, to see which features are configurable, and which parameter values are available. +### Configuration for SBG device and RTCM messages +The SBG Ros driver forwards RTCM messages received to the device. For the device to listen to those messages, a specific configuration must be applied manually. +Use `sbgCenter`, to configure the device. +In `Assignment panel`, `RTCM` should be set to `Port A`. + ### Calibrate the magnetometers Ellipse-A/E/N use magnemoter to provide heading. A calibration is then required to compensate soft and hard iron distortions due to the environmenent (motors, batteries, ...). The magnetic calibration procedure should be held in a non magnetic area (outside of buildings). From ec40b1945f01bf7255a5277280e30f038d74f263 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Fri, 11 Aug 2023 14:20:24 +0200 Subject: [PATCH 18/74] Reworked and improved main project README and small fixed in yaml examples --- README.md | 165 ++++++++++++++++---------- config/example/ellipse_A_default.yaml | 2 +- config/example/ellipse_D_default.yaml | 2 +- config/example/ellipse_E_default.yaml | 2 +- config/example/ellipse_N_default.yaml | 2 +- 5 files changed, 108 insertions(+), 65 deletions(-) diff --git a/README.md b/README.md index c96ce0b..88e0016 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,23 @@ # sbg_driver - -## Overview -ROS package for SBG Systems IMU.
-The driver allows the user to configure the IMU (if possible, according to the device), to receive messages from the Sbg message protocol, publish ROS standard messages , and to calibrate the magnetometers. +ROS2 driver package for SBG Systems IMU, AHRS and INS. +This driver package uses the [sbgECom binary protocol](https://github.com/SBG-Systems/sbgECom) to read data and configure SBG Systems devices. Initial work has been done by [ENSTA Bretagne](https://github.com/ENSTABretagneRobotics). -**Author : [SBG Systems](https://www.sbg-systems.com/)
-Maintainer : SBG Systems, support@sbg-systems.com** +**Author: [SBG Systems](https://www.sbg-systems.com/)** +**Maintainer: [SBG Systems](https://www.sbg-systems.com/)** +**Contact:** support@sbg-systems.com + +## Features +The driver supports the following features: + - Configure ELLIPSE products using yaml files (see note below) + - Parse IMU/AHRS/INS/GNSS using the sbgECom protocol + - Publish standard ROS messages and more detailed specific SBG Systems topics + - Subscribe and forward RTCM data to support DGPS/RTK mode with centimeters-level accuracy + - Calibrate 2D/3D magnetic field using the on-board ELLIPSE algorithms + +> [!NOTE] +> Only ELLIPSE devices can be configured from the ROS driver. For High Performance INS such as EKINOX, APOGEE and QUANTA, please use the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) ## Installation ### Installation from Packages @@ -18,8 +28,8 @@ User can install the sbg_ros2_driver through the standard ROS installation syste ### Building from sources #### Dependencies * [Robot Operating System (ROS)](http://wiki.ros.org/) -* SBG communication protocol sbgECom, v1.11.920-stable (full compatible with firmwares from 1.7.x). -* Boost +* [sbgECom C Library](https://github.com/SBG-Systems/sbgECom) (embeds v1.11.920-stable - compatible with ELLIPSE firmware above 1.7) +* [Boost C++ Library](https://www.boost.org/) #### Building 1. Clone the repository (use a Release version) @@ -52,54 +62,57 @@ ros2 launch sbg_driver sbg_device_mag_calibration_launch.py ## Config files ### Default config files -Every configuration file is defined according to the same structure.
+Every configuration file is defined according to the same structure. -* **sbg_device_uart_default.yaml**
-This config file is the default one for Uart connection with the device.
-It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node).
-It defines a few outputs for the device : +* **sbg_device_uart_default.yaml** +This config file is the default one for UART connection with the device. +It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node). +It defines a few outputs for the device: * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. -* **sbg_device_udp_default.yaml**
-This config file is the default one for an Udp connection with the device.
-It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node).
-It defines a few outputs for the device : +* **sbg_device_udp_default.yaml** +This config file is the default one for an Udp connection with the device. +It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node). +It defines a few outputs for the device: * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. ### Example config files -* **ellipse_A_default.yaml**
+* **ellipse_A_default.yaml** Default config file for an Ellipse-A. -* **ellipse_E_default.yaml**
-Default config file for an Ellipse-E with an external antenna and external Gnss. +* **ellipse_E_default.yaml** +Default config file for an Ellipse-E with an external NMEA GNSS. -* **ellipse_N_default.yaml**
-Default config file for an Ellipse-N with an external antenna and internal Gnss. +* **ellipse_N_default.yaml** +Default config file for an Ellipse-N using internal GNSS. -* **ellipse_D_default.yaml**
-Default config file for an Ellipse-D. +* **ellipse_D_default.yaml** +Default config file for an Ellipse-D using internal GNSS. ## Launch files ### Default launch files -* **sbg_device_launch.py**
-Launch the sbg_device node to handle the receivde data, and load the `sbg_device_uart_default.yaml` configuration. +* **sbg_device_launch.py** +Launch the sbg_device node to handle the received data, and load the `sbg_device_uart_default.yaml` configuration. -* **sbg_device_mag_calibration_launch.py**
+* **sbg_device_mag_calibration_launch.py** Launch the sbg_device_mag node to calibrate the magnetometers, and load the `ellipse_E_default.yaml` configuration. ## Nodes -### sbg_device -The `sbg_device` node handles the communication with the connected device, publishes the SBG output to the Ros environment and subscribes to topics. +### sbg_device node +The `sbg_device` node handles the communication with the connected device, publishes the SBG output to the Ros environment and subscribes to useful topics such as RTCM data streams. #### Published Topics -##### SBG specific topics +##### SBG Systems specific topics +SBG Systems has defined proprietary ROS messages to report more detailed information from the AHRS/INS. +These messages try to match as much as possible the sbgECom logs as they are output by the device. + * **`/sbg/status`** [sbg_driver/SbgStatus](http://docs.ros.org/api/sbg_driver/html/msg/SbgStatus.html) - Provides informations about the general status (Communication, Aiding, etc..). + Provides information about the general status (Communication, Aiding, etc..). * **`/sbg/utc_time`** [sbg_driver/SbgUtcTime](http://docs.ros.org/api/sbg_driver/html/msg/SbgUtcTime.html) @@ -123,7 +136,7 @@ The `sbg_device` node handles the communication with the connected device, publi * **`/sbg/mag`** [sbg_driver/SbgMag](http://docs.ros.org/api/sbg_driver/html/msg/SbgMag.html) - Magnetic data. + Calibrated magnetic field measurement. * **`/sbg/mag_calib`** [sbg_driver/SbgMagCalib](http://docs.ros.org/api/sbg_driver/html/msg/SbgMagCalib.html) @@ -209,28 +222,35 @@ For each ROS standard, you have to activate the needed SBG outputs. UTM projected position relative to the first valid INS position. Requires `/sbg/imu_data` and `/sbg/ekv_nav` and either `/sbg/ekf_euler` or `/sbg/ekf_quat`. - Disabled by default, set odometry.enable in configuration file. + Disabled by default, set `odometry.enable` in configuration file. ##### NMEA topics -GPS data in NMEA format can be published when `publish_nmea` is set to `true` in .yaml config file. -Data published on that topic can be used by a third party ROS2 module for NTRIP purpose. +The driver can publish NMEA GGA messages from the internal GNSS receiver. It can be used with third party [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) modules to support VRS networks providers. -* **`/ntrip_client/nmea`** [nmea_msgs/Sentence](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html) + Disabled by default, set `publish_nmea` to `true` in .yaml config file to use this feature. +* **`/ntrip_client/nmea`** [nmea_msgs/Sentence](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html) + Data from `/sbg/gps_pos` serialized into NMEA GGA format. Requires `/sbg/gps_pos`. Namespace `ntrip_client` and topic_name `nmea` can be customized in .yaml config files. #### Subscribed Topics ##### RTCM topics -SBG ROS Driver will listen to some topics published by third party ROS2 modules. +The `sbg_device` node can subscribe to RTCM topics published by third party ROS2 modules. +Incoming RTCM data are forwarded to the INS internal GNSS receiver to enable DGPS/RTK solutions. + + Disabled by default, set `rtcm.listen_rtcm` to `true` in .yaml config file to use this feature. * **`/ntrip_client/rtcm`** [rtcm_msgs/Message](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg) - RTCM data from `/ntrip_client/rtcm` will be forwarded to the IMU. + RTCM data from `/ntrip_client/rtcm` will be forwarded to the internal INS GNSS receiver. Namespace `ntrip_client` and topic_name `rtcm` can be customized in .yaml config files. -### sbg_device_mag -The sbg_device_mag node handles the magnetic calibration for suitable devices. +### sbg_device_mag node +The sbg_device_mag node is used to execute on board in-situ 2D or 3D magnetic field calibration. +If you are planning to use magnetic based heading, it is mandatory to perform a magnetic field calibration in a clean magnetic environnement. + +Only ELLIPSE products support magnetic based heading and feature the on-board magnetic field calibration process. #### Services * **`/sbg/mag_calibration`** [std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html) @@ -239,11 +259,11 @@ The sbg_device_mag node handles the magnetic calibration for suitable devices. * **`/sbg/mag_calibration_save`** [std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html) - Service to save the magnetic calibration to the connected device. + Service to save in FLASH memory the latest computed magnetic field calibration. ## HowTo ### Configure the SBG device -The SBG Ros driver allows the user to configure the device before starting the data handling.
+The SBG Ros driver allows the user to configure the device before starting data parsing. To do so, set the corresponding parameter in the used config file. ``` @@ -251,44 +271,67 @@ To do so, set the corresponding parameter in the used config file. confWithRos: true ``` -Then, modify the desired parameters in the config file, using the SBG Firmware Manual, to see which features are configurable, and which parameter values are available. +Then, modify the desired parameters in the config file, using the [Firmware Reference Manual](https://support.sbg-systems.com/sc/dev/latest/firmware-documentation), to see which features are configurable, and which parameter values are available. -### Configuration for SBG device and RTCM messages -The SBG Ros driver forwards RTCM messages received to the device. For the device to listen to those messages, a specific configuration must be applied manually. -Use `sbgCenter`, to configure the device. -In `Assignment panel`, `RTCM` should be set to `Port A`. +### Configure for RTK/DGPS +The `sbg_device` node can subscribe to [rtcm_msgs/Message](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg) topics to forward differential corrections to the INS internal GNSS receiver. + +The RTCM data stream is sent through the serial/ethernet interface used by ROS to communicate with the INS. +This enables simple and efficient RTK operations without requiring additional hardware or wiring. + +When combined with a third party [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client), it offers a turnkey solution to access local VRS providers and get centimeter-level accuracy solutions. + +The driver and the device should be properly setup: + - Configure the INS to accept RTCM corrections on the interface used by the ROS driver: + - For ELLIPSE, simply use the `sbgCenter` and in `Assignment panel`, `RTCM` should be set to `Port A`. + - For High Performance INS, either use the configuration web interface or the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). + - Install and configure a third party node that broadcast RTCM corrections such as a [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) + - Update the node config `yaml` file to set `rtcm.listen_rtcm` and `nmea.publish_nmea` to `true` + - If you use a different node to broadcast RTCM topics, you might have to update the config `yaml` file to update topics and namespaces. ### Calibrate the magnetometers -Ellipse-A/E/N use magnemoter to provide heading. A calibration is then required to compensate soft and hard iron distortions due to the environmenent (motors, batteries, ...). The magnetic calibration procedure should be held in a non magnetic area (outside of buildings). +ELLIPSE products can use magnetometers to determine the heading. A calibration is then required to compensate for soft and hard iron distortions due to the vehicle the product is installed on. The magnetic calibration procedure should be held in a clean magnetic environnement (outside of buildings). + +You can read more information about magnetic field calibration procedure from the SBG Systems [Support Center](https://support.sbg-systems.com/sc/kb/v3/inertial-sensors-installation/magnetic-calibration). + +The ROS driver provides a dedicated node to easily use ELLIPSE on board magnetic field calibration algorithms. +The ELLIPSE offers both a 2D and 3D magnetic field calibration mode. + +1) Make sure you have selected the desired 2D or 3D magnetic field calibration mode (`calibration.mode` in the configuration `yaml` file). +2) Start a new magnetic calibration session once you are ready to map the magnetic field: ``` roslaunch sbg_driver sbg_device_mag_calibration.launch rosservice call /sbg/mag_calibration ``` -> success: True
+> success: True > message: "Magnetometer calibration process started." -Proceed rotations of the IMU (every orientation if possible). +3) Rotate as much as possible the unit to map the surrounding magnetic field (ideally, perform a 360° with X then Y then Z axis pointing downward). +4) Once you believe you have covered enough orientations, compute a magnetic field calibration: ``` rosservice call /sbg/mag_calibration ``` -> success: True
-> message: "Magnetometer calibration is finished. See the output console to get calibration informations." +> success: True +> message: "Magnetometer calibration is finished. See the output console to get calibration information." -If the magnetic calibration is satisfaying (Quality, Confidence), it could be uploaded/saved to the device. +5) If you are happy with the results (Quality, Confidence), apply and save the new magnetic calibration parameters. + If not, you can continue to rotate the product and try to perform a new computation (and repeat step 4) ``` rosservice call /sbg/mag_calibration_save ``` -> success: True
+> success: True > message: "Magnetometer calibration has been uploaded to the device." +6) Reset/Power Cycle the device and you should now get an accurate magnetic based heading. + ### Enable communication with the SBG device -To be able to communicate with the device, be sure that your user is part of the dialout group.
+To be able to communicate with the device, be sure that your user is part of the dialout group. Once added, restart your machine to save and apply the changes. ``` @@ -299,7 +342,7 @@ sudo adduser $USER dialout Udev rules can be defined for communication port, in order to avoid modifying the port in configuration if it has changed. [Udev documentation](https://wiki.debian.org/udev) -A symlink can be configured and defined to uniquely identify the connected device.
+A symlink can be configured and defined to uniquely identify the connected device. Once it is done, configuration file could be updated `portName: "/dev/sbg"`. See the docs folder, to see an example of rules with the corresponding screenshot using the udev functions. @@ -336,8 +379,8 @@ frame_id: "imu_link_ned" #### Frame convention The frame convention can be set to NED or ENU -* In NED convention axises are the same as device axises. -* In ENU convention (x = X, y = -Y, z = -Z). +* The NED convention is SBG Systems native convention so no transformation is applied. +* The ENU convention requires the following transformation (x = Y, y = X, z = -Z). ``` # Frame convention: use_enu: true @@ -352,8 +395,8 @@ If you experience higher latency than expected and have connected the IMU via an ## Contributing ### Bugs and issues -Please report bugs and/or issues using the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros_driver/issues) +Please report bugs and/or issues using the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros2_driver/issues) ### Features requests or additions -In order to contribute to the code, please use Pull requests to the `devel` branch.
-If you have some feature requests, use the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros_driver/issues) as well. +In order to contribute to the code, please use Pull requests to the `devel` branch. +If you have some feature requests, use the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros2_driver/issues) as well. diff --git a/config/example/ellipse_A_default.yaml b/config/example/ellipse_A_default.yaml index 7283c0d..3d9bac2 100644 --- a/config/example/ellipse_A_default.yaml +++ b/config/example/ellipse_A_default.yaml @@ -160,7 +160,7 @@ gnss: # Gnss Model Id # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS + # 102 Default mode for Ellipse-E connection to external NMEA GNSS # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. diff --git a/config/example/ellipse_D_default.yaml b/config/example/ellipse_D_default.yaml index d76221e..687957a 100644 --- a/config/example/ellipse_D_default.yaml +++ b/config/example/ellipse_D_default.yaml @@ -160,7 +160,7 @@ gnss: # Gnss Model Id # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS + # 102 Default mode for Ellipse-E connection to external NMEA GNSS # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. diff --git a/config/example/ellipse_E_default.yaml b/config/example/ellipse_E_default.yaml index 3cc27d3..cbed1d6 100644 --- a/config/example/ellipse_E_default.yaml +++ b/config/example/ellipse_E_default.yaml @@ -160,7 +160,7 @@ gnss: # Gnss Model Id # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS + # 102 Default mode for Ellipse-E connection to external NMEA GNSS # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. diff --git a/config/example/ellipse_N_default.yaml b/config/example/ellipse_N_default.yaml index 3719f3d..353cdce 100644 --- a/config/example/ellipse_N_default.yaml +++ b/config/example/ellipse_N_default.yaml @@ -161,7 +161,7 @@ gnss: # Gnss Model Id # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS + # 102 Default mode for Ellipse-E connection to external NMEA GNSS # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. From 9c62d7d77940b0a8e2f62b673e64d725a14a8ca9 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Fri, 11 Aug 2023 14:35:16 +0200 Subject: [PATCH 19/74] Code cleanup - removed (void) as it is not recommended in C++ --- include/sbg_driver/config_applier.h | 2 +- include/sbg_driver/config_store.h | 78 ++++++++++++------------- include/sbg_driver/message_publisher.h | 8 +-- include/sbg_driver/message_wrapper.h | 2 +- include/sbg_driver/sbg_device.h | 30 +++++----- include/sbg_driver/sbg_matrix3.h | 6 +- include/sbg_driver/sbg_vector3.h | 4 +- src/config_applier.cpp | 2 +- src/config_store.cpp | 80 +++++++++++++------------- src/message_publisher.cpp | 8 +-- src/message_wrapper.cpp | 2 +- src/sbg_device.cpp | 30 +++++----- 12 files changed, 126 insertions(+), 126 deletions(-) diff --git a/include/sbg_driver/config_applier.h b/include/sbg_driver/config_applier.h index 8fee8aa..f125502 100644 --- a/include/sbg_driver/config_applier.h +++ b/include/sbg_driver/config_applier.h @@ -208,7 +208,7 @@ class ConfigApplier /*! * Save the configuration to the device. */ - void saveConfiguration(void); + void saveConfiguration(); }; } diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index f397167..ab4791b 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -272,7 +272,7 @@ class ConfigStore /*! * Default constructor. */ - ConfigStore(void); + ConfigStore(); //---------------------------------------------------------------------// //- Parameters -// @@ -283,182 +283,182 @@ class ConfigStore * * \return True if the ROS driver has to configure the device. */ - bool checkConfigWithRos(void) const; + bool checkConfigWithRos() const; /*! * Check if the interface configuration is a serial interface. * * \return True if the interface is serial, False otherwise. */ - bool isInterfaceSerial(void) const; + bool isInterfaceSerial() const; /*! * Get the UART port name. * * \return UART serial port name. */ - const std::string &getUartPortName(void) const; + const std::string &getUartPortName() const; /*! * Get the UART baudrate communication. * * \return UART serial baudrate. */ - uint32_t getBaudRate(void) const; + uint32_t getBaudRate() const; /*! * Get the output port of the device. * * \return SBG device output port. */ - SbgEComOutputPort getOutputPort(void) const; + SbgEComOutputPort getOutputPort() const; /*! * Check if the interface configuration is a UDP interface. * * \return True if the interface is UDP, False otherwise. */ - bool isInterfaceUdp(void) const; + bool isInterfaceUdp() const; /*! * Get the Ip address of the interface. * * \return Ip address. */ - sbgIpAddress getIpAddress(void) const; + sbgIpAddress getIpAddress() const; /*! * Get the output port. * * \return Output port. */ - uint32_t getOutputPortAddress(void) const; + uint32_t getOutputPortAddress() const; /*! * Get the input port. * * \return Input port. */ - uint32_t getInputPortAddress(void) const; + uint32_t getInputPortAddress() const; /*! * Get the initial conditions configuration. * * \return Initial conditions configuration. */ - const SbgEComInitConditionConf &getInitialConditions(void) const; + const SbgEComInitConditionConf &getInitialConditions() const; /*! * Get the motion profile configuration. * * \return Motion profile configuration. */ - const SbgEComModelInfo &getMotionProfile(void) const; + const SbgEComModelInfo &getMotionProfile() const; /*! * Get the sensor alignement configuration. * * \return Sensor alignement configuration. */ - const SbgEComSensorAlignmentInfo &getSensorAlignement(void) const; + const SbgEComSensorAlignmentInfo &getSensorAlignement() const; /*! * Get the sensor level arms. * * \return Sensor level arms vector (in meters). */ - const SbgVector3 &getSensorLevelArms(void) const; + const SbgVector3 &getSensorLevelArms() const; /*! * Get the aiding assignement configuration. * * \return Aiding assignement configuration. */ - const SbgEComAidingAssignConf &getAidingAssignement(void) const; + const SbgEComAidingAssignConf &getAidingAssignement() const; /*! * Get the magnetometer model configuration. * * \return Magnetometer model configuration. */ - const SbgEComModelInfo &getMagnetometerModel(void) const; + const SbgEComModelInfo &getMagnetometerModel() const; /*! * Get the magnetometer rejection configuration. * * \return Magnetometer rejection configuration. */ - const SbgEComMagRejectionConf &getMagnetometerRejection(void) const; + const SbgEComMagRejectionConf &getMagnetometerRejection() const; /*! * Get the magnetometer calibration mode. * * \return Magnetometer calibration mode. */ - const SbgEComMagCalibMode &getMagnetometerCalibMode(void) const; + const SbgEComMagCalibMode &getMagnetometerCalibMode() const; /*! * Get the magnetometer calibration bandwidth. * * \return Magnetometer calibration bandwidth. */ - const SbgEComMagCalibBandwidth &getMagnetometerCalibBandwidth(void) const; + const SbgEComMagCalibBandwidth &getMagnetometerCalibBandwidth() const; /*! * Get the Gnss model configuration. * * \return Gnss model configuration. */ - const SbgEComModelInfo &getGnssModel(void) const; + const SbgEComModelInfo &getGnssModel() const; /*! * Get the Gnss installation configuration. * * \return Gnss installation configuration. */ - const SbgEComGnssInstallation &getGnssInstallation(void) const; + const SbgEComGnssInstallation &getGnssInstallation() const; /*! * Get the Gnss rejection configuration. * * \return Gnss rejection configuration. */ - const SbgEComGnssRejectionConf &getGnssRejection(void) const; + const SbgEComGnssRejectionConf &getGnssRejection() const; /*! * Get the odometer configuration. * * \return Odometer configuration. */ - const SbgEComOdoConf &getOdometerConf(void) const; + const SbgEComOdoConf &getOdometerConf() const; /*! * Get the odometer level arms. * * \return Odometer level arms vector (in meters). */ - const SbgVector3 &getOdometerLevelArms(void) const; + const SbgVector3 &getOdometerLevelArms() const; /*! * Get the odometer rejection. * * \return Odometer rejection configuration. */ - const SbgEComOdoRejectionConf &getOdometerRejection(void) const; + const SbgEComOdoRejectionConf &getOdometerRejection() const; /*! * Get all the output modes. * * \return Output mode for this config store. */ - const std::vector &getOutputModes(void) const; + const std::vector &getOutputModes() const; /*! * Check if the ROS standard outputs are defined. * * \return True if standard ROS messages output are defined. */ - bool checkRosStandardMessages(void) const; + bool checkRosStandardMessages() const; /*! * Get the reading frequency defined in settings. @@ -466,42 +466,42 @@ class ConfigStore * * \return Rate frequency parameter (in Hz). */ - uint32_t getReadingRateFrequency(void) const; + uint32_t getReadingRateFrequency() const; /*! * Get the frame ID. * * \return Frame ID. */ - const std::string &getFrameId(void) const; + const std::string &getFrameId() const; /*! * Get use ENU. * * \return True if the frame convention to use is ENU. */ - bool getUseEnu(void) const; + bool getUseEnu() const; /*! * Get odom enable. * * \return True if the odometry is enabled. */ - bool getOdomEnable(void) const; + bool getOdomEnable() const; /*! * Get odom publish_tf. * * \return If true publish odometry transforms. */ - bool getOdomPublishTf(void) const; + bool getOdomPublishTf() const; /*! * Get the odometry frame ID. * * \return Odometry frame ID. */ - const std::string &getOdomFrameId(void) const; + const std::string &getOdomFrameId() const; /*! @@ -509,7 +509,7 @@ class ConfigStore * * \return Odometry base frame ID. */ - const std::string &getOdomBaseFrameId(void) const; + const std::string &getOdomBaseFrameId() const; /*! @@ -517,28 +517,28 @@ class ConfigStore * * \return Odometry init frame ID. */ - const std::string &getOdomInitFrameId(void) const; + const std::string &getOdomInitFrameId() const; /*! * Get the time reference. * * \return Time reference. */ - TimeReference getTimeReference(void) const; + TimeReference getTimeReference() const; /*! * Get RTCM enable. * * \return True if RTCM is enabled. */ - bool shouldListenRtcm(void) const; + bool shouldListenRtcm() const; /*! * Get RTCM full topic. * * \return String with RTCM namespace + topic. */ - const std::string &getRtcmFullTopic(void) const; + const std::string &getRtcmFullTopic() const; /*! * Get NMEA enable. @@ -552,7 +552,7 @@ class ConfigStore * * \return String with NMEA namespace + topic. */ - const std::string &getNmeaFullTopic(void) const; + const std::string &getNmeaFullTopic() const; //---------------------------------------------------------------------// //- Operations -// diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index f0baf46..7309659 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -128,17 +128,17 @@ class MessagePublisher /*! * Process a ROS Velocity standard message. */ - void processRosVelMessage(void); + void processRosVelMessage(); /*! * Process a ROS IMU standard message. */ - void processRosImuMessage(void); + void processRosImuMessage(); /*! * Process a ROS odometry standard message. */ - void processRosOdoMessage(void); + void processRosOdoMessage(); /*! * Publish a received SBG Magnetic log. @@ -185,7 +185,7 @@ class MessagePublisher /*! * Default constructor. */ - MessagePublisher(void); + MessagePublisher(); //---------------------------------------------------------------------// //- Operations -// diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 067d7f4..39fec87 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -356,7 +356,7 @@ class MessageWrapper : public rclcpp::Node /*! * Default constructor. */ - MessageWrapper(void); + MessageWrapper(); //---------------------------------------------------------------------// //- Parameters -// diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index 10cb4de..4ee2edf 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -82,21 +82,21 @@ class SbgDevice /*! * Load the parameters. */ - void loadParameters(void); + void loadParameters(); /*! * Create the connection to the SBG device. * * \throw Unable to connect to the SBG device. */ - void connect(void); + void connect(); /*! * Read the device informations. * * \throw Unable to read the device information. */ - void readDeviceInfo(void); + void readDeviceInfo(); /*! * Get the SBG version as a string. @@ -109,7 +109,7 @@ class SbgDevice /*! * Initialize the publishers according to the configuration. */ - void initPublishers(void); + void initPublishers(); /*! * Initialize the subscribers according to the configuration. @@ -123,7 +123,7 @@ class SbgDevice * * \throw Unable to configure the connected device. */ - void configure(void); + void configure(); /*! * Process the magnetometer calibration. @@ -148,31 +148,31 @@ class SbgDevice * * \return True if the calibration process has started successfully. */ - bool startMagCalibration(void); + bool startMagCalibration(); /*! * End the magnetometer calibration process. * * \return True if the calibration process has ended successfully. */ - bool endMagCalibration(void); + bool endMagCalibration(); /*! * Upload the magnetometers calibration results to the device. * * \return True if the magnetometers calibration has been successfully uploaded to the device. */ - bool uploadMagCalibrationToDevice(void); + bool uploadMagCalibrationToDevice(); /*! * Display magnetometers calibration status result. */ - void displayMagCalibrationStatusResult(void) const; + void displayMagCalibrationStatusResult() const; /*! * Export magnetometers calibration results. */ - void exportMagCalibrationResults(void) const; + void exportMagCalibrationResults() const; /*! * Handler for subscription to RTCM topic. @@ -197,7 +197,7 @@ class SbgDevice /*! * Default destructor. */ - ~SbgDevice(void); + ~SbgDevice(); //---------------------------------------------------------------------// //- Parameters -// @@ -208,7 +208,7 @@ class SbgDevice * * \return Device frequency to read the logs (in Hz). */ - uint32_t getUpdateFrequency(void) const; + uint32_t getUpdateFrequency() const; //---------------------------------------------------------------------// //- Public methods -// @@ -219,17 +219,17 @@ class SbgDevice * * \throw Unable to initialize the SBG device. */ - void initDeviceForReceivingData(void); + void initDeviceForReceivingData(); /*! * Initialize the device for magnetometers calibration. */ - void initDeviceForMagCalibration(void); + void initDeviceForMagCalibration(); /*! * Periodic handle of the connected SBG device. */ - void periodicHandle(void); + void periodicHandle(); }; } diff --git a/include/sbg_driver/sbg_matrix3.h b/include/sbg_driver/sbg_matrix3.h index 08c840e..b0158a8 100644 --- a/include/sbg_driver/sbg_matrix3.h +++ b/include/sbg_driver/sbg_matrix3.h @@ -68,7 +68,7 @@ class SbgMatrix3 /*! * Empty constructor. */ - SbgMatrix3(void) + SbgMatrix3() { m_data[0] = static_cast(0.0); m_data[1] = static_cast(0.0); @@ -155,7 +155,7 @@ class SbgMatrix3 * * \return Raw vector data. */ - const T *data(void) const + const T *data() const { return static_cast(m_data.data()); }; @@ -169,7 +169,7 @@ class SbgMatrix3 return SbgVector3(x, y, z); } - void transpose(void) + void transpose() { T swap = m_data[1]; m_data[1] = m_data[3]; diff --git a/include/sbg_driver/sbg_vector3.h b/include/sbg_driver/sbg_vector3.h index d82ea61..53c9276 100644 --- a/include/sbg_driver/sbg_vector3.h +++ b/include/sbg_driver/sbg_vector3.h @@ -51,7 +51,7 @@ class SbgVector3 /*! * Empty constructor. */ - SbgVector3(void) + SbgVector3() { m_data[0] = static_cast(0.0); m_data[1] = static_cast(0.0); @@ -136,7 +136,7 @@ class SbgVector3 * * \return Raw vector data. */ - const T *data(void) const + const T *data() const { return static_cast(m_data.data()); }; diff --git a/src/config_applier.cpp b/src/config_applier.cpp index e94e085..68100f1 100644 --- a/src/config_applier.cpp +++ b/src/config_applier.cpp @@ -439,7 +439,7 @@ void ConfigApplier::applyConfiguration(const ConfigStore& ref_config_store) } } -void ConfigApplier::saveConfiguration(void) +void ConfigApplier::saveConfiguration() { SbgErrorCode error_code; diff --git a/src/config_store.cpp b/src/config_store.cpp index 519cdf1..0417e67 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -10,7 +10,7 @@ using sbg::ConfigStore; //- Constructor -// //---------------------------------------------------------------------// -ConfigStore::ConfigStore(void): +ConfigStore::ConfigStore(): m_serial_communication_(false), m_upd_communication_(false), m_configure_through_ros_(false), @@ -213,197 +213,197 @@ void ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) //- Parameters -// //---------------------------------------------------------------------// -bool ConfigStore::checkConfigWithRos(void) const +bool ConfigStore::checkConfigWithRos() const { return m_configure_through_ros_; } -bool ConfigStore::isInterfaceSerial(void) const +bool ConfigStore::isInterfaceSerial() const { return m_serial_communication_; } -const std::string &ConfigStore::getUartPortName(void) const +const std::string &ConfigStore::getUartPortName() const { return m_uart_port_name_; } -uint32_t ConfigStore::getBaudRate(void) const +uint32_t ConfigStore::getBaudRate() const { return m_uart_baud_rate_; } -SbgEComOutputPort ConfigStore::getOutputPort(void) const +SbgEComOutputPort ConfigStore::getOutputPort() const { return m_output_port_; } -bool ConfigStore::isInterfaceUdp(void) const +bool ConfigStore::isInterfaceUdp() const { return m_upd_communication_; } -sbgIpAddress ConfigStore::getIpAddress(void) const +sbgIpAddress ConfigStore::getIpAddress() const { return m_sbg_ip_address_; } -uint32_t ConfigStore::getOutputPortAddress(void) const +uint32_t ConfigStore::getOutputPortAddress() const { return m_out_port_address_; } -uint32_t ConfigStore::getInputPortAddress(void) const +uint32_t ConfigStore::getInputPortAddress() const { return m_in_port_address_; } -const SbgEComInitConditionConf &ConfigStore::getInitialConditions(void) const +const SbgEComInitConditionConf &ConfigStore::getInitialConditions() const { return m_init_condition_conf_; } -const SbgEComModelInfo &ConfigStore::getMotionProfile(void) const +const SbgEComModelInfo &ConfigStore::getMotionProfile() const { return m_motion_profile_model_info_; } -const SbgEComSensorAlignmentInfo &ConfigStore::getSensorAlignement(void) const +const SbgEComSensorAlignmentInfo &ConfigStore::getSensorAlignement() const { return m_sensor_alignement_info_; } -const sbg::SbgVector3 &ConfigStore::getSensorLevelArms(void) const +const sbg::SbgVector3 &ConfigStore::getSensorLevelArms() const { return m_sensor_lever_arm_; } -const SbgEComAidingAssignConf &ConfigStore::getAidingAssignement(void) const +const SbgEComAidingAssignConf &ConfigStore::getAidingAssignement() const { return m_aiding_assignement_conf_; } -const SbgEComModelInfo &ConfigStore::getMagnetometerModel(void) const +const SbgEComModelInfo &ConfigStore::getMagnetometerModel() const { return m_mag_model_info_; } -const SbgEComMagRejectionConf &ConfigStore::getMagnetometerRejection(void) const +const SbgEComMagRejectionConf &ConfigStore::getMagnetometerRejection() const { return m_mag_rejection_conf_; } -const SbgEComMagCalibMode &ConfigStore::getMagnetometerCalibMode(void) const +const SbgEComMagCalibMode &ConfigStore::getMagnetometerCalibMode() const { return m_mag_calib_mode_; } -const SbgEComMagCalibBandwidth &ConfigStore::getMagnetometerCalibBandwidth(void) const +const SbgEComMagCalibBandwidth &ConfigStore::getMagnetometerCalibBandwidth() const { return m_mag_calib_bandwidth_; } -const SbgEComModelInfo &ConfigStore::getGnssModel(void) const +const SbgEComModelInfo &ConfigStore::getGnssModel() const { return m_gnss_model_info_; } -const SbgEComGnssInstallation &ConfigStore::getGnssInstallation(void) const +const SbgEComGnssInstallation &ConfigStore::getGnssInstallation() const { return m_gnss_installation_; } -const SbgEComGnssRejectionConf &ConfigStore::getGnssRejection(void) const +const SbgEComGnssRejectionConf &ConfigStore::getGnssRejection() const { return m_gnss_rejection_conf_; } -const SbgEComOdoConf &ConfigStore::getOdometerConf(void) const +const SbgEComOdoConf &ConfigStore::getOdometerConf() const { return m_odometer_conf_; } -const sbg::SbgVector3 &ConfigStore::getOdometerLevelArms(void) const +const sbg::SbgVector3 &ConfigStore::getOdometerLevelArms() const { return m_odometer_level_arm_; } -const SbgEComOdoRejectionConf &ConfigStore::getOdometerRejection(void) const +const SbgEComOdoRejectionConf &ConfigStore::getOdometerRejection() const { return m_odometer_rejection_conf_; } -const std::vector &ConfigStore::getOutputModes(void) const +const std::vector &ConfigStore::getOutputModes() const { return m_output_modes_; } -bool ConfigStore::checkRosStandardMessages(void) const +bool ConfigStore::checkRosStandardMessages() const { return m_ros_standard_output_; } -uint32_t ConfigStore::getReadingRateFrequency(void) const +uint32_t ConfigStore::getReadingRateFrequency() const { return m_rate_frequency_; } -const std::string &ConfigStore::getFrameId(void) const +const std::string &ConfigStore::getFrameId() const { return m_frame_id_; } -bool ConfigStore::getUseEnu(void) const +bool ConfigStore::getUseEnu() const { return m_use_enu_; } -sbg::TimeReference ConfigStore::getTimeReference(void) const +sbg::TimeReference ConfigStore::getTimeReference() const { return m_time_reference_; } -bool ConfigStore::getOdomEnable(void) const +bool ConfigStore::getOdomEnable() const { return m_odom_enable_; } -bool ConfigStore::getOdomPublishTf(void) const +bool ConfigStore::getOdomPublishTf() const { return m_odom_publish_tf_; } -const std::string &ConfigStore::getOdomFrameId(void) const +const std::string &ConfigStore::getOdomFrameId() const { return m_odom_frame_id_; } -const std::string &ConfigStore::getOdomBaseFrameId(void) const +const std::string &ConfigStore::getOdomBaseFrameId() const { return m_odom_base_frame_id_; } -const std::string &ConfigStore::getOdomInitFrameId(void) const +const std::string &ConfigStore::getOdomInitFrameId() const { return m_odom_init_frame_id_; } -bool ConfigStore::shouldListenRtcm(void) const +bool ConfigStore::shouldListenRtcm() const { return m_listen_rtcm_; } -const std::string &ConfigStore::getRtcmFullTopic(void) const +const std::string &ConfigStore::getRtcmFullTopic() const { return m_rtcm_full_topic_; } -bool ConfigStore::shouldPublishNmea(void) const +bool ConfigStore::shouldPublishNmea() const { return m_publish_nmea_; } -const std::string &ConfigStore::getNmeaFullTopic(void) const +const std::string &ConfigStore::getNmeaFullTopic() const { return m_nmea_full_topic_; } diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 995f7fe..af9b812 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -9,7 +9,7 @@ using sbg::MessagePublisher; //- Constructor -// //---------------------------------------------------------------------// -MessagePublisher::MessagePublisher(void): +MessagePublisher::MessagePublisher(): m_max_messages_(10) { } @@ -315,7 +315,7 @@ void MessagePublisher::publishIMUData(const SbgBinaryLogData &ref_sbg_log) processRosOdoMessage(); } -void MessagePublisher::processRosVelMessage(void) +void MessagePublisher::processRosVelMessage() { if (m_velocity_pub_) { @@ -330,7 +330,7 @@ void MessagePublisher::processRosVelMessage(void) } } -void MessagePublisher::processRosImuMessage(void) +void MessagePublisher::processRosImuMessage() { if (m_imu_pub_) { @@ -341,7 +341,7 @@ void MessagePublisher::processRosImuMessage(void) } } -void MessagePublisher::processRosOdoMessage(void) +void MessagePublisher::processRosOdoMessage() { if (m_odometry_pub_) { diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 56dd5eb..3205dfe 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -19,7 +19,7 @@ using sbg::MessageWrapper; //- Constructor -// //---------------------------------------------------------------------// -MessageWrapper::MessageWrapper(void): +MessageWrapper::MessageWrapper(): Node("tf_broadcaster") { m_first_valid_utc_ = false; diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index a9b406c..e38290e 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -65,7 +65,7 @@ m_mag_calibration_done_(false) connect(); } -SbgDevice::~SbgDevice(void) +SbgDevice::~SbgDevice() { SbgErrorCode error_code; @@ -117,7 +117,7 @@ void SbgDevice::onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const Sb m_message_publisher_.publish(msg_class, msg, ref_sbg_data); } -void SbgDevice::loadParameters(void) +void SbgDevice::loadParameters() { // // Get the ROS private nodeHandle, where the parameters are loaded from the launch file. @@ -128,7 +128,7 @@ void SbgDevice::loadParameters(void) m_config_store_.loadFromRosNodeHandle(n_private); } -void SbgDevice::connect(void) +void SbgDevice::connect() { SbgErrorCode error_code; error_code = SBG_NO_ERROR; @@ -168,7 +168,7 @@ void SbgDevice::connect(void) readDeviceInfo(); } -void SbgDevice::readDeviceInfo(void) +void SbgDevice::readDeviceInfo() { SbgEComDeviceInfo device_info; SbgErrorCode error_code; @@ -200,7 +200,7 @@ std::string SbgDevice::getVersionAsString(uint32 sbg_version_enc) const return std::string(version); } -void SbgDevice::initPublishers(void) +void SbgDevice::initPublishers() { m_message_publisher_.initPublishers(m_ref_node_, m_config_store_); @@ -225,7 +225,7 @@ void SbgDevice::initSubscribers() } } -void SbgDevice::configure(void) +void SbgDevice::configure() { if (m_config_store_.checkConfigWithRos()) { @@ -304,7 +304,7 @@ bool SbgDevice::saveMagCalibration(const std::shared_ptrsuccess; } -bool SbgDevice::startMagCalibration(void) +bool SbgDevice::startMagCalibration() { SbgErrorCode error_code; SbgEComMagCalibMode mag_calib_mode; @@ -329,7 +329,7 @@ bool SbgDevice::startMagCalibration(void) } } -bool SbgDevice::endMagCalibration(void) +bool SbgDevice::endMagCalibration() { SbgErrorCode error_code; @@ -349,7 +349,7 @@ bool SbgDevice::endMagCalibration(void) } } -bool SbgDevice::uploadMagCalibrationToDevice(void) +bool SbgDevice::uploadMagCalibrationToDevice() { SbgErrorCode error_code; @@ -377,7 +377,7 @@ bool SbgDevice::uploadMagCalibrationToDevice(void) } } -void SbgDevice::displayMagCalibrationStatusResult(void) const +void SbgDevice::displayMagCalibrationStatusResult() const { RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Quality of the calibration %s", g_mag_calib_quality_[m_magCalibResults.quality].c_str()); RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Calibration results confidence %s", g_mag_calib_confidence_[m_magCalibResults.confidence].c_str()); @@ -429,7 +429,7 @@ void SbgDevice::displayMagCalibrationStatusResult(void) const } } -void SbgDevice::exportMagCalibrationResults(void) const +void SbgDevice::exportMagCalibrationResults() const { SbgEComMagCalibMode mag_calib_mode; SbgEComMagCalibBandwidth mag_calib_bandwidth; @@ -486,7 +486,7 @@ void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPt //- Parameters -// //---------------------------------------------------------------------// -uint32_t SbgDevice::getUpdateFrequency(void) const +uint32_t SbgDevice::getUpdateFrequency() const { return m_rate_frequency_; } @@ -495,7 +495,7 @@ uint32_t SbgDevice::getUpdateFrequency(void) const //- Public methods -// //---------------------------------------------------------------------// -void SbgDevice::initDeviceForReceivingData(void) +void SbgDevice::initDeviceForReceivingData() { SbgErrorCode error_code; initPublishers(); @@ -511,7 +511,7 @@ void SbgDevice::initDeviceForReceivingData(void) initSubscribers(); } -void SbgDevice::initDeviceForMagCalibration(void) +void SbgDevice::initDeviceForMagCalibration() { m_calib_service_ = m_ref_node_.create_service("sbg/mag_calibration", std::bind(&SbgDevice::processMagCalibration, this, std::placeholders::_1, std::placeholders::_2)); m_calib_save_service_ = m_ref_node_.create_service("sbg/mag_calibration_save", std::bind(&SbgDevice::saveMagCalibration, this, std::placeholders::_1, std::placeholders::_2)); @@ -519,7 +519,7 @@ void SbgDevice::initDeviceForMagCalibration(void) RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Init] - SBG device is initialized for magnetometers calibration."); } -void SbgDevice::periodicHandle(void) +void SbgDevice::periodicHandle() { sbgEComHandle(&m_com_handle_); } From bf82be6dcce27762be98154849ef85e1b5686f8c Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Fri, 11 Aug 2023 15:08:00 +0200 Subject: [PATCH 20/74] Improved RTCM and NMEA parameters naming --- README.md | 18 +++++++------- config/sbg_device_uart_default.yaml | 6 ++--- config/sbg_device_udp_default.yaml | 6 ++--- include/sbg_driver/config_store.h | 18 ++++++-------- src/config_store.cpp | 38 +++++++++++++++++------------ src/sbg_device.cpp | 32 ++++++++++-------------- 6 files changed, 58 insertions(+), 60 deletions(-) diff --git a/README.md b/README.md index 88e0016..c488002 100644 --- a/README.md +++ b/README.md @@ -68,17 +68,17 @@ Every configuration file is defined according to the same structure. This config file is the default one for UART connection with the device. It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node). It defines a few outputs for the device: - * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz - * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz - * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. + * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz + * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz + * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. * **sbg_device_udp_default.yaml** This config file is the default one for an Udp connection with the device. It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node). It defines a few outputs for the device: - * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz - * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz - * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. + * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz + * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz + * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. ### Example config files * **ellipse_A_default.yaml** @@ -227,7 +227,7 @@ For each ROS standard, you have to activate the needed SBG outputs. ##### NMEA topics The driver can publish NMEA GGA messages from the internal GNSS receiver. It can be used with third party [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) modules to support VRS networks providers. - Disabled by default, set `publish_nmea` to `true` in .yaml config file to use this feature. + Disabled by default, set `nmea.publish` to `true` in .yaml config file to use this feature. * **`/ntrip_client/nmea`** [nmea_msgs/Sentence](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html) @@ -239,7 +239,7 @@ The driver can publish NMEA GGA messages from the internal GNSS receiver. It can The `sbg_device` node can subscribe to RTCM topics published by third party ROS2 modules. Incoming RTCM data are forwarded to the INS internal GNSS receiver to enable DGPS/RTK solutions. - Disabled by default, set `rtcm.listen_rtcm` to `true` in .yaml config file to use this feature. + Disabled by default, set `rtcm.subscribe` to `true` in .yaml config file to use this feature. * **`/ntrip_client/rtcm`** [rtcm_msgs/Message](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg) @@ -286,7 +286,7 @@ The driver and the device should be properly setup: - For ELLIPSE, simply use the `sbgCenter` and in `Assignment panel`, `RTCM` should be set to `Port A`. - For High Performance INS, either use the configuration web interface or the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). - Install and configure a third party node that broadcast RTCM corrections such as a [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) - - Update the node config `yaml` file to set `rtcm.listen_rtcm` and `nmea.publish_nmea` to `true` + - Update the node config `yaml` file to set `rtcm.subscribe` and `nmea.publish` to `true` - If you use a different node to broadcast RTCM topics, you might have to update the config `yaml` file to update topics and namespaces. ### Calibrate the magnetometers diff --git a/config/sbg_device_uart_default.yaml b/config/sbg_device_uart_default.yaml index a74751a..3fbb3c8 100644 --- a/config/sbg_device_uart_default.yaml +++ b/config/sbg_device_uart_default.yaml @@ -304,8 +304,8 @@ log_imu_short: 8 rtcm: - # Should ros driver listen to RTCM topic - listen_rtcm: false + # Should ros driver subscribe to RTCM topic + subscribe: false # Topic on which RTCM is published topic_name: rtcm # Namespace where topic is published @@ -313,7 +313,7 @@ nmea: # Should ros driver publish NMEA string - publish_nmea: false + publish: false # Topic on which to publish nmea data topic_name: nmea # Namespace where to publish topic diff --git a/config/sbg_device_udp_default.yaml b/config/sbg_device_udp_default.yaml index 542cfb1..4245fd7 100644 --- a/config/sbg_device_udp_default.yaml +++ b/config/sbg_device_udp_default.yaml @@ -294,8 +294,8 @@ log_imu_short: 0 rtcm: - # Should ros driver listen to RTCM topic - listen_rtcm: false + # Should ros driver subscribe to RTCM topic + subscribe: false # Topic on which RTCM is published topic_name: rtcm # Namespace where topic is published @@ -303,7 +303,7 @@ nmea: # Should ros driver publish NMEA string - publish_nmea: false + publish: false # Topic on which to publish nmea data topic_name: nmea # Namespace where to publish topic diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index ab4791b..6bc74e8 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -121,15 +121,11 @@ class ConfigStore std::string m_odom_base_frame_id_; std::string m_odom_init_frame_id_; - bool m_listen_rtcm_; - std::string m_rtcm_topic_name_; - std::string m_rtcm_topic_namespace_; - std::string m_rtcm_full_topic_; + bool rtcm_subscribe_; + std::string rtcm_full_topic_; - bool m_publish_nmea_; - std::string m_nmea_topic_name_; - std::string m_nmea_topic_namespace_; - std::string m_nmea_full_topic_; + bool nmea_publish_; + std::string nmea_full_topic_; //---------------------------------------------------------------------// //- Private methods -// @@ -527,11 +523,11 @@ class ConfigStore TimeReference getTimeReference() const; /*! - * Get RTCM enable. + * Returns if the node should subscribe to RTCM publisher * - * \return True if RTCM is enabled. + * \return True to subscribe to RTCM messages. */ - bool shouldListenRtcm() const; + bool shouldSubscribeToRtcm() const; /*! * Get RTCM full topic. diff --git a/src/config_store.cpp b/src/config_store.cpp index 0417e67..f9837d6 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -15,8 +15,8 @@ m_serial_communication_(false), m_upd_communication_(false), m_configure_through_ros_(false), m_ros_standard_output_(false), -m_listen_rtcm_(false), -m_publish_nmea_(false) +rtcm_subscribe_(false), +nmea_publish_(false) { } @@ -195,18 +195,26 @@ void ConfigStore::loadOutputTimeReference(const rclcpp::Node& ref_node_handle, c void ConfigStore::loadRtcmParameters(const rclcpp::Node &ref_node_handle) { - ref_node_handle.get_parameter_or("rtcm.listen_rtcm", m_listen_rtcm_, false); - ref_node_handle.get_parameter_or("rtcm.topic_name", m_rtcm_topic_name_, "rtcm"); - ref_node_handle.get_parameter_or("rtcm.namespace", m_rtcm_topic_namespace_, "ntrip_client"); - m_rtcm_full_topic_ = m_rtcm_topic_namespace_ + "/" + m_rtcm_topic_name_; + std::string topic_name; + std::string namespace; + + ref_node_handle.get_parameter_or("rtcm.subscribe", rtcm_subscribe_, false); + ref_node_handle.get_parameter_or("rtcm.topic_name", topic_name, "rtcm"); + ref_node_handle.get_parameter_or("rtcm.namespace", namespace, "ntrip_client"); + + rtcm_full_topic_ = namespace + "/" + topic_name; } void ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) { - ref_node_handle.get_parameter_or("nmea.publish_nmea", m_publish_nmea_, false); - ref_node_handle.get_parameter_or("nmea.topic_name", m_nmea_topic_name_, "nmea"); - ref_node_handle.get_parameter_or("nmea.namespace", m_nmea_topic_namespace_, "ntrip_client"); - m_nmea_full_topic_ = m_nmea_topic_namespace_ + "/" + m_nmea_topic_name_; + std::string topic_name; + std::string namespace; + + ref_node_handle.get_parameter_or("nmea.publish", nmea_publish_, false); + ref_node_handle.get_parameter_or("nmea.topic_name", topic_name, "nmea"); + ref_node_handle.get_parameter_or("nmea.namespace", namespace, "ntrip_client"); + + nmea_full_topic_ = namespace + "/" + topic_name; } //---------------------------------------------------------------------// @@ -388,24 +396,24 @@ const std::string &ConfigStore::getOdomInitFrameId() const return m_odom_init_frame_id_; } -bool ConfigStore::shouldListenRtcm() const +bool ConfigStore::shouldSubscribeToRtcm() const { - return m_listen_rtcm_; + return rtcm_subscribe_; } const std::string &ConfigStore::getRtcmFullTopic() const { - return m_rtcm_full_topic_; + return rtcm_full_topic_; } bool ConfigStore::shouldPublishNmea() const { - return m_publish_nmea_; + return nmea_publish_; } const std::string &ConfigStore::getNmeaFullTopic() const { - return m_nmea_full_topic_; + return nmea_full_topic_; } //---------------------------------------------------------------------// diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index e38290e..5fe23dd 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -209,20 +209,14 @@ void SbgDevice::initPublishers() void SbgDevice::initSubscribers() { - if (!m_config_store_.shouldListenRtcm()) - { - return; - } - + if (m_config_store_.shouldSubscribeToRtcm()) + { auto rtcm_cb = [&](const rtcm_msgs::msg::Message::SharedPtr msg) -> void { this->writeRtcmMessageToDevice(msg); }; - - if (m_config_store_.shouldListenRtcm()) - { - rtcm_sub_ = m_ref_node_.create_subscription( - m_config_store_.getRtcmFullTopic(), 10, rtcm_cb); - } + + rtcm_sub_ = m_ref_node_.create_subscription(m_config_store_.getRtcmFullTopic(), 10, rtcm_cb); + } } void SbgDevice::configure() @@ -471,15 +465,15 @@ void SbgDevice::exportMagCalibrationResults() const void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPtr msg) { - auto rtcm_data = msg->message; - auto error_code = sbgInterfaceWrite(&m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); - if (error_code != SBG_NO_ERROR) - { - char error_str[256]; + auto rtcm_data = msg->message; + auto error_code = sbgInterfaceWrite(&m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); + if (error_code != SBG_NO_ERROR) + { + char error_str[256]; - sbgEComErrorToString(error_code, error_str); - SBG_LOG_ERROR(SBG_ERROR, "Failed to sent RTCM data to device: %s", error_str); - } + sbgEComErrorToString(error_code, error_str); + SBG_LOG_ERROR(SBG_ERROR, "Failed to sent RTCM data to device: %s", error_str); + } } //---------------------------------------------------------------------// From 256391c45d7bbdc27b567dfd27a19d3608102416 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Fri, 11 Aug 2023 16:09:44 +0200 Subject: [PATCH 21/74] Improved GGA generation and code cleanup --- include/sbg_driver/config_store.h | 6 +- include/sbg_driver/message_publisher.h | 2 +- src/message_publisher.cpp | 6 +- src/message_wrapper.cpp | 101 ++++++++++++++----------- src/sbg_device.cpp | 1 + 5 files changed, 64 insertions(+), 52 deletions(-) diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index 6bc74e8..30d4626 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -499,15 +499,13 @@ class ConfigStore */ const std::string &getOdomFrameId() const; - /*! * Get the odometry base frame ID. * * \return Odometry base frame ID. */ const std::string &getOdomBaseFrameId() const; - - + /*! * Get the odometry init frame ID. * @@ -550,7 +548,7 @@ class ConfigStore */ const std::string &getNmeaFullTopic() const; - //---------------------------------------------------------------------// + //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index 7309659..300fca1 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -82,7 +82,7 @@ class MessagePublisher rclcpp::Publisher>::SharedPtr m_nav_sat_fix_pub_; rclcpp::Publisher>::SharedPtr m_odometry_pub_; - rclcpp::Publisher>::SharedPtr m_SbgGpsPos_gga_pub_; + rclcpp::Publisher>::SharedPtr sbgGpsPos_gga_pub_; MessageWrapper m_message_wrapper_; uint32_t m_max_messages_; diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index af9b812..c1e8c1e 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -450,9 +450,9 @@ void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log, Sb { m_nav_sat_fix_pub_->publish(m_message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); } - if (m_SbgGpsPos_gga_pub_ && sbg_msg_id == SBG_ECOM_LOG_GPS1_POS) + if (sbgGpsPos_gga_pub_ && sbg_msg_id == SBG_ECOM_LOG_GPS1_POS) { - m_SbgGpsPos_gga_pub_->publish(m_message_wrapper_.createSbgGpsPosMessageGGA(ref_sbg_log.gpsPosData)); + sbgGpsPos_gga_pub_->publish(m_message_wrapper_.createSbgGpsPosMessageGGA(ref_sbg_log.gpsPosData)); } } @@ -486,7 +486,7 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C if (ref_config_store.shouldPublishNmea()) { - m_SbgGpsPos_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(),m_max_messages_); + sbgGpsPos_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(), m_max_messages_); } if (ref_config_store.checkRosStandardMessages()) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 3205dfe..2ef8c13 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -512,29 +512,42 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT sbg::SbgNmeaGpsQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType) { - switch (sbgGpsType) - { - case SBG_ECOM_POS_SINGLE: - case SBG_ECOM_POS_UNKNOWN_TYPE: - case SBG_ECOM_POS_FIXED: - return SBG_NMEA_GPS_QUALITY_SINGLE; - case SBG_ECOM_POS_PSRDIFF: - case SBG_ECOM_POS_SBAS: - case SBG_ECOM_POS_OMNISTAR: - return SBG_NMEA_GPS_QUALITY_DGPS; - case SBG_ECOM_POS_PPP_FLOAT: - case SBG_ECOM_POS_PPP_INT: - return SBG_NMEA_GPS_QUALITY_PPS; - case SBG_ECOM_POS_RTK_INT: - return SBG_NMEA_GPS_QUALITY_RTK; - case SBG_ECOM_POS_RTK_FLOAT: - return SBG_NMEA_GPS_QUALITY_RTK_FLOAT; - case SBG_ECOM_POS_NO_SOLUTION: - default: - return SBG_NMEA_GPS_QUALITY_INVALID; - } -} + sbg::SbgNmeaGpsQuality nmeaQuality = SBG_NMEA_GPS_QUALITY_INVALID; + + switch (sbgGpsType) + { + case SBG_ECOM_POS_NO_SOLUTION: + nmeaQuality = SBG_NMEA_GPS_QUALITY_INVALID; + break; + + case SBG_ECOM_POS_UNKNOWN_TYPE: + case SBG_ECOM_POS_SINGLE: + case SBG_ECOM_POS_FIXED: + nmeaQuality = SBG_NMEA_GPS_QUALITY_SINGLE; + break; + + case SBG_ECOM_POS_PSRDIFF: + case SBG_ECOM_POS_SBAS: + case SBG_ECOM_POS_OMNISTAR: + nmeaQuality = SBG_NMEA_GPS_QUALITY_DGPS; + break; + + case SBG_ECOM_POS_PPP_FLOAT: + case SBG_ECOM_POS_PPP_INT: + nmeaQuality = SBG_NMEA_GPS_QUALITY_PPS; + break; + + case SBG_ECOM_POS_RTK_INT: + nmeaQuality = SBG_NMEA_GPS_QUALITY_RTK; + break; + + case SBG_ECOM_POS_RTK_FLOAT: + nmeaQuality = SBG_NMEA_GPS_QUALITY_RTK_FLOAT; + break; + } + return nmeaQuality; +} //---------------------------------------------------------------------// //- Parameters -// @@ -1341,7 +1354,7 @@ const nmea_msgs::msg::Sentence MessageWrapper::createSbgGpsPosMessageGGA(const S { nmea_msgs::msg::Sentence gps_pos_nmea_msg; - // Gps time of week To UTC + // GPS time of week to UTC uint32_t nb_hours = ref_log_gps_pos.timeOfWeek / (3600 * 1000); uint32_t nb_minutes = ref_log_gps_pos.timeOfWeek / (60 * 1000); uint32_t current_hour = nb_hours % 24; @@ -1355,7 +1368,7 @@ const nmea_msgs::msg::Sentence MessageWrapper::createSbgGpsPosMessageGGA(const S float latitude = ref_log_gps_pos.latitude; if (latitude >= 90.0f) { - latitude = 90.0; + latitude = 90.0f; } if (latitude <= -90.0f) { @@ -1399,42 +1412,42 @@ const nmea_msgs::msg::Sentence MessageWrapper::createSbgGpsPosMessageGGA(const S } // DOP computation - double h_dop = std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy); - if (h_dop >= 10.0) + float h_dop = std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy); + if (h_dop > 9.9f) { - h_dop = 9.9; + h_dop = 9.9f; } - double diff_age = (ref_log_gps_pos.differentialAge / 100.0f); - if (diff_age >= 10.0) + float diff_age = (ref_log_gps_pos.differentialAge / 100.0f); + if (diff_age > 9.9f) { - diff_age = 9.9; + diff_age = 9.9f; } uint8_t svUsed = ref_log_gps_pos.numSvUsed; - if (svUsed > 100) + if (svUsed > 99) { svUsed = 99; } - double altitude = ref_log_gps_pos.altitude; - if (altitude <= -100000.0) + float altitude = ref_log_gps_pos.altitude; + if (altitude < -99999.9f) { - altitude = -99999.9; + altitude = -99999.9f; } - if (altitude >= 1000000.0) + if (altitude > 99999.9f) { - altitude = 999999.9; + altitude = 99999.9f; } - double undulation = ref_log_gps_pos.undulation; - if (undulation <= -100000.0) + int32_t undulation = static_cast(ref_log_gps_pos.undulation); + if (undulation < -99999) { - undulation = -99999.9; + undulation = -99999; } - if (undulation >= 1000000.0) + if (undulation > 99999) { - undulation = 999999.9; + undulation = 99999; } uint16_t baseStationId = ref_log_gps_pos.baseStationId; @@ -1462,17 +1475,17 @@ const nmea_msgs::msg::Sentence MessageWrapper::createSbgGpsPosMessageGGA(const S svUsed, h_dop, altitude, - static_cast(undulation), + undulation, diff_age, baseStationId ); // Checksum computation uint8_t checksum = 0; - for(int32_t i = 1; i < len; i++) + for (int32_t i = 1; i < len; i++) { checksum ^= nmea_sentence_buff[i]; - } + } snprintf(&nmea_sentence_buff[len], nmea_sentence_buffer_size - len, "*%02X\r\n", checksum); // Populating ROS message diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 5fe23dd..f6c7f30 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -467,6 +467,7 @@ void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPt { auto rtcm_data = msg->message; auto error_code = sbgInterfaceWrite(&m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); + if (error_code != SBG_NO_ERROR) { char error_str[256]; From ad0de10f738ca5c4f2f329da5439618cbd931612 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 11 Aug 2023 16:29:52 +0200 Subject: [PATCH 22/74] Compilation fix --- src/config_store.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/config_store.cpp b/src/config_store.cpp index f9837d6..f068688 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -196,25 +196,25 @@ void ConfigStore::loadOutputTimeReference(const rclcpp::Node& ref_node_handle, c void ConfigStore::loadRtcmParameters(const rclcpp::Node &ref_node_handle) { std::string topic_name; - std::string namespace; + std::string rtcm_namespace; ref_node_handle.get_parameter_or("rtcm.subscribe", rtcm_subscribe_, false); ref_node_handle.get_parameter_or("rtcm.topic_name", topic_name, "rtcm"); - ref_node_handle.get_parameter_or("rtcm.namespace", namespace, "ntrip_client"); + ref_node_handle.get_parameter_or("rtcm.namespace", rtcm_namespace, "ntrip_client"); - rtcm_full_topic_ = namespace + "/" + topic_name; + rtcm_full_topic_ = rtcm_namespace + "/" + topic_name; } void ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) { std::string topic_name; - std::string namespace; + std::string nmea_namespace; ref_node_handle.get_parameter_or("nmea.publish", nmea_publish_, false); ref_node_handle.get_parameter_or("nmea.topic_name", topic_name, "nmea"); - ref_node_handle.get_parameter_or("nmea.namespace", namespace, "ntrip_client"); + ref_node_handle.get_parameter_or("nmea.namespace", nmea_namespace, "ntrip_client"); - nmea_full_topic_ = namespace + "/" + topic_name; + nmea_full_topic_ = nmea_namespace + "/" + topic_name; } //---------------------------------------------------------------------// From 540b27a9f215d1f3e3499b67812aa518117ae59e Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Mon, 14 Aug 2023 16:54:05 +0200 Subject: [PATCH 23/74] NTRIP: GGA generation Work In Progress with the following fixex: - GPS to UTC time correctly apply leap second offset - GGA only sent if a valid position is available - GGA is sent at 1 Hz only - Minor improvements Code is not yet tested nor build --- include/sbg_driver/config_store.h | 4 +- include/sbg_driver/message_publisher.h | 2 +- include/sbg_driver/message_wrapper.h | 58 ++++--- src/message_publisher.cpp | 12 +- src/message_wrapper.cpp | 217 +++++++++++-------------- 5 files changed, 143 insertions(+), 150 deletions(-) diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index 30d4626..c601aa7 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -535,7 +535,9 @@ class ConfigStore const std::string &getRtcmFullTopic() const; /*! - * Get NMEA enable. + * Returns if a specific NMEA GGA message should be published or not. + * + * This GGA message is dedicated for NTRIP VRS operations and not for navigation. * * \return True if NMEA is enabled. */ diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index 300fca1..770d05a 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -82,7 +82,7 @@ class MessagePublisher rclcpp::Publisher>::SharedPtr m_nav_sat_fix_pub_; rclcpp::Publisher>::SharedPtr m_odometry_pub_; - rclcpp::Publisher>::SharedPtr sbgGpsPos_gga_pub_; + rclcpp::Publisher>::SharedPtr nmea_gga_pub_; MessageWrapper m_message_wrapper_; uint32_t m_max_messages_; diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 39fec87..06eddfc 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -87,26 +87,31 @@ typedef struct _UTM0 int zone; } UTM0; -typedef enum _SbgNmeaGpsQuality -{ - SBG_NMEA_GPS_QUALITY_INVALID = 0, - SBG_NMEA_GPS_QUALITY_SINGLE = 1, - SBG_NMEA_GPS_QUALITY_DGPS = 2, - SBG_NMEA_GPS_QUALITY_PPS = 3, - SBG_NMEA_GPS_QUALITY_RTK = 4, - SBG_NMEA_GPS_QUALITY_RTK_FLOAT = 5, - SBG_NMEA_GPS_QUALITY_FIX_DEAD_RECKONING = 6, - SBG_NMEA_GPS_QUALITY_MANUAL_INPUT = 7, - SBG_NMEA_GPS_QUALITY_SIMULATED = 8, -} SbgNmeaGpsQuality; - /*! * Class to wrap the SBG logs into ROS messages. */ class MessageWrapper : public rclcpp::Node { -private: +public: + const int32_t DEFAULT_UTC_OFFSET = 18; /*!< Driver default GPS to UTC offset in seconds. */ + + /*! + * Standard NMEA GGA quality indicator value. + */ + enum class NmeaGGAQuality: int32_t + { + INVALID = 0, + SINGLE = 1, + DGPS = 2, + PPS = 3, + RTK_FIXED = 4, + RTK_FLOAT = 5, + DEAD_RECKONING = 6, + STATIC_POSITION = 7, + SIMULATED = 8, + }; +private: sbg_driver::msg::SbgUtcTime m_last_sbg_utc_; bool m_first_valid_utc_; std::string m_frame_id_; @@ -276,6 +281,16 @@ class MessageWrapper : public rclcpp::Node * \return Converted Epoch time (in s). */ const rclcpp::Time convertUtcTimeToUnix(const sbg_driver::msg::SbgUtcTime& ref_sbg_utc_msg) const; + + /*! + * Returns the GPS to UTC leap second offset: GPS_Time = UTC_Tme + utcOffset + * + * WARNING: The leap second is computed from the latest received SbgUtcTime message if any. + * If no SbgUtcTime message has been received, a default driver current value is used. + * + * \return Offset in seconds to apply to UTC time to get GPS time. + */ + int32_t getUtcOffset() const; /*! * Create a SBG-ROS air data status message. @@ -333,12 +348,12 @@ class MessageWrapper : public rclcpp::Node void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const; /*! - * Convert SbgEComGpsPosType enum to SbgNmeaGpsQuality enum + * Convert SbgEComGpsPosType enum to NmeaGGAQuality enum * * \param[in] sbgGpsType SbgECom GPS type * \return NMEA GPS type */ - static SbgNmeaGpsQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType); + static NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType); public: @@ -668,12 +683,17 @@ class MessageWrapper : public rclcpp::Node const sensor_msgs::msg::FluidPressure createRosFluidPressureMessage(const sbg_driver::msg::SbgAirData& ref_sbg_air_msg) const; /*! - * Create a SBG-ROS GPS-Position message. + * Create a ROS NMEA GGA message especially designed to support NTRIP VRS operations. + * + * This message is limited to 80 chars and only sent at up to 1 Hz to maximize VRS + * providers compatibility. + * + * WARNING: Don't use this GGA message for navigation purposes as the accuracy is limited. * * \param[in] ref_log_gps_pos SBG GPS Position log. - * \return GPS Position message in nmea format. + * \return ROS NMEA GGA message. */ - const nmea_msgs::msg::Sentence createSbgGpsPosMessageGGA(const SbgLogGpsPos& ref_log_gps_pos) const; + const nmea_msgs::msg::Sentence createNmeaGGAMessageForNtrip(const SbgLogGpsPos& ref_log_gps_pos) const; }; } diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index c1e8c1e..5c14ec5 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -450,9 +450,15 @@ void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log, Sb { m_nav_sat_fix_pub_->publish(m_message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); } - if (sbgGpsPos_gga_pub_ && sbg_msg_id == SBG_ECOM_LOG_GPS1_POS) + if (nmea_gga_pub_ && sbg_msg_id == SBG_ECOM_LOG_GPS1_POS) { - sbgGpsPos_gga_pub_->publish(m_message_wrapper_.createSbgGpsPosMessageGGA(ref_sbg_log.gpsPosData)); + const nmea_msgs::msg::Sentence nmea_gga_msg = m_message_wrapper_.createNmeaGGAMessageForNtrip(ref_sbg_log.gpsPosData); + + // Only publish if a valid NMEA GGA message has been generated + if (nmea_gga_msg.sentence.size() > 0) + { + nmea_gga_pub_->publish(nmea_gga_msg); + } } } @@ -486,7 +492,7 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C if (ref_config_store.shouldPublishNmea()) { - sbgGpsPos_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(), m_max_messages_); + nmea_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(), m_max_messages_); } if (ref_config_store.checkRosStandardMessages()) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 2ef8c13..7d4b224 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -331,7 +331,7 @@ const rclcpp::Time MessageWrapper::convertUtcTimeToUnix(const sbg_driver::msg::S uint64_t nanoseconds; // - // Convert the UTC time to Epoch(Unix) time, which is the elasped seconds since 1 Jan 1970. + // Convert the UTC time to Epoch(Unix) time, which is the elapsed seconds since 1 Jan 1970. // days = 0; nanoseconds = 0; @@ -359,6 +359,28 @@ const rclcpp::Time MessageWrapper::convertUtcTimeToUnix(const sbg_driver::msg::S return utc_to_epoch; } +int32_t MessageWrapper::getUtcOffset() const +{ + int32_t utcOffset; + + if (m_first_valid_utc_) + { + // Compute the leap second: GPS = UTC + utcOffset + utcOffset = (m_last_sbg_utc_.gps_tow/1000)%60 - m_last_sbg_utc_.sec; + + if (utcOffset < 0) + { + utcOffset = 60 + utcOffset; + } + } + else + { + utcOffset = DEFAULT_UTC_OFFSET; + } + + return utcOffset; +} + const sbg_driver::msg::SbgAirDataStatus MessageWrapper::createAirDataStatusMessage(const SbgLogAirData& ref_sbg_air_data) const { sbg_driver::msg::SbgAirDataStatus air_data_status_message; @@ -510,39 +532,39 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT } } -sbg::SbgNmeaGpsQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType) +sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType) { - sbg::SbgNmeaGpsQuality nmeaQuality = SBG_NMEA_GPS_QUALITY_INVALID; + sbg::MessageWrapper::NmeaGGAQuality nmeaQuality = NmeaGGAQuality::INVALID; switch (sbgGpsType) { case SBG_ECOM_POS_NO_SOLUTION: - nmeaQuality = SBG_NMEA_GPS_QUALITY_INVALID; + nmeaQuality = NmeaGGAQuality::INVALID; break; case SBG_ECOM_POS_UNKNOWN_TYPE: case SBG_ECOM_POS_SINGLE: case SBG_ECOM_POS_FIXED: - nmeaQuality = SBG_NMEA_GPS_QUALITY_SINGLE; + nmeaQuality = NmeaGGAQuality::SINGLE; break; case SBG_ECOM_POS_PSRDIFF: case SBG_ECOM_POS_SBAS: case SBG_ECOM_POS_OMNISTAR: - nmeaQuality = SBG_NMEA_GPS_QUALITY_DGPS; + nmeaQuality = NmeaGGAQuality::DGPS; break; case SBG_ECOM_POS_PPP_FLOAT: case SBG_ECOM_POS_PPP_INT: - nmeaQuality = SBG_NMEA_GPS_QUALITY_PPS; + nmeaQuality = NmeaGGAQuality::PPS; break; case SBG_ECOM_POS_RTK_INT: - nmeaQuality = SBG_NMEA_GPS_QUALITY_RTK; + nmeaQuality = NmeaGGAQuality::RTK_FIXED; break; case SBG_ECOM_POS_RTK_FLOAT: - nmeaQuality = SBG_NMEA_GPS_QUALITY_RTK_FLOAT; + nmeaQuality = NmeaGGAQuality::RTK_FLOAT; break; } @@ -1350,147 +1372,90 @@ const sensor_msgs::msg::FluidPressure MessageWrapper::createRosFluidPressureMess return fluid_pressure_message; } -const nmea_msgs::msg::Sentence MessageWrapper::createSbgGpsPosMessageGGA(const SbgLogGpsPos &ref_log_gps_pos) const +const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(const SbgLogGpsPos &ref_log_gps_pos) const { - nmea_msgs::msg::Sentence gps_pos_nmea_msg; + nmea_msgs::msg::Sentence nmea_gga_msg; + uint32_t gps_tow_ms; + uint32_t utc_hour; + uint32_t utc_minute; + uint32_t utc_second; + uint32_t utc_ms; - // GPS time of week to UTC - uint32_t nb_hours = ref_log_gps_pos.timeOfWeek / (3600 * 1000); - uint32_t nb_minutes = ref_log_gps_pos.timeOfWeek / (60 * 1000); - uint32_t current_hour = nb_hours % 24; - uint32_t current_minute = nb_minutes % 60; - uint32_t current_second = (ref_log_gps_pos.timeOfWeek / 1000) % 60; - uint32_t current_ms = ref_log_gps_pos.timeOfWeek % (60 * 1000); - current_ms = (current_ms - (current_second * 1000)) / 10; + // Offset GPS Time of Week by a full day to easily handle zero roll over while applying the leap second + gps_tow_ms = ref_log_gps_pos.timeOfWeek + (24ul * 3600ul * 1000ul); - // Latitude conversion - char lat_dir; - float latitude = ref_log_gps_pos.latitude; - if (latitude >= 90.0f) - { - latitude = 90.0f; - } - if (latitude <= -90.0f) - { - latitude = -90.0f; - } - int32_t lat_degs = latitude; - float lat_mins = (latitude - static_cast(lat_degs)) * 60.0f; - if(latitude < 0.0f) - { - lat_degs *= -1; - lat_mins *= -1.0f; - lat_dir = 'S'; - } - else - { - lat_dir = 'N'; - } + // Apply the GPS to UTC leap second offset + gps_tow_ms = gps_tow_ms - getUtcOffset() * 1000ul; - // Longitude conversion - char lon_dir; - float longitude = ref_log_gps_pos.longitude; - if (longitude >= 180.0f) - { - longitude = 180.0f; - } - if (longitude <= -180.0f) - { - longitude = -180.0f; - } - int32_t lon_degs = longitude; - float lon_mins = (longitude - static_cast(lon_degs)) * 60.0f; - if(longitude < 0.0f) - { - lon_degs *= -1; - lon_mins *= -1.0f; - lon_dir = 'W'; - } - else - { - lon_dir = 'E'; - } - - // DOP computation - float h_dop = std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy); - if (h_dop > 9.9f) - { - h_dop = 9.9f; - } + // Extract UTC hours/mins/seconds values + utc_hour = (gps_tow_ms / (3600 * 1000)) % 24; + utc_minute = (gps_tow_ms / (60 * 1000)) % 60; + utc_second = (gps_tow_ms / 1000) % 60; + utc_ms = (gps_tow_ms % 1000); - float diff_age = (ref_log_gps_pos.differentialAge / 100.0f); - if (diff_age > 9.9f) - { - diff_age = 9.9f; - } - - uint8_t svUsed = ref_log_gps_pos.numSvUsed; - if (svUsed > 99) - { - svUsed = 99; - } - - float altitude = ref_log_gps_pos.altitude; - if (altitude < -99999.9f) - { - altitude = -99999.9f; - } - if (altitude > 99999.9f) - { - altitude = 99999.9f; - } - - int32_t undulation = static_cast(ref_log_gps_pos.undulation); - if (undulation < -99999) - { - undulation = -99999; - } - if (undulation > 99999) - { - undulation = 99999; - } - - uint16_t baseStationId = ref_log_gps_pos.baseStationId; - if (baseStationId >= 10000) - { - baseStationId = 9999; - } + // Only output a GGA message at 1 Hz (plain second) and for valid positions + if ( (sbgEComLogGpsPosGetStatus(ref_log_gps_pos.status) == SBG_ECOM_POS_SOL_COMPUTED) && + (sbgEComLogGpsPosGetType(ref_log_gps_pos.status) != SBG_ECOM_POS_NO_SOLUTION) && + (utc_ms < 100) && (utc_ms > 900) ) + { + // Latitude conversion + char lat_dir; + float latitude = std::clamp(ref_log_gps_pos.latitude, -90.0f, 90.0f); + float latitude_abs = std::fabs(latitude); + int32_t lat_degs = static_cast(latitude_abs); + float lat_mins = (latitude_abs - static_cast(lat_degs)) * 60.0f; - // Writing NMEA sentence + // Longitude conversion + char lon_dir; + float longitude = std::clamp(ref_log_gps_pos.longitude, -180.0f, 180.0f); + float longitude_abs = std::fabs(longitude); + int32_t lon_degs = static_cast(longitude_abs); + float lon_mins = (longitude_abs - static_cast(lon_degs)) * 60.0f; + + // Compute and clamp each parameter + float h_dop = std::clamp(std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy), 0.0f, 9.9f); + float diff_age = std::clamp(ref_log_gps_pos.differentialAge / 100.0f, 0.0f, 9.9f); + uint8_t sv_used = std::clamp(ref_log_gps_pos.numSvUsed, 0, 99); + float altitude = std::clamp(ref_log_gps_pos.altitude, -99999.9f, 99999.9f); + int32_t undulation = std::clamp(static_cast(ref_log_gps_pos.undulation), -99999, 99999); + uint16_t base_station_id = std::clamp(ref_log_gps_pos.baseStationId, 0, 9999); + + // Write the NMEA sentence - 80 chars max constexpr uint32_t nmea_sentence_buffer_size = 128; char nmea_sentence_buff[nmea_sentence_buffer_size]{}; auto len = snprintf(nmea_sentence_buff, nmea_sentence_buffer_size, "$GPGGA,%02d%02d%02d.%02d,%02d%02.4f,%c,%03d%02.4f,%c,%d,%d,%.1f,%.1f,M,%d,M,%.1f,%04d", - current_hour, - current_minute, - current_second, - current_ms, + utc_hour, + utc_minute, + utc_second, + utc_ms/10, lat_degs, lat_mins, - lat_dir, + (latitude < 0.0f?'S':'N'), lon_degs, lon_mins, - lon_dir, + (longitude < 0.0f?'W':'E'), convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status)), - svUsed, + sv_used, h_dop, altitude, undulation, diff_age, - baseStationId - ); + base_station_id + ); - // Checksum computation + // Compute and append the NMEA checksum uint8_t checksum = 0; for (int32_t i = 1; i < len; i++) { - checksum ^= nmea_sentence_buff[i]; - } + checksum ^= nmea_sentence_buff[i]; + } snprintf(&nmea_sentence_buff[len], nmea_sentence_buffer_size - len, "*%02X\r\n", checksum); - // Populating ROS message - gps_pos_nmea_msg.header = createRosHeader(ref_log_gps_pos.timeStamp); - gps_pos_nmea_msg.sentence = nmea_sentence_buff; + // Fill the NMEA ROS message + nmea_gga_msg.header = createRosHeader(ref_log_gps_pos.timeStamp); + nmea_gga_msg.sentence = nmea_sentence_buff; + } - return gps_pos_nmea_msg; + return nmea_gga_msg; } From 675f85aa0b150112e27dd16a80d3ce5d9bd9456e Mon Sep 17 00:00:00 2001 From: cledant Date: Wed, 16 Aug 2023 11:12:10 +0200 Subject: [PATCH 24/74] Compilation fixes --- src/message_wrapper.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 7d4b224..3ff2222 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -10,6 +10,9 @@ // Project headers #include +// STL headers +#include + using sbg::MessageWrapper; /*! @@ -541,28 +544,28 @@ sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsTy case SBG_ECOM_POS_NO_SOLUTION: nmeaQuality = NmeaGGAQuality::INVALID; break; - + case SBG_ECOM_POS_UNKNOWN_TYPE: case SBG_ECOM_POS_SINGLE: case SBG_ECOM_POS_FIXED: nmeaQuality = NmeaGGAQuality::SINGLE; break; - + case SBG_ECOM_POS_PSRDIFF: case SBG_ECOM_POS_SBAS: case SBG_ECOM_POS_OMNISTAR: nmeaQuality = NmeaGGAQuality::DGPS; break; - + case SBG_ECOM_POS_PPP_FLOAT: case SBG_ECOM_POS_PPP_INT: nmeaQuality = NmeaGGAQuality::PPS; break; - + case SBG_ECOM_POS_RTK_INT: nmeaQuality = NmeaGGAQuality::RTK_FIXED; break; - + case SBG_ECOM_POS_RTK_FLOAT: nmeaQuality = NmeaGGAQuality::RTK_FLOAT; break; @@ -1399,15 +1402,13 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons (utc_ms < 100) && (utc_ms > 900) ) { // Latitude conversion - char lat_dir; - float latitude = std::clamp(ref_log_gps_pos.latitude, -90.0f, 90.0f); + float latitude = std::clamp(static_cast(ref_log_gps_pos.latitude), -90.0f, 90.0f); float latitude_abs = std::fabs(latitude); int32_t lat_degs = static_cast(latitude_abs); float lat_mins = (latitude_abs - static_cast(lat_degs)) * 60.0f; // Longitude conversion - char lon_dir; - float longitude = std::clamp(ref_log_gps_pos.longitude, -180.0f, 180.0f); + float longitude = std::clamp(static_cast(ref_log_gps_pos.longitude), -180.0f, 180.0f); float longitude_abs = std::fabs(longitude); int32_t lon_degs = static_cast(longitude_abs); float lon_mins = (longitude_abs - static_cast(lon_degs)) * 60.0f; @@ -1415,11 +1416,11 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons // Compute and clamp each parameter float h_dop = std::clamp(std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy), 0.0f, 9.9f); float diff_age = std::clamp(ref_log_gps_pos.differentialAge / 100.0f, 0.0f, 9.9f); - uint8_t sv_used = std::clamp(ref_log_gps_pos.numSvUsed, 0, 99); - float altitude = std::clamp(ref_log_gps_pos.altitude, -99999.9f, 99999.9f); + uint8_t sv_used = std::clamp(ref_log_gps_pos.numSvUsed, static_cast(0), static_cast(99)); + float altitude = std::clamp(static_cast(ref_log_gps_pos.altitude), -99999.9f, 99999.9f); int32_t undulation = std::clamp(static_cast(ref_log_gps_pos.undulation), -99999, 99999); - uint16_t base_station_id = std::clamp(ref_log_gps_pos.baseStationId, 0, 9999); - + uint16_t base_station_id = std::clamp(ref_log_gps_pos.baseStationId, static_cast(0), static_cast(9999)); + // Write the NMEA sentence - 80 chars max constexpr uint32_t nmea_sentence_buffer_size = 128; char nmea_sentence_buff[nmea_sentence_buffer_size]{}; @@ -1435,7 +1436,7 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons lon_degs, lon_mins, (longitude < 0.0f?'W':'E'), - convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status)), + static_cast(convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status))), sv_used, h_dop, altitude, From a27ac78dce94a2ccb11f0eb7dcab66b6212b25e4 Mon Sep 17 00:00:00 2001 From: cledant Date: Wed, 16 Aug 2023 11:34:48 +0200 Subject: [PATCH 25/74] Fixed nmea output condition --- src/message_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 3ff2222..df80eec 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1399,7 +1399,7 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons // Only output a GGA message at 1 Hz (plain second) and for valid positions if ( (sbgEComLogGpsPosGetStatus(ref_log_gps_pos.status) == SBG_ECOM_POS_SOL_COMPUTED) && (sbgEComLogGpsPosGetType(ref_log_gps_pos.status) != SBG_ECOM_POS_NO_SOLUTION) && - (utc_ms < 100) && (utc_ms > 900) ) + (utc_ms < 100 || utc_ms > 900) ) { // Latitude conversion float latitude = std::clamp(static_cast(ref_log_gps_pos.latitude), -90.0f, 90.0f); From 086cd4f250672c0cb7838e0759dffcd257d8ef99 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 30 Jun 2023 11:16:40 +0200 Subject: [PATCH 26/74] Fixed Euler / Quaternion orientation in ENU mode --- include/sbg_driver/message_wrapper.h | 8 ++++ src/message_wrapper.cpp | 61 ++++++++++++++++++---------- 2 files changed, 48 insertions(+), 21 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 06eddfc..045ee7f 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -137,6 +137,14 @@ class MessageWrapper : public rclcpp::Node */ float wrapAngle2Pi(float angle_rad) const; + /*! + * Wrap an angle to PI. + * + * \param[in] angle_rad Angle in rad. + * \return Wrapped angle. + */ + float wrapAnglePi(float angle_rad) const; + /*! * Wrap an angle to 360 degres. * diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index df80eec..4492209 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -53,6 +53,22 @@ float MessageWrapper::wrapAngle2Pi(float angle_rad) const return angle_rad; } +float MessageWrapper::wrapAnglePi(float angle_rad) const +{ + angle_rad = fmodf(angle_rad, SBG_PI_F * 2.0f); + if (angle_rad > SBG_PI_F) + { + angle_rad = SBG_PI_F * 2.0f - angle_rad; + } + + if (angle_rad < -SBG_PI_F) + { + angle_rad = SBG_PI_F * 2.0f + angle_rad; + } + + return angle_rad; +} + float MessageWrapper::wrapAngle360(float angle_deg) const { if ( (angle_deg < -360.0f) || (angle_deg > 360.0f) ) @@ -634,7 +650,7 @@ const sbg_driver::msg::SbgEkfEuler MessageWrapper::createSbgEkfEulerMessage(cons { ekf_euler_message.angle.x = ref_log_ekf_euler.euler[0]; ekf_euler_message.angle.y = -ref_log_ekf_euler.euler[1]; - ekf_euler_message.angle.z = wrapAngle2Pi((SBG_PI_F / 2.0f) - ref_log_ekf_euler.euler[2]); + ekf_euler_message.angle.z = wrapAnglePi((SBG_PI_F / 2.0f) - ref_log_ekf_euler.euler[2]); } else { @@ -709,10 +725,13 @@ const sbg_driver::msg::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const if (m_use_enu_) { - ekf_quat_message.quaternion.x = ref_log_ekf_quat.quaternion[1]; - ekf_quat_message.quaternion.y = -ref_log_ekf_quat.quaternion[2]; - ekf_quat_message.quaternion.z = -ref_log_ekf_quat.quaternion[3]; - ekf_quat_message.quaternion.w = ref_log_ekf_quat.quaternion[0]; + tf2::Quaternion q_NWU{ref_log_ekf_quat.quaternion[1], + -ref_log_ekf_quat.quaternion[2], + -ref_log_ekf_quat.quaternion[3], + ref_log_ekf_quat.quaternion[0]}; + const tf2::Quaternion q_ENU_NWU{0, 0, M_SQRT2 / 2, M_SQRT2 / 2}; + + ekf_quat_message.quaternion = tf2::toMsg(q_ENU_NWU * q_NWU); } else { @@ -864,20 +883,20 @@ const sbg_driver::msg::SbgImuData MessageWrapper::createSbgImuDataMessage(const if (m_use_enu_) { - imu_data_message.accel.x = ref_log_imu_data.accelerometers[0]; - imu_data_message.accel.y = -ref_log_imu_data.accelerometers[1]; + imu_data_message.accel.x = ref_log_imu_data.accelerometers[1]; + imu_data_message.accel.y = ref_log_imu_data.accelerometers[0]; imu_data_message.accel.z = -ref_log_imu_data.accelerometers[2]; - imu_data_message.gyro.x = ref_log_imu_data.gyroscopes[0]; - imu_data_message.gyro.y = -ref_log_imu_data.gyroscopes[1]; + imu_data_message.gyro.x = ref_log_imu_data.gyroscopes[1]; + imu_data_message.gyro.y = ref_log_imu_data.gyroscopes[0]; imu_data_message.gyro.z = -ref_log_imu_data.gyroscopes[2]; - imu_data_message.delta_vel.x = ref_log_imu_data.deltaVelocity[0]; - imu_data_message.delta_vel.y = -ref_log_imu_data.deltaVelocity[1]; + imu_data_message.delta_vel.x = ref_log_imu_data.deltaVelocity[1]; + imu_data_message.delta_vel.y = ref_log_imu_data.deltaVelocity[0]; imu_data_message.delta_vel.z = -ref_log_imu_data.deltaVelocity[2]; - imu_data_message.delta_angle.x = ref_log_imu_data.deltaAngle[0]; - imu_data_message.delta_angle.y = -ref_log_imu_data.deltaAngle[1]; + imu_data_message.delta_angle.x = ref_log_imu_data.deltaAngle[1]; + imu_data_message.delta_angle.y = ref_log_imu_data.deltaAngle[0]; imu_data_message.delta_angle.z = -ref_log_imu_data.deltaAngle[2]; } else @@ -912,12 +931,12 @@ const sbg_driver::msg::SbgMag MessageWrapper::createSbgMagMessage(const SbgLogMa if (m_use_enu_) { - mag_message.mag.x = ref_log_mag.magnetometers[0]; - mag_message.mag.y = -ref_log_mag.magnetometers[1]; + mag_message.mag.x = ref_log_mag.magnetometers[1]; + mag_message.mag.y = ref_log_mag.magnetometers[0]; mag_message.mag.z = -ref_log_mag.magnetometers[2]; - mag_message.accel.x = ref_log_mag.accelerometers[0]; - mag_message.accel.y = -ref_log_mag.accelerometers[1]; + mag_message.accel.x = ref_log_mag.accelerometers[1]; + mag_message.accel.y = ref_log_mag.accelerometers[0]; mag_message.accel.z = -ref_log_mag.accelerometers[2]; } else @@ -1058,12 +1077,12 @@ const sbg_driver::msg::SbgImuShort MessageWrapper::createSbgImuShortMessage(cons if (m_use_enu_) { - imu_short_message.delta_velocity.x = ref_short_imu_log.deltaVelocity[0]; - imu_short_message.delta_velocity.y = -ref_short_imu_log.deltaVelocity[1]; + imu_short_message.delta_velocity.x = ref_short_imu_log.deltaVelocity[1]; + imu_short_message.delta_velocity.y = ref_short_imu_log.deltaVelocity[0]; imu_short_message.delta_velocity.z = -ref_short_imu_log.deltaVelocity[2]; - imu_short_message.delta_angle.x = ref_short_imu_log.deltaAngle[0]; - imu_short_message.delta_angle.y = -ref_short_imu_log.deltaAngle[1]; + imu_short_message.delta_angle.x = ref_short_imu_log.deltaAngle[1]; + imu_short_message.delta_angle.y = ref_short_imu_log.deltaAngle[0]; imu_short_message.delta_angle.z = -ref_short_imu_log.deltaAngle[2]; } else From cbc2b1e6997a241bfb482d23b1025a5d68047707 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 30 Jun 2023 11:46:51 +0200 Subject: [PATCH 27/74] Updated ROS messages documentation --- msg/SbgGpsVel.msg | 2 +- msg/SbgImuData.msg | 16 ++++++++-------- msg/SbgImuShort.msg | 8 ++++---- msg/SbgMag.msg | 8 ++++---- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/msg/SbgGpsVel.msg b/msg/SbgGpsVel.msg index f595cd0..787b2cb 100644 --- a/msg/SbgGpsVel.msg +++ b/msg/SbgGpsVel.msg @@ -37,5 +37,5 @@ geometry_msgs/Vector3 velocity_accuracy # ENU convention: Zero when the X axis is pointing East. (opposite sign compared to NED) float32 course -# 1 sgima course accuracy +# 1 sigma course accuracy float32 course_acc diff --git a/msg/SbgImuData.msg b/msg/SbgImuData.msg index 137e8d0..8c79b22 100644 --- a/msg/SbgImuData.msg +++ b/msg/SbgImuData.msg @@ -15,8 +15,8 @@ SbgImuStatus imu_status # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 accel @@ -28,8 +28,8 @@ geometry_msgs/Vector3 accel # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 gyro @@ -44,8 +44,8 @@ float32 temp # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 delta_vel @@ -57,7 +57,7 @@ geometry_msgs/Vector3 delta_vel # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 delta_angle diff --git a/msg/SbgImuShort.msg b/msg/SbgImuShort.msg index 4095e57..94b5548 100644 --- a/msg/SbgImuShort.msg +++ b/msg/SbgImuShort.msg @@ -15,8 +15,8 @@ SbgImuStatus imu_status # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 delta_velocity @@ -28,8 +28,8 @@ geometry_msgs/Vector3 delta_velocity # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 delta_angle diff --git a/msg/SbgMag.msg b/msg/SbgMag.msg index 1a89157..c39d966 100644 --- a/msg/SbgMag.msg +++ b/msg/SbgMag.msg @@ -12,8 +12,8 @@ uint32 time_stamp # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 mag @@ -25,8 +25,8 @@ geometry_msgs/Vector3 mag # z: Z axis of the device frame # # ENU convention: -# x: X axis of the device frame -# y: -Y axis of the device frame +# x: Y axis of the device frame +# y: X axis of the device frame # z: -Z axis of the device frame geometry_msgs/Vector3 accel From ef1947b0b5d4a03a5701025aaaca4b50ce5a1a5c Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 30 Jun 2023 16:35:11 +0200 Subject: [PATCH 28/74] Refactored NED to ENU array conversion --- include/sbg_driver/message_wrapper.h | 17 ++++++++++ src/message_wrapper.cpp | 49 +++++++--------------------- 2 files changed, 28 insertions(+), 38 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 045ee7f..506e4b4 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -363,6 +363,23 @@ class MessageWrapper : public rclcpp::Node */ static NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType); + /*! + * Convert a NED array to an ENU vector. + * + * \param[in] nedArray Reference to a NED formatted array. + * \return An ENU formatted Vector3. + */ + template + static inline geometry_msgs::msg::Vector3 nedToEnu(const T (&nedArray)[3]) + { + geometry_msgs::msg::Vector3 enuVector; + + enuVector.x = nedArray[1]; + enuVector.y = nedArray[0]; + enuVector.z = -nedArray[2]; + return (enuVector); + } + public: //---------------------------------------------------------------------// diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 4492209..e02908a 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -681,9 +681,7 @@ const sbg_driver::msg::SbgEkfNav MessageWrapper::createSbgEkfNavMessage(const Sb if (m_use_enu_) { - ekf_nav_message.velocity.x = ref_log_ekf_nav.velocity[1]; - ekf_nav_message.velocity.y = ref_log_ekf_nav.velocity[0]; - ekf_nav_message.velocity.z = -ref_log_ekf_nav.velocity[2]; + ekf_nav_message.velocity = nedToEnu(ref_log_ekf_nav.velocity); ekf_nav_message.velocity_accuracy.x = ref_log_ekf_nav.velocityStdDev[1]; ekf_nav_message.velocity_accuracy.y = ref_log_ekf_nav.velocityStdDev[0]; @@ -846,9 +844,7 @@ const sbg_driver::msg::SbgGpsVel MessageWrapper::createSbgGpsVelMessage(const Sb if (m_use_enu_) { - gps_vel_message.velocity.x = ref_log_gps_vel.velocity[1]; - gps_vel_message.velocity.y = ref_log_gps_vel.velocity[0]; - gps_vel_message.velocity.z = -ref_log_gps_vel.velocity[2]; + gps_vel_message.velocity = nedToEnu(ref_log_gps_vel.velocity); gps_vel_message.velocity_accuracy.x = ref_log_gps_vel.velocityAcc[1]; gps_vel_message.velocity_accuracy.y = ref_log_gps_vel.velocityAcc[0]; @@ -883,21 +879,10 @@ const sbg_driver::msg::SbgImuData MessageWrapper::createSbgImuDataMessage(const if (m_use_enu_) { - imu_data_message.accel.x = ref_log_imu_data.accelerometers[1]; - imu_data_message.accel.y = ref_log_imu_data.accelerometers[0]; - imu_data_message.accel.z = -ref_log_imu_data.accelerometers[2]; - - imu_data_message.gyro.x = ref_log_imu_data.gyroscopes[1]; - imu_data_message.gyro.y = ref_log_imu_data.gyroscopes[0]; - imu_data_message.gyro.z = -ref_log_imu_data.gyroscopes[2]; - - imu_data_message.delta_vel.x = ref_log_imu_data.deltaVelocity[1]; - imu_data_message.delta_vel.y = ref_log_imu_data.deltaVelocity[0]; - imu_data_message.delta_vel.z = -ref_log_imu_data.deltaVelocity[2]; - - imu_data_message.delta_angle.x = ref_log_imu_data.deltaAngle[1]; - imu_data_message.delta_angle.y = ref_log_imu_data.deltaAngle[0]; - imu_data_message.delta_angle.z = -ref_log_imu_data.deltaAngle[2]; + imu_data_message.accel = nedToEnu(ref_log_imu_data.accelerometers); + imu_data_message.gyro = nedToEnu(ref_log_imu_data.gyroscopes); + imu_data_message.delta_vel = nedToEnu(ref_log_imu_data.deltaVelocity); + imu_data_message.delta_angle = nedToEnu(ref_log_imu_data.deltaAngle); } else { @@ -931,13 +916,8 @@ const sbg_driver::msg::SbgMag MessageWrapper::createSbgMagMessage(const SbgLogMa if (m_use_enu_) { - mag_message.mag.x = ref_log_mag.magnetometers[1]; - mag_message.mag.y = ref_log_mag.magnetometers[0]; - mag_message.mag.z = -ref_log_mag.magnetometers[2]; - - mag_message.accel.x = ref_log_mag.accelerometers[1]; - mag_message.accel.y = ref_log_mag.accelerometers[0]; - mag_message.accel.z = -ref_log_mag.accelerometers[2]; + mag_message.mag = nedToEnu(ref_log_mag.magnetometers); + mag_message.accel = nedToEnu(ref_log_mag.accelerometers); } else { @@ -1077,13 +1057,8 @@ const sbg_driver::msg::SbgImuShort MessageWrapper::createSbgImuShortMessage(cons if (m_use_enu_) { - imu_short_message.delta_velocity.x = ref_short_imu_log.deltaVelocity[1]; - imu_short_message.delta_velocity.y = ref_short_imu_log.deltaVelocity[0]; - imu_short_message.delta_velocity.z = -ref_short_imu_log.deltaVelocity[2]; - - imu_short_message.delta_angle.x = ref_short_imu_log.deltaAngle[1]; - imu_short_message.delta_angle.y = ref_short_imu_log.deltaAngle[0]; - imu_short_message.delta_angle.z = -ref_short_imu_log.deltaAngle[2]; + imu_short_message.delta_velocity = nedToEnu(ref_short_imu_log.deltaVelocity); + imu_short_message.delta_angle = nedToEnu(ref_short_imu_log.deltaAngle); } else { @@ -1105,7 +1080,7 @@ const sensor_msgs::msg::Imu MessageWrapper::createRosImuMessage(const sbg_driver imu_ros_message.header = createRosHeader(ref_sbg_imu_msg.time_stamp); - imu_ros_message.orientation = ref_sbg_quat_msg.quaternion; + imu_ros_message.orientation = ref_sbg_quat_msg.quaternion; imu_ros_message.angular_velocity = ref_sbg_imu_msg.delta_angle; imu_ros_message.linear_acceleration = ref_sbg_imu_msg.delta_vel; @@ -1127,8 +1102,6 @@ const sensor_msgs::msg::Imu MessageWrapper::createRosImuMessage(const sbg_driver void MessageWrapper::fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::msg::Pose &ref_pose, geometry_msgs::msg::TransformStamped &refTransformStamped) { - tf2::Quaternion q; - refTransformStamped.header.stamp = rclcpp::Clock().now(); refTransformStamped.header.frame_id = ref_parent_frame_id; refTransformStamped.child_frame_id = ref_child_frame_id; From 897875b889cb533c8906b72e4b8e2570f625605f Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 30 Jun 2023 18:02:47 +0200 Subject: [PATCH 29/74] odom->base_link is now correct in NED mode --- src/message_wrapper.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index e02908a..7076bab 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1155,9 +1155,19 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - pose.position.x = m_utm0_.easting; - pose.position.y = m_utm0_.northing; - pose.position.z = m_utm0_.altitude; + if (m_use_enu_) + { + pose.position.x = m_utm0_.easting; + pose.position.y = m_utm0_.northing; + pose.position.z = m_utm0_.altitude; + } + else + { + pose.position.x = m_utm0_.northing; + pose.position.y = m_utm0_.easting; + pose.position.z = m_utm0_.altitude; + } + fillTransform(m_odom_init_frame_id_, m_odom_frame_id_, pose, transform); m_static_tf_broadcaster_->sendTransform(transform); } From ff94ef54e43d19b935806294a027e50426c2dff2 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 3 Jul 2023 09:35:36 +0200 Subject: [PATCH 30/74] Fixed variable inversion. --- src/message_wrapper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 7076bab..28afb41 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1173,9 +1173,9 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv } } - LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, m_utm0_.zone, utm_easting, utm_northing); - odo_ros_msg.pose.pose.position.x = utm_northing - m_utm0_.easting; - odo_ros_msg.pose.pose.position.y = utm_easting - m_utm0_.northing; + LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, m_utm0_.zone, utm_northing, utm_easting); + odo_ros_msg.pose.pose.position.x = utm_easting - m_utm0_.easting; + odo_ros_msg.pose.pose.position.y = utm_northing - m_utm0_.northing; odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; // Compute convergence angle. From 54b3fc6cc94461da350dc29c84dab28cbb423b10 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 3 Jul 2023 17:55:08 +0200 Subject: [PATCH 31/74] Renamed function. --- include/sbg_driver/message_wrapper.h | 2 +- src/message_wrapper.cpp | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 506e4b4..37bace5 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -370,7 +370,7 @@ class MessageWrapper : public rclcpp::Node * \return An ENU formatted Vector3. */ template - static inline geometry_msgs::msg::Vector3 nedToEnu(const T (&nedArray)[3]) + static inline geometry_msgs::msg::Vector3 convertNedArrayToEnuVector(const T (&nedArray)[3]) { geometry_msgs::msg::Vector3 enuVector; diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 28afb41..91bd1d1 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -681,7 +681,7 @@ const sbg_driver::msg::SbgEkfNav MessageWrapper::createSbgEkfNavMessage(const Sb if (m_use_enu_) { - ekf_nav_message.velocity = nedToEnu(ref_log_ekf_nav.velocity); + ekf_nav_message.velocity = convertNedArrayToEnuVector(ref_log_ekf_nav.velocity); ekf_nav_message.velocity_accuracy.x = ref_log_ekf_nav.velocityStdDev[1]; ekf_nav_message.velocity_accuracy.y = ref_log_ekf_nav.velocityStdDev[0]; @@ -844,7 +844,7 @@ const sbg_driver::msg::SbgGpsVel MessageWrapper::createSbgGpsVelMessage(const Sb if (m_use_enu_) { - gps_vel_message.velocity = nedToEnu(ref_log_gps_vel.velocity); + gps_vel_message.velocity = convertNedArrayToEnuVector(ref_log_gps_vel.velocity); gps_vel_message.velocity_accuracy.x = ref_log_gps_vel.velocityAcc[1]; gps_vel_message.velocity_accuracy.y = ref_log_gps_vel.velocityAcc[0]; @@ -879,10 +879,10 @@ const sbg_driver::msg::SbgImuData MessageWrapper::createSbgImuDataMessage(const if (m_use_enu_) { - imu_data_message.accel = nedToEnu(ref_log_imu_data.accelerometers); - imu_data_message.gyro = nedToEnu(ref_log_imu_data.gyroscopes); - imu_data_message.delta_vel = nedToEnu(ref_log_imu_data.deltaVelocity); - imu_data_message.delta_angle = nedToEnu(ref_log_imu_data.deltaAngle); + imu_data_message.accel = convertNedArrayToEnuVector(ref_log_imu_data.accelerometers); + imu_data_message.gyro = convertNedArrayToEnuVector(ref_log_imu_data.gyroscopes); + imu_data_message.delta_vel = convertNedArrayToEnuVector(ref_log_imu_data.deltaVelocity); + imu_data_message.delta_angle = convertNedArrayToEnuVector(ref_log_imu_data.deltaAngle); } else { @@ -916,8 +916,8 @@ const sbg_driver::msg::SbgMag MessageWrapper::createSbgMagMessage(const SbgLogMa if (m_use_enu_) { - mag_message.mag = nedToEnu(ref_log_mag.magnetometers); - mag_message.accel = nedToEnu(ref_log_mag.accelerometers); + mag_message.mag = convertNedArrayToEnuVector(ref_log_mag.magnetometers); + mag_message.accel = convertNedArrayToEnuVector(ref_log_mag.accelerometers); } else { @@ -1057,8 +1057,8 @@ const sbg_driver::msg::SbgImuShort MessageWrapper::createSbgImuShortMessage(cons if (m_use_enu_) { - imu_short_message.delta_velocity = nedToEnu(ref_short_imu_log.deltaVelocity); - imu_short_message.delta_angle = nedToEnu(ref_short_imu_log.deltaAngle); + imu_short_message.delta_velocity = convertNedArrayToEnuVector(ref_short_imu_log.deltaVelocity); + imu_short_message.delta_angle = convertNedArrayToEnuVector(ref_short_imu_log.deltaAngle); } else { From c3850193c495b243a8165d37f483b4bec627de34 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 3 Jul 2023 17:59:43 +0200 Subject: [PATCH 32/74] odom->base_link is now correct in NED mode --- src/message_wrapper.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 91bd1d1..f92a011 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1174,9 +1174,18 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv } LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, m_utm0_.zone, utm_northing, utm_easting); - odo_ros_msg.pose.pose.position.x = utm_easting - m_utm0_.easting; - odo_ros_msg.pose.pose.position.y = utm_northing - m_utm0_.northing; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; + if (m_use_enu_) + { + odo_ros_msg.pose.pose.position.x = utm_easting - m_utm0_.easting; + odo_ros_msg.pose.pose.position.y = utm_northing - m_utm0_.northing; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; + } + else + { + odo_ros_msg.pose.pose.position.x = utm_northing - m_utm0_.northing; + odo_ros_msg.pose.pose.position.y = utm_easting - m_utm0_.easting; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; + } // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); From 33b208e5d6218b6dea7ca4e9f34377090854f463 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 4 Jul 2023 09:31:41 +0200 Subject: [PATCH 33/74] Documentation update --- msg/SbgEkfEuler.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/SbgEkfEuler.msg b/msg/SbgEkfEuler.msg index 025ac45..fc0b71c 100644 --- a/msg/SbgEkfEuler.msg +++ b/msg/SbgEkfEuler.msg @@ -18,7 +18,7 @@ uint32 time_stamp # ENU convention: # Roll: Rotation around X axis # Pitch: Rotation around Y axis (opposite sign compared to NED) -# Yaw: Rotation about the up axis. Zero when the X axis is pointing East. (opposite sign compared to NED) +# Yaw: Rotation about the up axis. Zero when the X axis is pointing East. geometry_msgs/Vector3 angle # Angle accuracy (Roll, Pitch, Yaw (heading)) (1 sigma) [rad] From a35035f44f0742d37a1407c9e3055e3ce735db2b Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 21 Aug 2023 13:38:04 +0200 Subject: [PATCH 34/74] Reverted NED to ENU array conversion --- include/sbg_driver/message_wrapper.h | 17 ----------- src/message_wrapper.cpp | 45 +++++++++++++++++++++------- 2 files changed, 35 insertions(+), 27 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 37bace5..045ee7f 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -363,23 +363,6 @@ class MessageWrapper : public rclcpp::Node */ static NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType); - /*! - * Convert a NED array to an ENU vector. - * - * \param[in] nedArray Reference to a NED formatted array. - * \return An ENU formatted Vector3. - */ - template - static inline geometry_msgs::msg::Vector3 convertNedArrayToEnuVector(const T (&nedArray)[3]) - { - geometry_msgs::msg::Vector3 enuVector; - - enuVector.x = nedArray[1]; - enuVector.y = nedArray[0]; - enuVector.z = -nedArray[2]; - return (enuVector); - } - public: //---------------------------------------------------------------------// diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index f92a011..3ce2b57 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -681,7 +681,9 @@ const sbg_driver::msg::SbgEkfNav MessageWrapper::createSbgEkfNavMessage(const Sb if (m_use_enu_) { - ekf_nav_message.velocity = convertNedArrayToEnuVector(ref_log_ekf_nav.velocity); + ekf_nav_message.velocity.x = ref_log_ekf_nav.velocity[1]; + ekf_nav_message.velocity.y = ref_log_ekf_nav.velocity[0]; + ekf_nav_message.velocity.z = -ref_log_ekf_nav.velocity[2]; ekf_nav_message.velocity_accuracy.x = ref_log_ekf_nav.velocityStdDev[1]; ekf_nav_message.velocity_accuracy.y = ref_log_ekf_nav.velocityStdDev[0]; @@ -844,7 +846,9 @@ const sbg_driver::msg::SbgGpsVel MessageWrapper::createSbgGpsVelMessage(const Sb if (m_use_enu_) { - gps_vel_message.velocity = convertNedArrayToEnuVector(ref_log_gps_vel.velocity); + gps_vel_message.velocity.x = ref_log_gps_vel.velocity[1]; + gps_vel_message.velocity.y = ref_log_gps_vel.velocity[0]; + gps_vel_message.velocity.z = -ref_log_gps_vel.velocity[2]; gps_vel_message.velocity_accuracy.x = ref_log_gps_vel.velocityAcc[1]; gps_vel_message.velocity_accuracy.y = ref_log_gps_vel.velocityAcc[0]; @@ -879,10 +883,21 @@ const sbg_driver::msg::SbgImuData MessageWrapper::createSbgImuDataMessage(const if (m_use_enu_) { - imu_data_message.accel = convertNedArrayToEnuVector(ref_log_imu_data.accelerometers); - imu_data_message.gyro = convertNedArrayToEnuVector(ref_log_imu_data.gyroscopes); - imu_data_message.delta_vel = convertNedArrayToEnuVector(ref_log_imu_data.deltaVelocity); - imu_data_message.delta_angle = convertNedArrayToEnuVector(ref_log_imu_data.deltaAngle); + imu_data_message.accel.x = ref_log_imu_data.accelerometers[0]; + imu_data_message.accel.y = -ref_log_imu_data.accelerometers[1]; + imu_data_message.accel.z = -ref_log_imu_data.accelerometers[2]; + + imu_data_message.gyro.x = ref_log_imu_data.gyroscopes[0]; + imu_data_message.gyro.y = -ref_log_imu_data.gyroscopes[1]; + imu_data_message.gyro.z = -ref_log_imu_data.gyroscopes[2]; + + imu_data_message.delta_vel.x = ref_log_imu_data.deltaVelocity[0]; + imu_data_message.delta_vel.y = -ref_log_imu_data.deltaVelocity[1]; + imu_data_message.delta_vel.z = -ref_log_imu_data.deltaVelocity[2]; + + imu_data_message.delta_angle.x = ref_log_imu_data.deltaAngle[0]; + imu_data_message.delta_angle.y = -ref_log_imu_data.deltaAngle[1]; + imu_data_message.delta_angle.z = -ref_log_imu_data.deltaAngle[2]; } else { @@ -916,8 +931,13 @@ const sbg_driver::msg::SbgMag MessageWrapper::createSbgMagMessage(const SbgLogMa if (m_use_enu_) { - mag_message.mag = convertNedArrayToEnuVector(ref_log_mag.magnetometers); - mag_message.accel = convertNedArrayToEnuVector(ref_log_mag.accelerometers); + mag_message.mag.x = ref_log_mag.magnetometers[0]; + mag_message.mag.y = -ref_log_mag.magnetometers[1]; + mag_message.mag.z = -ref_log_mag.magnetometers[2]; + + mag_message.accel.x = ref_log_mag.accelerometers[0]; + mag_message.accel.y = -ref_log_mag.accelerometers[1]; + mag_message.accel.z = -ref_log_mag.accelerometers[2]; } else { @@ -1057,8 +1077,13 @@ const sbg_driver::msg::SbgImuShort MessageWrapper::createSbgImuShortMessage(cons if (m_use_enu_) { - imu_short_message.delta_velocity = convertNedArrayToEnuVector(ref_short_imu_log.deltaVelocity); - imu_short_message.delta_angle = convertNedArrayToEnuVector(ref_short_imu_log.deltaAngle); + imu_short_message.delta_velocity.x = ref_short_imu_log.deltaVelocity[0]; + imu_short_message.delta_velocity.y = -ref_short_imu_log.deltaVelocity[1]; + imu_short_message.delta_velocity.z = -ref_short_imu_log.deltaVelocity[2]; + + imu_short_message.delta_angle.x = ref_short_imu_log.deltaAngle[0]; + imu_short_message.delta_angle.y = -ref_short_imu_log.deltaAngle[1]; + imu_short_message.delta_angle.z = -ref_short_imu_log.deltaAngle[2]; } else { From 0cc9f1cc24ec113996acd7c5c0f5d474c5e6b03f Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 21 Aug 2023 14:30:40 +0200 Subject: [PATCH 35/74] More explicit naming for quaternions --- src/message_wrapper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 3ce2b57..70039b2 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -725,13 +725,13 @@ const sbg_driver::msg::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const if (m_use_enu_) { - tf2::Quaternion q_NWU{ref_log_ekf_quat.quaternion[1], + tf2::Quaternion q_nwu{ref_log_ekf_quat.quaternion[1], -ref_log_ekf_quat.quaternion[2], -ref_log_ekf_quat.quaternion[3], ref_log_ekf_quat.quaternion[0]}; - const tf2::Quaternion q_ENU_NWU{0, 0, M_SQRT2 / 2, M_SQRT2 / 2}; + const tf2::Quaternion q_enu_to_nwu{0, 0, M_SQRT2 / 2, M_SQRT2 / 2}; - ekf_quat_message.quaternion = tf2::toMsg(q_ENU_NWU * q_NWU); + ekf_quat_message.quaternion = tf2::toMsg(q_enu_to_nwu * q_nwu); } else { From d035a94106c676db835f0b7a54a66df53adf416c Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 21 Aug 2023 14:53:28 +0200 Subject: [PATCH 36/74] Added range for Euler angle measurement. --- msg/SbgEkfEuler.msg | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/msg/SbgEkfEuler.msg b/msg/SbgEkfEuler.msg index fc0b71c..c232b6d 100644 --- a/msg/SbgEkfEuler.msg +++ b/msg/SbgEkfEuler.msg @@ -11,14 +11,14 @@ uint32 time_stamp # the fingers on your right hand curl when your thumb is pointing in the direction of the axis of rotation. # # NED convention: -# Roll: Rotation about the X axis -# Pitch: Rotation about the Y axis -# Yaw: Rotation about the down axis. Zero when the X axis is pointing North. +# Roll: Rotation about the X axis: [ -Pi ; Pi ]. +# Pitch: Rotation about the Y axis: [ -Pi/2 ; Pi/2 ]. +# Yaw: Rotation about the down axis: [ -Pi ; Pi ]. Zero when the X axis is pointing North. # # ENU convention: -# Roll: Rotation around X axis -# Pitch: Rotation around Y axis (opposite sign compared to NED) -# Yaw: Rotation about the up axis. Zero when the X axis is pointing East. +# Roll: Rotation around X axis: [ -Pi ; Pi ]. +# Pitch: Rotation around Y axis: [ -Pi/2 ; Pi/2 ]. (Opposite sign compared to NED) +# Yaw: Rotation about the up axis: [ -Pi ; Pi ]. Zero when the X axis is pointing East. geometry_msgs/Vector3 angle # Angle accuracy (Roll, Pitch, Yaw (heading)) (1 sigma) [rad] From 1286ebbf02b50954db3ef0520f524bef4c0e1bc0 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 21 Aug 2023 14:02:05 +0200 Subject: [PATCH 37/74] Reworked angle wrapping functions --- include/sbg_driver/message_wrapper.h | 18 ++++-------- src/message_wrapper.cpp | 42 ++++++++++------------------ 2 files changed, 19 insertions(+), 41 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 045ee7f..3fbff88 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -130,28 +130,20 @@ class MessageWrapper : public rclcpp::Node //---------------------------------------------------------------------// /*! - * Wrap an angle to 2 PI. + * Wrap an angle between [ -Pi ; Pi ] rad. * * \param[in] angle_rad Angle in rad. * \return Wrapped angle. */ - float wrapAngle2Pi(float angle_rad) const; + static float wrapAnglePi(float angle_rad); /*! - * Wrap an angle to PI. - * - * \param[in] angle_rad Angle in rad. - * \return Wrapped angle. - */ - float wrapAnglePi(float angle_rad) const; - - /*! - * Wrap an angle to 360 degres. + * Wrap an angle between [ 0 ; 360 ] degree. * * \param[in] angle_deg Angle in degree. * \return Wrapped angle. */ - float wrapAngle360(float angle_deg) const; + static float wrapAngle360(float angle_deg); /*! * Compute UTM zone meridian. @@ -159,7 +151,7 @@ class MessageWrapper : public rclcpp::Node * \param[in] zone_number UTM Zone number. * \return Meridian angle, in degrees. */ - double computeMeridian(int zone_number) const; + static double computeMeridian(int zone_number); /*! * Create a ROS message header. diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 70039b2..e7d4848 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -38,53 +38,39 @@ Node("tf_broadcaster") //- Internal methods -// //---------------------------------------------------------------------// -float MessageWrapper::wrapAngle2Pi(float angle_rad) const +float MessageWrapper::wrapAnglePi(float angle_rad) { - if ((angle_rad < -SBG_PI_F * 2.0f) || (angle_rad > SBG_PI_F * 2.0f)) + if (angle_rad > SBG_PI_F) { - angle_rad = fmodf(angle_rad, SBG_PI_F * 2.0f); + return (SBG_PI_F * 2.0f - fmodf(angle_rad, SBG_PI_F * 2.0f)); } - if (angle_rad < 0.0f) + if (angle_rad < -SBG_PI_F) { - angle_rad = SBG_PI_F * 2.0f + angle_rad; + return (SBG_PI_F * 2.0f + fmodf(angle_rad, SBG_PI_F * 2.0f)); } return angle_rad; } -float MessageWrapper::wrapAnglePi(float angle_rad) const +float MessageWrapper::wrapAngle360(float angle_deg) { - angle_rad = fmodf(angle_rad, SBG_PI_F * 2.0f); - if (angle_rad > SBG_PI_F) - { - angle_rad = SBG_PI_F * 2.0f - angle_rad; - } - - if (angle_rad < -SBG_PI_F) - { - angle_rad = SBG_PI_F * 2.0f + angle_rad; - } - - return angle_rad; -} + float wrapped_angle_deg = angle_deg; -float MessageWrapper::wrapAngle360(float angle_deg) const -{ - if ( (angle_deg < -360.0f) || (angle_deg > 360.0f) ) + if ( (wrapped_angle_deg < -360.0f) || (wrapped_angle_deg > 360.0f) ) { - angle_deg = fmodf(angle_deg, 360.0f); + wrapped_angle_deg = fmodf(wrapped_angle_deg, 360.0f); } - if (angle_deg < 0.0f) + if (wrapped_angle_deg < 0.0f) { - angle_deg = 360.0f + angle_deg; + wrapped_angle_deg = 360.0f + wrapped_angle_deg; } - return angle_deg; + return wrapped_angle_deg; } -double MessageWrapper::computeMeridian(int zone_number) const +double MessageWrapper::computeMeridian(int zone_number) { return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; } @@ -650,7 +636,7 @@ const sbg_driver::msg::SbgEkfEuler MessageWrapper::createSbgEkfEulerMessage(cons { ekf_euler_message.angle.x = ref_log_ekf_euler.euler[0]; ekf_euler_message.angle.y = -ref_log_ekf_euler.euler[1]; - ekf_euler_message.angle.z = wrapAnglePi((SBG_PI_F / 2.0f) - ref_log_ekf_euler.euler[2]); + ekf_euler_message.angle.z = -wrapAnglePi(-(SBG_PI_F / 2.0f) + ref_log_ekf_euler.euler[2]); } else { From c112e9f1c26bc9b4b2e913e45460f27f8356765c Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 21 Aug 2023 18:31:04 +0200 Subject: [PATCH 38/74] Updated readme about ENU frame convention --- README.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index c488002..aa32565 100644 --- a/README.md +++ b/README.md @@ -380,7 +380,16 @@ frame_id: "imu_link_ned" #### Frame convention The frame convention can be set to NED or ENU * The NED convention is SBG Systems native convention so no transformation is applied. -* The ENU convention requires the following transformation (x = Y, y = X, z = -Z). +* The ENU convention follows ROS standard [REP-103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions): + * Axis Orientation: + * In relation to a body the standard is: + * x forward + * y left + * z up + * Cartesian representation: + * X east + * Y north + * Z up ``` # Frame convention: use_enu: true From e882a927ea816322bfdb0ec62dcb9a7090f8e927 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 22 Aug 2023 09:41:21 +0200 Subject: [PATCH 39/74] Quaternion: NED to ENU conversion rework --- src/message_wrapper.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index e7d4848..af130cf 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -709,22 +709,20 @@ const sbg_driver::msg::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const ekf_quat_message.accuracy.y = ref_log_ekf_quat.eulerStdDev[1]; ekf_quat_message.accuracy.z = ref_log_ekf_quat.eulerStdDev[2]; + tf2::Quaternion q_ned{ref_log_ekf_quat.quaternion[1], + ref_log_ekf_quat.quaternion[2], + ref_log_ekf_quat.quaternion[3], + ref_log_ekf_quat.quaternion[0]}; if (m_use_enu_) { - tf2::Quaternion q_nwu{ref_log_ekf_quat.quaternion[1], - -ref_log_ekf_quat.quaternion[2], - -ref_log_ekf_quat.quaternion[3], - ref_log_ekf_quat.quaternion[0]}; - const tf2::Quaternion q_enu_to_nwu{0, 0, M_SQRT2 / 2, M_SQRT2 / 2}; + tf2::Quaternion q_ned_to_enu; + q_ned_to_enu.setRPY(sbgDegToRadF(180.0f), 0.0f, sbgDegToRadF(90.0f)); - ekf_quat_message.quaternion = tf2::toMsg(q_enu_to_nwu * q_nwu); + ekf_quat_message.quaternion = tf2::toMsg(q_ned_to_enu * q_ned); } else { - ekf_quat_message.quaternion.x = ref_log_ekf_quat.quaternion[1]; - ekf_quat_message.quaternion.y = ref_log_ekf_quat.quaternion[2]; - ekf_quat_message.quaternion.z = ref_log_ekf_quat.quaternion[3]; - ekf_quat_message.quaternion.w = ref_log_ekf_quat.quaternion[0]; + ekf_quat_message.quaternion = tf2::toMsg(q_ned); } return ekf_quat_message; From 7bbd4010fc5aea09129f27a912a68763f17ec161 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 22 Aug 2023 10:09:44 +0200 Subject: [PATCH 40/74] Reworked odometry message --- src/message_wrapper.cpp | 30 ++++++------------------------ 1 file changed, 6 insertions(+), 24 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index af130cf..60dbecd 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1164,18 +1164,9 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - if (m_use_enu_) - { - pose.position.x = m_utm0_.easting; - pose.position.y = m_utm0_.northing; - pose.position.z = m_utm0_.altitude; - } - else - { - pose.position.x = m_utm0_.northing; - pose.position.y = m_utm0_.easting; - pose.position.z = m_utm0_.altitude; - } + pose.position.x = m_utm0_.easting; + pose.position.y = m_utm0_.northing; + pose.position.z = m_utm0_.altitude; fillTransform(m_odom_init_frame_id_, m_odom_frame_id_, pose, transform); m_static_tf_broadcaster_->sendTransform(transform); @@ -1183,18 +1174,9 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv } LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, m_utm0_.zone, utm_northing, utm_easting); - if (m_use_enu_) - { - odo_ros_msg.pose.pose.position.x = utm_easting - m_utm0_.easting; - odo_ros_msg.pose.pose.position.y = utm_northing - m_utm0_.northing; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; - } - else - { - odo_ros_msg.pose.pose.position.x = utm_northing - m_utm0_.northing; - odo_ros_msg.pose.pose.position.y = utm_easting - m_utm0_.easting; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; - } + odo_ros_msg.pose.pose.position.x = utm_easting - m_utm0_.easting; + odo_ros_msg.pose.pose.position.y = utm_northing - m_utm0_.northing; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); From 61366586ebc8160fd494c1d4a92acd47fdb6f525 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 22 Aug 2023 11:15:38 +0200 Subject: [PATCH 41/74] Quaternion: cleaner version for NED to ENU conversion --- include/sbg_driver/message_wrapper.h | 2 ++ src/message_wrapper.cpp | 6 ++---- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 3fbff88..ae4e5da 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -125,6 +125,8 @@ class MessageWrapper : public rclcpp::Node std::string m_odom_base_frame_id_; std::string m_odom_init_frame_id_; + tf2::Quaternion q_ned_to_enu_; + //---------------------------------------------------------------------// //- Internal methods -// //---------------------------------------------------------------------// diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 60dbecd..f71324d 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -32,6 +32,7 @@ Node("tf_broadcaster") m_utm0_.northing = 0.0; m_utm0_.altitude = 0.0; m_utm0_.zone = 0; + tf2::Matrix3x3(0, 1, 0, 1, 0, 0, 0, 0, -1).getRotation(q_ned_to_enu_); } //---------------------------------------------------------------------// @@ -715,10 +716,7 @@ const sbg_driver::msg::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const ref_log_ekf_quat.quaternion[0]}; if (m_use_enu_) { - tf2::Quaternion q_ned_to_enu; - q_ned_to_enu.setRPY(sbgDegToRadF(180.0f), 0.0f, sbgDegToRadF(90.0f)); - - ekf_quat_message.quaternion = tf2::toMsg(q_ned_to_enu * q_ned); + ekf_quat_message.quaternion = tf2::toMsg(q_ned_to_enu_ * q_ned); } else { From 9b5f320bdea0e5ff24fba84ffaa74579f1cc1da0 Mon Sep 17 00:00:00 2001 From: cledant Date: Thu, 24 Aug 2023 17:04:28 +0200 Subject: [PATCH 42/74] Reverted NED to ENU quaternion conversion --- include/sbg_driver/message_wrapper.h | 2 -- src/message_wrapper.cpp | 18 ++++++++++++------ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index ae4e5da..3fbff88 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -125,8 +125,6 @@ class MessageWrapper : public rclcpp::Node std::string m_odom_base_frame_id_; std::string m_odom_init_frame_id_; - tf2::Quaternion q_ned_to_enu_; - //---------------------------------------------------------------------// //- Internal methods -// //---------------------------------------------------------------------// diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index f71324d..efe96d4 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -32,7 +32,6 @@ Node("tf_broadcaster") m_utm0_.northing = 0.0; m_utm0_.altitude = 0.0; m_utm0_.zone = 0; - tf2::Matrix3x3(0, 1, 0, 1, 0, 0, 0, 0, -1).getRotation(q_ned_to_enu_); } //---------------------------------------------------------------------// @@ -710,16 +709,23 @@ const sbg_driver::msg::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const ekf_quat_message.accuracy.y = ref_log_ekf_quat.eulerStdDev[1]; ekf_quat_message.accuracy.z = ref_log_ekf_quat.eulerStdDev[2]; - tf2::Quaternion q_ned{ref_log_ekf_quat.quaternion[1], - ref_log_ekf_quat.quaternion[2], - ref_log_ekf_quat.quaternion[3], - ref_log_ekf_quat.quaternion[0]}; if (m_use_enu_) { - ekf_quat_message.quaternion = tf2::toMsg(q_ned_to_enu_ * q_ned); + static const tf2::Quaternion q_enu_to_nwu{0, 0, M_SQRT2 / 2, M_SQRT2 / 2}; + tf2::Quaternion q_nwu{ref_log_ekf_quat.quaternion[1], + -ref_log_ekf_quat.quaternion[2], + -ref_log_ekf_quat.quaternion[3], + ref_log_ekf_quat.quaternion[0]}; + + ekf_quat_message.quaternion = tf2::toMsg(q_enu_to_nwu * q_nwu); } else { + tf2::Quaternion q_ned{ref_log_ekf_quat.quaternion[1], + ref_log_ekf_quat.quaternion[2], + ref_log_ekf_quat.quaternion[3], + ref_log_ekf_quat.quaternion[0]}; + ekf_quat_message.quaternion = tf2::toMsg(q_ned); } From 0bd0ea1cf041663feb0538bd150173fed7c7a436 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 25 Aug 2023 14:28:52 +0200 Subject: [PATCH 43/74] Disabling ROS standard message when in NED frame convention --- include/sbg_driver/message_publisher.h | 2 +- src/message_publisher.cpp | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index 770d05a..45209d9 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -116,7 +116,7 @@ class MessagePublisher * \param[in] ref_ros_node_handle Ros Node to advertise the publisher. * \param[in] odom_enable If true, enable odometry messages. */ - void defineRosStandardPublishers(rclcpp::Node& ref_ros_node_handle, bool odom_enable); + void defineRosStandardPublishers(rclcpp::Node& ref_ros_node_handle, bool odom_enable, bool enu_enable); /*! * Publish a received SBG IMU log. diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 5c14ec5..2ab7c09 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -207,8 +207,14 @@ void MessagePublisher::initPublisher(rclcpp::Node& ref_ros_node_handle, SbgEComM } } -void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_handle, bool odom_enable) +void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_handle, bool odom_enable, bool enu_enable) { + if (!enu_enable) + { + RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] Driver is configured in NED frame convention, ROS standard message are disabled."); + return; + } + if (m_sbgImuData_pub_ && m_sbgEkfQuat_pub_) { m_imu_pub_ = ref_ros_node_handle.create_publisher("imu/data", m_max_messages_); @@ -497,7 +503,7 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C if (ref_config_store.checkRosStandardMessages()) { - defineRosStandardPublishers(ref_ros_node_handle, ref_config_store.getOdomEnable()); + defineRosStandardPublishers(ref_ros_node_handle, ref_config_store.getOdomEnable(), ref_config_store.getUseEnu()); } } From 918aebb6b399ca0f42b02304bc96a2139cfe60c1 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Fri, 25 Aug 2023 15:00:36 +0200 Subject: [PATCH 44/74] Improved ENU/NED documentation --- README.md | 60 +++++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 43 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index aa32565..0fa5db2 100644 --- a/README.md +++ b/README.md @@ -224,6 +224,9 @@ For each ROS standard, you have to activate the needed SBG outputs. Requires `/sbg/imu_data` and `/sbg/ekv_nav` and either `/sbg/ekf_euler` or `/sbg/ekf_quat`. Disabled by default, set `odometry.enable` in configuration file. +> [!NOTE] +> Please update the driver configuration to enable standard ROS messages publication. Also, the driver only publish standard ROS messages if the driver is setup to use ENU frame convention. + ##### NMEA topics The driver can publish NMEA GGA messages from the internal GNSS receiver. It can be used with third party [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) modules to support VRS networks providers. @@ -369,32 +372,55 @@ Configuration example to use an absolute and accurate time reference to UNIX epo time_reference: "ins_unix" ``` -### Change frame parameters -#### Frame ID +## Frame parameters & conventions +### Frame ID The frame_id of the header can be set with this parameter: ``` -# Frame convention +# Frame name frame_id: "imu_link_ned" ``` -#### Frame convention -The frame convention can be set to NED or ENU -* The NED convention is SBG Systems native convention so no transformation is applied. -* The ENU convention follows ROS standard [REP-103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions): - * Axis Orientation: - * In relation to a body the standard is: - * x forward - * y left - * z up - * Cartesian representation: - * X east - * Y north - * Z up +### Frame convention +The frame convention can be set to NED or ENU: +* The NED convention is SBG Systems native convention so no transformation is applied +* The ENU convention follows ROS standard [REP-103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions) + +Please read the SBG Systems [Support Center article](https://support.sbg-systems.com/sc/kb/latest/underlying-maths-conventions) for more details. + +You can select the frame convention to use with the following parameter: ``` -# Frame convention: +# Frame convention use_enu: true ``` +> [!NOTE] +> The driver only publish standard ROS messages if the driver is setup to use ENU frame convention. + +#### Body/Vehicle Frame: +The X axis should point the vehicle **forward** direction for both NED and ENU frame conventions. +The table below summarizes the body/vehicle axis frame definitions for each convention: + +| NED Convention | ENU Convention | +| -------------- | -------------- | +| X Forward | X Forward | +| Y Right | Y Left | +| Z Downward | Z Upward | + +#### Navigation Frame: + +The navigation frame also referred by ROS as the cartesian representation is defined as follow: + +| NED Convention | ENU Convention | +| -------------- | -------------- | +| X North | X East | +| Y East | Y North | +| Z Down | Z Up | + +#### Heading Example: + +Based on the definitions above, when using a NED frame, if the vehicle X axis is pointing North, the INS should return a zero heading. +When using a ENU frame, the INS should return a zero heading when the vehicle X axis is pointing East. + ## Troubleshooting If you experience higher latency than expected and have connected the IMU via an USB interface, you can enable the serial driver low latency mode: From 40341b3b8d9faf6e4a66111752e286ff33786edb Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 25 Aug 2023 15:19:57 +0200 Subject: [PATCH 45/74] Updated naming convention --- include/sbg_driver/config_applier.h | 4 +- include/sbg_driver/config_store.h | 70 +++---- include/sbg_driver/message_publisher.h | 78 ++++---- include/sbg_driver/message_wrapper.h | 54 +++--- include/sbg_driver/sbg_device.h | 24 +-- include/sbg_driver/sbg_matrix3.h | 120 ++++++------ include/sbg_driver/sbg_vector3.h | 30 +-- src/config_applier.cpp | 64 +++---- src/config_store.cpp | 212 ++++++++++---------- src/message_publisher.cpp | 256 ++++++++++++------------- src/message_wrapper.cpp | 190 +++++++++--------- src/sbg_device.cpp | 200 +++++++++---------- 12 files changed, 651 insertions(+), 651 deletions(-) diff --git a/include/sbg_driver/config_applier.h b/include/sbg_driver/config_applier.h index f125502..0ffdb6b 100644 --- a/include/sbg_driver/config_applier.h +++ b/include/sbg_driver/config_applier.h @@ -52,8 +52,8 @@ class ConfigApplier { private: - bool m_reboot_needed_; - SbgEComHandle& m_ref_sbg_com_handle; + bool reboot_needed_; + SbgEComHandle& ref_sbg_com_handle_; //---------------------------------------------------------------------// //- Private methods -// diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index c601aa7..47223b7 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -73,53 +73,53 @@ class ConfigStore private: - std::string m_uart_port_name_; - SbgEComOutputPort m_output_port_; - uint32_t m_uart_baud_rate_; - bool m_serial_communication_; + std::string uart_port_name_; + SbgEComOutputPort output_port_; + uint32_t uart_baud_rate_; + bool serial_communication_; - sbgIpAddress m_sbg_ip_address_; - uint32_t m_out_port_address_; - uint32_t m_in_port_address_; - bool m_upd_communication_; + sbgIpAddress sbg_ip_address_; + uint32_t out_port_address_; + uint32_t in_port_address_; + bool upd_communication_; - bool m_configure_through_ros_; + bool configure_through_ros_; - SbgEComInitConditionConf m_init_condition_conf_; - SbgEComModelInfo m_motion_profile_model_info_; + SbgEComInitConditionConf init_condition_conf_; + SbgEComModelInfo motion_profile_model_info_; - SbgEComSensorAlignmentInfo m_sensor_alignement_info_; - SbgVector3 m_sensor_lever_arm_; + SbgEComSensorAlignmentInfo sensor_alignement_info_; + SbgVector3 sensor_lever_arm_; - SbgEComAidingAssignConf m_aiding_assignement_conf_; + SbgEComAidingAssignConf aiding_assignement_conf_; - SbgEComModelInfo m_mag_model_info_; - SbgEComMagRejectionConf m_mag_rejection_conf_; - SbgEComMagCalibMode m_mag_calib_mode_; - SbgEComMagCalibBandwidth m_mag_calib_bandwidth_; + SbgEComModelInfo mag_model_info_; + SbgEComMagRejectionConf mag_rejection_conf_; + SbgEComMagCalibMode mag_calib_mode_; + SbgEComMagCalibBandwidth mag_calib_bandwidth_; - SbgEComModelInfo m_gnss_model_info_; - SbgEComGnssInstallation m_gnss_installation_; - SbgEComGnssRejectionConf m_gnss_rejection_conf_; + SbgEComModelInfo gnss_model_info_; + SbgEComGnssInstallation gnss_installation_; + SbgEComGnssRejectionConf gnss_rejection_conf_; - SbgEComOdoConf m_odometer_conf_; - SbgVector3 m_odometer_level_arm_; - SbgEComOdoRejectionConf m_odometer_rejection_conf_; + SbgEComOdoConf odometer_conf_; + SbgVector3 odometer_level_arm_; + SbgEComOdoRejectionConf odometer_rejection_conf_; - std::vector m_output_modes_; - bool m_ros_standard_output_; + std::vector output_modes_; + bool ros_standard_output_; - TimeReference m_time_reference_; + TimeReference time_reference_; - uint32_t m_rate_frequency_; - std::string m_frame_id_; - bool m_use_enu_; + uint32_t rate_frequency_; + std::string frame_id_; + bool use_enu_; - bool m_odom_enable_; - bool m_odom_publish_tf_; - std::string m_odom_frame_id_; - std::string m_odom_base_frame_id_; - std::string m_odom_init_frame_id_; + bool odom_enable_; + bool odom_publish_tf_; + std::string odom_frame_id_; + std::string odom_base_frame_id_; + std::string odom_init_frame_id_; bool rtcm_subscribe_; std::string rtcm_full_topic_; diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index 45209d9..f6d83eb 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -45,48 +45,48 @@ class MessagePublisher { private: - rclcpp::Publisher>::SharedPtr m_sbgStatus_pub_; - rclcpp::Publisher>::SharedPtr m_sbgUtcTime_pub_; - rclcpp::Publisher>::SharedPtr m_sbgImuData_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEkfEuler_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEkfQuat_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEkfNav_pub_; - rclcpp::Publisher>::SharedPtr m_sbgShipMotion_pub_; - rclcpp::Publisher>::SharedPtr m_sbgMag_pub_; - rclcpp::Publisher>::SharedPtr m_sbgMagCalib_pub_; - rclcpp::Publisher>::SharedPtr m_sbgGpsVel_pub_; - rclcpp::Publisher>::SharedPtr m_sbgGpsPos_pub_; - rclcpp::Publisher>::SharedPtr m_sbgGpsHdt_pub_; - rclcpp::Publisher>::SharedPtr m_sbgGpsRaw_pub_; - rclcpp::Publisher>::SharedPtr m_sbgOdoVel_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEventA_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEventB_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEventC_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEventD_pub_; - rclcpp::Publisher>::SharedPtr m_sbgEventE_pub_; - rclcpp::Publisher>::SharedPtr m_SbgImuShort_pub_; - rclcpp::Publisher>::SharedPtr m_SbgAirData_pub_; - - rclcpp::Publisher>::SharedPtr m_imu_pub_; - sbg_driver::msg::SbgImuData m_sbg_imu_message_; - sbg_driver::msg::SbgEkfQuat m_sbg_ekf_quat_message_; - sbg_driver::msg::SbgEkfNav m_sbg_ekf_nav_message_; - sbg_driver::msg::SbgEkfEuler m_sbg_ekf_euler_message_; - - rclcpp::Publisher>::SharedPtr m_temp_pub_; - rclcpp::Publisher>::SharedPtr m_mag_pub_; - rclcpp::Publisher>::SharedPtr m_fluid_pub_; - rclcpp::Publisher>::SharedPtr m_pos_ecef_pub_; - rclcpp::Publisher>::SharedPtr m_velocity_pub_; - rclcpp::Publisher>::SharedPtr m_utc_reference_pub_; - rclcpp::Publisher>::SharedPtr m_nav_sat_fix_pub_; - rclcpp::Publisher>::SharedPtr m_odometry_pub_; + rclcpp::Publisher>::SharedPtr sbg_status_pub_; + rclcpp::Publisher>::SharedPtr sbg_utc_time_pub_; + rclcpp::Publisher>::SharedPtr sbg_imu_data_pub_; + rclcpp::Publisher>::SharedPtr sbg_ekf_euler_pub_; + rclcpp::Publisher>::SharedPtr sbg_ekf_quat_pub_; + rclcpp::Publisher>::SharedPtr sbg_ekf_nav_pub_; + rclcpp::Publisher>::SharedPtr sbg_ship_motion_pub_; + rclcpp::Publisher>::SharedPtr sbg_mag_pub_; + rclcpp::Publisher>::SharedPtr sbg_mag_calib_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_vel_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_pos_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_hdt_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_raw_pub_; + rclcpp::Publisher>::SharedPtr sbg_odo_vel_pub_; + rclcpp::Publisher>::SharedPtr sbg_event_a_pub_; + rclcpp::Publisher>::SharedPtr sbg_event_b_pub_; + rclcpp::Publisher>::SharedPtr sbg_event_c_pub_; + rclcpp::Publisher>::SharedPtr sbg_event_d_pub_; + rclcpp::Publisher>::SharedPtr sbg_event_e_pub_; + rclcpp::Publisher>::SharedPtr sbg_imu_short_pub_; + rclcpp::Publisher>::SharedPtr sbg_air_data_pub_; + + rclcpp::Publisher>::SharedPtr imu_pub_; + sbg_driver::msg::SbgImuData sbg_imu_message_; + sbg_driver::msg::SbgEkfQuat sbg_ekf_quat_message_; + sbg_driver::msg::SbgEkfNav sbg_ekf_nav_message_; + sbg_driver::msg::SbgEkfEuler sbg_ekf_euler_message_; + + rclcpp::Publisher>::SharedPtr temp_pub_; + rclcpp::Publisher>::SharedPtr mag_pub_; + rclcpp::Publisher>::SharedPtr fluid_pub_; + rclcpp::Publisher>::SharedPtr pos_ecef_pub_; + rclcpp::Publisher>::SharedPtr velocity_pub_; + rclcpp::Publisher>::SharedPtr utc_reference_pub_; + rclcpp::Publisher>::SharedPtr nav_sat_fix_pub_; + rclcpp::Publisher>::SharedPtr odometry_pub_; rclcpp::Publisher>::SharedPtr nmea_gga_pub_; - MessageWrapper m_message_wrapper_; - uint32_t m_max_messages_; - std::string m_frame_id_; + MessageWrapper message_wrapper_; + uint32_t max_messages_; + std::string frame_id_; //---------------------------------------------------------------------// //- Private methods -// diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 3fbff88..b4474d4 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -112,18 +112,18 @@ class MessageWrapper : public rclcpp::Node }; private: - sbg_driver::msg::SbgUtcTime m_last_sbg_utc_; - bool m_first_valid_utc_; - std::string m_frame_id_; - bool m_use_enu_; - TimeReference m_time_reference_; - UTM0 m_utm0_; - - bool m_odom_enable_; - bool m_odom_publish_tf_; - std::string m_odom_frame_id_; - std::string m_odom_base_frame_id_; - std::string m_odom_init_frame_id_; + sbg_driver::msg::SbgUtcTime last_sbg_utc_; + bool first_valid_utc_; + std::string frame_id_; + bool use_enu_; + TimeReference time_reference_; + UTM0 utm0_; + + bool odom_enable_; + bool odom_publish_tf_; + std::string odom_frame_id_; + std::string odom_base_frame_id_; + std::string odom_init_frame_id_; //---------------------------------------------------------------------// //- Internal methods -// @@ -322,38 +322,38 @@ class MessageWrapper : public rclcpp::Node /*! * Get UTM letter designator for the given latitude. * - * \param[in] Lat Latitude, in degrees. + * \param[in] latitude Latitude, in degrees. * \return UTM letter designator. */ - char UTMLetterDesignator(double Lat); + char UTMLetterDesignator(double latitude); /*! * Set UTM initial position. * - * \param[in] Lat Latitude, in degrees. - * \param[in] Long Longitude, in degrees. + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. * \param[in] altitude Altitude, in meters. */ - void initUTM(double Lat, double Long, double altitude); + void initUTM(double latitude, double longitude, double altitude); /*! * Convert latitude and longitude to a position relative to UTM initial position. * - * \param[in] Lat Latitude, in degrees. - * \param[in] Long Longitude, in degrees. - * \param[in] zoneNumber UTM zone number. - * \param[out] UTMNorthing UTM northing, in meters. - * \param[out] UTMEasting UTM easting, in meters. + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \param[in] zone_number UTM zone number. + * \param[out] utm_northing UTM northing, in meters. + * \param[out] utm_easting UTM easting, in meters. */ - void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const; + void LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) const; /*! * Convert SbgEComGpsPosType enum to NmeaGGAQuality enum * - * \param[in] sbgGpsType SbgECom GPS type + * \param[in] sbg_gps_type SbgECom GPS type * \return NMEA GPS type */ - static NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType); + static NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type); public: @@ -361,8 +361,8 @@ class MessageWrapper : public rclcpp::Node //- Transform broadcasters -// //---------------------------------------------------------------------// - std::shared_ptr m_tf_broadcaster_; - std::shared_ptr m_static_tf_broadcaster_; + std::shared_ptr tf_broadcaster_; + std::shared_ptr static_tf_broadcaster_; //---------------------------------------------------------------------// //- Constructor -// diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index 4ee2edf..ed0950a 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -32,25 +32,25 @@ class SbgDevice static std::map g_mag_calib_quality_; static std::map g_mag_calib_confidence_; static std::map g_mag_calib_mode_; - static std::map g_mag_calib_bandwidth; + static std::map g_mag_calib_bandwidth_; //---------------------------------------------------------------------// //- Private variables -// //---------------------------------------------------------------------// - SbgEComHandle m_com_handle_; - SbgInterface m_sbg_interface_; - rclcpp::Node& m_ref_node_; - MessagePublisher m_message_publisher_; - ConfigStore m_config_store_; + SbgEComHandle com_handle_; + SbgInterface sbg_interface_; + rclcpp::Node& ref_node_; + MessagePublisher message_publisher_; + ConfigStore config_store_; - uint32_t m_rate_frequency_; + uint32_t rate_frequency_; - bool m_mag_calibration_ongoing_; - bool m_mag_calibration_done_; - SbgEComMagCalibResults m_magCalibResults; - rclcpp::Service::SharedPtr m_calib_service_; - rclcpp::Service::SharedPtr m_calib_save_service_; + bool mag_calibration_ongoing_; + bool mag_calibration_done_; + SbgEComMagCalibResults mag_calib_results_; + rclcpp::Service::SharedPtr calib_service_; + rclcpp::Service::SharedPtr calib_save_service_; rclcpp::Subscription::SharedPtr rtcm_sub_; diff --git a/include/sbg_driver/sbg_matrix3.h b/include/sbg_driver/sbg_matrix3.h index b0158a8..beb54ff 100644 --- a/include/sbg_driver/sbg_matrix3.h +++ b/include/sbg_driver/sbg_matrix3.h @@ -57,7 +57,7 @@ class SbgMatrix3 //- Private variables -// //---------------------------------------------------------------------// - std::array m_data; + std::array data_; public: @@ -70,15 +70,15 @@ class SbgMatrix3 */ SbgMatrix3() { - m_data[0] = static_cast(0.0); - m_data[1] = static_cast(0.0); - m_data[2] = static_cast(0.0); - m_data[3] = static_cast(0.0); - m_data[4] = static_cast(0.0); - m_data[5] = static_cast(0.0); - m_data[6] = static_cast(0.0); - m_data[7] = static_cast(0.0); - m_data[8] = static_cast(0.0); + data_[0] = static_cast(0.0); + data_[1] = static_cast(0.0); + data_[2] = static_cast(0.0); + data_[3] = static_cast(0.0); + data_[4] = static_cast(0.0); + data_[5] = static_cast(0.0); + data_[6] = static_cast(0.0); + data_[7] = static_cast(0.0); + data_[8] = static_cast(0.0); } /*! @@ -96,15 +96,15 @@ class SbgMatrix3 */ SbgMatrix3(T value00, T value01, T value02, T value10, T value11, T value12, T value20, T value21, T value22) { - m_data[0] = value00; - m_data[1] = value01; - m_data[2] = value02; - m_data[3] = value10; - m_data[4] = value11; - m_data[5] = value12; - m_data[6] = value20; - m_data[7] = value21; - m_data[8] = value22; + data_[0] = value00; + data_[1] = value01; + data_[2] = value02; + data_[3] = value10; + data_[4] = value11; + data_[5] = value12; + data_[6] = value20; + data_[7] = value21; + data_[8] = value22; }; /*! @@ -117,15 +117,15 @@ class SbgMatrix3 { assert(array_size == 9); - m_data[0] = p_raw_data[0]; - m_data[1] = p_raw_data[1]; - m_data[2] = p_raw_data[2]; - m_data[3] = p_raw_data[3]; - m_data[4] = p_raw_data[4]; - m_data[5] = p_raw_data[5]; - m_data[6] = p_raw_data[6]; - m_data[7] = p_raw_data[7]; - m_data[8] = p_raw_data[8]; + data_[0] = p_raw_data[0]; + data_[1] = p_raw_data[1]; + data_[2] = p_raw_data[2]; + data_[3] = p_raw_data[3]; + data_[4] = p_raw_data[4]; + data_[5] = p_raw_data[5]; + data_[6] = p_raw_data[6]; + data_[7] = p_raw_data[7]; + data_[8] = p_raw_data[8]; }; //---------------------------------------------------------------------// @@ -147,7 +147,7 @@ class SbgMatrix3 { assert(i * 3 + j < 9); - return m_data[i * 3 + j]; + return data_[i * 3 + j]; } /*! @@ -157,31 +157,31 @@ class SbgMatrix3 */ const T *data() const { - return static_cast(m_data.data()); + return static_cast(data_.data()); }; const SbgVector3 operator*(const SbgVector3& vect) const { - T x = m_data[0] * vect(0) + m_data[1] * vect(1) + m_data[2] * vect(2); - T y = m_data[3] * vect(0) + m_data[4] * vect(1) + m_data[5] * vect(2); - T z = m_data[6] * vect(0) + m_data[7] * vect(1) + m_data[8] * vect(2); + T x = data_[0] * vect(0) + data_[1] * vect(1) + data_[2] * vect(2); + T y = data_[3] * vect(0) + data_[4] * vect(1) + data_[5] * vect(2); + T z = data_[6] * vect(0) + data_[7] * vect(1) + data_[8] * vect(2); return SbgVector3(x, y, z); } void transpose() { - T swap = m_data[1]; - m_data[1] = m_data[3]; - m_data[3] = swap; + T swap = data_[1]; + data_[1] = data_[3]; + data_[3] = swap; - swap = m_data[2]; - m_data[2] = m_data[6]; - m_data[6] = swap; + swap = data_[2]; + data_[2] = data_[6]; + data_[6] = swap; - swap = m_data[5]; - m_data[5] = m_data[7]; - m_data[7] = swap; + swap = data_[5]; + data_[5] = data_[7]; + data_[7] = swap; } /*! @@ -201,17 +201,17 @@ class SbgMatrix3 float cy = cosf(euler(2)); float sy = sinf(euler(2)); - m_data[0] = cp * cy; - m_data[3] = cp * sy; - m_data[6] = -sp; + data_[0] = cp * cy; + data_[3] = cp * sy; + data_[6] = -sp; - m_data[1] = (sr * sp * cy) - (cr * sy); - m_data[4] = (sr * sp * sy) + (cr * cy); - m_data[7] = sr * cp; + data_[1] = (sr * sp * cy) - (cr * sy); + data_[4] = (sr * sp * sy) + (cr * cy); + data_[7] = sr * cp; - m_data[2] = (cr * sp * cy) + (sy * sr); - m_data[5] = (cr * sp * sy) - (sr * cy); - m_data[8] = cr * cp; + data_[2] = (cr * sp * cy) + (sy * sr); + data_[5] = (cr * sp * sy) - (sr * cy); + data_[8] = cr * cp; } @@ -232,17 +232,17 @@ class SbgMatrix3 float xz = x * z; float yz = y * z; - m_data[0] = (2 * powf(w, 2)) + (2 * powf(x, 2)) - 1; - m_data[3] = (2 * xy) + (2 * wz); - m_data[6] = (2 * xz) - (2 * wy); + data_[0] = (2 * powf(w, 2)) + (2 * powf(x, 2)) - 1; + data_[3] = (2 * xy) + (2 * wz); + data_[6] = (2 * xz) - (2 * wy); - m_data[1] = (2 * xy) - (2 * wz); - m_data[4] = (2 * powf(w, 2)) + (2 * powf(y, 2)) - 1; - m_data[7] = (2 * yz) + (2 * wx); + data_[1] = (2 * xy) - (2 * wz); + data_[4] = (2 * powf(w, 2)) + (2 * powf(y, 2)) - 1; + data_[7] = (2 * yz) + (2 * wx); - m_data[2] = (2 * wy) + (2 * xz); - m_data[5] = (2 * yz) - (2 * wx); - m_data[8] = (2 * powf(w, 2)) + (2 * powf(z, 2)) - 1; + data_[2] = (2 * wy) + (2 * xz); + data_[5] = (2 * yz) - (2 * wx); + data_[8] = (2 * powf(w, 2)) + (2 * powf(z, 2)) - 1; } }; diff --git a/include/sbg_driver/sbg_vector3.h b/include/sbg_driver/sbg_vector3.h index 53c9276..44f7418 100644 --- a/include/sbg_driver/sbg_vector3.h +++ b/include/sbg_driver/sbg_vector3.h @@ -40,7 +40,7 @@ class SbgVector3 //- Private variables -// //---------------------------------------------------------------------// - std::array m_data; + std::array data_; public: @@ -53,9 +53,9 @@ class SbgVector3 */ SbgVector3() { - m_data[0] = static_cast(0.0); - m_data[1] = static_cast(0.0); - m_data[2] = static_cast(0.0); + data_[0] = static_cast(0.0); + data_[1] = static_cast(0.0); + data_[2] = static_cast(0.0); } /*! @@ -67,9 +67,9 @@ class SbgVector3 */ SbgVector3(T x_value, T y_value, T z_value) { - m_data[0] = x_value; - m_data[1] = y_value; - m_data[2] = z_value; + data_[0] = x_value; + data_[1] = y_value; + data_[2] = z_value; }; /*! @@ -82,9 +82,9 @@ class SbgVector3 { assert(array_size == 3); - m_data[0] = p_raw_data[0]; - m_data[1] = p_raw_data[1]; - m_data[2] = p_raw_data[2]; + data_[0] = p_raw_data[0]; + data_[1] = p_raw_data[1]; + data_[2] = p_raw_data[2]; }; //---------------------------------------------------------------------// @@ -103,9 +103,9 @@ class SbgVector3 */ bool operator==(const SbgVector3 &ref_vector) { - return ((areEquals(m_data[0], ref_vector.m_data[0])) - && (areEquals(m_data[1], ref_vector.m_data[1])) - && (areEquals(m_data[2], ref_vector.m_data[2]))); + return ((areEquals(data_[0], ref_vector.data_[0])) + && (areEquals(data_[1], ref_vector.data_[1])) + && (areEquals(data_[2], ref_vector.data_[2]))); }; /*! @@ -128,7 +128,7 @@ class SbgVector3 { assert(index < 3); - return m_data[index]; + return data_[index]; } /*! @@ -138,7 +138,7 @@ class SbgVector3 */ const T *data() const { - return static_cast(m_data.data()); + return static_cast(data_.data()); }; }; diff --git a/src/config_applier.cpp b/src/config_applier.cpp index 68100f1..64233a6 100644 --- a/src/config_applier.cpp +++ b/src/config_applier.cpp @@ -11,8 +11,8 @@ using sbg::ConfigApplier; //---------------------------------------------------------------------// ConfigApplier::ConfigApplier(SbgEComHandle &ref_sbg_com_handle): -m_reboot_needed_(false), -m_ref_sbg_com_handle(ref_sbg_com_handle) + reboot_needed_(false), + ref_sbg_com_handle_(ref_sbg_com_handle) { } @@ -56,7 +56,7 @@ void ConfigApplier::checkConfigurationApplied(const SbgErrorCode& ref_sbg_error_ else { RCLCPP_INFO(rclcpp::get_logger("Config"), "SBG_DRIVER - [Config] %s updated on the device.", ref_conf_title.c_str()); - m_reboot_needed_ = true; + reboot_needed_ = true; } } @@ -69,7 +69,7 @@ void ConfigApplier::configureInitCondition(const SbgEComInitConditionConf& ref_i SbgEComInitConditionConf init_condition; SbgErrorCode error_code; - error_code = sbgEComCmdSensorGetInitCondition(&m_ref_sbg_com_handle, &init_condition); + error_code = sbgEComCmdSensorGetInitCondition(&ref_sbg_com_handle_, &init_condition); checkConfigurationGet(error_code, std::string("Init conditions")); @@ -80,7 +80,7 @@ void ConfigApplier::configureInitCondition(const SbgEComInitConditionConf& ref_i || !areEquals(init_condition.latitude, ref_init_condition.latitude) || !areEquals(init_condition.longitude, ref_init_condition.longitude)) { - error_code = sbgEComCmdSensorSetInitCondition(&m_ref_sbg_com_handle, &ref_init_condition); + error_code = sbgEComCmdSensorSetInitCondition(&ref_sbg_com_handle_, &ref_init_condition); checkConfigurationApplied(error_code, std::string("Init conditions")); } @@ -95,13 +95,13 @@ void ConfigApplier::configureMotionProfile(const SbgEComModelInfo& ref_motion_pr SbgEComModelInfo motion_profile; SbgErrorCode error_code; - error_code = sbgEComCmdSensorGetMotionProfileInfo(&m_ref_sbg_com_handle, &motion_profile); + error_code = sbgEComCmdSensorGetMotionProfileInfo(&ref_sbg_com_handle_, &motion_profile); checkConfigurationGet(error_code, std::string("Motion profile")); if (motion_profile.id != ref_motion_profile.id) { - error_code = sbgEComCmdSensorSetMotionProfileId(&m_ref_sbg_com_handle, ref_motion_profile.id); + error_code = sbgEComCmdSensorSetMotionProfileId(&ref_sbg_com_handle_, ref_motion_profile.id); checkConfigurationApplied(error_code, std::string("Motion profile")); } @@ -117,7 +117,7 @@ void ConfigApplier::configureImuAlignement(const SbgEComSensorAlignmentInfo& ref SbgEComSensorAlignmentInfo sensor_alignement; float level_arms_device[3]; - error_code = sbgEComCmdSensorGetAlignmentAndLeverArm(&m_ref_sbg_com_handle, &sensor_alignement, level_arms_device); + error_code = sbgEComCmdSensorGetAlignmentAndLeverArm(&ref_sbg_com_handle_, &sensor_alignement, level_arms_device); checkConfigurationGet(error_code, std::string("IMU alignement")); @@ -130,7 +130,7 @@ void ConfigApplier::configureImuAlignement(const SbgEComSensorAlignmentInfo& ref || !areEquals(sensor_alignement.misPitch, ref_sensor_align.misPitch) || !areEquals(sensor_alignement.misYaw, ref_sensor_align.misYaw)) { - error_code = sbgEComCmdSensorSetAlignmentAndLeverArm(&m_ref_sbg_com_handle, &ref_sensor_align, level_arms_vector.data()); + error_code = sbgEComCmdSensorSetAlignmentAndLeverArm(&ref_sbg_com_handle_, &ref_sensor_align, level_arms_vector.data()); checkConfigurationApplied(error_code, std::string("IMU alignement")); } @@ -145,7 +145,7 @@ void ConfigApplier::configureAidingAssignement(const SbgEComAidingAssignConf& re SbgEComAidingAssignConf aiding_assign; SbgErrorCode error_code; - error_code = sbgEComCmdSensorGetAidingAssignment(&m_ref_sbg_com_handle, &aiding_assign); + error_code = sbgEComCmdSensorGetAidingAssignment(&ref_sbg_com_handle_, &aiding_assign); checkConfigurationGet(error_code, std::string("Aiding assignement")); @@ -154,7 +154,7 @@ void ConfigApplier::configureAidingAssignement(const SbgEComAidingAssignConf& re || (aiding_assign.odometerPinsConf != ref_aiding_assign.odometerPinsConf) || (aiding_assign.rtcmPort != ref_aiding_assign.rtcmPort)) { - error_code = sbgEComCmdSensorSetAidingAssignment(&m_ref_sbg_com_handle, &aiding_assign); + error_code = sbgEComCmdSensorSetAidingAssignment(&ref_sbg_com_handle_, &aiding_assign); checkConfigurationApplied(error_code, std::string("Aiding assignement")); } @@ -169,13 +169,13 @@ void ConfigApplier::configureMagModel(const SbgEComModelInfo& ref_mag_model) SbgEComModelInfo model_info; SbgErrorCode error_code; - error_code = sbgEComCmdMagGetModelInfo(&m_ref_sbg_com_handle, &model_info); + error_code = sbgEComCmdMagGetModelInfo(&ref_sbg_com_handle_, &model_info); checkConfigurationGet(error_code, std::string("Magnetometer model")); if (model_info.id != ref_mag_model.id) { - error_code = sbgEComCmdMagSetModelId(&m_ref_sbg_com_handle, ref_mag_model.id); + error_code = sbgEComCmdMagSetModelId(&ref_sbg_com_handle_, ref_mag_model.id); checkConfigurationApplied(error_code, std::string("Magnetometer model")); } @@ -190,13 +190,13 @@ void ConfigApplier::configureMagRejection(const SbgEComMagRejectionConf& ref_mag SbgEComMagRejectionConf mag_rejection; SbgErrorCode error_code; - error_code = sbgEComCmdMagGetRejection(&m_ref_sbg_com_handle, &mag_rejection); + error_code = sbgEComCmdMagGetRejection(&ref_sbg_com_handle_, &mag_rejection); checkConfigurationGet(error_code, std::string("Magnetometer rejection")); if (mag_rejection.magneticField != ref_mag_rejection.magneticField) { - error_code = sbgEComCmdMagSetRejection(&m_ref_sbg_com_handle, &ref_mag_rejection); + error_code = sbgEComCmdMagSetRejection(&ref_sbg_com_handle_, &ref_mag_rejection); checkConfigurationApplied(error_code, std::string("Magnetometer rejection")); } @@ -211,13 +211,13 @@ void ConfigApplier::configureGnssModel(const SbgEComModelInfo& ref_gnss_model) SbgEComModelInfo model_info; SbgErrorCode error_code; - error_code = sbgEComCmdGnss1GetModelInfo(&m_ref_sbg_com_handle, &model_info); + error_code = sbgEComCmdGnss1GetModelInfo(&ref_sbg_com_handle_, &model_info); checkConfigurationGet(error_code, std::string("Gnss model")); if (model_info.id != ref_gnss_model.id) { - error_code = sbgEComCmdGnss1SetModelId(&m_ref_sbg_com_handle, ref_gnss_model.id); + error_code = sbgEComCmdGnss1SetModelId(&ref_sbg_com_handle_, ref_gnss_model.id); checkConfigurationApplied(error_code, std::string("Gnss model")); } @@ -236,7 +236,7 @@ void ConfigApplier::configureGnssInstallation(const SbgEComGnssInstallation& ref SbgVector3 gnss_config_primary; SbgVector3 gnss_config_secondary; - error_code = sbgEComCmdGnss1InstallationGet(&m_ref_sbg_com_handle, &gnss_installation); + error_code = sbgEComCmdGnss1InstallationGet(&ref_sbg_com_handle_, &gnss_installation); checkConfigurationGet(error_code, std::string("Gnss level arms")); @@ -250,7 +250,7 @@ void ConfigApplier::configureGnssInstallation(const SbgEComGnssInstallation& ref || (gnss_installation.leverArmPrimaryPrecise != ref_gnss_installation.leverArmPrimaryPrecise) || (gnss_installation.leverArmSecondaryMode != ref_gnss_installation.leverArmSecondaryMode)) { - error_code = sbgEComCmdGnss1InstallationSet(&m_ref_sbg_com_handle, &ref_gnss_installation); + error_code = sbgEComCmdGnss1InstallationSet(&ref_sbg_com_handle_, &ref_gnss_installation); checkConfigurationApplied(error_code, std::string("Gnss level arms")); } @@ -265,7 +265,7 @@ void ConfigApplier::configureGnssRejection(const SbgEComGnssRejectionConf& ref_g SbgEComGnssRejectionConf rejection; SbgErrorCode error_code; - error_code = sbgEComCmdGnss1GetRejection(&m_ref_sbg_com_handle, &rejection); + error_code = sbgEComCmdGnss1GetRejection(&ref_sbg_com_handle_, &rejection); checkConfigurationGet(error_code, std::string("Gnss rejection")); @@ -273,7 +273,7 @@ void ConfigApplier::configureGnssRejection(const SbgEComGnssRejectionConf& ref_g || (rejection.position != ref_gnss_rejection.position) || (rejection.velocity != ref_gnss_rejection.velocity)) { - error_code = sbgEComCmdGnss1SetRejection(&m_ref_sbg_com_handle, &ref_gnss_rejection); + error_code = sbgEComCmdGnss1SetRejection(&ref_sbg_com_handle_, &ref_gnss_rejection); checkConfigurationApplied(error_code, std::string("Gnss rejection")); } @@ -288,7 +288,7 @@ void ConfigApplier::configureOdometer(const SbgEComOdoConf& ref_odometer) SbgEComOdoConf odom_conf; SbgErrorCode error_code; - error_code = sbgEComCmdOdoGetConf(&m_ref_sbg_com_handle, &odom_conf); + error_code = sbgEComCmdOdoGetConf(&ref_sbg_com_handle_, &odom_conf); checkConfigurationGet(error_code, std::string("Odometer")); @@ -296,7 +296,7 @@ void ConfigApplier::configureOdometer(const SbgEComOdoConf& ref_odometer) || (odom_conf.gainError != ref_odometer.gainError) || (odom_conf.reverseMode != ref_odometer.reverseMode)) { - error_code = sbgEComCmdOdoSetConf(&m_ref_sbg_com_handle, &ref_odometer); + error_code = sbgEComCmdOdoSetConf(&ref_sbg_com_handle_, &ref_odometer); checkConfigurationApplied(error_code, std::string("Odometer")); } @@ -311,7 +311,7 @@ void ConfigApplier::configureOdometerLevelArm(const SbgVector3& odometer_ float lever_arm[3]; SbgErrorCode error_code; - error_code = sbgEComCmdOdoGetLeverArm(&m_ref_sbg_com_handle, lever_arm); + error_code = sbgEComCmdOdoGetLeverArm(&ref_sbg_com_handle_, lever_arm); checkConfigurationGet(error_code, std::string("Odometer level arms")); @@ -319,7 +319,7 @@ void ConfigApplier::configureOdometerLevelArm(const SbgVector3& odometer_ if (lever_arm_device != odometer_level_arms) { - error_code = sbgEComCmdOdoSetLeverArm(&m_ref_sbg_com_handle, lever_arm_device.data()); + error_code = sbgEComCmdOdoSetLeverArm(&ref_sbg_com_handle_, lever_arm_device.data()); checkConfigurationApplied(error_code, std::string("Odometer level arms")); } @@ -334,13 +334,13 @@ void ConfigApplier::configureOdometerRejection(const SbgEComOdoRejectionConf& re SbgEComOdoRejectionConf odom_rejection; SbgErrorCode error_code; - error_code = sbgEComCmdOdoGetRejection(&m_ref_sbg_com_handle, &odom_rejection); + error_code = sbgEComCmdOdoGetRejection(&ref_sbg_com_handle_, &odom_rejection); checkConfigurationGet(error_code, std::string("Odometer rejection")); if (odom_rejection.velocity != ref_odometer_rejection.velocity) { - error_code = sbgEComCmdOdoSetRejection(&m_ref_sbg_com_handle, &ref_odometer_rejection); + error_code = sbgEComCmdOdoSetRejection(&ref_sbg_com_handle_, &ref_odometer_rejection); checkConfigurationApplied(error_code, std::string("Odometer rejection")); } @@ -355,7 +355,7 @@ void ConfigApplier::configureOutput(SbgEComOutputPort output_port, const ConfigS // Get the current output mode for the device and the selected log ID. // If output modes are different, udpate the device mode with the one loaded from the parameters. // - error_code = sbgEComCmdOutputGetConf(&m_ref_sbg_com_handle, output_port, ref_log_output.message_class, ref_log_output.message_id, ¤t_output_mode); + error_code = sbgEComCmdOutputGetConf(&ref_sbg_com_handle_, output_port, ref_log_output.message_class, ref_log_output.message_id, ¤t_output_mode); if (error_code == SBG_INVALID_PARAMETER) { @@ -374,7 +374,7 @@ void ConfigApplier::configureOutput(SbgEComOutputPort output_port, const ConfigS } else if (current_output_mode != ref_log_output.output_mode) { - error_code = sbgEComCmdOutputSetConf(&m_ref_sbg_com_handle, output_port, ref_log_output.message_class, ref_log_output.message_id, ref_log_output.output_mode); + error_code = sbgEComCmdOutputSetConf(&ref_sbg_com_handle_, output_port, ref_log_output.message_class, ref_log_output.message_id, ref_log_output.output_mode); if (error_code != SBG_NO_ERROR) { @@ -389,7 +389,7 @@ void ConfigApplier::configureOutput(SbgEComOutputPort output_port, const ConfigS } else { - m_reboot_needed_ = true; + reboot_needed_ = true; } } } @@ -433,7 +433,7 @@ void ConfigApplier::applyConfiguration(const ConfigStore& ref_config_store) // // Save configuration if needed. // - if (m_reboot_needed_) + if (reboot_needed_) { saveConfiguration(); } @@ -443,7 +443,7 @@ void ConfigApplier::saveConfiguration() { SbgErrorCode error_code; - error_code = sbgEComCmdSettingsAction(&m_ref_sbg_com_handle, SBG_ECOM_SAVE_SETTINGS); + error_code = sbgEComCmdSettingsAction(&ref_sbg_com_handle_, SBG_ECOM_SAVE_SETTINGS); if (error_code != SBG_NO_ERROR) { diff --git a/src/config_store.cpp b/src/config_store.cpp index f068688..9b14186 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -11,12 +11,12 @@ using sbg::ConfigStore; //---------------------------------------------------------------------// ConfigStore::ConfigStore(): -m_serial_communication_(false), -m_upd_communication_(false), -m_configure_through_ros_(false), -m_ros_standard_output_(false), -rtcm_subscribe_(false), -nmea_publish_(false) + serial_communication_(false), + upd_communication_(false), + configure_through_ros_(false), + ros_standard_output_(false), + rtcm_subscribe_(false), + nmea_publish_(false) { } @@ -27,39 +27,39 @@ nmea_publish_(false) void ConfigStore::loadDriverParameters(const rclcpp::Node& ref_node_handle) { - m_rate_frequency_ = getParameter(ref_node_handle, "driver.frequency", 400); + rate_frequency_ = getParameter(ref_node_handle, "driver.frequency", 400); } void ConfigStore::loadOdomParameters(const rclcpp::Node& ref_node_handle) { - ref_node_handle.get_parameter_or ("odometry.enable" , m_odom_enable_ , false); - ref_node_handle.get_parameter_or ("odometry.publishTf", m_odom_publish_tf_ , false); - ref_node_handle.get_parameter_or("odometry.odomFrameId", m_odom_frame_id_ , "odom"); - ref_node_handle.get_parameter_or("odometry.baseFrameId", m_odom_base_frame_id_ , "base_link"); - ref_node_handle.get_parameter_or("odometry.initFrameId", m_odom_init_frame_id_ , "map"); + ref_node_handle.get_parameter_or ("odometry.enable" , odom_enable_ , false); + ref_node_handle.get_parameter_or ("odometry.publishTf", odom_publish_tf_ , false); + ref_node_handle.get_parameter_or("odometry.odomFrameId", odom_frame_id_ , "odom"); + ref_node_handle.get_parameter_or("odometry.baseFrameId", odom_base_frame_id_ , "base_link"); + ref_node_handle.get_parameter_or("odometry.initFrameId", odom_init_frame_id_ , "map"); } void ConfigStore::loadCommunicationParameters(const rclcpp::Node& ref_node_handle) { - ref_node_handle.get_parameter_or("confWithRos", m_configure_through_ros_, false); + ref_node_handle.get_parameter_or("confWithRos", configure_through_ros_, false); if (ref_node_handle.has_parameter("uartConf.portName")) { - m_serial_communication_ = true; - ref_node_handle.get_parameter_or("uartConf.portName", m_uart_port_name_, "/dev/ttyUSB0"); + serial_communication_ = true; + ref_node_handle.get_parameter_or("uartConf.portName", uart_port_name_, "/dev/ttyUSB0"); - m_uart_baud_rate_ = getParameter(ref_node_handle, "uartConf.baudRate", 0); - m_output_port_ = getParameter(ref_node_handle, "uartConf.portID", SBG_ECOM_OUTPUT_PORT_A); + uart_baud_rate_ = getParameter(ref_node_handle, "uartConf.baudRate", 0); + output_port_ = getParameter(ref_node_handle, "uartConf.portID", SBG_ECOM_OUTPUT_PORT_A); } else if (ref_node_handle.has_parameter("ipConf.ipAddress")) { std::string ip_address; ref_node_handle.get_parameter_or("ipConf.ipAddress", ip_address, "0.0.0.0"); - m_upd_communication_ = true; - m_sbg_ip_address_ = sbgNetworkIpFromString(ip_address.c_str()); - m_out_port_address_ = getParameter(ref_node_handle, "ipConf.out_port", 0); - m_in_port_address_ = getParameter(ref_node_handle, "ipConf.in_port", 0); + upd_communication_ = true; + sbg_ip_address_ = sbgNetworkIpFromString(ip_address.c_str()); + out_port_address_ = getParameter(ref_node_handle, "ipConf.out_port", 0); + in_port_address_ = getParameter(ref_node_handle, "ipConf.in_port", 0); } else { @@ -69,83 +69,83 @@ void ConfigStore::loadCommunicationParameters(const rclcpp::Node& ref_node_handl void ConfigStore::loadSensorParameters(const rclcpp::Node& ref_node_handle) { - ref_node_handle.get_parameter_or("sensorParameters.initLat", m_init_condition_conf_.latitude, 48.419727); - ref_node_handle.get_parameter_or("sensorParameters.initLong", m_init_condition_conf_.longitude, -4.472119); - ref_node_handle.get_parameter_or("sensorParameters.initAlt", m_init_condition_conf_.altitude, 100); + ref_node_handle.get_parameter_or("sensorParameters.initLat", init_condition_conf_.latitude, 48.419727); + ref_node_handle.get_parameter_or("sensorParameters.initLong", init_condition_conf_.longitude, -4.472119); + ref_node_handle.get_parameter_or("sensorParameters.initAlt", init_condition_conf_.altitude, 100); - m_init_condition_conf_.year = getParameter(ref_node_handle, "sensorParameters.year", 2018); - m_init_condition_conf_.month = getParameter(ref_node_handle, "sensorParameters.year", 03); - m_init_condition_conf_.day = getParameter(ref_node_handle, "sensorParameters.year", 10); + init_condition_conf_.year = getParameter(ref_node_handle, "sensorParameters.year", 2018); + init_condition_conf_.month = getParameter(ref_node_handle, "sensorParameters.year", 03); + init_condition_conf_.day = getParameter(ref_node_handle, "sensorParameters.year", 10); - m_motion_profile_model_info_.id = getParameter(ref_node_handle, "sensorParameters.motionProfile", SBG_ECOM_MOTION_PROFILE_GENERAL_PURPOSE); + motion_profile_model_info_.id = getParameter(ref_node_handle, "sensorParameters.motionProfile", SBG_ECOM_MOTION_PROFILE_GENERAL_PURPOSE); } void ConfigStore::loadImuAlignementParameters(const rclcpp::Node& ref_node_handle) { - m_sensor_alignement_info_.axisDirectionX = getParameter(ref_node_handle, "imuAlignementLeverArm.axisDirectionX", SBG_ECOM_ALIGNMENT_FORWARD); - m_sensor_alignement_info_.axisDirectionY = getParameter(ref_node_handle, "imuAlignementLeverArm.axisDirectionY", SBG_ECOM_ALIGNMENT_FORWARD); + sensor_alignement_info_.axisDirectionX = getParameter(ref_node_handle, "imuAlignementLeverArm.axisDirectionX", SBG_ECOM_ALIGNMENT_FORWARD); + sensor_alignement_info_.axisDirectionY = getParameter(ref_node_handle, "imuAlignementLeverArm.axisDirectionY", SBG_ECOM_ALIGNMENT_FORWARD); - ref_node_handle.get_parameter_or("imuAlignementLeverArm.misRoll", m_sensor_alignement_info_.misRoll, 0.0f); - ref_node_handle.get_parameter_or("imuAlignementLeverArm.misPitch", m_sensor_alignement_info_.misPitch, 0.0f); - ref_node_handle.get_parameter_or("imuAlignementLeverArm.misYaw", m_sensor_alignement_info_.misYaw, 0.0f); + ref_node_handle.get_parameter_or("imuAlignementLeverArm.misRoll", sensor_alignement_info_.misRoll, 0.0f); + ref_node_handle.get_parameter_or("imuAlignementLeverArm.misPitch", sensor_alignement_info_.misPitch, 0.0f); + ref_node_handle.get_parameter_or("imuAlignementLeverArm.misYaw", sensor_alignement_info_.misYaw, 0.0f); float sensor_level_arm[3]; ref_node_handle.get_parameter_or("imuAlignementLeverArm.leverArmX", sensor_level_arm[0], 0.0f); ref_node_handle.get_parameter_or("imuAlignementLeverArm.leverArmY", sensor_level_arm[1], 0.0f); ref_node_handle.get_parameter_or("imuAlignementLeverArm.leverArmZ", sensor_level_arm[2], 0.0f); - m_sensor_lever_arm_ = SbgVector3(sensor_level_arm, 3); + sensor_lever_arm_ = SbgVector3(sensor_level_arm, 3); } void ConfigStore::loadAidingAssignementParameters(const rclcpp::Node& ref_node_handle) { - m_aiding_assignement_conf_.gps1Port = getParameter(ref_node_handle, "aidingAssignment.gnss1ModulePortAssignment", SBG_ECOM_MODULE_PORT_B); - m_aiding_assignement_conf_.gps1Sync = getParameter(ref_node_handle, "aidingAssignment.gnss1ModuleSyncAssignment", SBG_ECOM_MODULE_SYNC_DISABLED); - m_aiding_assignement_conf_.rtcmPort = getParameter(ref_node_handle, "aidingAssignment.rtcmPortAssignment", SBG_ECOM_MODULE_DISABLED); - m_aiding_assignement_conf_.odometerPinsConf = getParameter(ref_node_handle, "aidingAssignment.odometerPinAssignment", SBG_ECOM_MODULE_ODO_DISABLED); + aiding_assignement_conf_.gps1Port = getParameter(ref_node_handle, "aidingAssignment.gnss1ModulePortAssignment", SBG_ECOM_MODULE_PORT_B); + aiding_assignement_conf_.gps1Sync = getParameter(ref_node_handle, "aidingAssignment.gnss1ModuleSyncAssignment", SBG_ECOM_MODULE_SYNC_DISABLED); + aiding_assignement_conf_.rtcmPort = getParameter(ref_node_handle, "aidingAssignment.rtcmPortAssignment", SBG_ECOM_MODULE_DISABLED); + aiding_assignement_conf_.odometerPinsConf = getParameter(ref_node_handle, "aidingAssignment.odometerPinAssignment", SBG_ECOM_MODULE_ODO_DISABLED); } void ConfigStore::loadMagnetometersParameters(const rclcpp::Node& ref_node_handle) { - m_mag_model_info_.id = getParameter(ref_node_handle, "magnetometer.magnetometerModel", SBG_ECOM_MAG_MODEL_NORMAL); - m_mag_rejection_conf_.magneticField = getParameter(ref_node_handle, "magnetometer.magnetometerRejectMode", SBG_ECOM_AUTOMATIC_MODE); + mag_model_info_.id = getParameter(ref_node_handle, "magnetometer.magnetometerModel", SBG_ECOM_MAG_MODEL_NORMAL); + mag_rejection_conf_.magneticField = getParameter(ref_node_handle, "magnetometer.magnetometerRejectMode", SBG_ECOM_AUTOMATIC_MODE); - m_mag_calib_mode_ = getParameter(ref_node_handle, "magnetometer.calibration.mode", SBG_ECOM_MAG_CALIB_MODE_2D); - m_mag_calib_bandwidth_ = getParameter(ref_node_handle, "magnetometer.calibration.bandwidth", SBG_ECOM_MAG_CALIB_HIGH_BW); + mag_calib_mode_ = getParameter(ref_node_handle, "magnetometer.calibration.mode", SBG_ECOM_MAG_CALIB_MODE_2D); + mag_calib_bandwidth_ = getParameter(ref_node_handle, "magnetometer.calibration.bandwidth", SBG_ECOM_MAG_CALIB_HIGH_BW); } void ConfigStore::loadGnssParameters(const rclcpp::Node& ref_node_handle) { - m_gnss_model_info_.id = getParameter(ref_node_handle, "gnss.gnss_model_id", SBG_ECOM_GNSS_MODEL_NMEA); + gnss_model_info_.id = getParameter(ref_node_handle, "gnss.gnss_model_id", SBG_ECOM_GNSS_MODEL_NMEA); - ref_node_handle.get_parameter_or("gnss.primaryLeverArmX", m_gnss_installation_.leverArmPrimary[0], 0.0f); - ref_node_handle.get_parameter_or("gnss.primaryLeverArmY", m_gnss_installation_.leverArmPrimary[1], 0.0f); - ref_node_handle.get_parameter_or("gnss.primaryLeverArmZ", m_gnss_installation_.leverArmPrimary[2], 0.0f); - ref_node_handle.get_parameter_or("gnss.primaryLeverPrecise", m_gnss_installation_.leverArmPrimaryPrecise, true); - ref_node_handle.get_parameter_or("gnss.secondaryLeverArmX", m_gnss_installation_.leverArmSecondary[0], 0.0f); - ref_node_handle.get_parameter_or("gnss.secondaryLeverArmY", m_gnss_installation_.leverArmSecondary[1], 0.0f); - ref_node_handle.get_parameter_or("gnss.secondaryLeverArmZ", m_gnss_installation_.leverArmSecondary[2], 0.0f); - m_gnss_installation_.leverArmSecondaryMode = getParameter(ref_node_handle, "gnss.secondaryLeverMode", SBG_ECOM_GNSS_INSTALLATION_MODE_SINGLE); + ref_node_handle.get_parameter_or("gnss.primaryLeverArmX", gnss_installation_.leverArmPrimary[0], 0.0f); + ref_node_handle.get_parameter_or("gnss.primaryLeverArmY", gnss_installation_.leverArmPrimary[1], 0.0f); + ref_node_handle.get_parameter_or("gnss.primaryLeverArmZ", gnss_installation_.leverArmPrimary[2], 0.0f); + ref_node_handle.get_parameter_or("gnss.primaryLeverPrecise", gnss_installation_.leverArmPrimaryPrecise, true); + ref_node_handle.get_parameter_or("gnss.secondaryLeverArmX", gnss_installation_.leverArmSecondary[0], 0.0f); + ref_node_handle.get_parameter_or("gnss.secondaryLeverArmY", gnss_installation_.leverArmSecondary[1], 0.0f); + ref_node_handle.get_parameter_or("gnss.secondaryLeverArmZ", gnss_installation_.leverArmSecondary[2], 0.0f); + gnss_installation_.leverArmSecondaryMode = getParameter(ref_node_handle, "gnss.secondaryLeverMode", SBG_ECOM_GNSS_INSTALLATION_MODE_SINGLE); - m_gnss_rejection_conf_.position = getParameter(ref_node_handle, "gnss.posRejectMode", SBG_ECOM_AUTOMATIC_MODE); - m_gnss_rejection_conf_.velocity = getParameter(ref_node_handle, "gnss.velRejectMode", SBG_ECOM_AUTOMATIC_MODE); - m_gnss_rejection_conf_.hdt = getParameter(ref_node_handle, "gnss.hdtRejectMode", SBG_ECOM_AUTOMATIC_MODE); + gnss_rejection_conf_.position = getParameter(ref_node_handle, "gnss.posRejectMode", SBG_ECOM_AUTOMATIC_MODE); + gnss_rejection_conf_.velocity = getParameter(ref_node_handle, "gnss.velRejectMode", SBG_ECOM_AUTOMATIC_MODE); + gnss_rejection_conf_.hdt = getParameter(ref_node_handle, "gnss.hdtRejectMode", SBG_ECOM_AUTOMATIC_MODE); } void ConfigStore::loadOdometerParameters(const rclcpp::Node& ref_node_handle) { - ref_node_handle.get_parameter_or("odom.gain", m_odometer_conf_.gain, 4800.0f); - ref_node_handle.get_parameter_or("odom.direction", m_odometer_conf_.reverseMode, false); + ref_node_handle.get_parameter_or("odom.gain", odometer_conf_.gain, 4800.0f); + ref_node_handle.get_parameter_or("odom.direction", odometer_conf_.reverseMode, false); - float odometer_level_arm_[3]; - ref_node_handle.get_parameter_or("odom.leverArmX", odometer_level_arm_[0], 0.0f); - ref_node_handle.get_parameter_or("odom.leverArmY", odometer_level_arm_[1], 0.0f); - ref_node_handle.get_parameter_or("odom.leverArmZ", odometer_level_arm_[2], 0.0f); + float array_odometer_level_arm[3]; + ref_node_handle.get_parameter_or("odom.leverArmX", array_odometer_level_arm[0], 0.0f); + ref_node_handle.get_parameter_or("odom.leverArmY", array_odometer_level_arm[1], 0.0f); + ref_node_handle.get_parameter_or("odom.leverArmZ", array_odometer_level_arm[2], 0.0f); - m_odometer_level_arm_ = SbgVector3(odometer_level_arm_, 3); + odometer_level_arm_ = SbgVector3(array_odometer_level_arm, 3); - m_odometer_conf_.gainError = getParameter(ref_node_handle, "odom.gain_error", 0.1); - m_odometer_rejection_conf_.velocity = getParameter(ref_node_handle, "odom.rejectMode", SBG_ECOM_AUTOMATIC_MODE); + odometer_conf_.gainError = getParameter(ref_node_handle, "odom.gain_error", 0.1); + odometer_rejection_conf_.velocity = getParameter(ref_node_handle, "odom.rejectMode", SBG_ECOM_AUTOMATIC_MODE); } void ConfigStore::loadOutputConfiguration(const rclcpp::Node& ref_node_handle, const std::string& ref_key, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id) @@ -156,20 +156,20 @@ void ConfigStore::loadOutputConfiguration(const rclcpp::Node& ref_node_handle, c log_output.message_id = sbg_msg_id; log_output.output_mode = getParameter(ref_node_handle, ref_key, SBG_ECOM_OUTPUT_MODE_DISABLED); - m_output_modes_.push_back(log_output); + output_modes_.push_back(log_output); } void ConfigStore::loadOutputFrameParameters(const rclcpp::Node& ref_node_handle) { - ref_node_handle.get_parameter_or("output.use_enu", m_use_enu_, false); + ref_node_handle.get_parameter_or("output.use_enu", use_enu_, false); - if (m_use_enu_) + if (use_enu_) { - ref_node_handle.get_parameter_or("output.frame_id", m_frame_id_, "imu_link"); + ref_node_handle.get_parameter_or("output.frame_id", frame_id_, "imu_link"); } else { - ref_node_handle.get_parameter_or("output.frame_id", m_frame_id_, "imu_link_ned"); + ref_node_handle.get_parameter_or("output.frame_id", frame_id_, "imu_link_ned"); } } @@ -181,11 +181,11 @@ void ConfigStore::loadOutputTimeReference(const rclcpp::Node& ref_node_handle, c if (time_reference == "ros") { - m_time_reference_ = TimeReference::ROS; + time_reference_ = TimeReference::ROS; } else if (time_reference == "ins_unix") { - m_time_reference_ = TimeReference::INS_UNIX; + time_reference_ = TimeReference::INS_UNIX; } else { @@ -223,177 +223,177 @@ void ConfigStore::loadNmeaParameters(const rclcpp::Node &ref_node_handle) bool ConfigStore::checkConfigWithRos() const { - return m_configure_through_ros_; + return configure_through_ros_; } bool ConfigStore::isInterfaceSerial() const { - return m_serial_communication_; + return serial_communication_; } const std::string &ConfigStore::getUartPortName() const { - return m_uart_port_name_; + return uart_port_name_; } uint32_t ConfigStore::getBaudRate() const { - return m_uart_baud_rate_; + return uart_baud_rate_; } SbgEComOutputPort ConfigStore::getOutputPort() const { - return m_output_port_; + return output_port_; } bool ConfigStore::isInterfaceUdp() const { - return m_upd_communication_; + return upd_communication_; } sbgIpAddress ConfigStore::getIpAddress() const { - return m_sbg_ip_address_; + return sbg_ip_address_; } uint32_t ConfigStore::getOutputPortAddress() const { - return m_out_port_address_; + return out_port_address_; } uint32_t ConfigStore::getInputPortAddress() const { - return m_in_port_address_; + return in_port_address_; } const SbgEComInitConditionConf &ConfigStore::getInitialConditions() const { - return m_init_condition_conf_; + return init_condition_conf_; } const SbgEComModelInfo &ConfigStore::getMotionProfile() const { - return m_motion_profile_model_info_; + return motion_profile_model_info_; } const SbgEComSensorAlignmentInfo &ConfigStore::getSensorAlignement() const { - return m_sensor_alignement_info_; + return sensor_alignement_info_; } const sbg::SbgVector3 &ConfigStore::getSensorLevelArms() const { - return m_sensor_lever_arm_; + return sensor_lever_arm_; } const SbgEComAidingAssignConf &ConfigStore::getAidingAssignement() const { - return m_aiding_assignement_conf_; + return aiding_assignement_conf_; } const SbgEComModelInfo &ConfigStore::getMagnetometerModel() const { - return m_mag_model_info_; + return mag_model_info_; } const SbgEComMagRejectionConf &ConfigStore::getMagnetometerRejection() const { - return m_mag_rejection_conf_; + return mag_rejection_conf_; } const SbgEComMagCalibMode &ConfigStore::getMagnetometerCalibMode() const { - return m_mag_calib_mode_; + return mag_calib_mode_; } const SbgEComMagCalibBandwidth &ConfigStore::getMagnetometerCalibBandwidth() const { - return m_mag_calib_bandwidth_; + return mag_calib_bandwidth_; } const SbgEComModelInfo &ConfigStore::getGnssModel() const { - return m_gnss_model_info_; + return gnss_model_info_; } const SbgEComGnssInstallation &ConfigStore::getGnssInstallation() const { - return m_gnss_installation_; + return gnss_installation_; } const SbgEComGnssRejectionConf &ConfigStore::getGnssRejection() const { - return m_gnss_rejection_conf_; + return gnss_rejection_conf_; } const SbgEComOdoConf &ConfigStore::getOdometerConf() const { - return m_odometer_conf_; + return odometer_conf_; } const sbg::SbgVector3 &ConfigStore::getOdometerLevelArms() const { - return m_odometer_level_arm_; + return odometer_level_arm_; } const SbgEComOdoRejectionConf &ConfigStore::getOdometerRejection() const { - return m_odometer_rejection_conf_; + return odometer_rejection_conf_; } const std::vector &ConfigStore::getOutputModes() const { - return m_output_modes_; + return output_modes_; } bool ConfigStore::checkRosStandardMessages() const { - return m_ros_standard_output_; + return ros_standard_output_; } uint32_t ConfigStore::getReadingRateFrequency() const { - return m_rate_frequency_; + return rate_frequency_; } const std::string &ConfigStore::getFrameId() const { - return m_frame_id_; + return frame_id_; } bool ConfigStore::getUseEnu() const { - return m_use_enu_; + return use_enu_; } sbg::TimeReference ConfigStore::getTimeReference() const { - return m_time_reference_; + return time_reference_; } bool ConfigStore::getOdomEnable() const { - return m_odom_enable_; + return odom_enable_; } bool ConfigStore::getOdomPublishTf() const { - return m_odom_publish_tf_; + return odom_publish_tf_; } const std::string &ConfigStore::getOdomFrameId() const { - return m_odom_frame_id_; + return odom_frame_id_; } const std::string &ConfigStore::getOdomBaseFrameId() const { - return m_odom_base_frame_id_; + return odom_base_frame_id_; } const std::string &ConfigStore::getOdomInitFrameId() const { - return m_odom_init_frame_id_; + return odom_init_frame_id_; } bool ConfigStore::shouldSubscribeToRtcm() const @@ -459,5 +459,5 @@ void ConfigStore::loadFromRosNodeHandle(const rclcpp::Node& ref_node_handle) loadOutputConfiguration(ref_node_handle, "output.log_air_data", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA); loadOutputConfiguration(ref_node_handle, "output.log_imu_short", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_SHORT); - ref_node_handle.get_parameter_or("output.ros_standard", m_ros_standard_output_, false); + ref_node_handle.get_parameter_or("output.ros_standard", ros_standard_output_, false); } diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 2ab7c09..f6fed1f 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -10,7 +10,7 @@ using sbg::MessagePublisher; //---------------------------------------------------------------------// MessagePublisher::MessagePublisher(): -m_max_messages_(10) +max_messages_(10) { } @@ -100,105 +100,105 @@ void MessagePublisher::initPublisher(rclcpp::Node& ref_ros_node_handle, SbgEComM switch (sbg_msg_id) { case SBG_ECOM_LOG_STATUS: - m_sbgStatus_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_status_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_UTC_TIME: - m_sbgUtcTime_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_utc_time_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_IMU_DATA: - m_sbgImuData_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_imu_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_MAG: - m_sbgMag_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_mag_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_MAG_CALIB: - m_sbgMagCalib_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_mag_calib_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EKF_EULER: - m_sbgEkfEuler_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_ekf_euler_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EKF_QUAT: - m_sbgEkfQuat_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_ekf_quat_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EKF_NAV: - m_sbgEkfNav_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_ekf_nav_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_SHIP_MOTION: - m_sbgShipMotion_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_ship_motion_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_VEL: case SBG_ECOM_LOG_GPS2_VEL: - m_sbgGpsVel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_gps_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_POS: case SBG_ECOM_LOG_GPS2_POS: - m_sbgGpsPos_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_gps_pos_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_HDT: case SBG_ECOM_LOG_GPS2_HDT: - m_sbgGpsHdt_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_gps_hdt_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_RAW: case SBG_ECOM_LOG_GPS2_RAW: - m_sbgGpsRaw_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_gps_raw_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_ODO_VEL: - m_sbgOdoVel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_odo_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_A: - m_sbgEventA_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_event_a_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_B: - m_sbgEventB_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_event_b_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_C: - m_sbgEventC_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_event_c_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_D: - m_sbgEventD_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_event_d_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_E: - m_sbgEventE_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_event_e_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_IMU_SHORT: - m_SbgImuShort_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_imu_short_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_AIR_DATA: - m_SbgAirData_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, m_max_messages_); + sbg_air_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; default: @@ -215,27 +215,27 @@ void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_ha return; } - if (m_sbgImuData_pub_ && m_sbgEkfQuat_pub_) + if (sbg_imu_data_pub_ && sbg_ekf_quat_pub_) { - m_imu_pub_ = ref_ros_node_handle.create_publisher("imu/data", m_max_messages_); + imu_pub_ = ref_ros_node_handle.create_publisher("imu/data", max_messages_); } else { RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] SBG Imu and/or Quat output are not configured, the standard IMU can not be defined."); } - if (m_sbgImuData_pub_) + if (sbg_imu_data_pub_) { - m_temp_pub_ = ref_ros_node_handle.create_publisher("imu/temp", m_max_messages_); + temp_pub_ = ref_ros_node_handle.create_publisher("imu/temp", max_messages_); } else { RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] SBG Imu data output are not configured, the standard Temperature publisher can not be defined."); } - if (m_sbgMag_pub_) + if (sbg_mag_pub_) { - m_mag_pub_ = ref_ros_node_handle.create_publisher("imu/mag", m_max_messages_); + mag_pub_ = ref_ros_node_handle.create_publisher("imu/mag", max_messages_); } else { @@ -246,45 +246,45 @@ void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_ha // We need either Euler or quat angles, and we must have Nav and IMU data to // compute Body and angular velocity. // - if ((m_sbgEkfEuler_pub_ || m_sbgEkfQuat_pub_) && m_sbgEkfNav_pub_ && m_sbgImuData_pub_) + if ((sbg_ekf_euler_pub_ || sbg_ekf_quat_pub_) && sbg_ekf_nav_pub_ && sbg_imu_data_pub_) { - m_velocity_pub_ = ref_ros_node_handle.create_publisher("imu/velocity", m_max_messages_); + velocity_pub_ = ref_ros_node_handle.create_publisher("imu/velocity", max_messages_); } else { RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] SBG Imu, Nav or Angles data outputs are not configured, the standard Velocity publisher can not be defined."); } - if (m_SbgAirData_pub_) + if (sbg_air_data_pub_) { - m_fluid_pub_ = ref_ros_node_handle.create_publisher("imu/pres", m_max_messages_); + fluid_pub_ = ref_ros_node_handle.create_publisher("imu/pres", max_messages_); } else { RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] SBG AirData output are not configured, the standard FluidPressure publisher can not be defined."); } - if (m_sbgEkfNav_pub_) + if (sbg_ekf_nav_pub_) { - m_pos_ecef_pub_ = ref_ros_node_handle.create_publisher("imu/pos_ecef", m_max_messages_); + pos_ecef_pub_ = ref_ros_node_handle.create_publisher("imu/pos_ecef", max_messages_); } else { RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] SBG Ekf data output are not configured, the standard ECEF position publisher can not be defined."); } - if (m_sbgUtcTime_pub_) + if (sbg_utc_time_pub_) { - m_utc_reference_pub_ = ref_ros_node_handle.create_publisher("imu/utc_ref", m_max_messages_); + utc_reference_pub_ = ref_ros_node_handle.create_publisher("imu/utc_ref", max_messages_); } else { RCLCPP_WARN(ref_ros_node_handle.get_logger(), "SBG_DRIVER - [Publisher] SBG Utc data output are not configured, the UTC time reference publisher can not be defined."); } - if (m_sbgGpsPos_pub_) + if (sbg_gps_pos_pub_) { - m_nav_sat_fix_pub_ = ref_ros_node_handle.create_publisher("imu/nav_sat_fix", m_max_messages_); + nav_sat_fix_pub_ = ref_ros_node_handle.create_publisher("imu/nav_sat_fix", max_messages_); } else { @@ -293,9 +293,9 @@ void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_ha if (odom_enable) { - if (m_sbgImuData_pub_ && m_sbgEkfNav_pub_ && (m_sbgEkfEuler_pub_ || m_sbgEkfQuat_pub_)) + if (sbg_imu_data_pub_ && sbg_ekf_nav_pub_ && (sbg_ekf_euler_pub_ || sbg_ekf_quat_pub_)) { - m_odometry_pub_ = ref_ros_node_handle.create_publisher("imu/odometry", m_max_messages_); + odometry_pub_ = ref_ros_node_handle.create_publisher("imu/odometry", max_messages_); } else { @@ -306,14 +306,14 @@ void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_ha void MessagePublisher::publishIMUData(const SbgBinaryLogData &ref_sbg_log) { - if (m_sbgImuData_pub_) + if (sbg_imu_data_pub_) { - m_sbg_imu_message_ = m_message_wrapper_.createSbgImuDataMessage(ref_sbg_log.imuData); - m_sbgImuData_pub_->publish(m_sbg_imu_message_); + sbg_imu_message_ = message_wrapper_.createSbgImuDataMessage(ref_sbg_log.imuData); + sbg_imu_data_pub_->publish(sbg_imu_message_); } - if (m_temp_pub_) + if (temp_pub_) { - m_temp_pub_->publish(m_message_wrapper_.createRosTemperatureMessage(m_sbg_imu_message_)); + temp_pub_->publish(message_wrapper_.createRosTemperatureMessage(sbg_imu_message_)); } processRosImuMessage(); @@ -323,54 +323,54 @@ void MessagePublisher::publishIMUData(const SbgBinaryLogData &ref_sbg_log) void MessagePublisher::processRosVelMessage() { - if (m_velocity_pub_) + if (velocity_pub_) { - if (m_sbgEkfQuat_pub_) + if (sbg_ekf_quat_pub_) { - m_velocity_pub_->publish(m_message_wrapper_.createRosTwistStampedMessage(m_sbg_ekf_quat_message_, m_sbg_ekf_nav_message_, m_sbg_imu_message_)); + velocity_pub_->publish(message_wrapper_.createRosTwistStampedMessage(sbg_ekf_quat_message_, sbg_ekf_nav_message_, sbg_imu_message_)); } - else if (m_sbgEkfEuler_pub_) + else if (sbg_ekf_euler_pub_) { - m_velocity_pub_->publish(m_message_wrapper_.createRosTwistStampedMessage(m_sbg_ekf_euler_message_, m_sbg_ekf_nav_message_, m_sbg_imu_message_)); + velocity_pub_->publish(message_wrapper_.createRosTwistStampedMessage(sbg_ekf_euler_message_, sbg_ekf_nav_message_, sbg_imu_message_)); } } } void MessagePublisher::processRosImuMessage() { - if (m_imu_pub_) + if (imu_pub_) { - if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_quat_message_.time_stamp) + if (sbg_imu_message_.time_stamp == sbg_ekf_quat_message_.time_stamp) { - m_imu_pub_->publish(m_message_wrapper_.createRosImuMessage(m_sbg_imu_message_, m_sbg_ekf_quat_message_)); + imu_pub_->publish(message_wrapper_.createRosImuMessage(sbg_imu_message_, sbg_ekf_quat_message_)); } } } void MessagePublisher::processRosOdoMessage() { - if (m_odometry_pub_) + if (odometry_pub_) { - if (m_sbg_ekf_nav_message_.status.position_valid) + if (sbg_ekf_nav_message_.status.position_valid) { - if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_nav_message_.time_stamp) + if (sbg_imu_message_.time_stamp == sbg_ekf_nav_message_.time_stamp) { /* * Odometry message can be generated from quaternion or euler angles. * Quaternion is prefered if they are available. */ - if (m_sbgEkfQuat_pub_) + if (sbg_ekf_quat_pub_) { - if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_quat_message_.time_stamp) + if (sbg_imu_message_.time_stamp == sbg_ekf_quat_message_.time_stamp) { - m_odometry_pub_->publish(m_message_wrapper_.createRosOdoMessage(m_sbg_imu_message_, m_sbg_ekf_nav_message_, m_sbg_ekf_quat_message_, m_sbg_ekf_euler_message_)); + odometry_pub_->publish(message_wrapper_.createRosOdoMessage(sbg_imu_message_, sbg_ekf_nav_message_, sbg_ekf_quat_message_, sbg_ekf_euler_message_)); } } else { - if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_euler_message_.time_stamp) + if (sbg_imu_message_.time_stamp == sbg_ekf_euler_message_.time_stamp) { - m_odometry_pub_->publish(m_message_wrapper_.createRosOdoMessage(m_sbg_imu_message_, m_sbg_ekf_nav_message_, m_sbg_ekf_euler_message_)); + odometry_pub_->publish(message_wrapper_.createRosOdoMessage(sbg_imu_message_, sbg_ekf_nav_message_, sbg_ekf_euler_message_)); } } } @@ -381,44 +381,44 @@ void MessagePublisher::processRosOdoMessage() void MessagePublisher::publishMagData(const SbgBinaryLogData &ref_sbg_log) { sbg_driver::msg::SbgMag sbg_mag_message; - sbg_mag_message = m_message_wrapper_.createSbgMagMessage(ref_sbg_log.magData); + sbg_mag_message = message_wrapper_.createSbgMagMessage(ref_sbg_log.magData); - if (m_sbgMag_pub_) + if (sbg_mag_pub_) { - m_sbgMag_pub_->publish(sbg_mag_message); + sbg_mag_pub_->publish(sbg_mag_message); } - if (m_mag_pub_) + if (mag_pub_) { - m_mag_pub_->publish(m_message_wrapper_.createRosMagneticMessage(sbg_mag_message)); + mag_pub_->publish(message_wrapper_.createRosMagneticMessage(sbg_mag_message)); } } void MessagePublisher::publishFluidPressureData(const SbgBinaryLogData &ref_sbg_log) { sbg_driver::msg::SbgAirData sbg_air_data_message; - sbg_air_data_message = m_message_wrapper_.createSbgAirDataMessage(ref_sbg_log.airData); + sbg_air_data_message = message_wrapper_.createSbgAirDataMessage(ref_sbg_log.airData); - if (m_SbgAirData_pub_) + if (sbg_air_data_pub_) { - m_SbgAirData_pub_->publish(sbg_air_data_message); + sbg_air_data_pub_->publish(sbg_air_data_message); } - if (m_fluid_pub_) + if (fluid_pub_) { - m_fluid_pub_->publish(m_message_wrapper_.createRosFluidPressureMessage(sbg_air_data_message)); + fluid_pub_->publish(message_wrapper_.createRosFluidPressureMessage(sbg_air_data_message)); } } void MessagePublisher::publishEkfNavigationData(const SbgBinaryLogData &ref_sbg_log) { - m_sbg_ekf_nav_message_ = m_message_wrapper_.createSbgEkfNavMessage(ref_sbg_log.ekfNavData); + sbg_ekf_nav_message_ = message_wrapper_.createSbgEkfNavMessage(ref_sbg_log.ekfNavData); - if (m_sbgEkfNav_pub_) + if (sbg_ekf_nav_pub_) { - m_sbgEkfNav_pub_->publish(m_sbg_ekf_nav_message_); + sbg_ekf_nav_pub_->publish(sbg_ekf_nav_message_); } - if (m_pos_ecef_pub_) + if (pos_ecef_pub_) { - m_pos_ecef_pub_->publish(m_message_wrapper_.createRosPointStampedMessage(m_sbg_ekf_nav_message_)); + pos_ecef_pub_->publish(message_wrapper_.createRosPointStampedMessage(sbg_ekf_nav_message_)); } processRosVelMessage(); } @@ -427,17 +427,17 @@ void MessagePublisher::publishUtcData(const SbgBinaryLogData &ref_sbg_log) { sbg_driver::msg::SbgUtcTime sbg_utc_message; - sbg_utc_message = m_message_wrapper_.createSbgUtcTimeMessage(ref_sbg_log.utcData); + sbg_utc_message = message_wrapper_.createSbgUtcTimeMessage(ref_sbg_log.utcData); - if (m_sbgUtcTime_pub_) + if (sbg_utc_time_pub_) { - m_sbgUtcTime_pub_->publish(sbg_utc_message); + sbg_utc_time_pub_->publish(sbg_utc_message); } - if (m_utc_reference_pub_) + if (utc_reference_pub_) { if (sbg_utc_message.clock_status.clock_utc_status != SBG_ECOM_UTC_INVALID) { - m_utc_reference_pub_->publish(m_message_wrapper_.createRosUtcTimeReferenceMessage(sbg_utc_message)); + utc_reference_pub_->publish(message_wrapper_.createRosUtcTimeReferenceMessage(sbg_utc_message)); } } } @@ -446,19 +446,19 @@ void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log, Sb { sbg_driver::msg::SbgGpsPos sbg_gps_pos_message; - sbg_gps_pos_message = m_message_wrapper_.createSbgGpsPosMessage(ref_sbg_log.gpsPosData); + sbg_gps_pos_message = message_wrapper_.createSbgGpsPosMessage(ref_sbg_log.gpsPosData); - if (m_sbgGpsPos_pub_) + if (sbg_gps_pos_pub_) { - m_sbgGpsPos_pub_->publish(sbg_gps_pos_message); + sbg_gps_pos_pub_->publish(sbg_gps_pos_message); } - if (m_nav_sat_fix_pub_) + if (nav_sat_fix_pub_) { - m_nav_sat_fix_pub_->publish(m_message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); + nav_sat_fix_pub_->publish(message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); } if (nmea_gga_pub_ && sbg_msg_id == SBG_ECOM_LOG_GPS1_POS) { - const nmea_msgs::msg::Sentence nmea_gga_msg = m_message_wrapper_.createNmeaGGAMessageForNtrip(ref_sbg_log.gpsPosData); + const nmea_msgs::msg::Sentence nmea_gga_msg = message_wrapper_.createNmeaGGAMessageForNtrip(ref_sbg_log.gpsPosData); // Only publish if a valid NMEA GGA message has been generated if (nmea_gga_msg.sentence.size() > 0) @@ -479,17 +479,17 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C // const std::vector &ref_output_modes = ref_config_store.getOutputModes(); - m_message_wrapper_.setTimeReference(ref_config_store.getTimeReference()); + message_wrapper_.setTimeReference(ref_config_store.getTimeReference()); - m_message_wrapper_.setFrameId(ref_config_store.getFrameId()); + message_wrapper_.setFrameId(ref_config_store.getFrameId()); - m_message_wrapper_.setUseEnu(ref_config_store.getUseEnu()); + message_wrapper_.setUseEnu(ref_config_store.getUseEnu()); - m_message_wrapper_.setOdomEnable(ref_config_store.getOdomEnable()); - m_message_wrapper_.setOdomPublishTf(ref_config_store.getOdomPublishTf()); - m_message_wrapper_.setOdomFrameId(ref_config_store.getOdomFrameId()); - m_message_wrapper_.setOdomBaseFrameId(ref_config_store.getOdomBaseFrameId()); - m_message_wrapper_.setOdomInitFrameId(ref_config_store.getOdomInitFrameId()); + message_wrapper_.setOdomEnable(ref_config_store.getOdomEnable()); + message_wrapper_.setOdomPublishTf(ref_config_store.getOdomPublishTf()); + message_wrapper_.setOdomFrameId(ref_config_store.getOdomFrameId()); + message_wrapper_.setOdomBaseFrameId(ref_config_store.getOdomBaseFrameId()); + message_wrapper_.setOdomInitFrameId(ref_config_store.getOdomInitFrameId()); for (const ConfigStore::SbgLogOutput &ref_output : ref_output_modes) { @@ -498,7 +498,7 @@ void MessagePublisher::initPublishers(rclcpp::Node& ref_ros_node_handle, const C if (ref_config_store.shouldPublishNmea()) { - nmea_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(), m_max_messages_); + nmea_gga_pub_ = ref_ros_node_handle.create_publisher(ref_config_store.getNmeaFullTopic(), max_messages_); } if (ref_config_store.checkRosStandardMessages()) @@ -519,9 +519,9 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ { case SBG_ECOM_LOG_STATUS: - if (m_sbgStatus_pub_) + if (sbg_status_pub_) { - m_sbgStatus_pub_->publish(m_message_wrapper_.createSbgStatusMessage(ref_sbg_log.statusData)); + sbg_status_pub_->publish(message_wrapper_.createSbgStatusMessage(ref_sbg_log.statusData)); } break; @@ -542,18 +542,18 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_MAG_CALIB: - if (m_sbgMagCalib_pub_) + if (sbg_mag_calib_pub_) { - m_sbgMagCalib_pub_->publish(m_message_wrapper_.createSbgMagCalibMessage(ref_sbg_log.magCalibData)); + sbg_mag_calib_pub_->publish(message_wrapper_.createSbgMagCalibMessage(ref_sbg_log.magCalibData)); } break; case SBG_ECOM_LOG_EKF_EULER: - if (m_sbgEkfEuler_pub_) + if (sbg_ekf_euler_pub_) { - m_sbg_ekf_euler_message_ = m_message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); - m_sbgEkfEuler_pub_->publish(m_sbg_ekf_euler_message_); + sbg_ekf_euler_message_ = message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); + sbg_ekf_euler_pub_->publish(sbg_ekf_euler_message_); processRosVelMessage(); processRosOdoMessage(); } @@ -561,10 +561,10 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_EKF_QUAT: - if (m_sbgEkfQuat_pub_) + if (sbg_ekf_quat_pub_) { - m_sbg_ekf_quat_message_ = m_message_wrapper_.createSbgEkfQuatMessage(ref_sbg_log.ekfQuatData); - m_sbgEkfQuat_pub_->publish(m_sbg_ekf_quat_message_); + sbg_ekf_quat_message_ = message_wrapper_.createSbgEkfQuatMessage(ref_sbg_log.ekfQuatData); + sbg_ekf_quat_pub_->publish(sbg_ekf_quat_message_); processRosImuMessage(); processRosVelMessage(); } @@ -578,18 +578,18 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_SHIP_MOTION: - if (m_sbgShipMotion_pub_) + if (sbg_ship_motion_pub_) { - m_sbgShipMotion_pub_->publish(m_message_wrapper_.createSbgShipMotionMessage(ref_sbg_log.shipMotionData)); + sbg_ship_motion_pub_->publish(message_wrapper_.createSbgShipMotionMessage(ref_sbg_log.shipMotionData)); } break; case SBG_ECOM_LOG_GPS1_VEL: case SBG_ECOM_LOG_GPS2_VEL: - if (m_sbgGpsVel_pub_) + if (sbg_gps_vel_pub_) { - m_sbgGpsVel_pub_->publish(m_message_wrapper_.createSbgGpsVelMessage(ref_sbg_log.gpsVelData)); + sbg_gps_vel_pub_->publish(message_wrapper_.createSbgGpsVelMessage(ref_sbg_log.gpsVelData)); } break; @@ -602,74 +602,74 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_GPS1_HDT: case SBG_ECOM_LOG_GPS2_HDT: - if (m_sbgGpsHdt_pub_) + if (sbg_gps_hdt_pub_) { - m_sbgGpsHdt_pub_->publish(m_message_wrapper_.createSbgGpsHdtMessage(ref_sbg_log.gpsHdtData)); + sbg_gps_hdt_pub_->publish(message_wrapper_.createSbgGpsHdtMessage(ref_sbg_log.gpsHdtData)); } break; case SBG_ECOM_LOG_GPS1_RAW: case SBG_ECOM_LOG_GPS2_RAW: - if (m_sbgGpsRaw_pub_) + if (sbg_gps_raw_pub_) { - m_sbgGpsRaw_pub_->publish(m_message_wrapper_.createSbgGpsRawMessage(ref_sbg_log.gpsRawData)); + sbg_gps_raw_pub_->publish(message_wrapper_.createSbgGpsRawMessage(ref_sbg_log.gpsRawData)); } break; case SBG_ECOM_LOG_ODO_VEL: - if (m_sbgOdoVel_pub_) + if (sbg_odo_vel_pub_) { - m_sbgOdoVel_pub_->publish(m_message_wrapper_.createSbgOdoVelMessage(ref_sbg_log.odometerData)); + sbg_odo_vel_pub_->publish(message_wrapper_.createSbgOdoVelMessage(ref_sbg_log.odometerData)); } break; case SBG_ECOM_LOG_EVENT_A: - if (m_sbgEventA_pub_) + if (sbg_event_a_pub_) { - m_sbgEventA_pub_->publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + sbg_event_a_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); } break; case SBG_ECOM_LOG_EVENT_B: - if (m_sbgEventB_pub_) + if (sbg_event_b_pub_) { - m_sbgEventB_pub_->publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + sbg_event_b_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); } break; case SBG_ECOM_LOG_EVENT_C: - if (m_sbgEventC_pub_) + if (sbg_event_c_pub_) { - m_sbgEventC_pub_->publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + sbg_event_c_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); } break; case SBG_ECOM_LOG_EVENT_D: - if (m_sbgEventD_pub_) + if (sbg_event_d_pub_) { - m_sbgEventD_pub_->publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + sbg_event_d_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); } break; case SBG_ECOM_LOG_EVENT_E: - if (m_sbgEventE_pub_) + if (sbg_event_e_pub_) { - m_sbgEventE_pub_->publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + sbg_event_e_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); } break; case SBG_ECOM_LOG_IMU_SHORT: - if (m_SbgImuShort_pub_) + if (sbg_imu_short_pub_) { - m_SbgImuShort_pub_->publish(m_message_wrapper_.createSbgImuShortMessage(ref_sbg_log.imuShort)); + sbg_imu_short_pub_->publish(message_wrapper_.createSbgImuShortMessage(ref_sbg_log.imuShort)); } break; diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index efe96d4..a30b245 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -25,13 +25,13 @@ using sbg::MessageWrapper; MessageWrapper::MessageWrapper(): Node("tf_broadcaster") { - m_first_valid_utc_ = false; - m_tf_broadcaster_ = std::make_shared(this); - m_static_tf_broadcaster_ = std::make_shared(this); - m_utm0_.easting = 0.0; - m_utm0_.northing = 0.0; - m_utm0_.altitude = 0.0; - m_utm0_.zone = 0; + first_valid_utc_ = false; + tf_broadcaster_ = std::make_shared(this); + static_tf_broadcaster_ = std::make_shared(this); + utm0_.easting = 0.0; + utm0_.northing = 0.0; + utm0_.altitude = 0.0; + utm0_.zone = 0; } //---------------------------------------------------------------------// @@ -79,9 +79,9 @@ const std_msgs::msg::Header MessageWrapper::createRosHeader(uint32_t device_time { std_msgs::msg::Header header; - header.frame_id = m_frame_id_; + header.frame_id = frame_id_; - if (m_first_valid_utc_ && (m_time_reference_ == TimeReference::INS_UNIX)) + if (first_valid_utc_ && (time_reference_ == TimeReference::INS_UNIX)) { header.stamp = convertInsTimeToUnix(device_timestamp); } @@ -103,8 +103,8 @@ const rclcpp::Time MessageWrapper::convertInsTimeToUnix(uint32_t device_timestam uint32_t device_timestamp_diff; uint64_t nanoseconds; - utc_to_epoch = convertUtcTimeToUnix(m_last_sbg_utc_); - device_timestamp_diff = device_timestamp - m_last_sbg_utc_.time_stamp; + utc_to_epoch = convertUtcTimeToUnix(last_sbg_utc_); + device_timestamp_diff = device_timestamp - last_sbg_utc_.time_stamp; nanoseconds = utc_to_epoch.nanoseconds() + static_cast(device_timestamp_diff) * 1000; @@ -368,10 +368,10 @@ int32_t MessageWrapper::getUtcOffset() const { int32_t utcOffset; - if (m_first_valid_utc_) + if (first_valid_utc_) { // Compute the leap second: GPS = UTC + utcOffset - utcOffset = (m_last_sbg_utc_.gps_tow/1000)%60 - m_last_sbg_utc_.sec; + utcOffset = (last_sbg_utc_.gps_tow/1000)%60 - last_sbg_utc_.sec; if (utcOffset < 0) { @@ -407,51 +407,51 @@ const sbg_driver::msg::SbgAirDataStatus MessageWrapper::createAirDataStatusMessa * * Written by Chuck Gantz- chuck.gantz@globalstar.com */ -char MessageWrapper::UTMLetterDesignator(double Lat) +char MessageWrapper::UTMLetterDesignator(double latitude) { char LetterDesignator; - if ((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X'; - else if ((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W'; - else if ((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V'; - else if ((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U'; - else if ((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T'; - else if ((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S'; - else if ((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R'; - else if ((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q'; - else if ((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P'; - else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator = 'N'; - else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator = 'M'; - else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L'; - else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K'; - else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J'; - else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H'; - else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G'; - else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F'; - else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E'; - else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D'; - else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C'; + if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; + else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; + else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; + else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; + else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; + else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; + else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; + else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; + else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; + else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; + else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; + else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; + else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; + else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; + else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; + else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; + else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; + else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; + else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; + else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; // 'Z' is an error flag, the Latitude is outside the UTM limits else LetterDesignator = 'Z'; return LetterDesignator; } -void MessageWrapper::initUTM(double Lat, double Long, double altitude) +void MessageWrapper::initUTM(double latitude, double longitude, double altitude) { int zoneNumber; // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (Long+180)-int((Long+180)/360)*360-180; + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; zoneNumber = int((LongTemp + 180)/6) + 1; - if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + if(latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) { zoneNumber = 32; } // Special zones for Svalbard - if( Lat >= 72.0 && Lat < 84.0 ) + if(latitude >= 72.0 && latitude < 84.0 ) { if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; @@ -459,14 +459,14 @@ void MessageWrapper::initUTM(double Lat, double Long, double altitude) else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; } - m_utm0_.zone = zoneNumber; - m_utm0_.altitude = altitude; - LLtoUTM(Lat, Long, m_utm0_.zone, m_utm0_.northing, m_utm0_.easting); + utm0_.zone = zoneNumber; + utm0_.altitude = altitude; + LLtoUTM(latitude, longitude, utm0_.zone, utm0_.northing, utm0_.easting); RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" - , Lat, Long, m_utm0_.zone, UTMLetterDesignator(Lat) - , m_utm0_.easting, (int)(m_utm0_.easting)/1000 - , m_utm0_.northing, (int)(m_utm0_.northing)/1000 + , latitude, longitude, utm0_.zone, UTMLetterDesignator(latitude) + , utm0_.easting, (int)(utm0_.easting)/1000 + , utm0_.northing, (int)(utm0_.northing)/1000 ); } @@ -481,7 +481,7 @@ void MessageWrapper::initUTM(double Lat, double Long, double altitude) * * Originally written by Chuck Gantz- chuck.gantz@globalstar.com. */ -void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const +void MessageWrapper::LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) const { const double RADIANS_PER_DEGREE = M_PI/180.0; @@ -502,14 +502,14 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT double N, T, C, A, M; // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (Long+180)-int((Long+180)/360)*360-180; + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - double LatRad = Lat*RADIANS_PER_DEGREE; + double LatRad = latitude * RADIANS_PER_DEGREE; double LongRad = LongTemp*RADIANS_PER_DEGREE; double LongOriginRad; // +3 puts origin in middle of zone - LongOrigin = (zoneNumber - 1)*6 - 180 + 3; + LongOrigin = (zone_number - 1) * 6 - 180 + 3; LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; eccPrimeSquared = (eccSquared)/(1-eccSquared); @@ -524,24 +524,24 @@ void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UT + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); - UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); + utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); - UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); - if(Lat < 0) + if(latitude < 0) { - UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere + utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere } } -sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbgGpsType) +sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type) { sbg::MessageWrapper::NmeaGGAQuality nmeaQuality = NmeaGGAQuality::INVALID; - switch (sbgGpsType) + switch (sbg_gps_type) { case SBG_ECOM_POS_NO_SOLUTION: nmeaQuality = NmeaGGAQuality::INVALID; @@ -582,42 +582,42 @@ sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsTy void MessageWrapper::setTimeReference(TimeReference time_reference) { - m_time_reference_ = time_reference; + time_reference_ = time_reference; } void MessageWrapper::setFrameId(const std::string &frame_id) { - m_frame_id_ = frame_id; + frame_id_ = frame_id; } void MessageWrapper::setUseEnu(bool enu) { - m_use_enu_ = enu; + use_enu_ = enu; } void MessageWrapper::setOdomEnable(bool odom_enable) { - m_odom_enable_ = odom_enable; + odom_enable_ = odom_enable; } void MessageWrapper::setOdomPublishTf(bool publish_tf) { - m_odom_publish_tf_ = publish_tf; + odom_publish_tf_ = publish_tf; } void MessageWrapper::setOdomFrameId(const std::string &ref_frame_id) { - m_odom_frame_id_ = ref_frame_id; + odom_frame_id_ = ref_frame_id; } void MessageWrapper::setOdomBaseFrameId(const std::string &ref_frame_id) { - m_odom_base_frame_id_ = ref_frame_id; + odom_base_frame_id_ = ref_frame_id; } void MessageWrapper::setOdomInitFrameId(const std::string &ref_frame_id) { - m_odom_init_frame_id_ = ref_frame_id; + odom_init_frame_id_ = ref_frame_id; } //---------------------------------------------------------------------// @@ -632,7 +632,7 @@ const sbg_driver::msg::SbgEkfEuler MessageWrapper::createSbgEkfEulerMessage(cons ekf_euler_message.time_stamp = ref_log_ekf_euler.timeStamp; ekf_euler_message.status = createEkfStatusMessage(ref_log_ekf_euler.status); - if (m_use_enu_) + if (use_enu_) { ekf_euler_message.angle.x = ref_log_ekf_euler.euler[0]; ekf_euler_message.angle.y = -ref_log_ekf_euler.euler[1]; @@ -665,7 +665,7 @@ const sbg_driver::msg::SbgEkfNav MessageWrapper::createSbgEkfNavMessage(const Sb ekf_nav_message.longitude = ref_log_ekf_nav.position[1]; ekf_nav_message.altitude = ref_log_ekf_nav.position[2]; - if (m_use_enu_) + if (use_enu_) { ekf_nav_message.velocity.x = ref_log_ekf_nav.velocity[1]; ekf_nav_message.velocity.y = ref_log_ekf_nav.velocity[0]; @@ -709,7 +709,7 @@ const sbg_driver::msg::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const ekf_quat_message.accuracy.y = ref_log_ekf_quat.eulerStdDev[1]; ekf_quat_message.accuracy.z = ref_log_ekf_quat.eulerStdDev[2]; - if (m_use_enu_) + if (use_enu_) { static const tf2::Quaternion q_enu_to_nwu{0, 0, M_SQRT2 / 2, M_SQRT2 / 2}; tf2::Quaternion q_nwu{ref_log_ekf_quat.quaternion[1], @@ -765,7 +765,7 @@ const sbg_driver::msg::SbgGpsHdt MessageWrapper::createSbgGpsHdtMessage(const Sb gps_hdt_message.pitch_acc = ref_log_gps_hdt.pitchAccuracy; gps_hdt_message.baseline = ref_log_gps_hdt.baseline; - if (m_use_enu_) + if (use_enu_) { gps_hdt_message.true_heading = wrapAngle360(90.0f - ref_log_gps_hdt.heading); gps_hdt_message.pitch = -ref_log_gps_hdt.pitch; @@ -797,7 +797,7 @@ const sbg_driver::msg::SbgGpsPos MessageWrapper::createSbgGpsPosMessage(const Sb gps_pos_message.longitude = ref_log_gps_pos.longitude; gps_pos_message.altitude = ref_log_gps_pos.altitude; - if (m_use_enu_) + if (use_enu_) { gps_pos_message.position_accuracy.x = ref_log_gps_pos.longitudeAccuracy; gps_pos_message.position_accuracy.y = ref_log_gps_pos.latitudeAccuracy; @@ -832,7 +832,7 @@ const sbg_driver::msg::SbgGpsVel MessageWrapper::createSbgGpsVelMessage(const Sb gps_vel_message.gps_tow = ref_log_gps_vel.timeOfWeek; gps_vel_message.course_acc = ref_log_gps_vel.courseAcc; - if (m_use_enu_) + if (use_enu_) { gps_vel_message.velocity.x = ref_log_gps_vel.velocity[1]; gps_vel_message.velocity.y = ref_log_gps_vel.velocity[0]; @@ -869,7 +869,7 @@ const sbg_driver::msg::SbgImuData MessageWrapper::createSbgImuDataMessage(const imu_data_message.imu_status = createImuStatusMessage(ref_log_imu_data.status); imu_data_message.temp = ref_log_imu_data.temperature; - if (m_use_enu_) + if (use_enu_) { imu_data_message.accel.x = ref_log_imu_data.accelerometers[0]; imu_data_message.accel.y = -ref_log_imu_data.accelerometers[1]; @@ -917,7 +917,7 @@ const sbg_driver::msg::SbgMag MessageWrapper::createSbgMagMessage(const SbgLogMa mag_message.time_stamp = ref_log_mag.timeStamp; mag_message.status = createMagStatusMessage(ref_log_mag); - if (m_use_enu_) + if (use_enu_) { mag_message.mag.x = ref_log_mag.magnetometers[0]; mag_message.mag.y = -ref_log_mag.magnetometers[1]; @@ -1018,13 +1018,13 @@ const sbg_driver::msg::SbgUtcTime MessageWrapper::createSbgUtcTimeMessage(const utc_time_message.nanosec = ref_log_utc.nanoSecond; utc_time_message.gps_tow = ref_log_utc.gpsTimeOfWeek; - if (!m_first_valid_utc_) + if (!first_valid_utc_) { if (utc_time_message.clock_status.clock_stable && utc_time_message.clock_status.clock_utc_sync) { if (utc_time_message.clock_status.clock_status == SBG_ECOM_CLOCK_VALID) { - m_first_valid_utc_ = true; + first_valid_utc_ = true; RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "A full valid UTC log has been detected, timestamp will be synchronized with the UTC data."); } } @@ -1033,7 +1033,7 @@ const sbg_driver::msg::SbgUtcTime MessageWrapper::createSbgUtcTimeMessage(const // // Store the last UTC message. // - m_last_sbg_utc_ = utc_time_message; + last_sbg_utc_ = utc_time_message; return utc_time_message; } @@ -1063,7 +1063,7 @@ const sbg_driver::msg::SbgImuShort MessageWrapper::createSbgImuShortMessage(cons imu_short_message.imu_status = createImuStatusMessage(ref_short_imu_log.status); imu_short_message.temperature = ref_short_imu_log.temperature; - if (m_use_enu_) + if (use_enu_) { imu_short_message.delta_velocity.x = ref_short_imu_log.deltaVelocity[0]; imu_short_message.delta_velocity.y = -ref_short_imu_log.deltaVelocity[1]; @@ -1127,7 +1127,7 @@ void MessageWrapper::fillTransform(const std::string &ref_parent_frame_id, const refTransformStamped.transform.rotation.z = ref_pose.orientation.z; refTransformStamped.transform.rotation.w = ref_pose.orientation.w; - m_tf_broadcaster_->sendTransform(refTransformStamped); + tf_broadcaster_->sendTransform(refTransformStamped); } const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driver::msg::SbgImuData &ref_sbg_imu_msg, const sbg_driver::msg::SbgEkfNav &ref_ekf_nav_msg, const sbg_driver::msg::SbgEkfQuat &ref_ekf_quat_msg, const sbg_driver::msg::SbgEkfEuler &ref_ekf_euler_msg) @@ -1156,36 +1156,36 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv // The pose message provides the position and orientation of the robot relative to the frame specified in header.frame_id odo_ros_msg.header = createRosHeader(ref_sbg_imu_msg.time_stamp); - odo_ros_msg.header.frame_id = m_odom_frame_id_; + odo_ros_msg.header.frame_id = odom_frame_id_; tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); // Convert latitude and longitude to UTM coordinates. - if (m_utm0_.zone == 0) + if (utm0_.zone == 0) { initUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); - if (m_odom_publish_tf_) + if (odom_publish_tf_) { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - pose.position.x = m_utm0_.easting; - pose.position.y = m_utm0_.northing; - pose.position.z = m_utm0_.altitude; + pose.position.x = utm0_.easting; + pose.position.y = utm0_.northing; + pose.position.z = utm0_.altitude; - fillTransform(m_odom_init_frame_id_, m_odom_frame_id_, pose, transform); - m_static_tf_broadcaster_->sendTransform(transform); + fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); + static_tf_broadcaster_->sendTransform(transform); } } - LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, m_utm0_.zone, utm_northing, utm_easting); - odo_ros_msg.pose.pose.position.x = utm_easting - m_utm0_.easting; - odo_ros_msg.pose.pose.position.y = utm_northing - m_utm0_.northing; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; + LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm0_.zone, utm_northing, utm_easting); + odo_ros_msg.pose.pose.position.x = utm_easting - utm0_.easting; + odo_ros_msg.pose.pose.position.y = utm_northing - utm0_.northing; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - utm0_.altitude; // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(computeMeridian(m_utm0_.zone)); + double central_meridian = sbgDegToRadD(computeMeridian(utm0_.zone)); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. @@ -1202,7 +1202,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv odo_ros_msg.pose.covariance[5*6 + 5] = ref_ekf_euler_msg.accuracy.z * ref_ekf_euler_msg.accuracy.z; // The twist message gives the linear and angular velocity relative to the frame defined in child_frame_id - odo_ros_msg.child_frame_id = m_frame_id_; + odo_ros_msg.child_frame_id = frame_id_; odo_ros_msg.twist.twist.linear.x = ref_ekf_nav_msg.velocity.x; odo_ros_msg.twist.twist.linear.y = ref_ekf_nav_msg.velocity.y; odo_ros_msg.twist.twist.linear.z = ref_ekf_nav_msg.velocity.z; @@ -1216,11 +1216,11 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv odo_ros_msg.twist.covariance[4*6 + 4] = 0; odo_ros_msg.twist.covariance[5*6 + 5] = 0; - if (m_odom_publish_tf_) + if (odom_publish_tf_) { // Publish odom transformation. - fillTransform(odo_ros_msg.header.frame_id, m_odom_base_frame_id_, odo_ros_msg.pose.pose, transform); - m_tf_broadcaster_->sendTransform(transform); + fillTransform(odo_ros_msg.header.frame_id, odom_base_frame_id_, odo_ros_msg.pose.pose, transform); + tf_broadcaster_->sendTransform(transform); } return odo_ros_msg; diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index f6c7f30..1369850 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -45,9 +45,9 @@ std::map SbgDevice::g_mag_calib_confiden std::map SbgDevice::g_mag_calib_mode_ = { {SBG_ECOM_MAG_CALIB_MODE_2D, "Mode 2D"}, {SBG_ECOM_MAG_CALIB_MODE_3D, "Mode 3D"}}; -std::map SbgDevice::g_mag_calib_bandwidth = {{SBG_ECOM_MAG_CALIB_HIGH_BW, "High Bandwidth"}, - {SBG_ECOM_MAG_CALIB_MEDIUM_BW, "Medium Bandwidth"}, - {SBG_ECOM_MAG_CALIB_LOW_BW, "Low Bandwidth"}}; +std::map SbgDevice::g_mag_calib_bandwidth_ = {{SBG_ECOM_MAG_CALIB_HIGH_BW, "High Bandwidth"}, + {SBG_ECOM_MAG_CALIB_MEDIUM_BW, "Medium Bandwidth"}, + {SBG_ECOM_MAG_CALIB_LOW_BW, "Low Bandwidth"}}; /*! * Class to handle a connected SBG device. @@ -57,9 +57,9 @@ std::map SbgDevice::g_mag_calib_bandwidth //---------------------------------------------------------------------// SbgDevice::SbgDevice(rclcpp::Node& ref_node_handle): -m_ref_node_(ref_node_handle), -m_mag_calibration_ongoing_(false), -m_mag_calibration_done_(false) +ref_node_(ref_node_handle), +mag_calibration_ongoing_(false), +mag_calibration_done_(false) { loadParameters(); connect(); @@ -69,25 +69,25 @@ SbgDevice::~SbgDevice() { SbgErrorCode error_code; - error_code = sbgEComClose(&m_com_handle_); + error_code = sbgEComClose(&com_handle_); if (error_code != SBG_NO_ERROR) { - RCLCPP_ERROR(m_ref_node_.get_logger(), "Unable to close the SBG communication handle - %s.", sbgErrorCodeToString(error_code)); + RCLCPP_ERROR(ref_node_.get_logger(), "Unable to close the SBG communication handle - %s.", sbgErrorCodeToString(error_code)); } - if (m_config_store_.isInterfaceSerial()) + if (config_store_.isInterfaceSerial()) { - error_code = sbgInterfaceSerialDestroy(&m_sbg_interface_); + error_code = sbgInterfaceSerialDestroy(&sbg_interface_); } - else if (m_config_store_.isInterfaceUdp()) + else if (config_store_.isInterfaceUdp()) { - error_code = sbgInterfaceUdpDestroy(&m_sbg_interface_); + error_code = sbgInterfaceUdpDestroy(&sbg_interface_); } if (error_code != SBG_NO_ERROR) { - RCLCPP_ERROR(m_ref_node_.get_logger(), "SBG DRIVER - Unable to close the communication interface."); + RCLCPP_ERROR(ref_node_.get_logger(), "SBG DRIVER - Unable to close the communication interface."); } } @@ -114,7 +114,7 @@ void SbgDevice::onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const Sb // // Publish the received SBG log. // - m_message_publisher_.publish(msg_class, msg, ref_sbg_data); + message_publisher_.publish(msg_class, msg, ref_sbg_data); } void SbgDevice::loadParameters() @@ -125,7 +125,7 @@ void SbgDevice::loadParameters() rclcpp::NodeOptions node_opt; node_opt.automatically_declare_parameters_from_overrides(true); rclcpp::Node n_private("npv", "", node_opt); - m_config_store_.loadFromRosNodeHandle(n_private); + config_store_.loadFromRosNodeHandle(n_private); } void SbgDevice::connect() @@ -136,17 +136,17 @@ void SbgDevice::connect() // // Initialize the communication interface from the config store, then initialize the sbgECom protocol to communicate with the device. // - if (m_config_store_.isInterfaceSerial()) + if (config_store_.isInterfaceSerial()) { - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - serial interface %s at %d bps", m_config_store_.getUartPortName().c_str(), m_config_store_.getBaudRate()); - error_code = sbgInterfaceSerialCreate(&m_sbg_interface_, m_config_store_.getUartPortName().c_str(), m_config_store_.getBaudRate()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - serial interface %s at %d bps", config_store_.getUartPortName().c_str(), config_store_.getBaudRate()); + error_code = sbgInterfaceSerialCreate(&sbg_interface_, config_store_.getUartPortName().c_str(), config_store_.getBaudRate()); } - else if (m_config_store_.isInterfaceUdp()) + else if (config_store_.isInterfaceUdp()) { char ip[16]; - sbgNetworkIpToString(m_config_store_.getIpAddress(), ip, sizeof(ip)); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - UDP interface %s %d->%d", ip, m_config_store_.getInputPortAddress(), m_config_store_.getOutputPortAddress()); - error_code = sbgInterfaceUdpCreate(&m_sbg_interface_, m_config_store_.getIpAddress(), m_config_store_.getInputPortAddress(), m_config_store_.getOutputPortAddress()); + sbgNetworkIpToString(config_store_.getIpAddress(), ip, sizeof(ip)); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - UDP interface %s %d->%d", ip, config_store_.getInputPortAddress(), config_store_.getOutputPortAddress()); + error_code = sbgInterfaceUdpCreate(&sbg_interface_, config_store_.getIpAddress(), config_store_.getInputPortAddress(), config_store_.getOutputPortAddress()); } else { @@ -158,7 +158,7 @@ void SbgDevice::connect() rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the interface - " + std::string(sbgErrorCodeToString(error_code))); } - error_code = sbgEComInit(&m_com_handle_, &m_sbg_interface_); + error_code = sbgEComInit(&com_handle_, &sbg_interface_); if (error_code != SBG_NO_ERROR) { @@ -173,22 +173,22 @@ void SbgDevice::readDeviceInfo() SbgEComDeviceInfo device_info; SbgErrorCode error_code; - error_code = sbgEComCmdGetInfo(&m_com_handle_, &device_info); + error_code = sbgEComCmdGetInfo(&com_handle_, &device_info); if (error_code == SBG_NO_ERROR) { - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - productCode = %s", device_info.productCode); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - serialNumber = %u", device_info.serialNumber); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - productCode = %s", device_info.productCode); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - serialNumber = %u", device_info.serialNumber); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - calibationRev = %s", getVersionAsString(device_info.calibationRev).c_str()); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - calibrationDate = %u / %u / %u", device_info.calibrationDay, device_info.calibrationMonth, device_info.calibrationYear); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - calibationRev = %s", getVersionAsString(device_info.calibationRev).c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - calibrationDate = %u / %u / %u", device_info.calibrationDay, device_info.calibrationMonth, device_info.calibrationYear); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - hardwareRev = %s", getVersionAsString(device_info.hardwareRev).c_str()); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG_DRIVER - firmwareRev = %s", getVersionAsString(device_info.firmwareRev).c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - hardwareRev = %s", getVersionAsString(device_info.hardwareRev).c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - firmwareRev = %s", getVersionAsString(device_info.firmwareRev).c_str()); } else { - RCLCPP_ERROR(m_ref_node_.get_logger(), "Unable to get the device Info : %s", sbgErrorCodeToString(error_code)); + RCLCPP_ERROR(ref_node_.get_logger(), "Unable to get the device Info : %s", sbgErrorCodeToString(error_code)); } } @@ -202,29 +202,29 @@ std::string SbgDevice::getVersionAsString(uint32 sbg_version_enc) const void SbgDevice::initPublishers() { - m_message_publisher_.initPublishers(m_ref_node_, m_config_store_); + message_publisher_.initPublishers(ref_node_, config_store_); - m_rate_frequency_ = m_config_store_.getReadingRateFrequency(); + rate_frequency_ = config_store_.getReadingRateFrequency(); } void SbgDevice::initSubscribers() { - if (m_config_store_.shouldSubscribeToRtcm()) + if (config_store_.shouldSubscribeToRtcm()) { auto rtcm_cb = [&](const rtcm_msgs::msg::Message::SharedPtr msg) -> void { this->writeRtcmMessageToDevice(msg); }; - rtcm_sub_ = m_ref_node_.create_subscription(m_config_store_.getRtcmFullTopic(), 10, rtcm_cb); + rtcm_sub_ = ref_node_.create_subscription(config_store_.getRtcmFullTopic(), 10, rtcm_cb); } } void SbgDevice::configure() { - if (m_config_store_.checkConfigWithRos()) + if (config_store_.checkConfigWithRos()) { - ConfigApplier configApplier(m_com_handle_); - configApplier.applyConfiguration(m_config_store_); + ConfigApplier configApplier(com_handle_); + configApplier.applyConfiguration(config_store_); } } @@ -232,7 +232,7 @@ bool SbgDevice::processMagCalibration(const std::shared_ptrmessage = "Unable to end the calibration."; } - m_mag_calibration_ongoing_ = false; - m_mag_calibration_done_ = true; + mag_calibration_ongoing_ = false; + mag_calibration_done_ = true; } else { @@ -261,7 +261,7 @@ bool SbgDevice::processMagCalibration(const std::shared_ptrmessage = "Unable to start magnetometers calibration."; } - m_mag_calibration_ongoing_ = true; + mag_calibration_ongoing_ = true; } return ref_ros_response->success; @@ -271,12 +271,12 @@ bool SbgDevice::saveMagCalibration(const std::shared_ptrsuccess = false; ref_ros_response->message = "Magnetometer calibration process is still ongoing, finish it before trying to save it."; } - else if (m_mag_calibration_done_) + else if (mag_calibration_done_) { if (uploadMagCalibrationToDevice()) { @@ -304,21 +304,21 @@ bool SbgDevice::startMagCalibration() SbgEComMagCalibMode mag_calib_mode; SbgEComMagCalibBandwidth mag_calib_bandwidth; - mag_calib_mode = m_config_store_.getMagnetometerCalibMode(); - mag_calib_bandwidth = m_config_store_.getMagnetometerCalibBandwidth(); + mag_calib_mode = config_store_.getMagnetometerCalibMode(); + mag_calib_bandwidth = config_store_.getMagnetometerCalibBandwidth(); - error_code = sbgEComCmdMagStartCalib(&m_com_handle_, mag_calib_mode, mag_calib_bandwidth); + error_code = sbgEComCmdMagStartCalib(&com_handle_, mag_calib_mode, mag_calib_bandwidth); if (error_code != SBG_NO_ERROR) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to start the magnetometer calibration : %s", sbgErrorCodeToString(error_code)); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to start the magnetometer calibration : %s", sbgErrorCodeToString(error_code)); return false; } else { - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Start calibration"); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Mode : %s", g_mag_calib_mode_[mag_calib_mode].c_str()); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Bandwidth : %s", g_mag_calib_bandwidth[mag_calib_bandwidth].c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Start calibration"); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Mode : %s", g_mag_calib_mode_[mag_calib_mode].c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Bandwidth : %s", g_mag_calib_bandwidth_[mag_calib_bandwidth].c_str()); return true; } } @@ -327,11 +327,11 @@ bool SbgDevice::endMagCalibration() { SbgErrorCode error_code; - error_code = sbgEComCmdMagComputeCalib(&m_com_handle_, &m_magCalibResults); + error_code = sbgEComCmdMagComputeCalib(&com_handle_, &mag_calib_results_); if (error_code != SBG_NO_ERROR) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to compute the magnetometer calibration results : %s", sbgErrorCodeToString(error_code)); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to compute the magnetometer calibration results : %s", sbgErrorCodeToString(error_code)); return false; } else @@ -347,79 +347,79 @@ bool SbgDevice::uploadMagCalibrationToDevice() { SbgErrorCode error_code; - if (m_magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID) + if (mag_calib_results_.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID) { - error_code = sbgEComCmdMagSetCalibData(&m_com_handle_, m_magCalibResults.offset, m_magCalibResults.matrix); + error_code = sbgEComCmdMagSetCalibData(&com_handle_, mag_calib_results_.offset, mag_calib_results_.matrix); if (error_code != SBG_NO_ERROR) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to set the magnetometers calibration data to the device : %s", sbgErrorCodeToString(error_code)); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to set the magnetometers calibration data to the device : %s", sbgErrorCodeToString(error_code)); return false; } else { - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Saving data to the device"); - ConfigApplier configApplier(m_com_handle_); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Saving data to the device"); + ConfigApplier configApplier(com_handle_); configApplier.saveConfiguration(); return true; } } else { - RCLCPP_ERROR(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - The calibration was invalid, it can't be uploaded on the device."); + RCLCPP_ERROR(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - The calibration was invalid, it can't be uploaded on the device."); return false; } } void SbgDevice::displayMagCalibrationStatusResult() const { - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Quality of the calibration %s", g_mag_calib_quality_[m_magCalibResults.quality].c_str()); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Calibration results confidence %s", g_mag_calib_confidence_[m_magCalibResults.confidence].c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Quality of the calibration %s", g_mag_calib_quality_[mag_calib_results_.quality].c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Calibration results confidence %s", g_mag_calib_confidence_[mag_calib_results_.confidence].c_str()); SbgEComMagCalibMode mag_calib_mode; - mag_calib_mode = m_config_store_.getMagnetometerCalibMode(); + mag_calib_mode = config_store_.getMagnetometerCalibMode(); // // Check the magnetometers calibration status and display the warnings. // - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough valid points. Maybe you are moving too fast"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough valid points. Maybe you are moving too fast"); } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to find a calibration solution. Maybe there are too much non static distortions"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Unable to find a calibration solution. Maybe there are too much non static distortions"); } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment"); } if (mag_calib_mode == SBG_ECOM_MAG_CALIB_MODE_2D) { - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Too much roll motion for a 2D magnetic calibration"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Too much roll motion for a 2D magnetic calibration"); } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Too much pitch motion for a 2D magnetic calibration"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Too much pitch motion for a 2D magnetic calibration"); } } else { - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough roll motion for a 3D magnetic calibration"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough roll motion for a 3D magnetic calibration"); } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough pitch motion for a 3D magnetic calibration."); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough pitch motion for a 3D magnetic calibration."); } } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE) + if (mag_calib_results_.advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE) { - RCLCPP_WARN(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough yaw motion to compute a valid magnetic calibration"); + RCLCPP_WARN(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Not enough yaw motion to compute a valid magnetic calibration"); } } @@ -430,43 +430,43 @@ void SbgDevice::exportMagCalibrationResults() const ostringstream mag_results_stream; string output_filename; - mag_calib_mode = m_config_store_.getMagnetometerCalibMode(); - mag_calib_bandwidth = m_config_store_.getMagnetometerCalibBandwidth(); + mag_calib_mode = config_store_.getMagnetometerCalibMode(); + mag_calib_bandwidth = config_store_.getMagnetometerCalibBandwidth(); mag_results_stream << "SBG DRIVER [Mag Calib]" << endl; mag_results_stream << "======= Parameters =======" << endl; mag_results_stream << "* CALIB_MODE = " << g_mag_calib_mode_[mag_calib_mode] << endl; - mag_results_stream << "* CALIB_BW = " << g_mag_calib_bandwidth[mag_calib_bandwidth] << endl; + mag_results_stream << "* CALIB_BW = " << g_mag_calib_bandwidth_[mag_calib_bandwidth] << endl; mag_results_stream << "======= Results =======" << endl; - mag_results_stream << g_mag_calib_quality_[m_magCalibResults.quality] << endl; - mag_results_stream << g_mag_calib_confidence_[m_magCalibResults.confidence] << endl; + mag_results_stream << g_mag_calib_quality_[mag_calib_results_.quality] << endl; + mag_results_stream << g_mag_calib_confidence_[mag_calib_results_.confidence] << endl; mag_results_stream << "======= Infos =======" << endl; - mag_results_stream << "* Used points : " << m_magCalibResults.numPoints << "/" << m_magCalibResults.maxNumPoints << endl; + mag_results_stream << "* Used points : " << mag_calib_results_.numPoints << "/" << mag_calib_results_.maxNumPoints << endl; mag_results_stream << "* Mean, Std, Max" << endl; - mag_results_stream << "[Before]\t" << m_magCalibResults.beforeMeanError << "\t" << m_magCalibResults.beforeStdError << "\t" << m_magCalibResults.beforeMaxError << endl; - mag_results_stream << "[After]\t" << m_magCalibResults.afterMeanError << "\t" << m_magCalibResults.afterStdError << "\t" << m_magCalibResults.afterMaxError << endl; - mag_results_stream << "[Accuracy]\t" << sbgRadToDegF(m_magCalibResults.meanAccuracy) << "\t" << sbgRadToDegF(m_magCalibResults.stdAccuracy) << "\t" << sbgRadToDegF(m_magCalibResults.maxAccuracy) << endl; - mag_results_stream << "* Offset\t" << m_magCalibResults.offset[0] << "\t" << m_magCalibResults.offset[1] << "\t" << m_magCalibResults.offset[2] << endl; + mag_results_stream << "[Before]\t" << mag_calib_results_.beforeMeanError << "\t" << mag_calib_results_.beforeStdError << "\t" << mag_calib_results_.beforeMaxError << endl; + mag_results_stream << "[After]\t" << mag_calib_results_.afterMeanError << "\t" << mag_calib_results_.afterStdError << "\t" << mag_calib_results_.afterMaxError << endl; + mag_results_stream << "[Accuracy]\t" << sbgRadToDegF(mag_calib_results_.meanAccuracy) << "\t" << sbgRadToDegF(mag_calib_results_.stdAccuracy) << "\t" << sbgRadToDegF(mag_calib_results_.maxAccuracy) << endl; + mag_results_stream << "* Offset\t" << mag_calib_results_.offset[0] << "\t" << mag_calib_results_.offset[1] << "\t" << mag_calib_results_.offset[2] << endl; mag_results_stream << "* Matrix" << endl; - mag_results_stream << m_magCalibResults.matrix[0] << "\t" << m_magCalibResults.matrix[1] << "\t" << m_magCalibResults.matrix[2] << endl; - mag_results_stream << m_magCalibResults.matrix[3] << "\t" << m_magCalibResults.matrix[4] << "\t" << m_magCalibResults.matrix[5] << endl; - mag_results_stream << m_magCalibResults.matrix[6] << "\t" << m_magCalibResults.matrix[7] << "\t" << m_magCalibResults.matrix[8] << endl; + mag_results_stream << mag_calib_results_.matrix[0] << "\t" << mag_calib_results_.matrix[1] << "\t" << mag_calib_results_.matrix[2] << endl; + mag_results_stream << mag_calib_results_.matrix[3] << "\t" << mag_calib_results_.matrix[4] << "\t" << mag_calib_results_.matrix[5] << endl; + mag_results_stream << mag_calib_results_.matrix[6] << "\t" << mag_calib_results_.matrix[7] << "\t" << mag_calib_results_.matrix[8] << endl; output_filename = "mag_calib_" + timeToStr()/*(nullptrrclcpp::WallTimer::now())*/ + ".txt"; ofstream output_file(output_filename); output_file << mag_results_stream.str(); output_file.close(); - RCLCPP_INFO(m_ref_node_.get_logger(), "%s", mag_results_stream.str().c_str()); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Magnetometers calibration results saved to file %s", output_filename.c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "%s", mag_results_stream.str().c_str()); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Mag Calib] - Magnetometers calibration results saved to file %s", output_filename.c_str()); } void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPtr msg) { auto rtcm_data = msg->message; - auto error_code = sbgInterfaceWrite(&m_sbg_interface_, rtcm_data.data(), rtcm_data.size()); + auto error_code = sbgInterfaceWrite(&sbg_interface_, rtcm_data.data(), rtcm_data.size()); if (error_code != SBG_NO_ERROR) { @@ -483,7 +483,7 @@ void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPt uint32_t SbgDevice::getUpdateFrequency() const { - return m_rate_frequency_; + return rate_frequency_; } //---------------------------------------------------------------------// @@ -496,7 +496,7 @@ void SbgDevice::initDeviceForReceivingData() initPublishers(); configure(); - error_code = sbgEComSetReceiveLogCallback(&m_com_handle_, onLogReceivedCallback, this); + error_code = sbgEComSetReceiveLogCallback(&com_handle_, onLogReceivedCallback, this); if (error_code != SBG_NO_ERROR) { @@ -508,13 +508,13 @@ void SbgDevice::initDeviceForReceivingData() void SbgDevice::initDeviceForMagCalibration() { - m_calib_service_ = m_ref_node_.create_service("sbg/mag_calibration", std::bind(&SbgDevice::processMagCalibration, this, std::placeholders::_1, std::placeholders::_2)); - m_calib_save_service_ = m_ref_node_.create_service("sbg/mag_calibration_save", std::bind(&SbgDevice::saveMagCalibration, this, std::placeholders::_1, std::placeholders::_2)); + calib_service_ = ref_node_.create_service("sbg/mag_calibration", std::bind(&SbgDevice::processMagCalibration, this, std::placeholders::_1, std::placeholders::_2)); + calib_save_service_ = ref_node_.create_service("sbg/mag_calibration_save", std::bind(&SbgDevice::saveMagCalibration, this, std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(m_ref_node_.get_logger(), "SBG DRIVER [Init] - SBG device is initialized for magnetometers calibration."); + RCLCPP_INFO(ref_node_.get_logger(), "SBG DRIVER [Init] - SBG device is initialized for magnetometers calibration."); } void SbgDevice::periodicHandle() { - sbgEComHandle(&m_com_handle_); + sbgEComHandle(&com_handle_); } From db22cc50b80be324a442b8f687ad1aaa9a1d2e73 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 25 Aug 2023 16:01:32 +0200 Subject: [PATCH 46/74] Removed catkin reference in CMakeLists.txt --- CMakeLists.txt | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a35d16b..1e2442a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,17 +87,11 @@ ament_export_dependencies(rosidl_default_runtime) add_subdirectory(external/sbgECom) include_directories( - #${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include/sbg_driver/ ${PROJECT_SOURCE_DIR}/external/sbgECom/common ${PROJECT_SOURCE_DIR}/external/sbgECom/src ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(sbg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - ## Define common resources. set (SBG_COMMON_RESOURCES src/config_applier.cpp @@ -125,8 +119,8 @@ target_compile_options(sbg_device_mag PRIVATE -Wall -Wextra) endif() ## Specify libraries to link a library or executable target against -target_link_libraries(sbg_device ${catkin_LIBRARIES} sbgECom) -target_link_libraries(sbg_device_mag ${catkin_LIBRARIES} sbgECom) +target_link_libraries(sbg_device sbgECom) +target_link_libraries(sbg_device_mag sbgECom) ament_target_dependencies(sbg_device ${USED_LIBRARIES}) ament_target_dependencies(sbg_device_mag ${USED_LIBRARIES}) @@ -141,9 +135,6 @@ set_property(TARGET sbg_device_mag PROPERTY CXX_STANDARD 14) ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - ## Mark executables and/or libraries for installation install(TARGETS sbg_device sbg_device_mag DESTINATION lib/${PROJECT_NAME} @@ -151,7 +142,7 @@ install(TARGETS sbg_device sbg_device_mag ## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} #${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN ".git" EXCLUDE @@ -168,7 +159,7 @@ install(DIRECTORY config/ ################################### -## catkin specific configuration ## +## ament specific configuration ## ################################### ament_export_dependencies( From a06a68e313db7e2972bcb5fbe9ae153b2847418d Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 25 Aug 2023 17:57:54 +0200 Subject: [PATCH 47/74] Moved UTM initialization into its own class --- CMakeLists.txt | 2 + include/sbg_driver/message_publisher.h | 2 +- include/sbg_driver/message_wrapper.h | 38 +----- include/sbg_driver/sbg_ros_helpers.h | 60 +++++++++ include/sbg_driver/sbg_utm.h | 92 ++++++++++++++ src/message_wrapper.cpp | 167 +++---------------------- src/sbg_ros_helpers.cpp | 108 ++++++++++++++++ src/sbg_utm.cpp | 55 ++++++++ 8 files changed, 336 insertions(+), 188 deletions(-) create mode 100644 include/sbg_driver/sbg_ros_helpers.h create mode 100644 include/sbg_driver/sbg_utm.h create mode 100644 src/sbg_ros_helpers.cpp create mode 100644 src/sbg_utm.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e2442a..260c7a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,6 +99,8 @@ set (SBG_COMMON_RESOURCES src/message_wrapper.cpp src/config_store.cpp src/sbg_device.cpp + src/sbg_utm.cpp + src/sbg_ros_helpers.cpp ) ## Declare a C++ executable diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index f6d83eb..4e3e03f 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -3,7 +3,7 @@ * \author SBG Systems * \date 13/03/2020 * -* \brief Manage publishment of messages from logs. +* \brief Manage publishing of messages from logs. * * \section CodeCopyright Copyright Notice * MIT License diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index b4474d4..23dc01a 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -41,6 +41,7 @@ // Sbg header #include #include +#include // ROS headers #include @@ -79,13 +80,6 @@ namespace sbg { -typedef struct _UTM0 -{ - double easting; - double northing; - double altitude; - int zone; -} UTM0; /*! * Class to wrap the SBG logs into ROS messages. @@ -117,7 +111,6 @@ class MessageWrapper : public rclcpp::Node std::string frame_id_; bool use_enu_; TimeReference time_reference_; - UTM0 utm0_; bool odom_enable_; bool odom_publish_tf_; @@ -125,6 +118,7 @@ class MessageWrapper : public rclcpp::Node std::string odom_base_frame_id_; std::string odom_init_frame_id_; + sbg::utm utm_{}; //---------------------------------------------------------------------// //- Internal methods -// //---------------------------------------------------------------------// @@ -319,34 +313,6 @@ class MessageWrapper : public rclcpp::Node */ void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::msg::Pose &ref_pose, geometry_msgs::msg::TransformStamped &ref_transform_stamped); - /*! - * Get UTM letter designator for the given latitude. - * - * \param[in] latitude Latitude, in degrees. - * \return UTM letter designator. - */ - char UTMLetterDesignator(double latitude); - - /*! - * Set UTM initial position. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \param[in] altitude Altitude, in meters. - */ - void initUTM(double latitude, double longitude, double altitude); - - /*! - * Convert latitude and longitude to a position relative to UTM initial position. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \param[in] zone_number UTM zone number. - * \param[out] utm_northing UTM northing, in meters. - * \param[out] utm_easting UTM easting, in meters. - */ - void LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) const; - /*! * Convert SbgEComGpsPosType enum to NmeaGGAQuality enum * diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h new file mode 100644 index 0000000..4ef8762 --- /dev/null +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -0,0 +1,60 @@ +/*! +* \file sbg_helpers.h +* \author SBG Systems +* \date 25/08/2023 +* +* \brief Helpers for various tasks. +* +* Helpers for various tasks. +* +* \section CodeCopyright Copyright Notice +* MIT License +* +* Copyright (c) 2023 SBG Systems +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ + +#ifndef SBG_ROS_ROS_HELPERS_H +#define SBG_ROS_ROS_HELPERS_H + +namespace sbg::helpers +{ + /*! + * Convert latitude and longitude to a position relative to UTM initial position. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \param[in] zone_number UTM zone number. + * \param[out] utm_northing UTM northing, in meters. + * \param[out] utm_easting UTM easting, in meters. + */ + void LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting); + + /*! + * Get UTM letter designator for the given latitude. + * + * \param[in] latitude Latitude, in degrees. + * \return UTM letter designator. + */ + char UTMLetterDesignator(double latitude); +} + +#endif // #ifndef SBG_ROS_ROS_HELPERS_H + diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h new file mode 100644 index 0000000..ab35af4 --- /dev/null +++ b/include/sbg_driver/sbg_utm.h @@ -0,0 +1,92 @@ +/*! +* \file sbg_utm.h +* \author SBG Systems +* \date 25/08/2023 +* +* \brief Handle creation of utm class. +* +* Methods to create UTM from given data. +* +* \section CodeCopyright Copyright Notice +* MIT License +* +* Copyright (c) 2023 SBG Systems +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ + +#ifndef SBG_ROS_UTM_H +#define SBG_ROS_UTM_H + +namespace sbg +{ + +class utm final +{ +public: + utm() = default; + ~utm() = default; + + /*! + * Set UTM initial position. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \param[in] altitude Altitude, in meters. + */ + void init(double latitude, double longitude, double altitude); + + /*! + * Get odom publish_tf. + * + * \return If true publish odometry transforms. + */ + double getEasting() const; + + /*! + * Get odom publish_tf. + * + * \return If true publish odometry transforms. + */ + double getNorthing() const; + + /*! + * Get odom publish_tf. + * + * \return If true publish odometry transforms. + */ + double getAltitude() const; + + /*! + * Get odom publish_tf. + * + * \return If true publish odometry transforms. + */ + int getZone() const; + +private: + double easting_ = 0.0; + double northing_ = 0.0; + double altitude_ = 0.0; + int zone_ = 0; +}; + +} + +#endif // SBG_ROS_UTM_H diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index a30b245..a3cbaf6 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -9,6 +9,7 @@ // Project headers #include +#include // STL headers #include @@ -28,10 +29,6 @@ Node("tf_broadcaster") first_valid_utc_ = false; tf_broadcaster_ = std::make_shared(this); static_tf_broadcaster_ = std::make_shared(this); - utm0_.easting = 0.0; - utm0_.northing = 0.0; - utm0_.altitude = 0.0; - utm0_.zone = 0; } //---------------------------------------------------------------------// @@ -400,143 +397,6 @@ const sbg_driver::msg::SbgAirDataStatus MessageWrapper::createAirDataStatusMessa return air_data_status_message; } -/** - * Get UTM letter designator for the given latitude. - * - * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S - * - * Written by Chuck Gantz- chuck.gantz@globalstar.com - */ -char MessageWrapper::UTMLetterDesignator(double latitude) -{ - char LetterDesignator; - - if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; - else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; - else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; - else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; - else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; - else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; - else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; - else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; - else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; - else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; - else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; - else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; - else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; - else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; - else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; - else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; - else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; - else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; - else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; - else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; - // 'Z' is an error flag, the Latitude is outside the UTM limits - else LetterDesignator = 'Z'; - return LetterDesignator; -} - -void MessageWrapper::initUTM(double latitude, double longitude, double altitude) -{ - int zoneNumber; - - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - - zoneNumber = int((LongTemp + 180)/6) + 1; - - if(latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) - { - zoneNumber = 32; - } - - // Special zones for Svalbard - if(latitude >= 72.0 && latitude < 84.0 ) - { - if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; - else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; - else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; - else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; - } - - utm0_.zone = zoneNumber; - utm0_.altitude = altitude; - LLtoUTM(latitude, longitude, utm0_.zone, utm0_.northing, utm0_.easting); - - RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" - , latitude, longitude, utm0_.zone, UTMLetterDesignator(latitude) - , utm0_.easting, (int)(utm0_.easting)/1000 - , utm0_.northing, (int)(utm0_.northing)/1000 - ); -} - -/* - * Modification of gps_common::LLtoUTM() to use a constant UTM zone. - * - * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 - * - * East Longitudes are positive, West longitudes are negative. - * North latitudes are positive, South latitudes are negative - * Lat and Long are in fractional degrees - * - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com. - */ -void MessageWrapper::LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) const -{ - const double RADIANS_PER_DEGREE = M_PI/180.0; - - // WGS84 Parameters - const double WGS84_A = 6378137.0; // major axis - const double WGS84_E = 0.0818191908; // first eccentricity - - // UTM Parameters - const double UTM_K0 = 0.9996; // scale factor - const double UTM_E2 = (WGS84_E*WGS84_E); // e^2 - - double a = WGS84_A; - double eccSquared = UTM_E2; - double k0 = UTM_K0; - - double LongOrigin; - double eccPrimeSquared; - double N, T, C, A, M; - - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - - double LatRad = latitude * RADIANS_PER_DEGREE; - double LongRad = LongTemp*RADIANS_PER_DEGREE; - double LongOriginRad; - - // +3 puts origin in middle of zone - LongOrigin = (zone_number - 1) * 6 - 180 + 3; - LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; - - eccPrimeSquared = (eccSquared)/(1-eccSquared); - - N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); - T = tan(LatRad)*tan(LatRad); - C = eccPrimeSquared*cos(LatRad)*cos(LatRad); - A = cos(LatRad)*(LongRad-LongOriginRad); - - M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad - - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) - + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) - - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); - - utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); - - utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); - - if(latitude < 0) - { - utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere - } -} - sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type) { sbg::MessageWrapper::NmeaGGAQuality nmeaQuality = NmeaGGAQuality::INVALID; @@ -1160,32 +1020,37 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); // Convert latitude and longitude to UTM coordinates. - if (utm0_.zone == 0) + if (utm_.getZone() == 0) { - initUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); + utm_.init(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); + RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" + , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZone(), sbg::helpers::UTMLetterDesignator(ref_ekf_nav_msg.latitude) + , utm_.getEasting(), (int)(utm_.getEasting())/1000 + , utm_.getNorthing(), (int)(utm_.getNorthing())/1000 + ); if (odom_publish_tf_) { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - pose.position.x = utm0_.easting; - pose.position.y = utm0_.northing; - pose.position.z = utm0_.altitude; + pose.position.x = utm_.getEasting(); + pose.position.y = utm_.getNorthing(); + pose.position.z = utm_.getAltitude(); fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); static_tf_broadcaster_->sendTransform(transform); } } - LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm0_.zone, utm_northing, utm_easting); - odo_ros_msg.pose.pose.position.x = utm_easting - utm0_.easting; - odo_ros_msg.pose.pose.position.y = utm_northing - utm0_.northing; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - utm0_.altitude; + sbg::helpers::LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZone(), utm_northing, utm_easting); + odo_ros_msg.pose.pose.position.x = utm_easting - utm_.getEasting(); + odo_ros_msg.pose.pose.position.y = utm_northing - utm_.getNorthing(); + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - utm_.getAltitude(); // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(computeMeridian(utm0_.zone)); + double central_meridian = sbgDegToRadD(computeMeridian(utm_.getZone())); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp new file mode 100644 index 0000000..013c085 --- /dev/null +++ b/src/sbg_ros_helpers.cpp @@ -0,0 +1,108 @@ +// File header +#include "sbg_ros_helpers.h" + +// STL includes +#include + +/* + * Modification of gps_common::LLtoUTM() to use a constant UTM zone. + * + * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 + * + * East Longitudes are positive, West longitudes are negative. + * North latitudes are positive, South latitudes are negative + * Lat and Long are in fractional degrees + * + * Originally written by Chuck Gantz- chuck.gantz@globalstar.com. + */ +void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) +{ + const double RADIANS_PER_DEGREE = M_PI/180.0; + + // WGS84 Parameters + const double WGS84_A = 6378137.0; // major axis + const double WGS84_E = 0.0818191908; // first eccentricity + + // UTM Parameters + const double UTM_K0 = 0.9996; // scale factor + const double UTM_E2 = (WGS84_E*WGS84_E); // e^2 + + double a = WGS84_A; + double eccSquared = UTM_E2; + double k0 = UTM_K0; + + double LongOrigin; + double eccPrimeSquared; + double N, T, C, A, M; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + + double LatRad = latitude * RADIANS_PER_DEGREE; + double LongRad = LongTemp*RADIANS_PER_DEGREE; + double LongOriginRad; + + // +3 puts origin in middle of zone + LongOrigin = (zone_number - 1) * 6 - 180 + 3; + LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; + + eccPrimeSquared = (eccSquared)/(1-eccSquared); + + N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = eccPrimeSquared*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); + + M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad + - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) + + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) + - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); + + utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); + + utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + + if(latitude < 0) + { + utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere + } +} + +/** + * Get UTM letter designator for the given latitude. + * + * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S + * + * Written by Chuck Gantz- chuck.gantz@globalstar.com + */ +char sbg::helpers::UTMLetterDesignator(double latitude) +{ + char LetterDesignator; + + if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; + else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; + else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; + else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; + else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; + else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; + else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; + else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; + else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; + else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; + else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; + else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; + else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; + else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; + else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; + else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; + else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; + else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; + else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; + else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; + // 'Z' is an error flag, the Latitude is outside the UTM limits + else LetterDesignator = 'Z'; + return LetterDesignator; +} \ No newline at end of file diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp new file mode 100644 index 0000000..0e0cc29 --- /dev/null +++ b/src/sbg_utm.cpp @@ -0,0 +1,55 @@ +// File header +#include "sbg_utm.h" + +// Project headers +#include + +using sbg::utm; + +void utm::init(double latitude, double longitude, double altitude) +{ + int zoneNumber; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + + zoneNumber = int((LongTemp + 180)/6) + 1; + + if(latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + { + zoneNumber = 32; + } + + // Special zones for Svalbard + if(latitude >= 72.0 && latitude < 84.0 ) + { + if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; + else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; + else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; + else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; + } + + zone_ = zoneNumber; + altitude_ = altitude; + sbg::helpers::LLtoUTM(latitude, longitude, zone_, northing_, easting_); +} + +double utm::getEasting() const +{ + return easting_; +} + +double utm::getNorthing() const +{ + return northing_; +} + +double utm::getAltitude() const +{ + return altitude_; +} + +int utm::getZone() const +{ + return zone_; +} From 2af8917e260666960e9eb097b8139637fc6f4801 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 10:55:41 +0200 Subject: [PATCH 48/74] Moved functions into helper namespace --- include/sbg_driver/message_wrapper.h | 104 ++---------- include/sbg_driver/sbg_ros_helpers.h | 92 +++++++++++ src/message_wrapper.cpp | 232 ++++++--------------------- src/sbg_ros_helpers.cpp | 222 ++++++++++++++++++++----- 4 files changed, 330 insertions(+), 320 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 23dc01a..87b2aae 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -86,25 +86,6 @@ namespace sbg */ class MessageWrapper : public rclcpp::Node { -public: - const int32_t DEFAULT_UTC_OFFSET = 18; /*!< Driver default GPS to UTC offset in seconds. */ - - /*! - * Standard NMEA GGA quality indicator value. - */ - enum class NmeaGGAQuality: int32_t - { - INVALID = 0, - SINGLE = 1, - DGPS = 2, - PPS = 3, - RTK_FIXED = 4, - RTK_FLOAT = 5, - DEAD_RECKONING = 6, - STATIC_POSITION = 7, - SIMULATED = 8, - }; - private: sbg_driver::msg::SbgUtcTime last_sbg_utc_; bool first_valid_utc_; @@ -123,30 +104,6 @@ class MessageWrapper : public rclcpp::Node //- Internal methods -// //---------------------------------------------------------------------// - /*! - * Wrap an angle between [ -Pi ; Pi ] rad. - * - * \param[in] angle_rad Angle in rad. - * \return Wrapped angle. - */ - static float wrapAnglePi(float angle_rad); - - /*! - * Wrap an angle between [ 0 ; 360 ] degree. - * - * \param[in] angle_deg Angle in degree. - * \return Wrapped angle. - */ - static float wrapAngle360(float angle_deg); - - /*! - * Compute UTM zone meridian. - * - * \param[in] zone_number UTM Zone number. - * \return Meridian angle, in degrees. - */ - static double computeMeridian(int zone_number); - /*! * Create a ROS message header. * @@ -163,6 +120,14 @@ class MessageWrapper : public rclcpp::Node */ const rclcpp::Time convertInsTimeToUnix(uint32_t device_timestamp) const; + /*! + * Convert the UTC time to an Unix time. + * + * \param[in] ref_sbg_utc_msg UTC message. + * \return Converted Epoch time (in s). + */ + const rclcpp::Time convertUtcTimeToUnix(const sbg_driver::msg::SbgUtcTime& ref_sbg_utc_msg) const; + /*! * Create SBG-ROS Ekf status message. * @@ -243,49 +208,6 @@ class MessageWrapper : public rclcpp::Node */ const sbg_driver::msg::SbgUtcTimeStatus createUtcStatusMessage(const SbgLogUtcData& ref_log_utc) const; - /*! - * Get the number of days in the year. - * - * \param[in] year Year to get the number of days. - * \return Number of days in the year. - */ - uint32_t getNumberOfDaysInYear(uint16_t year) const; - - /*! - * Get the number of days of the month index. - * - * \param[in] year Year. - * \param[in] month_index Month index [1..12]. - * \return Number of days in the month. - */ - uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) const; - - /*! - * Check if the given year is a leap year. - * - * \param[in] year Year to check. - * \return True if the year is a leap year. - */ - bool isLeapYear(uint16_t year) const; - - /*! - * Convert the UTC time to an Unix time. - * - * \param[in] ref_sbg_utc_msg UTC message. - * \return Converted Epoch time (in s). - */ - const rclcpp::Time convertUtcTimeToUnix(const sbg_driver::msg::SbgUtcTime& ref_sbg_utc_msg) const; - - /*! - * Returns the GPS to UTC leap second offset: GPS_Time = UTC_Tme + utcOffset - * - * WARNING: The leap second is computed from the latest received SbgUtcTime message if any. - * If no SbgUtcTime message has been received, a default driver current value is used. - * - * \return Offset in seconds to apply to UTC time to get GPS time. - */ - int32_t getUtcOffset() const; - /*! * Create a SBG-ROS air data status message. * @@ -313,14 +235,6 @@ class MessageWrapper : public rclcpp::Node */ void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::msg::Pose &ref_pose, geometry_msgs::msg::TransformStamped &ref_transform_stamped); - /*! - * Convert SbgEComGpsPosType enum to NmeaGGAQuality enum - * - * \param[in] sbg_gps_type SbgECom GPS type - * \return NMEA GPS type - */ - static NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type); - public: //---------------------------------------------------------------------// @@ -659,7 +573,7 @@ class MessageWrapper : public rclcpp::Node * \param[in] ref_log_gps_pos SBG GPS Position log. * \return ROS NMEA GGA message. */ - const nmea_msgs::msg::Sentence createNmeaGGAMessageForNtrip(const SbgLogGpsPos& ref_log_gps_pos) const; + const nmea_msgs::msg::Sentence createNmeaGGAMessageForNtrip(const SbgLogGpsPos& ref_log_gps_pos) const; }; } diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h index 4ef8762..ee743d7 100644 --- a/include/sbg_driver/sbg_ros_helpers.h +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -34,8 +34,30 @@ #ifndef SBG_ROS_ROS_HELPERS_H #define SBG_ROS_ROS_HELPERS_H +// SbgECom headers +#include + +// STL headers +#include + namespace sbg::helpers { + /*! + * Standard NMEA GGA quality indicator value. + */ + enum class NmeaGGAQuality: int32_t + { + INVALID = 0, + SINGLE = 1, + DGPS = 2, + PPS = 3, + RTK_FIXED = 4, + RTK_FLOAT = 5, + DEAD_RECKONING = 6, + STATIC_POSITION = 7, + SIMULATED = 8, + }; + /*! * Convert latitude and longitude to a position relative to UTM initial position. * @@ -54,6 +76,76 @@ namespace sbg::helpers * \return UTM letter designator. */ char UTMLetterDesignator(double latitude); + + /*! + * Wrap an angle between [ -Pi ; Pi ] rad. + * + * \param[in] angle_rad Angle in rad. + * \return Wrapped angle. + */ + float wrapAnglePi(float angle_rad); + + /*! + * Wrap an angle between [ 0 ; 360 ] degree. + * + * \param[in] angle_deg Angle in degree. + * \return Wrapped angle. + */ + float wrapAngle360(float angle_deg); + + /*! + * Compute UTM zone meridian. + * + * \param[in] zone_number UTM Zone number. + * \return Meridian angle, in degrees. + */ + double computeMeridian(int zone_number); + + /*! + * Get the number of days in the year. + * + * \param[in] year Year to get the number of days. + * \return Number of days in the year. + */ + uint32_t getNumberOfDaysInYear(uint16_t year); + + /*! + * Get the number of days of the month index. + * + * \param[in] year Year. + * \param[in] month_index Month index [1..12]. + * \return Number of days in the month. + */ + uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index); + + /*! + * Check if the given year is a leap year. + * + * \param[in] year Year to check. + * \return True if the year is a leap year. + */ + bool isLeapYear(uint16_t year); + + /*! + * Returns the GPS to UTC leap second offset: GPS_Time = UTC_Tme + utcOffset + * + * WARNING: The leap second is computed from the latest received SbgUtcTime message if any. + * If no SbgUtcTime message has been received, a default driver current value is used. + * + * \param[in] first_valid_utc First valid utc. + * \param[in] gps_tow Current GPS time of the week. + * \param[in] sec Current second. + * \return Offset in seconds to apply to UTC time to get GPS time. + */ + int32_t getUtcOffset(bool first_valid_utc, uint32_t gps_tow, uint8_t sec); + + /*! + * Convert SbgEComGpsPosType enum to NmeaGGAQuality enum + * + * \param[in] sbg_gps_type SbgECom GPS type + * \return NMEA GPS type + */ + NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type); } #endif // #ifndef SBG_ROS_ROS_HELPERS_H diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index a3cbaf6..0dabc74 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -35,43 +35,6 @@ Node("tf_broadcaster") //- Internal methods -// //---------------------------------------------------------------------// -float MessageWrapper::wrapAnglePi(float angle_rad) -{ - if (angle_rad > SBG_PI_F) - { - return (SBG_PI_F * 2.0f - fmodf(angle_rad, SBG_PI_F * 2.0f)); - } - - if (angle_rad < -SBG_PI_F) - { - return (SBG_PI_F * 2.0f + fmodf(angle_rad, SBG_PI_F * 2.0f)); - } - - return angle_rad; -} - -float MessageWrapper::wrapAngle360(float angle_deg) -{ - float wrapped_angle_deg = angle_deg; - - if ( (wrapped_angle_deg < -360.0f) || (wrapped_angle_deg > 360.0f) ) - { - wrapped_angle_deg = fmodf(wrapped_angle_deg, 360.0f); - } - - if (wrapped_angle_deg < 0.0f) - { - wrapped_angle_deg = 360.0f + wrapped_angle_deg; - } - - return wrapped_angle_deg; -} - -double MessageWrapper::computeMeridian(int zone_number) -{ - return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; -} - const std_msgs::msg::Header MessageWrapper::createRosHeader(uint32_t device_timestamp) const { std_msgs::msg::Header header; @@ -110,6 +73,41 @@ const rclcpp::Time MessageWrapper::convertInsTimeToUnix(uint32_t device_timestam return utc_to_epoch; } +const rclcpp::Time MessageWrapper::convertUtcTimeToUnix(const sbg_driver::msg::SbgUtcTime& ref_sbg_utc_msg) const +{ + rclcpp::Time utc_to_epoch; + uint32_t days; + uint64_t nanoseconds; + + // + // Convert the UTC time to Epoch(Unix) time, which is the elapsed seconds since 1 Jan 1970. + // + days = 0; + nanoseconds = 0; + + for (uint16_t yearIndex = 1970; yearIndex < ref_sbg_utc_msg.year; yearIndex++) + { + days += sbg::helpers::getNumberOfDaysInYear(yearIndex); + } + + for (uint8_t monthIndex = 1; monthIndex < ref_sbg_utc_msg.month; monthIndex++) + { + days += sbg::helpers::getNumberOfDaysInMonth(ref_sbg_utc_msg.year, monthIndex); + } + + days += ref_sbg_utc_msg.day - 1; + + nanoseconds = days * 24; + nanoseconds = (nanoseconds + ref_sbg_utc_msg.hour) * 60; + nanoseconds = (nanoseconds + ref_sbg_utc_msg.min) * 60; + nanoseconds = nanoseconds + ref_sbg_utc_msg.sec; + nanoseconds = nanoseconds * 1000000000 + ref_sbg_utc_msg.nanosec; + + utc_to_epoch = rclcpp::Time(nanoseconds); + + return utc_to_epoch; +} + const sbg_driver::msg::SbgEkfStatus MessageWrapper::createEkfStatusMessage(uint32_t ekf_status) const { sbg_driver::msg::SbgEkfStatus ekf_status_message; @@ -286,103 +284,6 @@ const sbg_driver::msg::SbgUtcTimeStatus MessageWrapper::createUtcStatusMessage(c return utc_status_message; } -uint32_t MessageWrapper::getNumberOfDaysInYear(uint16_t year) const -{ - if (isLeapYear(year)) - { - return 366; - } - else - { - return 365; - } -} - -uint32_t MessageWrapper::getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) const -{ - if ((month_index == 4) || (month_index == 6) || (month_index == 9) || (month_index == 11)) - { - return 30; - } - else if ((month_index == 2)) - { - if (isLeapYear(year)) - { - return 29; - } - else - { - return 28; - } - } - else - { - return 31; - } -} - -bool MessageWrapper::isLeapYear(uint16_t year) const -{ - return ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0); -} - -const rclcpp::Time MessageWrapper::convertUtcTimeToUnix(const sbg_driver::msg::SbgUtcTime& ref_sbg_utc_msg) const -{ - rclcpp::Time utc_to_epoch; - uint32_t days; - uint64_t nanoseconds; - - // - // Convert the UTC time to Epoch(Unix) time, which is the elapsed seconds since 1 Jan 1970. - // - days = 0; - nanoseconds = 0; - - for (uint16_t yearIndex = 1970; yearIndex < ref_sbg_utc_msg.year; yearIndex++) - { - days += getNumberOfDaysInYear(yearIndex); - } - - for (uint8_t monthIndex = 1; monthIndex < ref_sbg_utc_msg.month; monthIndex++) - { - days += getNumberOfDaysInMonth(ref_sbg_utc_msg.year, monthIndex); - } - - days += ref_sbg_utc_msg.day - 1; - - nanoseconds = days * 24; - nanoseconds = (nanoseconds + ref_sbg_utc_msg.hour) * 60; - nanoseconds = (nanoseconds + ref_sbg_utc_msg.min) * 60; - nanoseconds = nanoseconds + ref_sbg_utc_msg.sec; - nanoseconds = nanoseconds * 1000000000 + ref_sbg_utc_msg.nanosec; - - utc_to_epoch = rclcpp::Time(nanoseconds); - - return utc_to_epoch; -} - -int32_t MessageWrapper::getUtcOffset() const -{ - int32_t utcOffset; - - if (first_valid_utc_) - { - // Compute the leap second: GPS = UTC + utcOffset - utcOffset = (last_sbg_utc_.gps_tow/1000)%60 - last_sbg_utc_.sec; - - if (utcOffset < 0) - { - utcOffset = 60 + utcOffset; - } - } - else - { - utcOffset = DEFAULT_UTC_OFFSET; - } - - return utcOffset; -} - const sbg_driver::msg::SbgAirDataStatus MessageWrapper::createAirDataStatusMessage(const SbgLogAirData& ref_sbg_air_data) const { sbg_driver::msg::SbgAirDataStatus air_data_status_message; @@ -397,45 +298,6 @@ const sbg_driver::msg::SbgAirDataStatus MessageWrapper::createAirDataStatusMessa return air_data_status_message; } -sbg::MessageWrapper::NmeaGGAQuality MessageWrapper::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type) -{ - sbg::MessageWrapper::NmeaGGAQuality nmeaQuality = NmeaGGAQuality::INVALID; - - switch (sbg_gps_type) - { - case SBG_ECOM_POS_NO_SOLUTION: - nmeaQuality = NmeaGGAQuality::INVALID; - break; - - case SBG_ECOM_POS_UNKNOWN_TYPE: - case SBG_ECOM_POS_SINGLE: - case SBG_ECOM_POS_FIXED: - nmeaQuality = NmeaGGAQuality::SINGLE; - break; - - case SBG_ECOM_POS_PSRDIFF: - case SBG_ECOM_POS_SBAS: - case SBG_ECOM_POS_OMNISTAR: - nmeaQuality = NmeaGGAQuality::DGPS; - break; - - case SBG_ECOM_POS_PPP_FLOAT: - case SBG_ECOM_POS_PPP_INT: - nmeaQuality = NmeaGGAQuality::PPS; - break; - - case SBG_ECOM_POS_RTK_INT: - nmeaQuality = NmeaGGAQuality::RTK_FIXED; - break; - - case SBG_ECOM_POS_RTK_FLOAT: - nmeaQuality = NmeaGGAQuality::RTK_FLOAT; - break; - } - - return nmeaQuality; -} - //---------------------------------------------------------------------// //- Parameters -// //---------------------------------------------------------------------// @@ -496,7 +358,7 @@ const sbg_driver::msg::SbgEkfEuler MessageWrapper::createSbgEkfEulerMessage(cons { ekf_euler_message.angle.x = ref_log_ekf_euler.euler[0]; ekf_euler_message.angle.y = -ref_log_ekf_euler.euler[1]; - ekf_euler_message.angle.z = -wrapAnglePi(-(SBG_PI_F / 2.0f) + ref_log_ekf_euler.euler[2]); + ekf_euler_message.angle.z = -sbg::helpers::wrapAnglePi(-(SBG_PI_F / 2.0f) + ref_log_ekf_euler.euler[2]); } else { @@ -627,7 +489,7 @@ const sbg_driver::msg::SbgGpsHdt MessageWrapper::createSbgGpsHdtMessage(const Sb if (use_enu_) { - gps_hdt_message.true_heading = wrapAngle360(90.0f - ref_log_gps_hdt.heading); + gps_hdt_message.true_heading = sbg::helpers::wrapAngle360(90.0f - ref_log_gps_hdt.heading); gps_hdt_message.pitch = -ref_log_gps_hdt.pitch; } else @@ -702,7 +564,7 @@ const sbg_driver::msg::SbgGpsVel MessageWrapper::createSbgGpsVelMessage(const Sb gps_vel_message.velocity_accuracy.y = ref_log_gps_vel.velocityAcc[0]; gps_vel_message.velocity_accuracy.z = ref_log_gps_vel.velocityAcc[2]; - gps_vel_message.course = wrapAngle360(90.0f - ref_log_gps_vel.course); + gps_vel_message.course = sbg::helpers::wrapAngle360(90.0f - ref_log_gps_vel.course); } else { @@ -1050,7 +912,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(computeMeridian(utm_.getZone())); + double central_meridian = sbgDegToRadD(sbg::helpers::computeMeridian(utm_.getZone())); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. @@ -1259,7 +1121,7 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons gps_tow_ms = ref_log_gps_pos.timeOfWeek + (24ul * 3600ul * 1000ul); // Apply the GPS to UTC leap second offset - gps_tow_ms = gps_tow_ms - getUtcOffset() * 1000ul; + gps_tow_ms = gps_tow_ms - (sbg::helpers::getUtcOffset(first_valid_utc_, last_sbg_utc_.gps_tow, last_sbg_utc_.sec) * 1000ul); // Extract UTC hours/mins/seconds values utc_hour = (gps_tow_ms / (3600 * 1000)) % 24; @@ -1273,16 +1135,16 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons (utc_ms < 100 || utc_ms > 900) ) { // Latitude conversion - float latitude = std::clamp(static_cast(ref_log_gps_pos.latitude), -90.0f, 90.0f); + float latitude = std::clamp(static_cast(ref_log_gps_pos.latitude), -90.0f, 90.0f); float latitude_abs = std::fabs(latitude); - int32_t lat_degs = static_cast(latitude_abs); - float lat_mins = (latitude_abs - static_cast(lat_degs)) * 60.0f; + int32_t lat_degs = static_cast(latitude_abs); + float lat_mins = (latitude_abs - static_cast(lat_degs)) * 60.0f; // Longitude conversion - float longitude = std::clamp(static_cast(ref_log_gps_pos.longitude), -180.0f, 180.0f); + float longitude = std::clamp(static_cast(ref_log_gps_pos.longitude), -180.0f, 180.0f); float longitude_abs = std::fabs(longitude); - int32_t lon_degs = static_cast(longitude_abs); - float lon_mins = (longitude_abs - static_cast(lon_degs)) * 60.0f; + int32_t lon_degs = static_cast(longitude_abs); + float lon_mins = (longitude_abs - static_cast(lon_degs)) * 60.0f; // Compute and clamp each parameter float h_dop = std::clamp(std::hypot(ref_log_gps_pos.latitudeAccuracy, ref_log_gps_pos.longitudeAccuracy), 0.0f, 9.9f); @@ -1307,7 +1169,7 @@ const nmea_msgs::msg::Sentence MessageWrapper::createNmeaGGAMessageForNtrip(cons lon_degs, lon_mins, (longitude < 0.0f?'W':'E'), - static_cast(convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status))), + static_cast(sbg::helpers::convertSbgGpsTypeToNmeaGpsType(sbgEComLogGpsPosGetType(ref_log_gps_pos.status))), sv_used, h_dop, altitude, diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp index 013c085..5897c55 100644 --- a/src/sbg_ros_helpers.cpp +++ b/src/sbg_ros_helpers.cpp @@ -1,7 +1,10 @@ // File header #include "sbg_ros_helpers.h" -// STL includes +// SBG headers +#include + +// STL headers #include /* @@ -17,58 +20,58 @@ */ void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) { - const double RADIANS_PER_DEGREE = M_PI/180.0; + constexpr double RADIANS_PER_DEGREE = M_PI/180.0; - // WGS84 Parameters - const double WGS84_A = 6378137.0; // major axis - const double WGS84_E = 0.0818191908; // first eccentricity + // WGS84 Parameters + constexpr double WGS84_A = 6378137.0; // major axis + constexpr double WGS84_E = 0.0818191908; // first eccentricity - // UTM Parameters - const double UTM_K0 = 0.9996; // scale factor - const double UTM_E2 = (WGS84_E*WGS84_E); // e^2 + // UTM Parameters + constexpr double UTM_K0 = 0.9996; // scale factor + constexpr double UTM_E2 = (WGS84_E*WGS84_E); // e^2 - double a = WGS84_A; - double eccSquared = UTM_E2; - double k0 = UTM_K0; + double a = WGS84_A; + double eccSquared = UTM_E2; + double k0 = UTM_K0; - double LongOrigin; - double eccPrimeSquared; - double N, T, C, A, M; + double LongOrigin; + double eccPrimeSquared; + double N, T, C, A, M; - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - double LatRad = latitude * RADIANS_PER_DEGREE; - double LongRad = LongTemp*RADIANS_PER_DEGREE; - double LongOriginRad; + double LatRad = latitude * RADIANS_PER_DEGREE; + double LongRad = LongTemp*RADIANS_PER_DEGREE; + double LongOriginRad; - // +3 puts origin in middle of zone - LongOrigin = (zone_number - 1) * 6 - 180 + 3; - LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; + // +3 puts origin in middle of zone + LongOrigin = (zone_number - 1) * 6 - 180 + 3; + LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; - eccPrimeSquared = (eccSquared)/(1-eccSquared); + eccPrimeSquared = (eccSquared)/(1-eccSquared); - N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); - T = tan(LatRad)*tan(LatRad); - C = eccPrimeSquared*cos(LatRad)*cos(LatRad); - A = cos(LatRad)*(LongRad-LongOriginRad); + N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = eccPrimeSquared*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); - M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad - - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) - + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) - - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); + M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad + - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) + + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) + - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); - utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); + utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); - utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); - if(latitude < 0) - { - utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere - } + if(latitude < 0) + { + utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere + } } /** @@ -105,4 +108,143 @@ char sbg::helpers::UTMLetterDesignator(double latitude) // 'Z' is an error flag, the Latitude is outside the UTM limits else LetterDesignator = 'Z'; return LetterDesignator; +} + +float sbg::helpers::wrapAnglePi(float angle_rad) +{ + if (angle_rad > SBG_PI_F) + { + return (SBG_PI_F * 2.0f - fmodf(angle_rad, SBG_PI_F * 2.0f)); + } + + if (angle_rad < -SBG_PI_F) + { + return (SBG_PI_F * 2.0f + fmodf(angle_rad, SBG_PI_F * 2.0f)); + } + + return angle_rad; +} + +float sbg::helpers::wrapAngle360(float angle_deg) +{ + float wrapped_angle_deg = angle_deg; + + if ( (wrapped_angle_deg < -360.0f) || (wrapped_angle_deg > 360.0f) ) + { + wrapped_angle_deg = fmodf(wrapped_angle_deg, 360.0f); + } + + if (wrapped_angle_deg < 0.0f) + { + wrapped_angle_deg = 360.0f + wrapped_angle_deg; + } + + return wrapped_angle_deg; +} + +double sbg::helpers::computeMeridian(int zone_number) +{ + return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; +} + +uint32_t sbg::helpers::getNumberOfDaysInYear(uint16_t year) +{ + if (isLeapYear(year)) + { + return 366; + } + else + { + return 365; + } +} + +uint32_t sbg::helpers::getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) +{ + if ((month_index == 4) || (month_index == 6) || (month_index == 9) || (month_index == 11)) + { + return 30; + } + else if ((month_index == 2)) + { + if (isLeapYear(year)) + { + return 29; + } + else + { + return 28; + } + } + else + { + return 31; + } +} + +bool sbg::helpers::isLeapYear(uint16_t year) +{ + return ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0); +} + +int32_t sbg::helpers::getUtcOffset(bool first_valid_utc, uint32_t gps_tow, uint8_t sec) +{ + constexpr int32_t DEFAULT_UTC_OFFSET = 18; /*!< Driver default GPS to UTC offset in seconds. */ + int32_t utcOffset; + + if (first_valid_utc) + { + // Compute the leap second: GPS = UTC + utcOffset + utcOffset = (gps_tow/1000)%60 - sec; + + if (utcOffset < 0) + { + utcOffset = 60 + utcOffset; + } + } + else + { + utcOffset = DEFAULT_UTC_OFFSET; + } + + return utcOffset; +} + +sbg::helpers::NmeaGGAQuality sbg::helpers::convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type) +{ + sbg::helpers::NmeaGGAQuality nmeaQuality = NmeaGGAQuality::INVALID; + + switch (sbg_gps_type) + { + case SBG_ECOM_POS_NO_SOLUTION: + nmeaQuality = NmeaGGAQuality::INVALID; + break; + + case SBG_ECOM_POS_UNKNOWN_TYPE: + case SBG_ECOM_POS_SINGLE: + case SBG_ECOM_POS_FIXED: + nmeaQuality = NmeaGGAQuality::SINGLE; + break; + + case SBG_ECOM_POS_PSRDIFF: + case SBG_ECOM_POS_SBAS: + case SBG_ECOM_POS_OMNISTAR: + nmeaQuality = NmeaGGAQuality::DGPS; + break; + + case SBG_ECOM_POS_PPP_FLOAT: + case SBG_ECOM_POS_PPP_INT: + nmeaQuality = NmeaGGAQuality::PPS; + break; + + case SBG_ECOM_POS_RTK_INT: + nmeaQuality = NmeaGGAQuality::RTK_FIXED; + break; + + case SBG_ECOM_POS_RTK_FLOAT: + nmeaQuality = NmeaGGAQuality::RTK_FLOAT; + break; + } + + return nmeaQuality; } \ No newline at end of file From 28453b5e07ecaa61785aed9649dd3e23194db10e Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 13:53:08 +0200 Subject: [PATCH 49/74] Removed sendTransform in fillTransform method --- src/message_wrapper.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 0dabc74..2b04131 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -848,8 +848,6 @@ void MessageWrapper::fillTransform(const std::string &ref_parent_frame_id, const refTransformStamped.transform.rotation.y = ref_pose.orientation.y; refTransformStamped.transform.rotation.z = ref_pose.orientation.z; refTransformStamped.transform.rotation.w = ref_pose.orientation.w; - - tf_broadcaster_->sendTransform(refTransformStamped); } const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driver::msg::SbgImuData &ref_sbg_imu_msg, const sbg_driver::msg::SbgEkfNav &ref_ekf_nav_msg, const sbg_driver::msg::SbgEkfQuat &ref_ekf_quat_msg, const sbg_driver::msg::SbgEkfEuler &ref_ekf_euler_msg) @@ -900,6 +898,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv pose.position.z = utm_.getAltitude(); fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); + tf_broadcaster_->sendTransform(transform); static_tf_broadcaster_->sendTransform(transform); } } From 4f0f39804729eb02b84b70aa422ee27b39531e05 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 14:49:02 +0200 Subject: [PATCH 50/74] Reworked createRosPointStampedMessage computations --- include/sbg_driver/message_wrapper.h | 6 ++++++ src/message_wrapper.cpp | 18 +++++++----------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 87b2aae..199dc28 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -100,6 +100,12 @@ class MessageWrapper : public rclcpp::Node std::string odom_init_frame_id_; sbg::utm utm_{}; + + static constexpr double equatorial_radius_ = 6378137.0; + static constexpr double polar_radius_ = 6356752.314245; + double point_stamped_compute_cte_{}; + double eccentricity_{}; + //---------------------------------------------------------------------// //- Internal methods -// //---------------------------------------------------------------------// diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 2b04131..e0edba6 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -29,6 +29,9 @@ Node("tf_broadcaster") first_valid_utc_ = false; tf_broadcaster_ = std::make_shared(this); static_tf_broadcaster_ = std::make_shared(this); + + point_stamped_compute_cte_ = pow(polar_radius_, 2) / pow(equatorial_radius_, 2); + eccentricity_ = 1.0 - point_stamped_compute_cte_; } //---------------------------------------------------------------------// @@ -1018,24 +1021,17 @@ const geometry_msgs::msg::PointStamped MessageWrapper::createRosPointStampedMess // Conversion from Geodetic coordinates to ECEF is based on World Geodetic System 1984 (WGS84). // Radius are expressed in meters, and latitude/longitude in radian. // - double equatorial_radius; - double polar_radius; double prime_vertical_radius; - double eccentricity; double latitude; double longitude; - equatorial_radius = 6378137.0; - polar_radius = 6356752.314245; - eccentricity = 1 - pow(polar_radius, 2) / pow(equatorial_radius, 2); - latitude = sbgDegToRadD(ref_sbg_ekf_msg.latitude); - longitude = sbgDegToRadD(ref_sbg_ekf_msg.longitude); - - prime_vertical_radius = equatorial_radius / sqrt(1.0 - pow(eccentricity, 2) * pow(sin(latitude), 2)); + latitude = sbgDegToRadD(ref_sbg_ekf_msg.latitude); + longitude = sbgDegToRadD(ref_sbg_ekf_msg.longitude); + prime_vertical_radius = equatorial_radius_ / sqrt(1.0 - (pow(eccentricity_, 2) * pow(sin(latitude), 2))); point_stamped_message.point.x = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) * cos(latitude) * cos(longitude); point_stamped_message.point.y = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) * cos(latitude) * sin(longitude); - point_stamped_message.point.z = ((pow(polar_radius, 2) / pow(equatorial_radius, 2)) * prime_vertical_radius + ref_sbg_ekf_msg.altitude) * sin(latitude); + point_stamped_message.point.z = ((point_stamped_compute_cte_ * prime_vertical_radius) + ref_sbg_ekf_msg.altitude) * sin(latitude); return point_stamped_message; } From e4417a0bebc0259550989640f57bf017bc43e263 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 15:13:12 +0200 Subject: [PATCH 51/74] Using pow instead of multiply --- src/message_wrapper.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index e0edba6..7033e12 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -822,9 +822,9 @@ const sensor_msgs::msg::Imu MessageWrapper::createRosImuMessage(const sbg_driver imu_ros_message.angular_velocity = ref_sbg_imu_msg.delta_angle; imu_ros_message.linear_acceleration = ref_sbg_imu_msg.delta_vel; - imu_ros_message.orientation_covariance[0] = ref_sbg_quat_msg.accuracy.x * ref_sbg_quat_msg.accuracy.x; - imu_ros_message.orientation_covariance[4] = ref_sbg_quat_msg.accuracy.y * ref_sbg_quat_msg.accuracy.y; - imu_ros_message.orientation_covariance[8] = ref_sbg_quat_msg.accuracy.z * ref_sbg_quat_msg.accuracy.z; + imu_ros_message.orientation_covariance[0] = pow(ref_sbg_quat_msg.accuracy.x, 2); + imu_ros_message.orientation_covariance[4] = pow(ref_sbg_quat_msg.accuracy.y, 2); + imu_ros_message.orientation_covariance[8] = pow(ref_sbg_quat_msg.accuracy.z, 2); // // Angular velocity and linear acceleration covariances are not provided. @@ -926,9 +926,9 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv odo_ros_msg.pose.covariance[0*6 + 0] = std_x * std_x; odo_ros_msg.pose.covariance[1*6 + 1] = std_y * std_y; odo_ros_msg.pose.covariance[2*6 + 2] = std_z * std_z; - odo_ros_msg.pose.covariance[3*6 + 3] = ref_ekf_euler_msg.accuracy.x * ref_ekf_euler_msg.accuracy.x; - odo_ros_msg.pose.covariance[4*6 + 4] = ref_ekf_euler_msg.accuracy.y * ref_ekf_euler_msg.accuracy.y; - odo_ros_msg.pose.covariance[5*6 + 5] = ref_ekf_euler_msg.accuracy.z * ref_ekf_euler_msg.accuracy.z; + odo_ros_msg.pose.covariance[3*6 + 3] = pow(ref_ekf_euler_msg.accuracy.x, 2); + odo_ros_msg.pose.covariance[4*6 + 4] = pow(ref_ekf_euler_msg.accuracy.y, 2); + odo_ros_msg.pose.covariance[5*6 + 5] = pow(ref_ekf_euler_msg.accuracy.z, 2); // The twist message gives the linear and angular velocity relative to the frame defined in child_frame_id odo_ros_msg.child_frame_id = frame_id_; @@ -938,9 +938,9 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv odo_ros_msg.twist.twist.angular.x = ref_sbg_imu_msg.gyro.x; odo_ros_msg.twist.twist.angular.y = ref_sbg_imu_msg.gyro.y; odo_ros_msg.twist.twist.angular.z = ref_sbg_imu_msg.gyro.z; - odo_ros_msg.twist.covariance[0*6 + 0] = ref_ekf_nav_msg.velocity_accuracy.x * ref_ekf_nav_msg.velocity_accuracy.x; - odo_ros_msg.twist.covariance[1*6 + 1] = ref_ekf_nav_msg.velocity_accuracy.y * ref_ekf_nav_msg.velocity_accuracy.y; - odo_ros_msg.twist.covariance[2*6 + 2] = ref_ekf_nav_msg.velocity_accuracy.z * ref_ekf_nav_msg.velocity_accuracy.z; + odo_ros_msg.twist.covariance[0*6 + 0] = pow(ref_ekf_nav_msg.velocity_accuracy.x, 2); + odo_ros_msg.twist.covariance[1*6 + 1] = pow(ref_ekf_nav_msg.velocity_accuracy.y, 2); + odo_ros_msg.twist.covariance[2*6 + 2] = pow(ref_ekf_nav_msg.velocity_accuracy.z, 2); odo_ros_msg.twist.covariance[3*6 + 3] = 0; odo_ros_msg.twist.covariance[4*6 + 4] = 0; odo_ros_msg.twist.covariance[5*6 + 5] = 0; @@ -1083,9 +1083,9 @@ const sensor_msgs::msg::NavSatFix MessageWrapper::createRosNavSatFixMessage(cons nav_sat_fix_message.longitude = ref_sbg_gps_msg.longitude; nav_sat_fix_message.altitude = ref_sbg_gps_msg.altitude + ref_sbg_gps_msg.undulation; - nav_sat_fix_message.position_covariance[0] = ref_sbg_gps_msg.position_accuracy.x * ref_sbg_gps_msg.position_accuracy.x; - nav_sat_fix_message.position_covariance[4] = ref_sbg_gps_msg.position_accuracy.y * ref_sbg_gps_msg.position_accuracy.y; - nav_sat_fix_message.position_covariance[8] = ref_sbg_gps_msg.position_accuracy.z * ref_sbg_gps_msg.position_accuracy.z; + nav_sat_fix_message.position_covariance[0] = pow(ref_sbg_gps_msg.position_accuracy.x, 2); + nav_sat_fix_message.position_covariance[4] = pow(ref_sbg_gps_msg.position_accuracy.y, 2); + nav_sat_fix_message.position_covariance[8] = pow(ref_sbg_gps_msg.position_accuracy.z, 2); nav_sat_fix_message.position_covariance_type = nav_sat_fix_message.COVARIANCE_TYPE_DIAGONAL_KNOWN; From bb8dbdd66a42917d13c316c64a550b85a836a8a0 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 15:32:29 +0200 Subject: [PATCH 52/74] Added constexpr to some variables --- include/sbg_driver/sbg_ros_helpers.h | 2 +- src/sbg_ros_helpers.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h index ee743d7..205fadd 100644 --- a/include/sbg_driver/sbg_ros_helpers.h +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -1,5 +1,5 @@ /*! -* \file sbg_helpers.h +* \file sbg_ros_helpers.h * \author SBG Systems * \date 25/08/2023 * diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp index 5897c55..388370e 100644 --- a/src/sbg_ros_helpers.cpp +++ b/src/sbg_ros_helpers.cpp @@ -28,11 +28,11 @@ void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, d // UTM Parameters constexpr double UTM_K0 = 0.9996; // scale factor - constexpr double UTM_E2 = (WGS84_E*WGS84_E); // e^2 + constexpr double UTM_E2 = (WGS84_E * WGS84_E); // e^2 - double a = WGS84_A; - double eccSquared = UTM_E2; - double k0 = UTM_K0; + constexpr double a = WGS84_A; + constexpr double eccSquared = UTM_E2; + constexpr double k0 = UTM_K0; double LongOrigin; double eccPrimeSquared; @@ -42,7 +42,7 @@ void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, d double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; double LatRad = latitude * RADIANS_PER_DEGREE; - double LongRad = LongTemp*RADIANS_PER_DEGREE; + double LongRad = LongTemp * RADIANS_PER_DEGREE; double LongOriginRad; // +3 puts origin in middle of zone From 196aa297490ddc3b24ed5076cd2106ebd67b05cb Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 15:36:53 +0200 Subject: [PATCH 53/74] Using fma for computation --- src/message_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 7033e12..2bbe4f1 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1031,7 +1031,7 @@ const geometry_msgs::msg::PointStamped MessageWrapper::createRosPointStampedMess point_stamped_message.point.x = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) * cos(latitude) * cos(longitude); point_stamped_message.point.y = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) * cos(latitude) * sin(longitude); - point_stamped_message.point.z = ((point_stamped_compute_cte_ * prime_vertical_radius) + ref_sbg_ekf_msg.altitude) * sin(latitude); + point_stamped_message.point.z = fma(point_stamped_compute_cte_, prime_vertical_radius, ref_sbg_ekf_msg.altitude) * sin(latitude); return point_stamped_message; } From 6afffc4ba385fcf4d64be3503a5c71ed53fd6fe2 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 28 Aug 2023 16:22:54 +0200 Subject: [PATCH 54/74] SbgUtm documentation --- include/sbg_driver/message_wrapper.h | 2 +- include/sbg_driver/sbg_utm.h | 62 +++++++++++++--------- src/message_wrapper.cpp | 8 +-- src/sbg_utm.cpp | 78 +++++++++++++++------------- 4 files changed, 86 insertions(+), 64 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 199dc28..8adab84 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -99,7 +99,7 @@ class MessageWrapper : public rclcpp::Node std::string odom_base_frame_id_; std::string odom_init_frame_id_; - sbg::utm utm_{}; + sbg::SbgUtm utm_{}; static constexpr double equatorial_radius_ = 6378137.0; static constexpr double polar_radius_ = 6356752.314245; diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index ab35af4..a08e708 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -37,54 +37,68 @@ namespace sbg { -class utm final +/*! + * Class to wrap UTM computation. + */ +class SbgUtm final { public: - utm() = default; - ~utm() = default; + //---------------------------------------------------------------------// + //- Constructor -// + //---------------------------------------------------------------------// - /*! - * Set UTM initial position. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \param[in] altitude Altitude, in meters. - */ - void init(double latitude, double longitude, double altitude); + SbgUtm() = default; + + //---------------------------------------------------------------------// + //- Parameters -// + //---------------------------------------------------------------------// /*! - * Get odom publish_tf. + * Get UTM easting. * - * \return If true publish odometry transforms. + * \return Easting, in degrees. */ double getEasting() const; /*! - * Get odom publish_tf. + * Get UTM northing. * - * \return If true publish odometry transforms. + * \return Northing, in degrees. */ double getNorthing() const; /*! - * Get odom publish_tf. + * Get UTM altitude. * - * \return If true publish odometry transforms. + * \return Altitude, in meters. */ double getAltitude() const; /*! - * Get odom publish_tf. + * Get UTM zone. * - * \return If true publish odometry transforms. + * \return Zone number. */ - int getZone() const; + int getZoneNumber() const; + + //---------------------------------------------------------------------// + //- Operations -// + //---------------------------------------------------------------------// + + /*! + * Set UTM initial position. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \param[in] altitude Altitude, in meters. + */ + void init(double latitude, double longitude, double altitude); private: - double easting_ = 0.0; - double northing_ = 0.0; - double altitude_ = 0.0; - int zone_ = 0; + double easting_ = 0.0; + double northing_ = 0.0; + double altitude_ = 0.0; + int zone_ = 0; }; } diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 2bbe4f1..9f73e76 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -883,11 +883,11 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); // Convert latitude and longitude to UTM coordinates. - if (utm_.getZone() == 0) + if (utm_.getZoneNumber() == 0) { utm_.init(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" - , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZone(), sbg::helpers::UTMLetterDesignator(ref_ekf_nav_msg.latitude) + , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZoneNumber(), sbg::helpers::UTMLetterDesignator(ref_ekf_nav_msg.latitude) , utm_.getEasting(), (int)(utm_.getEasting())/1000 , utm_.getNorthing(), (int)(utm_.getNorthing())/1000 ); @@ -906,7 +906,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv } } - sbg::helpers::LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZone(), utm_northing, utm_easting); + sbg::helpers::LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZoneNumber(), utm_northing, utm_easting); odo_ros_msg.pose.pose.position.x = utm_easting - utm_.getEasting(); odo_ros_msg.pose.pose.position.y = utm_northing - utm_.getNorthing(); odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - utm_.getAltitude(); @@ -914,7 +914,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(sbg::helpers::computeMeridian(utm_.getZone())); + double central_meridian = sbgDegToRadD(sbg::helpers::computeMeridian(utm_.getZoneNumber())); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index 0e0cc29..297e2d5 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -4,52 +4,60 @@ // Project headers #include -using sbg::utm; +using sbg::SbgUtm; -void utm::init(double latitude, double longitude, double altitude) +//---------------------------------------------------------------------// +//- Parameters -// +//---------------------------------------------------------------------// + +double SbgUtm::getEasting() const { - int zoneNumber; - - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - - zoneNumber = int((LongTemp + 180)/6) + 1; - - if(latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) - { - zoneNumber = 32; - } - - // Special zones for Svalbard - if(latitude >= 72.0 && latitude < 84.0 ) - { - if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; - else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; - else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; - else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; - } - - zone_ = zoneNumber; - altitude_ = altitude; - sbg::helpers::LLtoUTM(latitude, longitude, zone_, northing_, easting_); + return easting_; } -double utm::getEasting() const +double SbgUtm::getNorthing() const { - return easting_; + return northing_; } -double utm::getNorthing() const +double SbgUtm::getAltitude() const { - return northing_; + return altitude_; } -double utm::getAltitude() const +int SbgUtm::getZoneNumber() const { - return altitude_; + return zone_; } -int utm::getZone() const +//---------------------------------------------------------------------// +//- Operations -// +//---------------------------------------------------------------------// + +void SbgUtm::init(double latitude, double longitude, double altitude) { - return zone_; -} + int zoneNumber; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + + zoneNumber = int((LongTemp + 180)/6) + 1; + + if( latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + { + zoneNumber = 32; + } + + // Special zones for Svalbard + if( latitude >= 72.0 && latitude < 84.0 ) + { + if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; + else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; + else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; + else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; + } + + zone_ = zoneNumber; + altitude_ = altitude; + sbg::helpers::LLtoUTM(latitude, longitude, zone_, northing_, easting_); +} \ No newline at end of file From de978c5aa8ebc9a0f890245553a7c66659601603 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 29 Aug 2023 11:02:08 +0200 Subject: [PATCH 55/74] Code indentation --- include/sbg_driver/sbg_ros_helpers.h | 18 +- src/message_publisher.cpp | 334 +++++++++++++-------------- src/sbg_device.cpp | 15 +- src/sbg_ros_helpers.cpp | 48 ++-- 4 files changed, 208 insertions(+), 207 deletions(-) diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h index 205fadd..333cbe5 100644 --- a/include/sbg_driver/sbg_ros_helpers.h +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -47,15 +47,15 @@ namespace sbg::helpers */ enum class NmeaGGAQuality: int32_t { - INVALID = 0, - SINGLE = 1, - DGPS = 2, - PPS = 3, - RTK_FIXED = 4, - RTK_FLOAT = 5, - DEAD_RECKONING = 6, - STATIC_POSITION = 7, - SIMULATED = 8, + INVALID = 0, + SINGLE = 1, + DGPS = 2, + PPS = 3, + RTK_FIXED = 4, + RTK_FLOAT = 5, + DEAD_RECKONING = 6, + STATIC_POSITION = 7, + SIMULATED = 8, }; /*! diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index f6fed1f..90f779d 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -22,71 +22,71 @@ std::string MessagePublisher::getOutputTopicName(SbgEComMsgId sbg_message_id) co { switch (sbg_message_id) { - case SBG_ECOM_LOG_STATUS: - return "sbg/status"; + case SBG_ECOM_LOG_STATUS: + return "sbg/status"; - case SBG_ECOM_LOG_UTC_TIME: - return "sbg/utc_time"; + case SBG_ECOM_LOG_UTC_TIME: + return "sbg/utc_time"; - case SBG_ECOM_LOG_IMU_DATA: - return "sbg/imu_data"; + case SBG_ECOM_LOG_IMU_DATA: + return "sbg/imu_data"; - case SBG_ECOM_LOG_MAG: - return "sbg/mag"; + case SBG_ECOM_LOG_MAG: + return "sbg/mag"; - case SBG_ECOM_LOG_MAG_CALIB: - return "sbg/mag_calib"; + case SBG_ECOM_LOG_MAG_CALIB: + return "sbg/mag_calib"; - case SBG_ECOM_LOG_EKF_EULER: - return "sbg/ekf_euler"; + case SBG_ECOM_LOG_EKF_EULER: + return "sbg/ekf_euler"; - case SBG_ECOM_LOG_EKF_QUAT: - return "sbg/ekf_quat"; + case SBG_ECOM_LOG_EKF_QUAT: + return "sbg/ekf_quat"; - case SBG_ECOM_LOG_EKF_NAV: - return "sbg/ekf_nav"; + case SBG_ECOM_LOG_EKF_NAV: + return "sbg/ekf_nav"; - case SBG_ECOM_LOG_SHIP_MOTION: - return "sbg/ship_motion"; + case SBG_ECOM_LOG_SHIP_MOTION: + return "sbg/ship_motion"; - case SBG_ECOM_LOG_GPS1_VEL: - return "sbg/gps_vel"; + case SBG_ECOM_LOG_GPS1_VEL: + return "sbg/gps_vel"; - case SBG_ECOM_LOG_GPS1_POS: - return "sbg/gps_pos"; + case SBG_ECOM_LOG_GPS1_POS: + return "sbg/gps_pos"; - case SBG_ECOM_LOG_GPS1_HDT: - return "sbg/gps_hdt"; + case SBG_ECOM_LOG_GPS1_HDT: + return "sbg/gps_hdt"; - case SBG_ECOM_LOG_GPS1_RAW: - return "sbg/gps_raw"; + case SBG_ECOM_LOG_GPS1_RAW: + return "sbg/gps_raw"; - case SBG_ECOM_LOG_ODO_VEL: - return "sbg/odo_vel"; + case SBG_ECOM_LOG_ODO_VEL: + return "sbg/odo_vel"; - case SBG_ECOM_LOG_EVENT_A: - return "sbg/eventA"; + case SBG_ECOM_LOG_EVENT_A: + return "sbg/eventA"; - case SBG_ECOM_LOG_EVENT_B: - return "sbg/eventB"; + case SBG_ECOM_LOG_EVENT_B: + return "sbg/eventB"; - case SBG_ECOM_LOG_EVENT_C: - return "sbg/eventC"; + case SBG_ECOM_LOG_EVENT_C: + return "sbg/eventC"; - case SBG_ECOM_LOG_EVENT_D: - return "sbg/eventD"; + case SBG_ECOM_LOG_EVENT_D: + return "sbg/eventD"; - case SBG_ECOM_LOG_EVENT_E: - return "sbg/eventE"; + case SBG_ECOM_LOG_EVENT_E: + return "sbg/eventE"; - case SBG_ECOM_LOG_AIR_DATA: - return "sbg/air_data"; + case SBG_ECOM_LOG_AIR_DATA: + return "sbg/air_data"; - case SBG_ECOM_LOG_IMU_SHORT: - return "sbg/imu_short"; + case SBG_ECOM_LOG_IMU_SHORT: + return "sbg/imu_short"; - default: - return "undefined"; + default: + return "undefined"; } } @@ -517,177 +517,177 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ { switch (sbg_msg_id) { - case SBG_ECOM_LOG_STATUS: + case SBG_ECOM_LOG_STATUS: - if (sbg_status_pub_) - { - sbg_status_pub_->publish(message_wrapper_.createSbgStatusMessage(ref_sbg_log.statusData)); - } - break; + if (sbg_status_pub_) + { + sbg_status_pub_->publish(message_wrapper_.createSbgStatusMessage(ref_sbg_log.statusData)); + } + break; - case SBG_ECOM_LOG_UTC_TIME: + case SBG_ECOM_LOG_UTC_TIME: - publishUtcData(ref_sbg_log); - break; + publishUtcData(ref_sbg_log); + break; - case SBG_ECOM_LOG_IMU_DATA: + case SBG_ECOM_LOG_IMU_DATA: - publishIMUData(ref_sbg_log); - break; + publishIMUData(ref_sbg_log); + break; - case SBG_ECOM_LOG_MAG: + case SBG_ECOM_LOG_MAG: - publishMagData(ref_sbg_log); - break; + publishMagData(ref_sbg_log); + break; - case SBG_ECOM_LOG_MAG_CALIB: + case SBG_ECOM_LOG_MAG_CALIB: - if (sbg_mag_calib_pub_) - { - sbg_mag_calib_pub_->publish(message_wrapper_.createSbgMagCalibMessage(ref_sbg_log.magCalibData)); - } - break; + if (sbg_mag_calib_pub_) + { + sbg_mag_calib_pub_->publish(message_wrapper_.createSbgMagCalibMessage(ref_sbg_log.magCalibData)); + } + break; - case SBG_ECOM_LOG_EKF_EULER: + case SBG_ECOM_LOG_EKF_EULER: - if (sbg_ekf_euler_pub_) - { - sbg_ekf_euler_message_ = message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); - sbg_ekf_euler_pub_->publish(sbg_ekf_euler_message_); - processRosVelMessage(); - processRosOdoMessage(); - } - break; + if (sbg_ekf_euler_pub_) + { + sbg_ekf_euler_message_ = message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); + sbg_ekf_euler_pub_->publish(sbg_ekf_euler_message_); + processRosVelMessage(); + processRosOdoMessage(); + } + break; - case SBG_ECOM_LOG_EKF_QUAT: + case SBG_ECOM_LOG_EKF_QUAT: - if (sbg_ekf_quat_pub_) - { - sbg_ekf_quat_message_ = message_wrapper_.createSbgEkfQuatMessage(ref_sbg_log.ekfQuatData); - sbg_ekf_quat_pub_->publish(sbg_ekf_quat_message_); - processRosImuMessage(); - processRosVelMessage(); - } - break; + if (sbg_ekf_quat_pub_) + { + sbg_ekf_quat_message_ = message_wrapper_.createSbgEkfQuatMessage(ref_sbg_log.ekfQuatData); + sbg_ekf_quat_pub_->publish(sbg_ekf_quat_message_); + processRosImuMessage(); + processRosVelMessage(); + } + break; - case SBG_ECOM_LOG_EKF_NAV: + case SBG_ECOM_LOG_EKF_NAV: - publishEkfNavigationData(ref_sbg_log); - processRosOdoMessage(); - break; + publishEkfNavigationData(ref_sbg_log); + processRosOdoMessage(); + break; - case SBG_ECOM_LOG_SHIP_MOTION: + case SBG_ECOM_LOG_SHIP_MOTION: - if (sbg_ship_motion_pub_) - { - sbg_ship_motion_pub_->publish(message_wrapper_.createSbgShipMotionMessage(ref_sbg_log.shipMotionData)); - } - break; + if (sbg_ship_motion_pub_) + { + sbg_ship_motion_pub_->publish(message_wrapper_.createSbgShipMotionMessage(ref_sbg_log.shipMotionData)); + } + break; - case SBG_ECOM_LOG_GPS1_VEL: - case SBG_ECOM_LOG_GPS2_VEL: + case SBG_ECOM_LOG_GPS1_VEL: + case SBG_ECOM_LOG_GPS2_VEL: - if (sbg_gps_vel_pub_) - { - sbg_gps_vel_pub_->publish(message_wrapper_.createSbgGpsVelMessage(ref_sbg_log.gpsVelData)); - } - break; + if (sbg_gps_vel_pub_) + { + sbg_gps_vel_pub_->publish(message_wrapper_.createSbgGpsVelMessage(ref_sbg_log.gpsVelData)); + } + break; - case SBG_ECOM_LOG_GPS1_POS: - case SBG_ECOM_LOG_GPS2_POS: + case SBG_ECOM_LOG_GPS1_POS: + case SBG_ECOM_LOG_GPS2_POS: - publishGpsPosData(ref_sbg_log, sbg_msg_id); - break; + publishGpsPosData(ref_sbg_log, sbg_msg_id); + break; - case SBG_ECOM_LOG_GPS1_HDT: - case SBG_ECOM_LOG_GPS2_HDT: + case SBG_ECOM_LOG_GPS1_HDT: + case SBG_ECOM_LOG_GPS2_HDT: - if (sbg_gps_hdt_pub_) - { - sbg_gps_hdt_pub_->publish(message_wrapper_.createSbgGpsHdtMessage(ref_sbg_log.gpsHdtData)); - } - break; + if (sbg_gps_hdt_pub_) + { + sbg_gps_hdt_pub_->publish(message_wrapper_.createSbgGpsHdtMessage(ref_sbg_log.gpsHdtData)); + } + break; - case SBG_ECOM_LOG_GPS1_RAW: - case SBG_ECOM_LOG_GPS2_RAW: + case SBG_ECOM_LOG_GPS1_RAW: + case SBG_ECOM_LOG_GPS2_RAW: - if (sbg_gps_raw_pub_) - { - sbg_gps_raw_pub_->publish(message_wrapper_.createSbgGpsRawMessage(ref_sbg_log.gpsRawData)); - } - break; + if (sbg_gps_raw_pub_) + { + sbg_gps_raw_pub_->publish(message_wrapper_.createSbgGpsRawMessage(ref_sbg_log.gpsRawData)); + } + break; - case SBG_ECOM_LOG_ODO_VEL: + case SBG_ECOM_LOG_ODO_VEL: - if (sbg_odo_vel_pub_) - { - sbg_odo_vel_pub_->publish(message_wrapper_.createSbgOdoVelMessage(ref_sbg_log.odometerData)); - } - break; + if (sbg_odo_vel_pub_) + { + sbg_odo_vel_pub_->publish(message_wrapper_.createSbgOdoVelMessage(ref_sbg_log.odometerData)); + } + break; - case SBG_ECOM_LOG_EVENT_A: + case SBG_ECOM_LOG_EVENT_A: - if (sbg_event_a_pub_) - { - sbg_event_a_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; + if (sbg_event_a_pub_) + { + sbg_event_a_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + } + break; - case SBG_ECOM_LOG_EVENT_B: + case SBG_ECOM_LOG_EVENT_B: - if (sbg_event_b_pub_) - { - sbg_event_b_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; + if (sbg_event_b_pub_) + { + sbg_event_b_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + } + break; - case SBG_ECOM_LOG_EVENT_C: + case SBG_ECOM_LOG_EVENT_C: - if (sbg_event_c_pub_) - { - sbg_event_c_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; + if (sbg_event_c_pub_) + { + sbg_event_c_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + } + break; - case SBG_ECOM_LOG_EVENT_D: + case SBG_ECOM_LOG_EVENT_D: - if (sbg_event_d_pub_) - { - sbg_event_d_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; + if (sbg_event_d_pub_) + { + sbg_event_d_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + } + break; - case SBG_ECOM_LOG_EVENT_E: + case SBG_ECOM_LOG_EVENT_E: - if (sbg_event_e_pub_) - { - sbg_event_e_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; + if (sbg_event_e_pub_) + { + sbg_event_e_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); + } + break; - case SBG_ECOM_LOG_IMU_SHORT: + case SBG_ECOM_LOG_IMU_SHORT: - if (sbg_imu_short_pub_) - { - sbg_imu_short_pub_->publish(message_wrapper_.createSbgImuShortMessage(ref_sbg_log.imuShort)); - } - break; + if (sbg_imu_short_pub_) + { + sbg_imu_short_pub_->publish(message_wrapper_.createSbgImuShortMessage(ref_sbg_log.imuShort)); + } + break; - case SBG_ECOM_LOG_AIR_DATA: + case SBG_ECOM_LOG_AIR_DATA: - publishFluidPressureData(ref_sbg_log); - break; + publishFluidPressureData(ref_sbg_log); + break; - default: - break; + default: + break; } } else if (sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_1) { switch (sbg_msg_id) { - default: - break; + default: + break; } } } diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 1369850..66ce10a 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -21,13 +21,14 @@ using sbg::SbgDevice; // From ros_com/recorder std::string timeToStr() //rclcpp::WallTimer> ros_t) //TODO: FIXME { - //(void)ros_t; - std::stringstream msg; - const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time(); - boost::posix_time::time_facet *const f = new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S"); - msg.imbue(std::locale(msg.getloc(),f)); - msg << now; - return msg.str(); + //(void)ros_t; + std::stringstream msg; + const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time(); + boost::posix_time::time_facet *const f = new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S"); + msg.imbue(std::locale(msg.getloc(),f)); + msg << now; + + return msg.str(); } // diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp index 388370e..854a476 100644 --- a/src/sbg_ros_helpers.cpp +++ b/src/sbg_ros_helpers.cpp @@ -83,31 +83,31 @@ void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, d */ char sbg::helpers::UTMLetterDesignator(double latitude) { - char LetterDesignator; - - if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; - else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; - else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; - else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; - else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; - else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; - else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; - else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; - else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; - else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; - else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; - else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; - else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; - else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; - else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; - else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; - else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; - else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; - else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; - else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; + char LetterDesignator; + + if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; + else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; + else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; + else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; + else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; + else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; + else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; + else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; + else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; + else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; + else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; + else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; + else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; + else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; + else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; + else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; + else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; + else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; + else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; + else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; // 'Z' is an error flag, the Latitude is outside the UTM limits - else LetterDesignator = 'Z'; - return LetterDesignator; + else LetterDesignator = 'Z'; + return LetterDesignator; } float sbg::helpers::wrapAnglePi(float angle_rad) From 35a4e1a97189b1cfd8653235bdcf7c7067bf8249 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Fri, 1 Sep 2023 15:33:22 +0200 Subject: [PATCH 56/74] Fixed space / tabluations issues --- include/sbg_driver/config_applier.h | 49 ++++++++------- include/sbg_driver/config_store.h | 64 ++++++++++---------- include/sbg_driver/message_publisher.h | 84 +++++++++++++------------- include/sbg_driver/message_wrapper.h | 46 +++++++------- include/sbg_driver/sbg_device.h | 63 ++++++++++++++----- include/sbg_driver/sbg_matrix3.h | 54 ++++++++--------- include/sbg_driver/sbg_ros_helpers.h | 81 ++++++++++++------------- include/sbg_driver/sbg_utm.h | 54 ++++++++--------- include/sbg_driver/sbg_vector3.h | 41 ++++++++++++- src/config_applier.cpp | 4 +- src/config_store.cpp | 12 ++-- src/message_publisher.cpp | 38 +----------- src/sbg_device.cpp | 4 +- src/sbg_ros_helpers.cpp | 18 +++--- src/sbg_utm.cpp | 12 ++-- 15 files changed, 328 insertions(+), 296 deletions(-) diff --git a/include/sbg_driver/config_applier.h b/include/sbg_driver/config_applier.h index 0ffdb6b..b68c5b3 100644 --- a/include/sbg_driver/config_applier.h +++ b/include/sbg_driver/config_applier.h @@ -1,38 +1,37 @@ /*! -* \file config_applier.h -* \author SBG Systems -* \date 13/03/2020 +* \file config_applier.h +* \author SBG Systems +* \date 13/03/2020 * -* \brief Apply configuration to the device. +* \brief Apply configuration to the device. * -* Class takes a configuration from config_store and send all commands to the -* device to apply it. +* Class takes a configuration from config_store and send all commands to the +* device to apply it. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2020 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ - #ifndef CONFIG_APPLIER_H #define CONFIG_APPLIER_H diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index 47223b7..8f28b24 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -1,34 +1,34 @@ /*! -* \file config_store.h -* \author SBG Systems -* \date 13/03/2020 +* \file config_store.h +* \author SBG Systems +* \date 13/03/2020 * -* \brief Class to handle the device configuration. +* \brief Class to handle the device configuration. * -* Methods to extract configuration values and load it to the ROS node. +* Methods to extract configuration values and load it to the ROS node. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2020 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ #ifndef SBG_ROS_CONFIG_STORE_H @@ -66,9 +66,9 @@ class ConfigStore */ struct SbgLogOutput { - SbgEComClass message_class; - SbgEComMsgId message_id; - SbgEComOutputMode output_mode; + SbgEComClass message_class; + SbgEComMsgId message_id; + SbgEComOutputMode output_mode; }; private: @@ -113,10 +113,10 @@ class ConfigStore uint32_t rate_frequency_; std::string frame_id_; - bool use_enu_; + bool use_enu_; - bool odom_enable_; - bool odom_publish_tf_; + bool odom_enable_; + bool odom_publish_tf_; std::string odom_frame_id_; std::string odom_base_frame_id_; std::string odom_init_frame_id_; @@ -474,21 +474,21 @@ class ConfigStore /*! * Get use ENU. * - * \return True if the frame convention to use is ENU. + * \return True if the frame convention to use is ENU. */ bool getUseEnu() const; /*! * Get odom enable. * - * \return True if the odometry is enabled. + * \return True if the odometry is enabled. */ bool getOdomEnable() const; /*! * Get odom publish_tf. * - * \return If true publish odometry transforms. + * \return If true publish odometry transforms. */ bool getOdomPublishTf() const; diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index 4e3e03f..4e43746 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -1,32 +1,32 @@ /*! -* \file message_publisher.h -* \author SBG Systems -* \date 13/03/2020 +* \file message_publisher.h +* \author SBG Systems +* \date 13/03/2020 * -* \brief Manage publishing of messages from logs. +* \brief Manage publishing of messages from logs. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2020 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ #ifndef SBG_ROS_MESSAGE_PUBLISHER_H @@ -45,33 +45,33 @@ class MessagePublisher { private: - rclcpp::Publisher>::SharedPtr sbg_status_pub_; - rclcpp::Publisher>::SharedPtr sbg_utc_time_pub_; - rclcpp::Publisher>::SharedPtr sbg_imu_data_pub_; - rclcpp::Publisher>::SharedPtr sbg_ekf_euler_pub_; + rclcpp::Publisher>::SharedPtr sbg_status_pub_; + rclcpp::Publisher>::SharedPtr sbg_utc_time_pub_; + rclcpp::Publisher>::SharedPtr sbg_imu_data_pub_; + rclcpp::Publisher>::SharedPtr sbg_ekf_euler_pub_; rclcpp::Publisher>::SharedPtr sbg_ekf_quat_pub_; rclcpp::Publisher>::SharedPtr sbg_ekf_nav_pub_; rclcpp::Publisher>::SharedPtr sbg_ship_motion_pub_; - rclcpp::Publisher>::SharedPtr sbg_mag_pub_; - rclcpp::Publisher>::SharedPtr sbg_mag_calib_pub_; - rclcpp::Publisher>::SharedPtr sbg_gps_vel_pub_; - rclcpp::Publisher>::SharedPtr sbg_gps_pos_pub_; - rclcpp::Publisher>::SharedPtr sbg_gps_hdt_pub_; - rclcpp::Publisher>::SharedPtr sbg_gps_raw_pub_; - rclcpp::Publisher>::SharedPtr sbg_odo_vel_pub_; + rclcpp::Publisher>::SharedPtr sbg_mag_pub_; + rclcpp::Publisher>::SharedPtr sbg_mag_calib_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_vel_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_pos_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_hdt_pub_; + rclcpp::Publisher>::SharedPtr sbg_gps_raw_pub_; + rclcpp::Publisher>::SharedPtr sbg_odo_vel_pub_; rclcpp::Publisher>::SharedPtr sbg_event_a_pub_; rclcpp::Publisher>::SharedPtr sbg_event_b_pub_; rclcpp::Publisher>::SharedPtr sbg_event_c_pub_; rclcpp::Publisher>::SharedPtr sbg_event_d_pub_; rclcpp::Publisher>::SharedPtr sbg_event_e_pub_; - rclcpp::Publisher>::SharedPtr sbg_imu_short_pub_; - rclcpp::Publisher>::SharedPtr sbg_air_data_pub_; + rclcpp::Publisher>::SharedPtr sbg_imu_short_pub_; + rclcpp::Publisher>::SharedPtr sbg_air_data_pub_; rclcpp::Publisher>::SharedPtr imu_pub_; - sbg_driver::msg::SbgImuData sbg_imu_message_; - sbg_driver::msg::SbgEkfQuat sbg_ekf_quat_message_; - sbg_driver::msg::SbgEkfNav sbg_ekf_nav_message_; - sbg_driver::msg::SbgEkfEuler sbg_ekf_euler_message_; + sbg_driver::msg::SbgImuData sbg_imu_message_; + sbg_driver::msg::SbgEkfQuat sbg_ekf_quat_message_; + sbg_driver::msg::SbgEkfNav sbg_ekf_nav_message_; + sbg_driver::msg::SbgEkfEuler sbg_ekf_euler_message_; rclcpp::Publisher>::SharedPtr temp_pub_; rclcpp::Publisher>::SharedPtr mag_pub_; @@ -84,9 +84,9 @@ class MessagePublisher rclcpp::Publisher>::SharedPtr nmea_gga_pub_; - MessageWrapper message_wrapper_; - uint32_t max_messages_; - std::string frame_id_; + MessageWrapper message_wrapper_; + uint32_t max_messages_; + std::string frame_id_; //---------------------------------------------------------------------// //- Private methods -// diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 8adab84..90a8d01 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -1,34 +1,34 @@ /*! -* \file message_wrapper.h -* \author SBG Systems -* \date 13/03/2020 +* \file message_wrapper.h +* \author SBG Systems +* \date 13/03/2020 * -* \brief Handle creation of messages. +* \brief Handle creation of messages. * -* Methods to create ROS messages from given data. +* Methods to create ROS messages from given data. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2020 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ #ifndef SBG_ROS_MESSAGE_WRAPPER_H diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index ed0950a..7741ef4 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -1,3 +1,34 @@ +/*! +* \file sbg_device.h +* \author SBG Systems +* \date 13/03/2020 +* +* \brief Implement device connection / parsing. +* +* \section CodeCopyright Copyright Notice +* MIT License +* +* Copyright (c) 2023 SBG Systems +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ + #ifndef SBG_ROS_SBG_DEVICE_H #define SBG_ROS_SBG_DEVICE_H @@ -29,30 +60,30 @@ class SbgDevice //- Static members definition -// //---------------------------------------------------------------------// - static std::map g_mag_calib_quality_; - static std::map g_mag_calib_confidence_; - static std::map g_mag_calib_mode_; - static std::map g_mag_calib_bandwidth_; + static std::map g_mag_calib_quality_; + static std::map g_mag_calib_confidence_; + static std::map g_mag_calib_mode_; + static std::map g_mag_calib_bandwidth_; //---------------------------------------------------------------------// //- Private variables -// //---------------------------------------------------------------------// - SbgEComHandle com_handle_; - SbgInterface sbg_interface_; - rclcpp::Node& ref_node_; - MessagePublisher message_publisher_; - ConfigStore config_store_; + SbgEComHandle com_handle_; + SbgInterface sbg_interface_; + rclcpp::Node& ref_node_; + MessagePublisher message_publisher_; + ConfigStore config_store_; - uint32_t rate_frequency_; + uint32_t rate_frequency_; - bool mag_calibration_ongoing_; - bool mag_calibration_done_; - SbgEComMagCalibResults mag_calib_results_; - rclcpp::Service::SharedPtr calib_service_; - rclcpp::Service::SharedPtr calib_save_service_; + bool mag_calibration_ongoing_; + bool mag_calibration_done_; + SbgEComMagCalibResults mag_calib_results_; + rclcpp::Service::SharedPtr calib_service_; + rclcpp::Service::SharedPtr calib_save_service_; - rclcpp::Subscription::SharedPtr rtcm_sub_; + rclcpp::Subscription::SharedPtr rtcm_sub_; //---------------------------------------------------------------------// //- Private methods -// diff --git a/include/sbg_driver/sbg_matrix3.h b/include/sbg_driver/sbg_matrix3.h index beb54ff..25f118e 100644 --- a/include/sbg_driver/sbg_matrix3.h +++ b/include/sbg_driver/sbg_matrix3.h @@ -1,38 +1,38 @@ /*! -* \file sbg_matrix3.h -* \author SBG Systems -* \date 13/03/2020 +* \file sbg_matrix3.h +* \author SBG Systems +* \date 13/03/2020 * -* \brief Handle a 3x3 matrix. +* \brief Handle a 3x3 matrix. * -* SBG Systems Ros driver needs some basic matrix operations. -* Ros uses Eigen for mathematical computations but to avoid dependancy on -* Eigen we chose to implement a basic custom matrix class with basic -* mathematical operations needed. -* This class also defines SbgMatrix3f and SbgMatrix3d for floats and doubles. +* SBG Systems Ros driver needs some basic matrix operations. +* Ros uses Eigen for mathematical computations but to avoid dependency on +* Eigen we chose to implement a basic custom matrix class with basic +* mathematical operations needed. +* This class also defines SbgMatrix3f and SbgMatrix3d for floats and doubles. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2020 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ #ifndef SBG_MATRIX_3_H diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h index 333cbe5..a848889 100644 --- a/include/sbg_driver/sbg_ros_helpers.h +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -1,34 +1,34 @@ /*! -* \file sbg_ros_helpers.h -* \author SBG Systems -* \date 25/08/2023 +* \file sbg_ros_helpers.h +* \author SBG Systems +* \date 25/08/2023 * -* \brief Helpers for various tasks. +* \brief Helpers for various tasks. * -* Helpers for various tasks. +* Helpers for various tasks. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2023 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ #ifndef SBG_ROS_ROS_HELPERS_H @@ -80,49 +80,49 @@ namespace sbg::helpers /*! * Wrap an angle between [ -Pi ; Pi ] rad. * - * \param[in] angle_rad Angle in rad. - * \return Wrapped angle. + * \param[in] angle_rad Angle in rad. + * \return Wrapped angle. */ float wrapAnglePi(float angle_rad); /*! * Wrap an angle between [ 0 ; 360 ] degree. * - * \param[in] angle_deg Angle in degree. - * \return Wrapped angle. + * \param[in] angle_deg Angle in degree. + * \return Wrapped angle. */ float wrapAngle360(float angle_deg); /*! * Compute UTM zone meridian. * - * \param[in] zone_number UTM Zone number. - * \return Meridian angle, in degrees. + * \param[in] zone_number UTM Zone number. + * \return Meridian angle, in degrees. */ double computeMeridian(int zone_number); /*! * Get the number of days in the year. * - * \param[in] year Year to get the number of days. - * \return Number of days in the year. + * \param[in] year Year to get the number of days. + * \return Number of days in the year. */ uint32_t getNumberOfDaysInYear(uint16_t year); /*! * Get the number of days of the month index. * - * \param[in] year Year. - * \param[in] month_index Month index [1..12]. - * \return Number of days in the month. + * \param[in] year Year. + * \param[in] month_index Month index [1..12]. + * \return Number of days in the month. */ uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index); /*! * Check if the given year is a leap year. * - * \param[in] year Year to check. - * \return True if the year is a leap year. + * \param[in] year Year to check. + * \return True if the year is a leap year. */ bool isLeapYear(uint16_t year); @@ -132,10 +132,10 @@ namespace sbg::helpers * WARNING: The leap second is computed from the latest received SbgUtcTime message if any. * If no SbgUtcTime message has been received, a default driver current value is used. * - * \param[in] first_valid_utc First valid utc. - * \param[in] gps_tow Current GPS time of the week. - * \param[in] sec Current second. - * \return Offset in seconds to apply to UTC time to get GPS time. + * \param[in] first_valid_utc First valid utc. + * \param[in] gps_tow Current GPS time of the week. + * \param[in] sec Current second. + * \return Offset in seconds to apply to UTC time to get GPS time. */ int32_t getUtcOffset(bool first_valid_utc, uint32_t gps_tow, uint8_t sec); @@ -149,4 +149,3 @@ namespace sbg::helpers } #endif // #ifndef SBG_ROS_ROS_HELPERS_H - diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index a08e708..2c83352 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -1,34 +1,34 @@ /*! -* \file sbg_utm.h -* \author SBG Systems -* \date 25/08/2023 +* \file sbg_utm.h +* \author SBG Systems +* \date 25/08/2023 * -* \brief Handle creation of utm class. +* \brief Handle creation of utm class. * -* Methods to create UTM from given data. +* Methods to create UTM from given data. * -* \section CodeCopyright Copyright Notice -* MIT License +* \section CodeCopyright Copyright Notice +* MIT License * -* Copyright (c) 2023 SBG Systems +* Copyright (c) 2023 SBG Systems * -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: * -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. */ #ifndef SBG_ROS_UTM_H @@ -56,28 +56,28 @@ class SbgUtm final /*! * Get UTM easting. * - * \return Easting, in degrees. + * \return Easting, in degrees. */ double getEasting() const; /*! * Get UTM northing. * - * \return Northing, in degrees. + * \return Northing, in degrees. */ double getNorthing() const; /*! * Get UTM altitude. * - * \return Altitude, in meters. + * \return Altitude, in meters. */ double getAltitude() const; /*! * Get UTM zone. * - * \return Zone number. + * \return Zone number. */ int getZoneNumber() const; diff --git a/include/sbg_driver/sbg_vector3.h b/include/sbg_driver/sbg_vector3.h index 44f7418..0bc5b80 100644 --- a/include/sbg_driver/sbg_vector3.h +++ b/include/sbg_driver/sbg_vector3.h @@ -1,3 +1,40 @@ +/*! +* \file sbg_vector3.h +* \author SBG Systems +* \date 13/03/2020 +* +* \brief Handle a X,Y,Z vector. +* +* SBG Systems Ros driver needs some basic matrix operations. +* Ros uses Eigen for mathematical computations but to avoid dependency on +* Eigen we chose to implement a basic custom matrix class with basic +* mathematical operations needed. +* This class also defines SbgMatrix3f and SbgMatrix3d for floats and doubles. +* +* \section CodeCopyright Copyright Notice +* MIT License +* +* Copyright (c) 2023 SBG Systems +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ + #ifndef SBG_VECTOR_3_H #define SBG_VECTOR_3_H @@ -121,8 +158,8 @@ class SbgVector3 /*! * Getter parenthesis operator - * \param[in] index Index of value to retrieve. - * \return Value at index. + * \param[in] index Index of value to retrieve. + * \return Value at index. */ const T operator()(size_t index) const { diff --git a/src/config_applier.cpp b/src/config_applier.cpp index 64233a6..cf8eace 100644 --- a/src/config_applier.cpp +++ b/src/config_applier.cpp @@ -11,8 +11,8 @@ using sbg::ConfigApplier; //---------------------------------------------------------------------// ConfigApplier::ConfigApplier(SbgEComHandle &ref_sbg_com_handle): - reboot_needed_(false), - ref_sbg_com_handle_(ref_sbg_com_handle) +reboot_needed_(false), +ref_sbg_com_handle_(ref_sbg_com_handle) { } diff --git a/src/config_store.cpp b/src/config_store.cpp index 9b14186..8439729 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -11,12 +11,12 @@ using sbg::ConfigStore; //---------------------------------------------------------------------// ConfigStore::ConfigStore(): - serial_communication_(false), - upd_communication_(false), - configure_through_ros_(false), - ros_standard_output_(false), - rtcm_subscribe_(false), - nmea_publish_(false) +serial_communication_(false), +upd_communication_(false), +configure_through_ros_(false), +ros_standard_output_(false), +rtcm_subscribe_(false), +nmea_publish_(false) { } diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 90f779d..11df729 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -128,76 +128,62 @@ void MessagePublisher::initPublisher(rclcpp::Node& ref_ros_node_handle, SbgEComM break; case SBG_ECOM_LOG_EKF_NAV: - sbg_ekf_nav_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_SHIP_MOTION: - sbg_ship_motion_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_VEL: case SBG_ECOM_LOG_GPS2_VEL: - sbg_gps_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_POS: case SBG_ECOM_LOG_GPS2_POS: - sbg_gps_pos_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_HDT: case SBG_ECOM_LOG_GPS2_HDT: - sbg_gps_hdt_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_RAW: case SBG_ECOM_LOG_GPS2_RAW: - sbg_gps_raw_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_ODO_VEL: - sbg_odo_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_A: - sbg_event_a_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_B: - sbg_event_b_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_C: - sbg_event_c_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_D: - sbg_event_d_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_E: - sbg_event_e_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_IMU_SHORT: - sbg_imu_short_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_AIR_DATA: - sbg_air_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; @@ -226,7 +212,7 @@ void MessagePublisher::defineRosStandardPublishers(rclcpp::Node& ref_ros_node_ha if (sbg_imu_data_pub_) { - temp_pub_ = ref_ros_node_handle.create_publisher("imu/temp", max_messages_); + temp_pub_ = ref_ros_node_handle.create_publisher("imu/temp", max_messages_); } else { @@ -513,7 +499,7 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ // Publish the message with the corresponding publisher and SBG message ID. // For each log, check if the publisher has been initialized. // - if(sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_0) + if (sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_0) { switch (sbg_msg_id) { @@ -526,22 +512,18 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_UTC_TIME: - publishUtcData(ref_sbg_log); break; case SBG_ECOM_LOG_IMU_DATA: - publishIMUData(ref_sbg_log); break; case SBG_ECOM_LOG_MAG: - publishMagData(ref_sbg_log); break; case SBG_ECOM_LOG_MAG_CALIB: - if (sbg_mag_calib_pub_) { sbg_mag_calib_pub_->publish(message_wrapper_.createSbgMagCalibMessage(ref_sbg_log.magCalibData)); @@ -549,7 +531,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EKF_EULER: - if (sbg_ekf_euler_pub_) { sbg_ekf_euler_message_ = message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); @@ -560,7 +541,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EKF_QUAT: - if (sbg_ekf_quat_pub_) { sbg_ekf_quat_message_ = message_wrapper_.createSbgEkfQuatMessage(ref_sbg_log.ekfQuatData); @@ -571,13 +551,11 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EKF_NAV: - publishEkfNavigationData(ref_sbg_log); processRosOdoMessage(); break; case SBG_ECOM_LOG_SHIP_MOTION: - if (sbg_ship_motion_pub_) { sbg_ship_motion_pub_->publish(message_wrapper_.createSbgShipMotionMessage(ref_sbg_log.shipMotionData)); @@ -586,7 +564,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_GPS1_VEL: case SBG_ECOM_LOG_GPS2_VEL: - if (sbg_gps_vel_pub_) { sbg_gps_vel_pub_->publish(message_wrapper_.createSbgGpsVelMessage(ref_sbg_log.gpsVelData)); @@ -595,13 +572,11 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_GPS1_POS: case SBG_ECOM_LOG_GPS2_POS: - publishGpsPosData(ref_sbg_log, sbg_msg_id); break; case SBG_ECOM_LOG_GPS1_HDT: case SBG_ECOM_LOG_GPS2_HDT: - if (sbg_gps_hdt_pub_) { sbg_gps_hdt_pub_->publish(message_wrapper_.createSbgGpsHdtMessage(ref_sbg_log.gpsHdtData)); @@ -610,7 +585,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_GPS1_RAW: case SBG_ECOM_LOG_GPS2_RAW: - if (sbg_gps_raw_pub_) { sbg_gps_raw_pub_->publish(message_wrapper_.createSbgGpsRawMessage(ref_sbg_log.gpsRawData)); @@ -618,7 +592,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_ODO_VEL: - if (sbg_odo_vel_pub_) { sbg_odo_vel_pub_->publish(message_wrapper_.createSbgOdoVelMessage(ref_sbg_log.odometerData)); @@ -626,7 +599,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EVENT_A: - if (sbg_event_a_pub_) { sbg_event_a_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); @@ -634,7 +606,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EVENT_B: - if (sbg_event_b_pub_) { sbg_event_b_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); @@ -642,7 +613,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EVENT_C: - if (sbg_event_c_pub_) { sbg_event_c_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); @@ -650,7 +620,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EVENT_D: - if (sbg_event_d_pub_) { sbg_event_d_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); @@ -658,7 +627,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_EVENT_E: - if (sbg_event_e_pub_) { sbg_event_e_pub_->publish(message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); @@ -666,7 +634,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_IMU_SHORT: - if (sbg_imu_short_pub_) { sbg_imu_short_pub_->publish(message_wrapper_.createSbgImuShortMessage(ref_sbg_log.imuShort)); @@ -674,7 +641,6 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ break; case SBG_ECOM_LOG_AIR_DATA: - publishFluidPressureData(ref_sbg_log); break; diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 66ce10a..3402544 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -144,8 +144,8 @@ void SbgDevice::connect() } else if (config_store_.isInterfaceUdp()) { - char ip[16]; - sbgNetworkIpToString(config_store_.getIpAddress(), ip, sizeof(ip)); + char ip[16]; + sbgNetworkIpToString(config_store_.getIpAddress(), ip, sizeof(ip)); RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - UDP interface %s %d->%d", ip, config_store_.getInputPortAddress(), config_store_.getOutputPortAddress()); error_code = sbgInterfaceUdpCreate(&sbg_interface_, config_store_.getIpAddress(), config_store_.getInputPortAddress(), config_store_.getOutputPortAddress()); } diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp index 854a476..e6eedcd 100644 --- a/src/sbg_ros_helpers.cpp +++ b/src/sbg_ros_helpers.cpp @@ -68,7 +68,7 @@ void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, d utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); - if(latitude < 0) + if (latitude < 0) { utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere } @@ -97,14 +97,14 @@ char sbg::helpers::UTMLetterDesignator(double latitude) else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; - else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; - else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; - else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; - else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; - else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; - else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; - else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; - else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; + else if ((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; + else if ((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; + else if ((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; + else if ((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; + else if ((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; + else if ((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; + else if ((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; + else if ((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; // 'Z' is an error flag, the Latitude is outside the UTM limits else LetterDesignator = 'Z'; return LetterDesignator; diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index 297e2d5..97abb38 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -43,18 +43,18 @@ void SbgUtm::init(double latitude, double longitude, double altitude) zoneNumber = int((LongTemp + 180)/6) + 1; - if( latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + if ( latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) { zoneNumber = 32; } // Special zones for Svalbard - if( latitude >= 72.0 && latitude < 84.0 ) + if ( latitude >= 72.0 && latitude < 84.0 ) { - if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; - else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; - else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; - else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; + if ( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; + else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; + else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; + else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; } zone_ = zoneNumber; From 873b6c395886fb703a3d8dfcf99c021303b366a1 Mon Sep 17 00:00:00 2001 From: cledant Date: Fri, 1 Sep 2023 18:13:27 +0200 Subject: [PATCH 57/74] Factory for UTM data --- include/sbg_driver/message_wrapper.h | 4 +- include/sbg_driver/sbg_ros_helpers.h | 27 ----- include/sbg_driver/sbg_utm.h | 68 ++---------- src/message_wrapper.cpp | 31 +++--- src/sbg_ros_helpers.cpp | 108 ------------------- src/sbg_utm.cpp | 152 ++++++++++++++++++++++----- 6 files changed, 156 insertions(+), 234 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 90a8d01..e97b51e 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -99,7 +99,9 @@ class MessageWrapper : public rclcpp::Node std::string odom_base_frame_id_; std::string odom_init_frame_id_; - sbg::SbgUtm utm_{}; + bool has_first_valid_position = false; + sbg::Utm first_valid_utm_{}; + double first_valid_altitude_{}; static constexpr double equatorial_radius_ = 6378137.0; static constexpr double polar_radius_ = 6356752.314245; diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h index a848889..3dc842a 100644 --- a/include/sbg_driver/sbg_ros_helpers.h +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -58,25 +58,6 @@ namespace sbg::helpers SIMULATED = 8, }; - /*! - * Convert latitude and longitude to a position relative to UTM initial position. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \param[in] zone_number UTM zone number. - * \param[out] utm_northing UTM northing, in meters. - * \param[out] utm_easting UTM easting, in meters. - */ - void LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting); - - /*! - * Get UTM letter designator for the given latitude. - * - * \param[in] latitude Latitude, in degrees. - * \return UTM letter designator. - */ - char UTMLetterDesignator(double latitude); - /*! * Wrap an angle between [ -Pi ; Pi ] rad. * @@ -93,14 +74,6 @@ namespace sbg::helpers */ float wrapAngle360(float angle_deg); - /*! - * Compute UTM zone meridian. - * - * \param[in] zone_number UTM Zone number. - * \return Meridian angle, in degrees. - */ - double computeMeridian(int zone_number); - /*! * Get the number of days in the year. * diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 2c83352..c3aaccb 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -38,69 +38,19 @@ namespace sbg { /*! - * Class to wrap UTM computation. + * Struct for UTM values */ -class SbgUtm final +struct Utm final { -public: - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - SbgUtm() = default; - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - /*! - * Get UTM easting. - * - * \return Easting, in degrees. - */ - double getEasting() const; - - /*! - * Get UTM northing. - * - * \return Northing, in degrees. - */ - double getNorthing() const; - - /*! - * Get UTM altitude. - * - * \return Altitude, in meters. - */ - double getAltitude() const; - - /*! - * Get UTM zone. - * - * \return Zone number. - */ - int getZoneNumber() const; - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Set UTM initial position. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \param[in] altitude Altitude, in meters. - */ - void init(double latitude, double longitude, double altitude); - -private: - double easting_ = 0.0; - double northing_ = 0.0; - double altitude_ = 0.0; - int zone_ = 0; + double easting_{}; + double northing_{}; + double meridian_{}; + int zone_number_{}; + char letter_designator_{}; }; +Utm convertLLtoUTM(double latitude, double longitude); + } #endif // SBG_ROS_UTM_H diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 9f73e76..f3f7e7e 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -873,7 +873,6 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driver::msg::SbgImuData &ref_sbg_imu_msg, const sbg_driver::msg::SbgEkfNav &ref_ekf_nav_msg, const tf2::Quaternion &ref_orientation, const sbg_driver::msg::SbgEkfEuler &ref_ekf_euler_msg) { nav_msgs::msg::Odometry odo_ros_msg; - double utm_northing, utm_easting; std::string utm_zone; geometry_msgs::msg::TransformStamped transform; @@ -883,38 +882,42 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); // Convert latitude and longitude to UTM coordinates. - if (utm_.getZoneNumber() == 0) + if (!has_first_valid_position) { - utm_.init(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); + first_valid_altitude_ = ref_ekf_nav_msg.altitude; + first_valid_utm_ = sbg::convertLLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); + RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" - , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZoneNumber(), sbg::helpers::UTMLetterDesignator(ref_ekf_nav_msg.latitude) - , utm_.getEasting(), (int)(utm_.getEasting())/1000 - , utm_.getNorthing(), (int)(utm_.getNorthing())/1000 + , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, first_valid_utm_.zone_number_, first_valid_utm_.letter_designator_ + , first_valid_utm_.easting_, (int)(first_valid_utm_.easting_)/1000 + , first_valid_utm_.northing_, (int)(first_valid_utm_.northing_)/1000 ); if (odom_publish_tf_) { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - pose.position.x = utm_.getEasting(); - pose.position.y = utm_.getNorthing(); - pose.position.z = utm_.getAltitude(); + pose.position.x = first_valid_utm_.easting_; + pose.position.y = first_valid_utm_.northing_; + pose.position.z = first_valid_altitude_; fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); tf_broadcaster_->sendTransform(transform); static_tf_broadcaster_->sendTransform(transform); } + + has_first_valid_position = true; } - sbg::helpers::LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZoneNumber(), utm_northing, utm_easting); - odo_ros_msg.pose.pose.position.x = utm_easting - utm_.getEasting(); - odo_ros_msg.pose.pose.position.y = utm_northing - utm_.getNorthing(); - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - utm_.getAltitude(); + auto current_utm = sbg::convertLLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); + odo_ros_msg.pose.pose.position.x = current_utm.easting_ - first_valid_utm_.easting_; + odo_ros_msg.pose.pose.position.y = current_utm.northing_ - first_valid_utm_.northing_; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - first_valid_altitude_; // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(sbg::helpers::computeMeridian(utm_.getZoneNumber())); + double central_meridian = sbgDegToRadD(current_utm.meridian_); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp index e6eedcd..ec38814 100644 --- a/src/sbg_ros_helpers.cpp +++ b/src/sbg_ros_helpers.cpp @@ -7,109 +7,6 @@ // STL headers #include -/* - * Modification of gps_common::LLtoUTM() to use a constant UTM zone. - * - * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 - * - * East Longitudes are positive, West longitudes are negative. - * North latitudes are positive, South latitudes are negative - * Lat and Long are in fractional degrees - * - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com. - */ -void sbg::helpers::LLtoUTM(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) -{ - constexpr double RADIANS_PER_DEGREE = M_PI/180.0; - - // WGS84 Parameters - constexpr double WGS84_A = 6378137.0; // major axis - constexpr double WGS84_E = 0.0818191908; // first eccentricity - - // UTM Parameters - constexpr double UTM_K0 = 0.9996; // scale factor - constexpr double UTM_E2 = (WGS84_E * WGS84_E); // e^2 - - constexpr double a = WGS84_A; - constexpr double eccSquared = UTM_E2; - constexpr double k0 = UTM_K0; - - double LongOrigin; - double eccPrimeSquared; - double N, T, C, A, M; - - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - - double LatRad = latitude * RADIANS_PER_DEGREE; - double LongRad = LongTemp * RADIANS_PER_DEGREE; - double LongOriginRad; - - // +3 puts origin in middle of zone - LongOrigin = (zone_number - 1) * 6 - 180 + 3; - LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; - - eccPrimeSquared = (eccSquared)/(1-eccSquared); - - N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); - T = tan(LatRad)*tan(LatRad); - C = eccPrimeSquared*cos(LatRad)*cos(LatRad); - A = cos(LatRad)*(LongRad-LongOriginRad); - - M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad - - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) - + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) - - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); - - utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); - - utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); - - if (latitude < 0) - { - utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere - } -} - -/** - * Get UTM letter designator for the given latitude. - * - * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S - * - * Written by Chuck Gantz- chuck.gantz@globalstar.com - */ -char sbg::helpers::UTMLetterDesignator(double latitude) -{ - char LetterDesignator; - - if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; - else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; - else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; - else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; - else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; - else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; - else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; - else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; - else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; - else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; - else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; - else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; - else if ((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; - else if ((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; - else if ((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; - else if ((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; - else if ((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; - else if ((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; - else if ((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; - else if ((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; - // 'Z' is an error flag, the Latitude is outside the UTM limits - else LetterDesignator = 'Z'; - return LetterDesignator; -} - float sbg::helpers::wrapAnglePi(float angle_rad) { if (angle_rad > SBG_PI_F) @@ -142,11 +39,6 @@ float sbg::helpers::wrapAngle360(float angle_deg) return wrapped_angle_deg; } -double sbg::helpers::computeMeridian(int zone_number) -{ - return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; -} - uint32_t sbg::helpers::getNumberOfDaysInYear(uint16_t year) { if (isLeapYear(year)) diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index 97abb38..a4b2ab9 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -1,40 +1,132 @@ // File header #include "sbg_utm.h" -// Project headers -#include +// STL headers +#include -using sbg::SbgUtm; +/*! + * Convert latitude and longitude to a position relative to UTM initial position. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \param[in] zone_number UTM zone number. + * \param[out] utm_northing UTM northing, in meters. + * \param[out] utm_easting UTM easting, in meters. + * + * Originally written by Chuck Gantz- chuck.gantz@globalstar.com. + */ +static void computeEastingNorthing(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) +{ + constexpr double RADIANS_PER_DEGREE = M_PI/180.0; -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// + // WGS84 Parameters + constexpr double WGS84_A = 6378137.0; // major axis + constexpr double WGS84_E = 0.0818191908; // first eccentricity -double SbgUtm::getEasting() const -{ - return easting_; -} + // UTM Parameters + constexpr double UTM_K0 = 0.9996; // scale factor + constexpr double UTM_E2 = (WGS84_E * WGS84_E); // e^2 -double SbgUtm::getNorthing() const -{ - return northing_; + constexpr double a = WGS84_A; + constexpr double eccSquared = UTM_E2; + constexpr double k0 = UTM_K0; + + double LongOrigin; + double eccPrimeSquared; + double N, T, C, A, M; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + + double LatRad = latitude * RADIANS_PER_DEGREE; + double LongRad = LongTemp * RADIANS_PER_DEGREE; + double LongOriginRad; + + // +3 puts origin in middle of zone + LongOrigin = (zone_number - 1) * 6 - 180 + 3; + LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; + + eccPrimeSquared = (eccSquared)/(1-eccSquared); + + N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = eccPrimeSquared*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); + + M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad + - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) + + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) + - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); + + utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); + + utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + + if (latitude < 0) + { + utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere + } } -double SbgUtm::getAltitude() const +/*! + * Compute UTM zone meridian. + * + * \param[in] zone_number UTM Zone number. + * \return Meridian angle, in degrees. + */ +static double computeMeridian(int zone_number) { - return altitude_; + return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; } -int SbgUtm::getZoneNumber() const +/*! + * Get UTM letter designator for the given latitude. + * + * \param[in] latitude Latitude, in degrees. + * \return UTM letter designator. + * + * Written by Chuck Gantz- chuck.gantz@globalstar.com + */ +static char computeUTMLetterDesignator(double latitude) { - return zone_; -} + char LetterDesignator; -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// + if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; + else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; + else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; + else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; + else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; + else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; + else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; + else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; + else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; + else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; + else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; + else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; + else if ((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; + else if ((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; + else if ((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; + else if ((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; + else if ((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; + else if ((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; + else if ((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; + else if ((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; + // 'Z' is an error flag, the Latitude is outside the UTM limits + else LetterDesignator = 'Z'; + return LetterDesignator; +} -void SbgUtm::init(double latitude, double longitude, double altitude) +/*! + * Convert latitude and longitude to a position relative to UTM initial position. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \return UTM letter designator. + */ +static int computeZoneNumber(double latitude, double longitude) { int zoneNumber; @@ -57,7 +149,17 @@ void SbgUtm::init(double latitude, double longitude, double altitude) else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; } - zone_ = zoneNumber; - altitude_ = altitude; - sbg::helpers::LLtoUTM(latitude, longitude, zone_, northing_, easting_); + return zoneNumber; +} + +sbg::Utm sbg::convertLLtoUTM(double latitude, double longitude) +{ + sbg::Utm converted_ll; + + converted_ll.zone_number_ = computeZoneNumber(latitude, longitude); + converted_ll.meridian_ = computeMeridian(converted_ll.zone_number_); + converted_ll.letter_designator_ = computeUTMLetterDesignator(latitude); + computeEastingNorthing(latitude, longitude, converted_ll.zone_number_, converted_ll.northing_, converted_ll.easting_); + + return (converted_ll); } \ No newline at end of file From 7820f4c7ec93db095cea5bdbf51590cd0d35efd0 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 10:16:45 +0200 Subject: [PATCH 58/74] Added Position class --- CMakeLists.txt | 1 + include/sbg_driver/message_wrapper.h | 5 +-- include/sbg_driver/sbg_position.h | 61 ++++++++++++++++++++++++++++ include/sbg_driver/sbg_utm.h | 10 ++--- src/message_wrapper.cpp | 29 +++++++------ src/sbg_position.cpp | 37 +++++++++++++++++ src/sbg_utm.cpp | 8 ++-- 7 files changed, 124 insertions(+), 27 deletions(-) create mode 100644 include/sbg_driver/sbg_position.h create mode 100644 src/sbg_position.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 260c7a0..a0175a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,6 +100,7 @@ set (SBG_COMMON_RESOURCES src/config_store.cpp src/sbg_device.cpp src/sbg_utm.cpp + src/sbg_position.cpp src/sbg_ros_helpers.cpp ) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index e97b51e..de0a7aa 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -41,6 +41,7 @@ // Sbg header #include #include +#include #include // ROS headers @@ -99,9 +100,7 @@ class MessageWrapper : public rclcpp::Node std::string odom_base_frame_id_; std::string odom_init_frame_id_; - bool has_first_valid_position = false; - sbg::Utm first_valid_utm_{}; - double first_valid_altitude_{}; + sbg::Position first_valid_position_{}; static constexpr double equatorial_radius_ = 6378137.0; static constexpr double polar_radius_ = 6356752.314245; diff --git a/include/sbg_driver/sbg_position.h b/include/sbg_driver/sbg_position.h new file mode 100644 index 0000000..5cca7bb --- /dev/null +++ b/include/sbg_driver/sbg_position.h @@ -0,0 +1,61 @@ +/*! +* \file sbg_position.h +* \author SBG Systems +* \date 04/09/2023 +* +* \brief Handle creation of position class. +* +* Methods to create Position from given data. +* +* \section CodeCopyright Copyright Notice +* MIT License +* +* Copyright (c) 2023 SBG Systems +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ + +#ifndef SBG_DRIVER_SBG_POSITION_H +#define SBG_DRIVER_SBG_POSITION_H + +#include + +namespace sbg +{ + class Position + { + public: + Position() = default; + Position(double latitude, double longitude, double altitude); + + void init(double latitude, double longitude, double altitude); + void clear(); + + bool isInit() const; + const Utm &getUtm() const; + double getAltitude() const; + + private: + bool is_init_ = false; + Utm utm_position_{}; + double altitude_{}; + }; +} + +#endif //SBG_DRIVER_SBG_POSITION_H diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index c3aaccb..1b36ad0 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -42,11 +42,11 @@ namespace sbg */ struct Utm final { - double easting_{}; - double northing_{}; - double meridian_{}; - int zone_number_{}; - char letter_designator_{}; + double easting{}; + double northing{}; + double meridian{}; + int zone_number{}; + char letter_designator{}; }; Utm convertLLtoUTM(double latitude, double longitude); diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index f3f7e7e..5aa8026 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -882,42 +882,41 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); // Convert latitude and longitude to UTM coordinates. - if (!has_first_valid_position) + if (!first_valid_position_.isInit()) { - first_valid_altitude_ = ref_ekf_nav_msg.altitude; - first_valid_utm_ = sbg::convertLLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); + first_valid_position_.init(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); + const auto &first_valid_position_utm = first_valid_position_.getUtm(); RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" - , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, first_valid_utm_.zone_number_, first_valid_utm_.letter_designator_ - , first_valid_utm_.easting_, (int)(first_valid_utm_.easting_)/1000 - , first_valid_utm_.northing_, (int)(first_valid_utm_.northing_)/1000 + , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, first_valid_position_utm.zone_number, first_valid_position_utm.letter_designator + , first_valid_position_utm.easting, (int)(first_valid_position_utm.easting) / 1000 + , first_valid_position_utm.northing, (int)(first_valid_position_utm.northing) / 1000 ); if (odom_publish_tf_) { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - pose.position.x = first_valid_utm_.easting_; - pose.position.y = first_valid_utm_.northing_; - pose.position.z = first_valid_altitude_; + pose.position.x = first_valid_position_utm.easting; + pose.position.y = first_valid_position_utm.northing; + pose.position.z = first_valid_position_.getAltitude(); fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); tf_broadcaster_->sendTransform(transform); static_tf_broadcaster_->sendTransform(transform); } - - has_first_valid_position = true; } auto current_utm = sbg::convertLLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); - odo_ros_msg.pose.pose.position.x = current_utm.easting_ - first_valid_utm_.easting_; - odo_ros_msg.pose.pose.position.y = current_utm.northing_ - first_valid_utm_.northing_; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - first_valid_altitude_; + const auto &first_valid_position_utm = first_valid_position_.getUtm(); + odo_ros_msg.pose.pose.position.x = current_utm.easting - first_valid_position_utm.easting; + odo_ros_msg.pose.pose.position.y = current_utm.northing - first_valid_position_utm.northing; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - first_valid_position_.getAltitude(); // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(current_utm.meridian_); + double central_meridian = sbgDegToRadD(current_utm.meridian); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. diff --git a/src/sbg_position.cpp b/src/sbg_position.cpp new file mode 100644 index 0000000..609eced --- /dev/null +++ b/src/sbg_position.cpp @@ -0,0 +1,37 @@ +#include "sbg_position.h" + +using sbg::Position; + +Position::Position(double latitude, double longitude, double altitude) +{ + init(latitude, longitude, altitude); +} + +void Position::init(double latitude, double longitude, double altitude) +{ + utm_position_ = convertLLtoUTM(latitude, longitude); + altitude_ = altitude; + is_init_ = true; +} + +void Position::clear() +{ + is_init_ = false; + altitude_ = 0.0; + utm_position_ = {}; +} + +bool Position::isInit() const +{ + return is_init_; +} + +const sbg::Utm &Position::getUtm() const +{ + return utm_position_; +} + +double Position::getAltitude() const +{ + return altitude_; +} diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index a4b2ab9..2593994 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -156,10 +156,10 @@ sbg::Utm sbg::convertLLtoUTM(double latitude, double longitude) { sbg::Utm converted_ll; - converted_ll.zone_number_ = computeZoneNumber(latitude, longitude); - converted_ll.meridian_ = computeMeridian(converted_ll.zone_number_); - converted_ll.letter_designator_ = computeUTMLetterDesignator(latitude); - computeEastingNorthing(latitude, longitude, converted_ll.zone_number_, converted_ll.northing_, converted_ll.easting_); + converted_ll.zone_number = computeZoneNumber(latitude, longitude); + converted_ll.meridian = computeMeridian(converted_ll.zone_number); + converted_ll.letter_designator = computeUTMLetterDesignator(latitude); + computeEastingNorthing(latitude, longitude, converted_ll.zone_number, converted_ll.northing, converted_ll.easting); return (converted_ll); } \ No newline at end of file From 74aa60894e9a662d9cc3a786d9fbc6eb8e3e0f5d Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 11:06:53 +0200 Subject: [PATCH 59/74] Added documentation on Position class --- include/sbg_driver/sbg_position.h | 59 +++++++++++++++++++++++++++++-- src/sbg_position.cpp | 38 +++++++++++++------- 2 files changed, 82 insertions(+), 15 deletions(-) diff --git a/include/sbg_driver/sbg_position.h b/include/sbg_driver/sbg_position.h index 5cca7bb..46351a1 100644 --- a/include/sbg_driver/sbg_position.h +++ b/include/sbg_driver/sbg_position.h @@ -38,19 +38,74 @@ namespace sbg { + /*! + * Class to describe a position using Utm and altitude. + */ class Position { public: + + //---------------------------------------------------------------------// + //- Constructor -// + //---------------------------------------------------------------------// + + /*! + * Default constructor. + */ Position() = default; + + /*! + * Constructor. + * + * \param[in] latitude Latitude in degree. + * \param[in] longitude Longitude in degree. + * \param[in] altitude Altitude is meter. + */ Position(double latitude, double longitude, double altitude); - void init(double latitude, double longitude, double altitude); - void clear(); + //---------------------------------------------------------------------// + //- Parameters -// + //---------------------------------------------------------------------// + /*! + * Returns if class is initialized. + * + * \return True if class is initialized. + */ bool isInit() const; + + /*! + * Get UTM parameters + * + * \return Utm structure. + */ const Utm &getUtm() const; + + /*! + * Get altitude in meter + * + * \return Altitude. + */ double getAltitude() const; + //---------------------------------------------------------------------// + //- Operations -// + //---------------------------------------------------------------------// + + /*! + * Initialize position. + * + * \param[in] latitude Latitude in degree. + * \param[in] longitude Longitude in degree. + * \param[in] altitude Altitude in meter. + */ + void init(double latitude, double longitude, double altitude); + + /*! + * Clear values and set them to zero / false. + */ + void clear(); + private: bool is_init_ = false; Utm utm_position_{}; diff --git a/src/sbg_position.cpp b/src/sbg_position.cpp index 609eced..729eb91 100644 --- a/src/sbg_position.cpp +++ b/src/sbg_position.cpp @@ -2,24 +2,18 @@ using sbg::Position; +//---------------------------------------------------------------------// +//- Constructor -// +//---------------------------------------------------------------------// + Position::Position(double latitude, double longitude, double altitude) { init(latitude, longitude, altitude); } -void Position::init(double latitude, double longitude, double altitude) -{ - utm_position_ = convertLLtoUTM(latitude, longitude); - altitude_ = altitude; - is_init_ = true; -} - -void Position::clear() -{ - is_init_ = false; - altitude_ = 0.0; - utm_position_ = {}; -} +//---------------------------------------------------------------------// +//- Parameters -// +//---------------------------------------------------------------------// bool Position::isInit() const { @@ -35,3 +29,21 @@ double Position::getAltitude() const { return altitude_; } + +//---------------------------------------------------------------------// +//- Operations -// +//---------------------------------------------------------------------// + +void Position::init(double latitude, double longitude, double altitude) +{ + utm_position_ = convertLLtoUTM(latitude, longitude); + altitude_ = altitude; + is_init_ = true; +} + +void Position::clear() +{ + is_init_ = false; + altitude_ = 0.0; + utm_position_ = {}; +} \ No newline at end of file From 2e35488071577b9e085ed13f63357a07a0d82753 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 11:15:17 +0200 Subject: [PATCH 60/74] Added documentation on Utm structure --- include/sbg_driver/sbg_utm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 1b36ad0..762ea20 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -5,7 +5,7 @@ * * \brief Handle creation of utm class. * -* Methods to create UTM from given data. +* Methods to create Utm from given data. * * \section CodeCopyright Copyright Notice * MIT License @@ -38,7 +38,7 @@ namespace sbg { /*! - * Struct for UTM values + * Struct for Utm related data */ struct Utm final { From f5556739114d43ac0ad0ec162e10363beae54fed Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 11:46:57 +0200 Subject: [PATCH 61/74] Updated sbg_utm documentation and function prototype --- include/sbg_driver/sbg_utm.h | 7 ++++++ src/sbg_position.cpp | 1 + src/sbg_utm.cpp | 41 +++++++++++++++++++++--------------- 3 files changed, 32 insertions(+), 17 deletions(-) diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 762ea20..976c912 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -49,6 +49,13 @@ struct Utm final char letter_designator{}; }; +/*! + * Convert latitude and longitude to UTM structure. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \return UTM structure. + */ Utm convertLLtoUTM(double latitude, double longitude); } diff --git a/src/sbg_position.cpp b/src/sbg_position.cpp index 729eb91..35ad75e 100644 --- a/src/sbg_position.cpp +++ b/src/sbg_position.cpp @@ -1,3 +1,4 @@ +// File header #include "sbg_position.h" using sbg::Position; diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index 2593994..a7d4537 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -1,21 +1,23 @@ // File header #include "sbg_utm.h" +// SBG headers +#include + // STL headers #include /*! - * Convert latitude and longitude to a position relative to UTM initial position. + * Convert latitude, longitude, zone number to a UTM position. + * Originally written by Chuck Gantz- chuck.gantz@globalstar.com * * \param[in] latitude Latitude, in degrees. * \param[in] longitude Longitude, in degrees. * \param[in] zone_number UTM zone number. - * \param[out] utm_northing UTM northing, in meters. - * \param[out] utm_easting UTM easting, in meters. + * \return Array containing easting then northing. * - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com. */ -static void computeEastingNorthing(double latitude, double longitude, int zone_number, double &utm_northing, double &utm_easting) +static std::array computeEastingNorthing(double latitude, double longitude, int zone_number) { constexpr double RADIANS_PER_DEGREE = M_PI/180.0; @@ -58,21 +60,24 @@ static void computeEastingNorthing(double latitude, double longitude, int zone_n + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); - utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); + double utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); - utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + double utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); if (latitude < 0) { utm_northing += 10000000.0; //10000000 meter offset for southern hemisphere } + + std::array easting_northing{utm_easting, utm_northing}; + return (easting_northing); } /*! - * Compute UTM zone meridian. + * Compute UTM zone meridian from UTM zone number. * * \param[in] zone_number UTM Zone number. * \return Meridian angle, in degrees. @@ -84,13 +89,13 @@ static double computeMeridian(int zone_number) /*! * Get UTM letter designator for the given latitude. + * Originally written by Chuck Gantz- chuck.gantz@globalstar.com * * \param[in] latitude Latitude, in degrees. * \return UTM letter designator. * - * Written by Chuck Gantz- chuck.gantz@globalstar.com */ -static char computeUTMLetterDesignator(double latitude) +static char computeLetterDesignator(double latitude) { char LetterDesignator; @@ -120,11 +125,11 @@ static char computeUTMLetterDesignator(double latitude) } /*! - * Convert latitude and longitude to a position relative to UTM initial position. + * Convert latitude and longitude to an UTM zone number. * * \param[in] latitude Latitude, in degrees. * \param[in] longitude Longitude, in degrees. - * \return UTM letter designator. + * \return UTM zone number. */ static int computeZoneNumber(double latitude, double longitude) { @@ -158,8 +163,10 @@ sbg::Utm sbg::convertLLtoUTM(double latitude, double longitude) converted_ll.zone_number = computeZoneNumber(latitude, longitude); converted_ll.meridian = computeMeridian(converted_ll.zone_number); - converted_ll.letter_designator = computeUTMLetterDesignator(latitude); - computeEastingNorthing(latitude, longitude, converted_ll.zone_number, converted_ll.northing, converted_ll.easting); + converted_ll.letter_designator = computeLetterDesignator(latitude); + const auto easting_northing = computeEastingNorthing(latitude, longitude, converted_ll.zone_number); + converted_ll.easting = easting_northing[0]; + converted_ll.northing = easting_northing[1]; return (converted_ll); } \ No newline at end of file From afc89e2e44bd5ab374397bdc616e10ddcdc463ef Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 16:16:01 +0200 Subject: [PATCH 62/74] Indentation fix --- src/config_store.cpp | 12 +++++------ src/message_publisher.cpp | 42 +++++++++++++++++++-------------------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/config_store.cpp b/src/config_store.cpp index 8439729..eb4d22f 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -45,11 +45,11 @@ void ConfigStore::loadCommunicationParameters(const rclcpp::Node& ref_node_handl if (ref_node_handle.has_parameter("uartConf.portName")) { - serial_communication_ = true; + serial_communication_ = true; ref_node_handle.get_parameter_or("uartConf.portName", uart_port_name_, "/dev/ttyUSB0"); - uart_baud_rate_ = getParameter(ref_node_handle, "uartConf.baudRate", 0); - output_port_ = getParameter(ref_node_handle, "uartConf.portID", SBG_ECOM_OUTPUT_PORT_A); + uart_baud_rate_ = getParameter(ref_node_handle, "uartConf.baudRate", 0); + output_port_ = getParameter(ref_node_handle, "uartConf.portID", SBG_ECOM_OUTPUT_PORT_A); } else if (ref_node_handle.has_parameter("ipConf.ipAddress")) { @@ -57,9 +57,9 @@ void ConfigStore::loadCommunicationParameters(const rclcpp::Node& ref_node_handl ref_node_handle.get_parameter_or("ipConf.ipAddress", ip_address, "0.0.0.0"); upd_communication_ = true; - sbg_ip_address_ = sbgNetworkIpFromString(ip_address.c_str()); - out_port_address_ = getParameter(ref_node_handle, "ipConf.out_port", 0); - in_port_address_ = getParameter(ref_node_handle, "ipConf.in_port", 0); + sbg_ip_address_ = sbgNetworkIpFromString(ip_address.c_str()); + out_port_address_ = getParameter(ref_node_handle, "ipConf.out_port", 0); + in_port_address_ = getParameter(ref_node_handle, "ipConf.in_port", 0); } else { diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 11df729..989a2e2 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -100,91 +100,91 @@ void MessagePublisher::initPublisher(rclcpp::Node& ref_ros_node_handle, SbgEComM switch (sbg_msg_id) { case SBG_ECOM_LOG_STATUS: - sbg_status_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_status_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_UTC_TIME: - sbg_utc_time_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_utc_time_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_IMU_DATA: - sbg_imu_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_imu_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_MAG: - sbg_mag_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_mag_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_MAG_CALIB: - sbg_mag_calib_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_mag_calib_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EKF_EULER: - sbg_ekf_euler_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_ekf_euler_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EKF_QUAT: - sbg_ekf_quat_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_ekf_quat_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EKF_NAV: - sbg_ekf_nav_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_ekf_nav_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_SHIP_MOTION: - sbg_ship_motion_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_ship_motion_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_VEL: case SBG_ECOM_LOG_GPS2_VEL: - sbg_gps_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_gps_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_POS: case SBG_ECOM_LOG_GPS2_POS: - sbg_gps_pos_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_gps_pos_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_HDT: case SBG_ECOM_LOG_GPS2_HDT: - sbg_gps_hdt_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_gps_hdt_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_GPS1_RAW: case SBG_ECOM_LOG_GPS2_RAW: - sbg_gps_raw_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_gps_raw_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_ODO_VEL: - sbg_odo_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_odo_vel_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_A: - sbg_event_a_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_event_a_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_B: - sbg_event_b_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_event_b_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_C: - sbg_event_c_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_event_c_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_D: - sbg_event_d_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_event_d_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_EVENT_E: - sbg_event_e_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_event_e_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_IMU_SHORT: - sbg_imu_short_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_imu_short_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; case SBG_ECOM_LOG_AIR_DATA: - sbg_air_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); + sbg_air_data_pub_ = ref_ros_node_handle.create_publisher(ref_output_topic, max_messages_); break; default: From 9007017005d8b9b6172bef7866f8e065efd0d4df Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 17:28:14 +0200 Subject: [PATCH 63/74] Utm as class --- CMakeLists.txt | 1 - include/sbg_driver/message_wrapper.h | 6 +- include/sbg_driver/sbg_position.h | 116 ------------------- include/sbg_driver/sbg_utm.h | 67 ++++++++--- src/message_wrapper.cpp | 32 +++--- src/sbg_position.cpp | 50 --------- src/sbg_utm.cpp | 161 ++++++++++++++------------- 7 files changed, 153 insertions(+), 280 deletions(-) delete mode 100644 include/sbg_driver/sbg_position.h delete mode 100644 src/sbg_position.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a0175a3..260c7a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,6 @@ set (SBG_COMMON_RESOURCES src/config_store.cpp src/sbg_device.cpp src/sbg_utm.cpp - src/sbg_position.cpp src/sbg_ros_helpers.cpp ) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index de0a7aa..d058f2c 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -41,7 +41,6 @@ // Sbg header #include #include -#include #include // ROS headers @@ -100,7 +99,10 @@ class MessageWrapper : public rclcpp::Node std::string odom_base_frame_id_; std::string odom_init_frame_id_; - sbg::Position first_valid_position_{}; + Utm utm_{}; + double first_valid_easting_{}; + double first_valid_northing_{}; + double first_valid_altitude_{}; static constexpr double equatorial_radius_ = 6378137.0; static constexpr double polar_radius_ = 6356752.314245; diff --git a/include/sbg_driver/sbg_position.h b/include/sbg_driver/sbg_position.h deleted file mode 100644 index 46351a1..0000000 --- a/include/sbg_driver/sbg_position.h +++ /dev/null @@ -1,116 +0,0 @@ -/*! -* \file sbg_position.h -* \author SBG Systems -* \date 04/09/2023 -* -* \brief Handle creation of position class. -* -* Methods to create Position from given data. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2023 SBG Systems -* -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: -* -* The above copyright notice and this permission notice shall be included in all -* copies or substantial portions of the Software. -* -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. -*/ - -#ifndef SBG_DRIVER_SBG_POSITION_H -#define SBG_DRIVER_SBG_POSITION_H - -#include - -namespace sbg -{ - /*! - * Class to describe a position using Utm and altitude. - */ - class Position - { - public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - */ - Position() = default; - - /*! - * Constructor. - * - * \param[in] latitude Latitude in degree. - * \param[in] longitude Longitude in degree. - * \param[in] altitude Altitude is meter. - */ - Position(double latitude, double longitude, double altitude); - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - /*! - * Returns if class is initialized. - * - * \return True if class is initialized. - */ - bool isInit() const; - - /*! - * Get UTM parameters - * - * \return Utm structure. - */ - const Utm &getUtm() const; - - /*! - * Get altitude in meter - * - * \return Altitude. - */ - double getAltitude() const; - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Initialize position. - * - * \param[in] latitude Latitude in degree. - * \param[in] longitude Longitude in degree. - * \param[in] altitude Altitude in meter. - */ - void init(double latitude, double longitude, double altitude); - - /*! - * Clear values and set them to zero / false. - */ - void clear(); - - private: - bool is_init_ = false; - Utm utm_position_{}; - double altitude_{}; - }; -} - -#endif //SBG_DRIVER_SBG_POSITION_H diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 976c912..7d3cc9e 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -34,29 +34,64 @@ #ifndef SBG_ROS_UTM_H #define SBG_ROS_UTM_H +// STL headers +#include + namespace sbg { /*! - * Struct for Utm related data + * Class for Utm related data */ -struct Utm final +class Utm final { - double easting{}; - double northing{}; - double meridian{}; - int zone_number{}; - char letter_designator{}; -}; + public: + Utm() = default; + Utm(double latitude, double longitude); -/*! - * Convert latitude and longitude to UTM structure. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \return UTM structure. - */ -Utm convertLLtoUTM(double latitude, double longitude); + void init(double latitude, double longitude); + void clear(); + void reset(double latitude, double longitude); + + bool isInit() const; + int getZoneNumber() const; + double getMeridian() const; + char getLetterDesignator() const; + + std::array computeEastingNorthing(double latitude, double longitude) const; + + private: + + /*! + * Convert latitude and longitude to an UTM zone number. + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \return UTM zone number. + */ + static int computeZoneNumber(double latitude, double longitude); + + /*! + * Get UTM letter designator for the given latitude. + * Originally written by Chuck Gantz- chuck.gantz@globalstar.com + * + * \param[in] latitude Latitude, in degrees. + * \return UTM letter designator. + */ + static char computeLetterDesignator(double latitude); + + /*! + * Compute UTM zone meridian from UTM zone number. + * + * \return Meridian angle, in degrees. + */ + double computeMeridian() const; + + bool is_init_ = false; + double meridian_{}; + int zone_number_{}; + char letter_designator_{}; +}; } diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 5aa8026..1745274 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -882,24 +882,27 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); // Convert latitude and longitude to UTM coordinates. - if (!first_valid_position_.isInit()) + if (!utm_.isInit()) { - first_valid_position_.init(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); - const auto &first_valid_position_utm = first_valid_position_.getUtm(); + utm_.init(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); + const auto first_valid_easting_northing = utm_.computeEastingNorthing(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); + first_valid_easting_ = first_valid_easting_northing[0]; + first_valid_northing_ = first_valid_easting_northing[1]; + first_valid_altitude_ = ref_ekf_nav_msg.altitude; RCLCPP_INFO(rclcpp::get_logger("Message wrapper"), "initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" - , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, first_valid_position_utm.zone_number, first_valid_position_utm.letter_designator - , first_valid_position_utm.easting, (int)(first_valid_position_utm.easting) / 1000 - , first_valid_position_utm.northing, (int)(first_valid_position_utm.northing) / 1000 + , ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, utm_.getZoneNumber(), utm_.getLetterDesignator() + , first_valid_easting_, (int)(first_valid_easting_) / 1000 + , first_valid_northing_, (int)(first_valid_northing_) / 1000 ); if (odom_publish_tf_) { // Publish UTM initial transformation. geometry_msgs::msg::Pose pose; - pose.position.x = first_valid_position_utm.easting; - pose.position.y = first_valid_position_utm.northing; - pose.position.z = first_valid_position_.getAltitude(); + pose.position.x = first_valid_easting_; + pose.position.y = first_valid_northing_; + pose.position.z = first_valid_altitude_; fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); tf_broadcaster_->sendTransform(transform); @@ -907,16 +910,15 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv } } - auto current_utm = sbg::convertLLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); - const auto &first_valid_position_utm = first_valid_position_.getUtm(); - odo_ros_msg.pose.pose.position.x = current_utm.easting - first_valid_position_utm.easting; - odo_ros_msg.pose.pose.position.y = current_utm.northing - first_valid_position_utm.northing; - odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - first_valid_position_.getAltitude(); + const auto easting_northing = utm_.computeEastingNorthing(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude); + odo_ros_msg.pose.pose.position.x = easting_northing[0] - first_valid_easting_; + odo_ros_msg.pose.pose.position.y = easting_northing[1] - first_valid_northing_; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - first_valid_altitude_; // Compute convergence angle. double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); - double central_meridian = sbgDegToRadD(current_utm.meridian); + double central_meridian = sbgDegToRadD(utm_.getMeridian()); double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); // Convert position standard deviations to UTM frame. diff --git a/src/sbg_position.cpp b/src/sbg_position.cpp deleted file mode 100644 index 35ad75e..0000000 --- a/src/sbg_position.cpp +++ /dev/null @@ -1,50 +0,0 @@ -// File header -#include "sbg_position.h" - -using sbg::Position; - -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -Position::Position(double latitude, double longitude, double altitude) -{ - init(latitude, longitude, altitude); -} - -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// - -bool Position::isInit() const -{ - return is_init_; -} - -const sbg::Utm &Position::getUtm() const -{ - return utm_position_; -} - -double Position::getAltitude() const -{ - return altitude_; -} - -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// - -void Position::init(double latitude, double longitude, double altitude) -{ - utm_position_ = convertLLtoUTM(latitude, longitude); - altitude_ = altitude; - is_init_ = true; -} - -void Position::clear() -{ - is_init_ = false; - altitude_ = 0.0; - utm_position_ = {}; -} \ No newline at end of file diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index a7d4537..11b41fc 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -1,23 +1,59 @@ // File header #include "sbg_utm.h" -// SBG headers -#include - // STL headers #include -/*! - * Convert latitude, longitude, zone number to a UTM position. - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \param[in] zone_number UTM zone number. - * \return Array containing easting then northing. - * - */ -static std::array computeEastingNorthing(double latitude, double longitude, int zone_number) +using sbg::Utm; + +Utm::Utm(double latitude, double longitude) +{ + init(latitude, longitude); +} + +void Utm::init(double latitude, double longitude) +{ + zone_number_ = computeZoneNumber(latitude, longitude); + letter_designator_ = computeLetterDesignator(latitude); + meridian_ = computeMeridian(); + is_init_ = true; +} + +void Utm::clear() +{ + is_init_ = false; + meridian_ = {}; + zone_number_ = {}; + letter_designator_ = {}; +} + +void Utm::reset(double latitude, double longitude) +{ + clear(); + init(latitude, longitude); +} + +bool Utm::isInit() const +{ + return is_init_; +} + +int Utm::getZoneNumber() const +{ + return zone_number_; +} + +double Utm::getMeridian() const +{ + return meridian_; +} + +char Utm::getLetterDesignator() const +{ + return letter_designator_; +} + +std::array Utm::computeEastingNorthing(double latitude, double longitude) const { constexpr double RADIANS_PER_DEGREE = M_PI/180.0; @@ -26,7 +62,7 @@ static std::array computeEastingNorthing(double latitude, double long constexpr double WGS84_E = 0.0818191908; // first eccentricity // UTM Parameters - constexpr double UTM_K0 = 0.9996; // scale factor + constexpr double UTM_K0 = 0.9996; // scale factor constexpr double UTM_E2 = (WGS84_E * WGS84_E); // e^2 constexpr double a = WGS84_A; @@ -45,7 +81,7 @@ static std::array computeEastingNorthing(double latitude, double long double LongOriginRad; // +3 puts origin in middle of zone - LongOrigin = (zone_number - 1) * 6 - 180 + 3; + LongOrigin = (zone_number_ - 1) * 6 - 180 + 3; LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; eccPrimeSquared = (eccSquared)/(1-eccSquared); @@ -61,11 +97,11 @@ static std::array computeEastingNorthing(double latitude, double long - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); double utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); double utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); if (latitude < 0) { @@ -76,26 +112,33 @@ static std::array computeEastingNorthing(double latitude, double long return (easting_northing); } -/*! - * Compute UTM zone meridian from UTM zone number. - * - * \param[in] zone_number UTM Zone number. - * \return Meridian angle, in degrees. - */ -static double computeMeridian(int zone_number) +int Utm::computeZoneNumber(double latitude, double longitude) { - return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; + int zoneNumber; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + + zoneNumber = int((LongTemp + 180)/6) + 1; + + if ( latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + { + zoneNumber = 32; + } + + // Special zones for Svalbard + if ( latitude >= 72.0 && latitude < 84.0 ) + { + if ( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; + else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; + else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; + else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; + } + + return zoneNumber; } -/*! - * Get UTM letter designator for the given latitude. - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com - * - * \param[in] latitude Latitude, in degrees. - * \return UTM letter designator. - * - */ -static char computeLetterDesignator(double latitude) +char Utm::computeLetterDesignator(double latitude) { char LetterDesignator; @@ -124,49 +167,7 @@ static char computeLetterDesignator(double latitude) return LetterDesignator; } -/*! - * Convert latitude and longitude to an UTM zone number. - * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \return UTM zone number. - */ -static int computeZoneNumber(double latitude, double longitude) +double Utm::computeMeridian() const { - int zoneNumber; - - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - - zoneNumber = int((LongTemp + 180)/6) + 1; - - if ( latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) - { - zoneNumber = 32; - } - - // Special zones for Svalbard - if ( latitude >= 72.0 && latitude < 84.0 ) - { - if ( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; - else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; - else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; - else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; - } - - return zoneNumber; + return (zone_number_ == 0) ? 0.0 : (zone_number_ - 1) * 6.0 - 177.0; } - -sbg::Utm sbg::convertLLtoUTM(double latitude, double longitude) -{ - sbg::Utm converted_ll; - - converted_ll.zone_number = computeZoneNumber(latitude, longitude); - converted_ll.meridian = computeMeridian(converted_ll.zone_number); - converted_ll.letter_designator = computeLetterDesignator(latitude); - const auto easting_northing = computeEastingNorthing(latitude, longitude, converted_ll.zone_number); - converted_ll.easting = easting_northing[0]; - converted_ll.northing = easting_northing[1]; - - return (converted_ll); -} \ No newline at end of file From 9c86709f28b63b88f2f998ff96568dc86eccb194 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 18:02:56 +0200 Subject: [PATCH 64/74] Code indentation --- src/sbg_utm.cpp | 156 +++++++++++++++++++++++++++++++++++------------- 1 file changed, 116 insertions(+), 40 deletions(-) diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index 11b41fc..ea147fd 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -55,7 +55,7 @@ char Utm::getLetterDesignator() const std::array Utm::computeEastingNorthing(double latitude, double longitude) const { - constexpr double RADIANS_PER_DEGREE = M_PI/180.0; + constexpr double RADIANS_PER_DEGREE = M_PI / 180.0; // WGS84 Parameters constexpr double WGS84_A = 6378137.0; // major axis @@ -84,24 +84,25 @@ std::array Utm::computeEastingNorthing(double latitude, double longit LongOrigin = (zone_number_ - 1) * 6 - 180 + 3; LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; - eccPrimeSquared = (eccSquared)/(1-eccSquared); + eccPrimeSquared = (eccSquared)/(1 - eccSquared); - N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); - T = tan(LatRad)*tan(LatRad); - C = eccPrimeSquared*cos(LatRad)*cos(LatRad); - A = cos(LatRad)*(LongRad-LongOriginRad); + N = a/sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad)); + T = tan(LatRad) * tan(LatRad); + C = eccPrimeSquared * cos(LatRad) * cos(LatRad); + A = cos(LatRad) * (LongRad-LongOriginRad); - M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad - - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) - + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) - - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); + M = a * ((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256) * LatRad + - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024) * sin(2*LatRad) + + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024) * sin(4*LatRad) + - (35*eccSquared*eccSquared*eccSquared/3072) * sin(6*LatRad)); - double utm_easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) - + 500000.0); + double utm_easting = (double)(k0*N*(A+(1-T+C)*A*A*A/6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); + + double utm_northing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); - double utm_northing = (double)(k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); if (latitude < 0) { @@ -129,10 +130,22 @@ int Utm::computeZoneNumber(double latitude, double longitude) // Special zones for Svalbard if ( latitude >= 72.0 && latitude < 84.0 ) { - if ( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; - else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; - else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; - else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; + if ( LongTemp >= 0.0 && LongTemp < 9.0 ) + { + zoneNumber = 31; + } + else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) + { + zoneNumber = 33; + } + else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) + { + zoneNumber = 35; + } + else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) + { + zoneNumber = 37; + } } return zoneNumber; @@ -142,28 +155,91 @@ char Utm::computeLetterDesignator(double latitude) { char LetterDesignator; - if ((84 >= latitude) && (latitude >= 72)) LetterDesignator = 'X'; - else if ((72 > latitude) && (latitude >= 64)) LetterDesignator = 'W'; - else if ((64 > latitude) && (latitude >= 56)) LetterDesignator = 'V'; - else if ((56 > latitude) && (latitude >= 48)) LetterDesignator = 'U'; - else if ((48 > latitude) && (latitude >= 40)) LetterDesignator = 'T'; - else if ((40 > latitude) && (latitude >= 32)) LetterDesignator = 'S'; - else if ((32 > latitude) && (latitude >= 24)) LetterDesignator = 'R'; - else if ((24 > latitude) && (latitude >= 16)) LetterDesignator = 'Q'; - else if ((16 > latitude) && (latitude >= 8)) LetterDesignator = 'P'; - else if ((8 > latitude) && (latitude >= 0)) LetterDesignator = 'N'; - else if ((0 > latitude) && (latitude >= -8)) LetterDesignator = 'M'; - else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L'; - else if ((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K'; - else if ((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J'; - else if ((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H'; - else if ((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G'; - else if ((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F'; - else if ((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E'; - else if ((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D'; - else if ((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C'; + if ((84 >= latitude) && (latitude >= 72)) + { + LetterDesignator = 'X'; + } + else if ((72 > latitude) && (latitude >= 64)) + { + LetterDesignator = 'W'; + } + else if ((64 > latitude) && (latitude >= 56)) + { + LetterDesignator = 'V'; + } + else if ((56 > latitude) && (latitude >= 48)) + { + LetterDesignator = 'U'; + } + else if ((48 > latitude) && (latitude >= 40)) + { + LetterDesignator = 'T'; + } + else if ((40 > latitude) && (latitude >= 32)) + { + LetterDesignator = 'S'; + } + else if ((32 > latitude) && (latitude >= 24)) + { + LetterDesignator = 'R'; + } + else if ((24 > latitude) && (latitude >= 16)) + { + LetterDesignator = 'Q'; + } + else if ((16 > latitude) && (latitude >= 8)) + { + LetterDesignator = 'P'; + } + else if ((8 > latitude) && (latitude >= 0)) + { + LetterDesignator = 'N'; + } + else if ((0 > latitude) && (latitude >= -8)) + { + LetterDesignator = 'M'; + } + else if ((-8 > latitude) && (latitude >= -16)) + { + LetterDesignator = 'L'; + } + else if ((-16 > latitude) && (latitude >= -24)) + { + LetterDesignator = 'K'; + } + else if ((-24 > latitude) && (latitude >= -32)) + { + LetterDesignator = 'J'; + } + else if ((-32 > latitude) && (latitude >= -40)) + { + LetterDesignator = 'H'; + } + else if ((-40 > latitude) && (latitude >= -48)) + { + LetterDesignator = 'G'; + } + else if ((-48 > latitude) && (latitude >= -56)) + { + LetterDesignator = 'F'; + } + else if ((-56 > latitude) && (latitude >= -64)) + { + LetterDesignator = 'E'; + } + else if ((-64 > latitude) && (latitude >= -72)) + { + LetterDesignator = 'D'; + } + else if ((-72 > latitude) && (latitude >= -80)) + { + LetterDesignator = 'C'; + } + else + { // 'Z' is an error flag, the Latitude is outside the UTM limits - else LetterDesignator = 'Z'; + LetterDesignator = 'Z'; + } return LetterDesignator; } From 6b64d9dce485303f424e212bd871aca258c9ad31 Mon Sep 17 00:00:00 2001 From: cledant Date: Mon, 4 Sep 2023 18:22:49 +0200 Subject: [PATCH 65/74] Class documentation --- include/sbg_driver/sbg_utm.h | 78 ++++++++++++++++++++++++++++++++++-- src/sbg_utm.cpp | 52 +++++++++++++++--------- 2 files changed, 107 insertions(+), 23 deletions(-) diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 7d3cc9e..0e160ee 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -46,18 +46,90 @@ namespace sbg class Utm final { public: + + //---------------------------------------------------------------------// + //- Constructor -// + //---------------------------------------------------------------------// + + /*! + * Default constructor. + */ Utm() = default; + + /*! + * Constructor. + * + * \param[in] latitude Latitude in degree. + * \param[in] longitude Longitude in degree. + */ Utm(double latitude, double longitude); - void init(double latitude, double longitude); - void clear(); - void reset(double latitude, double longitude); + //---------------------------------------------------------------------// + //- Parameters -// + //---------------------------------------------------------------------// + /*! + * Returns if class is initialized. + * + * \return True if class is initialized. + */ bool isInit() const; + + /*! + * Returns UTM zone number. + * + * \return Zone number. + */ int getZoneNumber() const; + + /*! + * Returns UTM meridian. + * + * \return Meridian in degree. + */ double getMeridian() const; + + /*! + * Returns UTM letter designator. + * + * \return Letter designator. + */ char getLetterDesignator() const; + //---------------------------------------------------------------------// + //- Operations -// + //---------------------------------------------------------------------// + + /*! + * Initialize UTM zone. + * + * \param[in] latitude Latitude in degree. + * \param[in] longitude Longitude in degree. + */ + void init(double latitude, double longitude); + + /*! + * Clear values and set them to zero / false. + */ + void clear(); + + /*! + * Clear and initialize UTM zone. + * + * \param[in] latitude Latitude in degree. + * \param[in] longitude Longitude in degree. + */ + void reset(double latitude, double longitude); + + /*! + * Convert latitude, longitude, to easting and northing. + * Originally written by Chuck Gantz- chuck.gantz@globalstar.com + * + * \param[in] latitude Latitude, in degrees. + * \param[in] longitude Longitude, in degrees. + * \return Array containing easting then northing. + * + */ std::array computeEastingNorthing(double latitude, double longitude) const; private: diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index ea147fd..6fead16 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -6,11 +6,43 @@ using sbg::Utm; +//---------------------------------------------------------------------// +//- Constructor -// +//---------------------------------------------------------------------// + Utm::Utm(double latitude, double longitude) { init(latitude, longitude); } +//---------------------------------------------------------------------// +//- Parameters -// +//---------------------------------------------------------------------// + +bool Utm::isInit() const +{ + return is_init_; +} + +int Utm::getZoneNumber() const +{ + return zone_number_; +} + +double Utm::getMeridian() const +{ + return meridian_; +} + +char Utm::getLetterDesignator() const +{ + return letter_designator_; +} + +//---------------------------------------------------------------------// +//- Operations -// +//---------------------------------------------------------------------// + void Utm::init(double latitude, double longitude) { zone_number_ = computeZoneNumber(latitude, longitude); @@ -33,26 +65,6 @@ void Utm::reset(double latitude, double longitude) init(latitude, longitude); } -bool Utm::isInit() const -{ - return is_init_; -} - -int Utm::getZoneNumber() const -{ - return zone_number_; -} - -double Utm::getMeridian() const -{ - return meridian_; -} - -char Utm::getLetterDesignator() const -{ - return letter_designator_; -} - std::array Utm::computeEastingNorthing(double latitude, double longitude) const { constexpr double RADIANS_PER_DEGREE = M_PI / 180.0; From 2e605858915008eb6844d60c12bf921c114e4869 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Tue, 5 Sep 2023 10:03:51 +0200 Subject: [PATCH 66/74] WIP code cleanup --- include/sbg_driver/sbg_utm.h | 43 ++++++------- src/sbg_utm.cpp | 114 ++++++++++++++++------------------- 2 files changed, 69 insertions(+), 88 deletions(-) diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 0e160ee..5a463e8 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -3,9 +3,7 @@ * \author SBG Systems * \date 25/08/2023 * -* \brief Handle creation of utm class. -* -* Methods to create Utm from given data. +* \brief Implement simple UTM projections * * \section CodeCopyright Copyright Notice * MIT License @@ -31,8 +29,8 @@ * SOFTWARE. */ -#ifndef SBG_ROS_UTM_H -#define SBG_ROS_UTM_H +#ifndef SBG_UTM_H +#define SBG_UTM_H // STL headers #include @@ -59,8 +57,8 @@ class Utm final /*! * Constructor. * - * \param[in] latitude Latitude in degree. - * \param[in] longitude Longitude in degree. + * \param[in] latitude Latitude in degree [-90 to +90]. + * \param[in] longitude Longitude in degree [-180 to +180]. */ Utm(double latitude, double longitude); @@ -69,9 +67,9 @@ class Utm final //---------------------------------------------------------------------// /*! - * Returns if class is initialized. + * Returns if the UTM zone has been initialized. * - * \return True if class is initialized. + * \return True if the UTM zone has been initialized. */ bool isInit() const; @@ -103,31 +101,24 @@ class Utm final /*! * Initialize UTM zone. * - * \param[in] latitude Latitude in degree. - * \param[in] longitude Longitude in degree. + * \param[in] latitude Latitude in degree [-90 to +90]. + * \param[in] longitude Longitude in degree [-180 to +180]. */ void init(double latitude, double longitude); /*! - * Clear values and set them to zero / false. + * Reset the instance to uninitialized UTM zone. */ void clear(); - /*! - * Clear and initialize UTM zone. - * - * \param[in] latitude Latitude in degree. - * \param[in] longitude Longitude in degree. - */ - void reset(double latitude, double longitude); - /*! * Convert latitude, longitude, to easting and northing. - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com + * + * Originally written by Chuck Gantz - chuck.gantz@globalstar.com * - * \param[in] latitude Latitude, in degrees. - * \param[in] longitude Longitude, in degrees. - * \return Array containing easting then northing. + * \param[in] latitude Latitude, in degrees [-90 to +90]. + * \param[in] longitude Longitude, in degrees [-180 to +180]. + * \return Array containing easting then northing in meters. * */ std::array computeEastingNorthing(double latitude, double longitude) const; @@ -145,7 +136,7 @@ class Utm final /*! * Get UTM letter designator for the given latitude. - * Originally written by Chuck Gantz- chuck.gantz@globalstar.com + * Originally written by Chuck Gantz - chuck.gantz@globalstar.com * * \param[in] latitude Latitude, in degrees. * \return UTM letter designator. @@ -167,4 +158,4 @@ class Utm final } -#endif // SBG_ROS_UTM_H +#endif // SBG_UTM_H diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index 6fead16..c328d75 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -59,12 +59,6 @@ void Utm::clear() letter_designator_ = {}; } -void Utm::reset(double latitude, double longitude) -{ - clear(); - init(latitude, longitude); -} - std::array Utm::computeEastingNorthing(double latitude, double longitude) const { constexpr double RADIANS_PER_DEGREE = M_PI / 180.0; @@ -77,43 +71,39 @@ std::array Utm::computeEastingNorthing(double latitude, double longit constexpr double UTM_K0 = 0.9996; // scale factor constexpr double UTM_E2 = (WGS84_E * WGS84_E); // e^2 - constexpr double a = WGS84_A; - constexpr double eccSquared = UTM_E2; - constexpr double k0 = UTM_K0; - - double LongOrigin; - double eccPrimeSquared; - double N, T, C, A, M; + double long_origin; + double ecc_prime_squared; + double n, t, c, a, m; // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + double long_temp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - double LatRad = latitude * RADIANS_PER_DEGREE; - double LongRad = LongTemp * RADIANS_PER_DEGREE; - double LongOriginRad; + double lat_rad = latitude * RADIANS_PER_DEGREE; + double long_rad = long_temp * RADIANS_PER_DEGREE; + double long_origin_rad; // +3 puts origin in middle of zone - LongOrigin = (zone_number_ - 1) * 6 - 180 + 3; - LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; + long_origin = (zone_number_ - 1) * 6 - 180 + 3; + long_origin_rad = long_origin * RADIANS_PER_DEGREE; - eccPrimeSquared = (eccSquared)/(1 - eccSquared); + ecc_prime_squared = (UTM_E2)/(1 - UTM_E2); - N = a/sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad)); - T = tan(LatRad) * tan(LatRad); - C = eccPrimeSquared * cos(LatRad) * cos(LatRad); - A = cos(LatRad) * (LongRad-LongOriginRad); + n = WGS84_A/sqrt(1 - UTM_E2 * sin(lat_rad) * sin(lat_rad)); + t = tan(lat_rad) * tan(lat_rad); + c = ecc_prime_squared * cos(lat_rad) * cos(lat_rad); + a = cos(lat_rad) * (long_rad-long_origin_rad); - M = a * ((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256) * LatRad - - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024) * sin(2*LatRad) - + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024) * sin(4*LatRad) - - (35*eccSquared*eccSquared*eccSquared/3072) * sin(6*LatRad)); + m = WGS84_A * ((1 - UTM_E2/4 - 3*UTM_E2*UTM_E2/64 - 5*UTM_E2*UTM_E2*UTM_E2/256) * lat_rad + - (3 * UTM_E2/8 + 3*UTM_E2*UTM_E2/32 + 45*UTM_E2*UTM_E2*UTM_E2/1024) * sin(2*lat_rad) + + (15*UTM_E2*UTM_E2/256 + 45*UTM_E2*UTM_E2*UTM_E2/1024) * sin(4*lat_rad) + - (35*UTM_E2*UTM_E2*UTM_E2/3072) * sin(6*lat_rad)); - double utm_easting = (double)(k0*N*(A+(1-T+C)*A*A*A/6 - + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + double utm_easting = (double)(UTM_K0*n*(a+(1-t+c)*a*a*a/6 + + (5-18*t+t*t+72*c-58*ecc_prime_squared)*a*a*a*a*a/120) + 500000.0); - double utm_northing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 - + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + double utm_northing = (double)(UTM_K0*(m+n*tan(lat_rad)*(a*a/2+(5-t+9*c+4*c*c)*a*a*a*a/24 + + (61-58*t+t*t+600*c-330*ecc_prime_squared)*a*a*a*a*a*a/720))); if (latitude < 0) @@ -130,11 +120,11 @@ int Utm::computeZoneNumber(double latitude, double longitude) int zoneNumber; // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; + double long_temp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - zoneNumber = int((LongTemp + 180)/6) + 1; + zoneNumber = int((long_temp + 180)/6) + 1; - if ( latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + if ( latitude >= 56.0 && latitude < 64.0 && long_temp >= 3.0 && long_temp < 12.0 ) { zoneNumber = 32; } @@ -142,19 +132,19 @@ int Utm::computeZoneNumber(double latitude, double longitude) // Special zones for Svalbard if ( latitude >= 72.0 && latitude < 84.0 ) { - if ( LongTemp >= 0.0 && LongTemp < 9.0 ) + if ( long_temp >= 0.0 && long_temp < 9.0 ) { zoneNumber = 31; } - else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) + else if ( long_temp >= 9.0 && long_temp < 21.0 ) { zoneNumber = 33; } - else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) + else if ( long_temp >= 21.0 && long_temp < 33.0 ) { zoneNumber = 35; } - else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) + else if ( long_temp >= 33.0 && long_temp < 42.0 ) { zoneNumber = 37; } @@ -165,94 +155,94 @@ int Utm::computeZoneNumber(double latitude, double longitude) char Utm::computeLetterDesignator(double latitude) { - char LetterDesignator; + char letter_designator; if ((84 >= latitude) && (latitude >= 72)) { - LetterDesignator = 'X'; + letter_designator = 'X'; } else if ((72 > latitude) && (latitude >= 64)) { - LetterDesignator = 'W'; + letter_designator = 'W'; } else if ((64 > latitude) && (latitude >= 56)) { - LetterDesignator = 'V'; + letter_designator = 'V'; } else if ((56 > latitude) && (latitude >= 48)) { - LetterDesignator = 'U'; + letter_designator = 'U'; } else if ((48 > latitude) && (latitude >= 40)) { - LetterDesignator = 'T'; + letter_designator = 'T'; } else if ((40 > latitude) && (latitude >= 32)) { - LetterDesignator = 'S'; + letter_designator = 'S'; } else if ((32 > latitude) && (latitude >= 24)) { - LetterDesignator = 'R'; + letter_designator = 'R'; } else if ((24 > latitude) && (latitude >= 16)) { - LetterDesignator = 'Q'; + letter_designator = 'Q'; } else if ((16 > latitude) && (latitude >= 8)) { - LetterDesignator = 'P'; + letter_designator = 'P'; } else if ((8 > latitude) && (latitude >= 0)) { - LetterDesignator = 'N'; + letter_designator = 'N'; } else if ((0 > latitude) && (latitude >= -8)) { - LetterDesignator = 'M'; + letter_designator = 'M'; } else if ((-8 > latitude) && (latitude >= -16)) { - LetterDesignator = 'L'; + letter_designator = 'L'; } else if ((-16 > latitude) && (latitude >= -24)) { - LetterDesignator = 'K'; + letter_designator = 'K'; } else if ((-24 > latitude) && (latitude >= -32)) { - LetterDesignator = 'J'; + letter_designator = 'J'; } else if ((-32 > latitude) && (latitude >= -40)) { - LetterDesignator = 'H'; + letter_designator = 'H'; } else if ((-40 > latitude) && (latitude >= -48)) { - LetterDesignator = 'G'; + letter_designator = 'G'; } else if ((-48 > latitude) && (latitude >= -56)) { - LetterDesignator = 'F'; + letter_designator = 'F'; } else if ((-56 > latitude) && (latitude >= -64)) { - LetterDesignator = 'E'; + letter_designator = 'E'; } else if ((-64 > latitude) && (latitude >= -72)) { - LetterDesignator = 'D'; + letter_designator = 'D'; } else if ((-72 > latitude) && (latitude >= -80)) { - LetterDesignator = 'C'; + letter_designator = 'C'; } else { // 'Z' is an error flag, the Latitude is outside the UTM limits - LetterDesignator = 'Z'; + letter_designator = 'Z'; } - return LetterDesignator; + return letter_designator; } double Utm::computeMeridian() const From 9dece1ce788123a17e74bfdd65fad1f7bb44c87d Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 5 Sep 2023 11:26:05 +0200 Subject: [PATCH 67/74] Variable naming --- src/sbg_utm.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index c328d75..a5592e5 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -117,16 +117,16 @@ std::array Utm::computeEastingNorthing(double latitude, double longit int Utm::computeZoneNumber(double latitude, double longitude) { - int zoneNumber; + int zone_number; // Make sure the longitude is between -180.00 .. 179.9 double long_temp = (longitude + 180) - int((longitude + 180) / 360) * 360 - 180; - zoneNumber = int((long_temp + 180)/6) + 1; + zone_number = int((long_temp + 180) / 6) + 1; if ( latitude >= 56.0 && latitude < 64.0 && long_temp >= 3.0 && long_temp < 12.0 ) { - zoneNumber = 32; + zone_number = 32; } // Special zones for Svalbard @@ -134,23 +134,23 @@ int Utm::computeZoneNumber(double latitude, double longitude) { if ( long_temp >= 0.0 && long_temp < 9.0 ) { - zoneNumber = 31; + zone_number = 31; } else if ( long_temp >= 9.0 && long_temp < 21.0 ) { - zoneNumber = 33; + zone_number = 33; } else if ( long_temp >= 21.0 && long_temp < 33.0 ) { - zoneNumber = 35; + zone_number = 35; } else if ( long_temp >= 33.0 && long_temp < 42.0 ) { - zoneNumber = 37; + zone_number = 37; } } - return zoneNumber; + return zone_number; } char Utm::computeLetterDesignator(double latitude) From be1d61719f06def124264ddb4c33856c21434964 Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 5 Sep 2023 13:52:49 +0200 Subject: [PATCH 68/74] Moved LLAtoECEF into a helper --- include/sbg_driver/message_wrapper.h | 5 ----- include/sbg_driver/sbg_ros_helpers.h | 13 +++++++++++++ src/message_wrapper.cpp | 22 ++++------------------ src/sbg_ros_helpers.cpp | 21 +++++++++++++++++++++ 4 files changed, 38 insertions(+), 23 deletions(-) diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index d058f2c..9d771e6 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -104,11 +104,6 @@ class MessageWrapper : public rclcpp::Node double first_valid_northing_{}; double first_valid_altitude_{}; - static constexpr double equatorial_radius_ = 6378137.0; - static constexpr double polar_radius_ = 6356752.314245; - double point_stamped_compute_cte_{}; - double eccentricity_{}; - //---------------------------------------------------------------------// //- Internal methods -// //---------------------------------------------------------------------// diff --git a/include/sbg_driver/sbg_ros_helpers.h b/include/sbg_driver/sbg_ros_helpers.h index 3dc842a..1fd87de 100644 --- a/include/sbg_driver/sbg_ros_helpers.h +++ b/include/sbg_driver/sbg_ros_helpers.h @@ -37,6 +37,9 @@ // SbgECom headers #include +// Sbg headers +#include + // STL headers #include @@ -119,6 +122,16 @@ namespace sbg::helpers * \return NMEA GPS type */ NmeaGGAQuality convertSbgGpsTypeToNmeaGpsType(SbgEComGpsPosType sbg_gps_type); + + /* + * Convert latitude, longitude, altitude to ECEF coordinates. + * + * \param[in] latitude Latitude, in degrees [-90 to +90]. + * \param[in] longitude Longitude, in degrees [-180 to +180]. + * \param[in] altitude Altitude, in meters. + * \return Vector containing ECEF coordinates in meters. + */ + sbg::SbgVector3d convertLLAtoECEF(double latitude, double longitude, double altitude); } #endif // #ifndef SBG_ROS_ROS_HELPERS_H diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 1745274..e5be00b 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -29,9 +29,6 @@ Node("tf_broadcaster") first_valid_utc_ = false; tf_broadcaster_ = std::make_shared(this); static_tf_broadcaster_ = std::make_shared(this); - - point_stamped_compute_cte_ = pow(polar_radius_, 2) / pow(equatorial_radius_, 2); - eccentricity_ = 1.0 - point_stamped_compute_cte_; } //---------------------------------------------------------------------// @@ -1021,21 +1018,10 @@ const geometry_msgs::msg::PointStamped MessageWrapper::createRosPointStampedMess point_stamped_message.header = createRosHeader(ref_sbg_ekf_msg.time_stamp); - // - // Conversion from Geodetic coordinates to ECEF is based on World Geodetic System 1984 (WGS84). - // Radius are expressed in meters, and latitude/longitude in radian. - // - double prime_vertical_radius; - double latitude; - double longitude; - - latitude = sbgDegToRadD(ref_sbg_ekf_msg.latitude); - longitude = sbgDegToRadD(ref_sbg_ekf_msg.longitude); - prime_vertical_radius = equatorial_radius_ / sqrt(1.0 - (pow(eccentricity_, 2) * pow(sin(latitude), 2))); - - point_stamped_message.point.x = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) * cos(latitude) * cos(longitude); - point_stamped_message.point.y = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) * cos(latitude) * sin(longitude); - point_stamped_message.point.z = fma(point_stamped_compute_cte_, prime_vertical_radius, ref_sbg_ekf_msg.altitude) * sin(latitude); + const auto ecef_coordinates = helpers::convertLLAtoECEF(ref_sbg_ekf_msg.latitude, ref_sbg_ekf_msg.longitude, ref_sbg_ekf_msg.altitude); + point_stamped_message.point.x = ecef_coordinates(0); + point_stamped_message.point.y = ecef_coordinates(1); + point_stamped_message.point.z = ecef_coordinates(2); return point_stamped_message; } diff --git a/src/sbg_ros_helpers.cpp b/src/sbg_ros_helpers.cpp index ec38814..dcb2779 100644 --- a/src/sbg_ros_helpers.cpp +++ b/src/sbg_ros_helpers.cpp @@ -139,4 +139,25 @@ sbg::helpers::NmeaGGAQuality sbg::helpers::convertSbgGpsTypeToNmeaGpsType(SbgECo } return nmeaQuality; +} + +sbg::SbgVector3d sbg::helpers::convertLLAtoECEF(double latitude, double longitude, double altitude) +{ + // + // Conversion from Geodetic coordinates to ECEF is based on World Geodetic System 1984 (WGS84). + // Radius are expressed in meters, and latitude/longitude in radian. + // + static constexpr double EQUATORIAL_RADIUS = 6378137.0; + static constexpr double POLAR_RADIUS = 6356752.314245; + + double latitude_rad = sbgDegToRadD(latitude); + double longitude_rad = sbgDegToRadD(longitude); + double compute_cte = pow(POLAR_RADIUS, 2) / pow(EQUATORIAL_RADIUS, 2); + double eccentricity = 1.0 - compute_cte; + double prime_vertical_radius = EQUATORIAL_RADIUS / sqrt(1.0 - (pow(eccentricity, 2) * pow(sin(latitude_rad), 2))); + + sbg::SbgVector3d ecef_vector((prime_vertical_radius + altitude) * cos(latitude_rad) * cos(longitude_rad), + (prime_vertical_radius + altitude) * cos(latitude_rad) * sin(longitude_rad), + fma(compute_cte, prime_vertical_radius, altitude) * sin(latitude_rad)); + return ecef_vector; } \ No newline at end of file From 59879d96a3894277cef274d38cc2a4e49a396cbd Mon Sep 17 00:00:00 2001 From: cledant Date: Tue, 5 Sep 2023 13:56:47 +0200 Subject: [PATCH 69/74] Cleanup --- include/sbg_driver/sbg_utm.h | 3 --- src/sbg_utm.cpp | 6 ++++++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/sbg_driver/sbg_utm.h b/include/sbg_driver/sbg_utm.h index 5a463e8..68445ac 100644 --- a/include/sbg_driver/sbg_utm.h +++ b/include/sbg_driver/sbg_utm.h @@ -113,8 +113,6 @@ class Utm final /*! * Convert latitude, longitude, to easting and northing. - * - * Originally written by Chuck Gantz - chuck.gantz@globalstar.com * * \param[in] latitude Latitude, in degrees [-90 to +90]. * \param[in] longitude Longitude, in degrees [-180 to +180]. @@ -136,7 +134,6 @@ class Utm final /*! * Get UTM letter designator for the given latitude. - * Originally written by Chuck Gantz - chuck.gantz@globalstar.com * * \param[in] latitude Latitude, in degrees. * \return UTM letter designator. diff --git a/src/sbg_utm.cpp b/src/sbg_utm.cpp index a5592e5..b49e84c 100644 --- a/src/sbg_utm.cpp +++ b/src/sbg_utm.cpp @@ -59,6 +59,9 @@ void Utm::clear() letter_designator_ = {}; } +/* + * Originally written by Chuck Gantz - chuck.gantz@globalstar.com + */ std::array Utm::computeEastingNorthing(double latitude, double longitude) const { constexpr double RADIANS_PER_DEGREE = M_PI / 180.0; @@ -153,6 +156,9 @@ int Utm::computeZoneNumber(double latitude, double longitude) return zone_number; } +/* + * Originally written by Chuck Gantz - chuck.gantz@globalstar.com + */ char Utm::computeLetterDesignator(double latitude) { char letter_designator; From 4f5ffcaff8ebecbb879e1eb3dda2fddca495cea0 Mon Sep 17 00:00:00 2001 From: SanderVanDijk-StreetDrone <91061301+SanderVanDijk-StreetDrone@users.noreply.github.com> Date: Thu, 27 Oct 2022 14:49:17 +0100 Subject: [PATCH 70/74] Fix deprecated use of rosidl_target_interfaces The use of rosidl_target_interfaces is deprecated (see [Humble release notes](http://docs.ros.org.ros.informatik.uni-freiburg.de/en/humble/Releases/Release-Humble-Hawksbill.html#deprecation-of-rosidl-target-interfaces). Here that actually causes an issue with CMake not setting the right include directory paths, breaking `colcon build` on humble. This applies the documented update, making the driver build under Humble (cherry picked from commit 84dd91a5e323ffb65255f8b1e2b68a26f4af60d6) --- CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 260c7a0..152b17a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,8 +127,9 @@ target_link_libraries(sbg_device_mag sbgECom) ament_target_dependencies(sbg_device ${USED_LIBRARIES}) ament_target_dependencies(sbg_device_mag ${USED_LIBRARIES}) -rosidl_target_interfaces(sbg_device ${PROJECT_NAME} "rosidl_typesupport_cpp") -rosidl_target_interfaces(sbg_device_mag ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") +target_link_libraries(sbg_device "${cpp_typesupport_target}") +target_link_libraries(sbg_device_mag "${cpp_typesupport_target}") set_property(TARGET sbg_device PROPERTY CXX_STANDARD 14) set_property(TARGET sbg_device_mag PROPERTY CXX_STANDARD 14) From 766770ac046134fac302e470f6c94a08b87f7a4a Mon Sep 17 00:00:00 2001 From: VladimirL Date: Thu, 23 Mar 2023 12:29:06 +0300 Subject: [PATCH 71/74] time_reference parameter fix (cherry picked from commit 08727555c4f681db43b3f3a8def041a8956923d3) --- src/config_store.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config_store.cpp b/src/config_store.cpp index eb4d22f..99b0dba 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -435,7 +435,7 @@ void ConfigStore::loadFromRosNodeHandle(const rclcpp::Node& ref_node_handle) loadRtcmParameters(ref_node_handle); loadNmeaParameters(ref_node_handle); - loadOutputTimeReference(ref_node_handle, "output/time_reference"); + loadOutputTimeReference(ref_node_handle, "output.time_reference"); loadOutputConfiguration(ref_node_handle, "output.log_status", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_STATUS); loadOutputConfiguration(ref_node_handle, "output.log_imu_data", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_DATA); From 7e1e1eedc86671b25793ece73c58f4d0eeb5794a Mon Sep 17 00:00:00 2001 From: Timon Mentink Date: Wed, 29 Nov 2023 09:13:49 +0100 Subject: [PATCH 72/74] Remove boost dependency (cherry picked from commit ab54c33f1e442c3737dd8e1c09a8b6f36c2c1afa) --- CMakeLists.txt | 1 - package.xml | 1 - src/sbg_device.cpp | 27 +++++++++------------------ 3 files changed, 9 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 152b17a..b058924 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(tf2_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(nmea_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) -find_package(Boost REQUIRED) ################################################ ## Declare ROS messages, services and actions ## diff --git a/package.xml b/package.xml index 06e4bad..39edaf1 100644 --- a/package.xml +++ b/package.xml @@ -25,7 +25,6 @@ tf2_msgs tf2_geometry_msgs nmea_msgs - boost rosidl_default_runtime diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 3402544..8ea3d99 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -6,12 +6,6 @@ #include #include -// Boost headers -#include -#include -#include -#include - // SbgECom headers #include @@ -19,16 +13,13 @@ using namespace std; using sbg::SbgDevice; // From ros_com/recorder -std::string timeToStr() //rclcpp::WallTimer> ros_t) //TODO: FIXME +std::string timeToStr() { - //(void)ros_t; - std::stringstream msg; - const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time(); - boost::posix_time::time_facet *const f = new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S"); - msg.imbue(std::locale(msg.getloc(),f)); - msg << now; - - return msg.str(); + std::stringstream msg; + auto now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + auto tm = *std::localtime(&now); + msg << std::put_time(&tm, "%Y-%m-%d-%H-%M-%S"); + return msg.str(); } // @@ -215,7 +206,7 @@ void SbgDevice::initSubscribers() auto rtcm_cb = [&](const rtcm_msgs::msg::Message::SharedPtr msg) -> void { this->writeRtcmMessageToDevice(msg); }; - + rtcm_sub_ = ref_node_.create_subscription(config_store_.getRtcmFullTopic(), 10, rtcm_cb); } } @@ -455,7 +446,7 @@ void SbgDevice::exportMagCalibrationResults() const mag_results_stream << mag_calib_results_.matrix[3] << "\t" << mag_calib_results_.matrix[4] << "\t" << mag_calib_results_.matrix[5] << endl; mag_results_stream << mag_calib_results_.matrix[6] << "\t" << mag_calib_results_.matrix[7] << "\t" << mag_calib_results_.matrix[8] << endl; - output_filename = "mag_calib_" + timeToStr()/*(nullptrrclcpp::WallTimer::now())*/ + ".txt"; + output_filename = "mag_calib_" + timeToStr() + ".txt"; ofstream output_file(output_filename); output_file << mag_results_stream.str(); output_file.close(); @@ -468,7 +459,7 @@ void SbgDevice::writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPt { auto rtcm_data = msg->message; auto error_code = sbgInterfaceWrite(&sbg_interface_, rtcm_data.data(), rtcm_data.size()); - + if (error_code != SBG_NO_ERROR) { char error_str[256]; From fdbd2a6a76c2ce52efbdc14beb53270f273f24fe Mon Sep 17 00:00:00 2001 From: Christoph Gruber Date: Tue, 24 Sep 2024 15:27:59 +0200 Subject: [PATCH 73/74] uart: set baudrate as configured in config file --- include/sbg_driver/sbg_device.h | 18 ++++++- src/sbg_device.cpp | 86 +++++++++++++++++++++++++++++++-- 2 files changed, 100 insertions(+), 4 deletions(-) diff --git a/include/sbg_driver/sbg_device.h b/include/sbg_driver/sbg_device.h index 7741ef4..520168f 100644 --- a/include/sbg_driver/sbg_device.h +++ b/include/sbg_driver/sbg_device.h @@ -126,8 +126,24 @@ class SbgDevice * Read the device informations. * * \throw Unable to read the device information. + * \return SBG_NO_ERROR if reading device info succeeded. */ - void readDeviceInfo(); + SbgErrorCode readDeviceInfo(); + + /*! + * Find the baudrate currently configured on the device. Leaves the ECOM interface in an initialized state. + * + * \return SBG_NO_ERROR if the current device baudrate was found. + * \throw Unable to read the device information. + */ + SbgErrorCode findCurrentDeviceBaudrate(); + + /*! + * Use the current baudrate to set the baudrate configured in the config file. + * + * \throw Unable to read the device information. + */ + void setDeviceBaudrate(); /*! * Get the SBG version as a string. diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 8ea3d99..138ef84 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -157,13 +157,90 @@ void SbgDevice::connect() rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code))); } - readDeviceInfo(); + error_code = readDeviceInfo(); + + if (error_code == SBG_NO_ERROR) + { + return; + } + + if (error_code == SBG_TIME_OUT && config_store_.isInterfaceSerial()) + { + // readDeviceInfo() not successful - error could be that the baudrate configured on the device + // is different from the one configured in the config file. Re-try with different baudrate + error_code = findCurrentDeviceBaudrate(); + } + + if (error_code == SBG_NO_ERROR) + { + setDeviceBaudrate(); + } } -void SbgDevice::readDeviceInfo() +SbgErrorCode SbgDevice::findCurrentDeviceBaudrate() +{ + SbgErrorCode error_code; + error_code = SBG_ERROR; + static constexpr uint32_t kBaudRates[8] = {115200, 921600, 460800, 230400, 38400, 19200, 9600, 4800}; + + for (const auto br : kBaudRates) + { + RCLCPP_INFO(ref_node_.get_logger(), "Not successful with %d bps, trying with %d bps instead", config_store_.getBaudRate(), br); + + sbgEComClose(&com_handle_); + sbgInterfaceSerialDestroy(&sbg_interface_); + + error_code = sbgInterfaceSerialCreate(&sbg_interface_, config_store_.getUartPortName().c_str(), br); + + if (error_code != SBG_NO_ERROR) + { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the interface - " + std::string(sbgErrorCodeToString(error_code))); + } + + error_code = sbgEComInit(&com_handle_, &sbg_interface_); + + if (error_code != SBG_NO_ERROR) + { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code))); + } + + error_code = readDeviceInfo(); + + if (error_code == SBG_NO_ERROR) { + // current device baud rate found + return SBG_NO_ERROR; + } + } + return error_code; +} + +void SbgDevice::setDeviceBaudrate() +{ + SbgErrorCode error_code; + error_code = sbgInterfaceSerialChangeBaudrate(&sbg_interface_, config_store_.getBaudRate()); + sbgEComClose(&com_handle_); + sbgInterfaceSerialDestroy(&sbg_interface_); + + error_code = sbgInterfaceSerialCreate(&sbg_interface_, config_store_.getUartPortName().c_str(), config_store_.getBaudRate()); + + if (error_code != SBG_NO_ERROR) + { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the interface - " + std::string(sbgErrorCodeToString(error_code))); + } + + error_code = sbgEComInit(&com_handle_, &sbg_interface_); + + if (error_code != SBG_NO_ERROR) + { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code))); + } + RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - successfully reconfigured baudrate to %d", config_store_.getBaudRate()); +} + +SbgErrorCode SbgDevice::readDeviceInfo() { SbgEComDeviceInfo device_info; - SbgErrorCode error_code; + SbgErrorCode error_code; error_code = sbgEComCmdGetInfo(&com_handle_, &device_info); @@ -177,11 +254,14 @@ void SbgDevice::readDeviceInfo() RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - hardwareRev = %s", getVersionAsString(device_info.hardwareRev).c_str()); RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - firmwareRev = %s", getVersionAsString(device_info.firmwareRev).c_str()); + return SBG_NO_ERROR; } else { RCLCPP_ERROR(ref_node_.get_logger(), "Unable to get the device Info : %s", sbgErrorCodeToString(error_code)); } + + return error_code; } std::string SbgDevice::getVersionAsString(uint32 sbg_version_enc) const From ebf06abca0545f03e18f5aa1f03949886b80aa2b Mon Sep 17 00:00:00 2001 From: Christoph Gruber Date: Tue, 1 Oct 2024 16:38:32 +0200 Subject: [PATCH 74/74] fix: change device baudrate, not local interface baudrate --- src/sbg_device.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index 138ef84..f1b0aab 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -8,6 +8,7 @@ // SbgECom headers #include +#include using namespace std; using sbg::SbgDevice; @@ -217,7 +218,23 @@ SbgErrorCode SbgDevice::findCurrentDeviceBaudrate() void SbgDevice::setDeviceBaudrate() { SbgErrorCode error_code; - error_code = sbgInterfaceSerialChangeBaudrate(&sbg_interface_, config_store_.getBaudRate()); + SbgEComInterfaceConf com_conf; + error_code = sbgEComCmdInterfaceGetUartConf(&com_handle_, SBG_ECOM_IF_COM_A, &com_conf); + if (error_code != SBG_NO_ERROR) { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Reconfig] Unable to get config of device - " + std::string(sbgErrorCodeToString(error_code))); + } + + com_conf.baudRate = config_store_.getBaudRate(); + error_code = sbgEComCmdInterfaceSetUartConf(&com_handle_, SBG_ECOM_IF_COM_A, &com_conf); + if (error_code != SBG_NO_ERROR) { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Reconfig] Unable to set new baudrate of device - " + std::string(sbgErrorCodeToString(error_code))); + } + + error_code = sbgEComCmdSettingsAction(&com_handle_, SBG_ECOM_SAVE_SETTINGS); + if (error_code != SBG_NO_ERROR) { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Reconfig] Unable to save settings on device - " + std::string(sbgErrorCodeToString(error_code))); + } + sbgEComClose(&com_handle_); sbgInterfaceSerialDestroy(&sbg_interface_);