From 29214f20e993a8857720f50b16aa3ead09e64947 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Oct 2022 14:11:14 +0200 Subject: [PATCH 001/170] Add local to ecef transforms --- .../parsers/parsing_utilities.hpp | 50 +++++++++- .../communication/rx_message.cpp | 11 ++- .../parsers/parsing_utilities.cpp | 98 ++++++++++++++++--- 3 files changed, 139 insertions(+), 20 deletions(-) diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index 3c38055c..e5b69a39 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -300,12 +300,54 @@ namespace parsing_utilities { /** * @brief Transforms Euler angles to a quaternion - * @param[in] yaw Yaw, i.e. heading, about the Up-axis - * @param[in] pitch Pitch about the new North-axis - * @param[in] roll Roll about the new East-axis + * @param[in] yaw Yaw, i.e. heading, about the z-axis [rad] + * @param[in] pitch Pitch about the new y-axis [rad] + * @param[in] roll Roll about the new y-axis [rad] + * @return quaternion + */ + Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, + double yaw); + + /** + * @brief Transforms Euler angles to a QuaternionMsg + * @param[in] yaw Yaw, i.e. heading, about the z-axis [rad] + * @param[in] pitch Pitch about the new y-axis [rad] + * @param[in] roll Roll about the new x-axis [rad] * @return ROS message representing a quaternion */ - QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll); + QuaternionMsg convertEulerToQuaternionMsg(double yaw, double pitch, double roll); + + /** + * @brief Generates the quaternion to rotate from local ENU to ECEF + * @param[in] lat gedoetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return quaternion + */ + Eigen::Quaterniond q_enu_ecef(double lat, double lon); + + /** + * @brief Generates the quaternion to rotate from local NED to ECEF + * @param[in] lat geodetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return rotation matrix + */ + Eigen::Quaterniond q_ned_ecef(double lat, double lon); + + /** + * @brief Generates the matrix to rotate from local ENU to ECEF + * @param[in] lat geodetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return rotation matrix + */ + Eigen::Matrix3d R_enu_ecef(double lat, double lon); + + /** + * @brief Generates the matrix to rotate from local NED to ECEF + * @param[in] lat geodetic latitude [rad] + * @param[in] lon geodetic longitude [rad] + * @return rotation matrix + */ + Eigen::Matrix3d R_ned_ecef(double lat, double lon); /** * @brief Transforms the input polling period [milliseconds] into a std::string diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 8ea09e94..ca8c9194 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -67,7 +67,7 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() double roll = 0.0; if (validValue(last_atteuler_.roll)) roll = last_atteuler_.roll; - msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion( + msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternionMsg( deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); @@ -126,8 +126,9 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() if (validValue(last_insnavgeod_.roll)) roll = last_insnavgeod_.roll; // Attitude - msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion( - deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); + msg.pose.pose.orientation = + parsing_utilities::convertEulerToQuaternionMsg( + deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); } else { msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); @@ -330,7 +331,7 @@ ImuMsg io_comm_rx::RxMessage::ImuCallback() validValue(last_insnavgeod_.roll)) { msg.orientation = - parsing_utilities::convertEulerToQuaternion( + parsing_utilities::convertEulerToQuaternionMsg( deg2rad(last_insnavgeod_.heading), deg2rad(last_insnavgeod_.pitch), deg2rad(last_insnavgeod_.roll)); @@ -711,7 +712,7 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() { // Attitude msg.pose.pose.orientation = - parsing_utilities::convertEulerToQuaternion(yaw, pitch, roll); + parsing_utilities::convertEulerToQuaternionMsg(yaw, pitch, roll); } else { msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index c7e3502e..dd9246a4 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -303,12 +303,11 @@ namespace parsing_utilities { //! The rotational sequence convention we adopt here (and Septentrio receivers' //! pitch, roll, yaw definition too) is the yaw-pitch-roll sequence, i.e. the - //! 3-2-1 sequence: The body first does yaw around the Z=Down-axis, then pitches - //! around the new Y=East=right-axis and finally rolls around the new - //! X=North=forward-axis. - QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll) + //! 3-2-1 sequence: The body first does yaw around the z-axis, then pitches + //! around the new y-axis and finally rolls around the new x-axis. + Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, + double yaw) { - // Abbreviations for the angular functions double cy = std::cos(yaw * 0.5); double sy = std::sin(yaw * 0.5); double cp = std::cos(pitch * 0.5); @@ -316,13 +315,89 @@ namespace parsing_utilities { double cr = std::cos(roll * 0.5); double sr = std::sin(roll * 0.5); - QuaternionMsg q; - q.w = cr * cp * cy + sr * sp * sy; - q.x = sr * cp * cy - cr * sp * sy; - q.y = cr * sp * cy + sr * cp * sy; - q.z = cr * cp * sy - sr * sp * cy; + return Eigen::Quaterniond( + cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy); + } + + QuaternionMsg convertEulerToQuaternionMsg(double yaw, double pitch, double roll) + { + QuaternionMsg qm; + + Eigen::Quaterniond q = convertEulerToQuaternion(roll, pitch, yaw); + + qm.w = q.w(); + qm.x = q.x(); + qm.y = q.y(); + qm.z = q.z(); + + return qm; + } + + Eigen::Quaterniond q_enu_ecef(double lat, double lon) + { + static double pihalf = boost::math::constants::pi() / 2.0; + double sr = sin((pihalf - lat) / 2.0); + double cr = cos((pihalf - lat) / 2.0); + double sy = sin((lon + pihalf) / 2.0); + double cy = cos((lon + pihalf) / 2.0); + + return Eigen::Quaterniond(cr * cy, sr * cy, sr * sy, cr * sy); + } + + Eigen::Quaterniond q_ned_ecef(double lat, double lon) + { + static double pihalf = boost::math::constants::pi() / 2.0; + double sp = sin((-lat - pihalf) / 2); + double cp = cos((-lat - pihalf) / 2); + double sy = sin(lon / 2); + double cy = cos(lon / 2); + + return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy); + } - return q; + Eigen::Matrix3d R_enu_ecef(double lat, double lon) + { + Eigen::Matrix3d R; + + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double sin_lon = sin(lon); + double cos_lon = cos(lon); + + R(0, 0) = -sin_lon; + R(0, 1) = -cos_lon * sin_lat; + R(0, 2) = cos_lon * cos_lat; + R(1, 0) = cos_lon; + R(1, 1) = -sin_lon * sin_lat; + R(1, 2) = sin_lon * cos_lat; + R(2, 0) = 0.0; + R(2, 1) = cos_lat; + R(2, 2) = sin_lat; + + return R; + } + + Eigen::Matrix3d R_ned_ecef(double lat, double lon) + { + Eigen::Matrix3d R; + + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double sin_lon = sin(lon); + double cos_lon = cos(lon); + + R(0, 0) = cos_lon; + R(0, 1) = -sin_lon * sin_lat; + R(0, 2) = sin_lon * cos_lat; + R(1, 0) = -sin_lon; + R(1, 1) = -cos_lon * sin_lat; + R(1, 2) = cos_lon * cos_lat; + R(2, 0) = 0.0; + R(2, 1) = -cos_lat; + R(2, 2) = -sin_lat; + + return R; } std::string convertUserPeriodToRxCommand(uint32_t period_user) @@ -356,4 +431,5 @@ namespace parsing_utilities { uint32_t getTow(const uint8_t* buffer) { return parseUInt32(buffer + 8); } uint16_t getWnc(const uint8_t* buffer) { return parseUInt16(buffer + 12); } + } // namespace parsing_utilities From 1b17ef79f3281cea7ad77191fb94de9c64fa67ae Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Oct 2022 15:59:50 +0200 Subject: [PATCH 002/170] Add ecef localization msg --- .../abstraction/typedefs.hpp | 4 +- .../communication/rx_message.hpp | 34 +- .../parsers/parsing_utilities.hpp | 9 +- .../communication/communication_core.cpp | 2 +- .../communication/rx_message.cpp | 630 +++++++++++------- .../parsers/parsing_utilities.cpp | 9 +- 6 files changed, 432 insertions(+), 256 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 1513ff0e..c8ad3a64 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -102,7 +102,7 @@ typedef sensor_msgs::NavSatFix NavSatFixMsg; typedef sensor_msgs::NavSatStatus NavSatStatusMsg; typedef sensor_msgs::TimeReference TimeReferenceMsg; typedef sensor_msgs::Imu ImuMsg; -typedef nav_msgs::Odometry LocalizationUtmMsg; +typedef nav_msgs::Odometry LocalizationMsg; // Septentrio GNSS SBF messages typedef septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg; @@ -282,7 +282,7 @@ class ROSaicNodeBase * @brief Publishing function for tf * @param[in] msg ROS localization message to be converted to tf */ - void publishTf(const LocalizationUtmMsg& loc) + void publishTf(const LocalizationMsg& loc) { if (std::isnan(loc.pose.pose.orientation.w)) return; diff --git a/include/septentrio_gnss_driver/communication/rx_message.hpp b/include/septentrio_gnss_driver/communication/rx_message.hpp index 4dbfde04..3c78b3e4 100644 --- a/include/septentrio_gnss_driver/communication/rx_message.hpp +++ b/include/septentrio_gnss_driver/communication/rx_message.hpp @@ -379,7 +379,7 @@ namespace io_comm_rx { * @brief Publishing function * @param[in] msg Localization message */ - void publishTf(const LocalizationUtmMsg& msg); + void publishTf(const LocalizationMsg& msg); /** * @brief Performs the CRC check (if SBF) and publishes ROS messages @@ -495,11 +495,17 @@ namespace io_comm_rx { AttCovEulerMsg last_attcoveuler_; /** - * @brief Since NavSatFix, GPSFix, Imu and Pose. need INSNavGeod, incoming + * @brief Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming * INSNavGeod blocks need to be stored */ INSNavGeodMsg last_insnavgeod_; + /** + * @brief Since LoclaizationEcef needs INSNavCart, incoming + * INSNavCart blocks need to be stored + */ + INSNavCartMsg last_insnavcart_; + /** * @brief Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks * need to be stored @@ -694,11 +700,29 @@ namespace io_comm_rx { /** * @brief "Callback" function when constructing - * LocalizationUtmMsg messages + * LocalizationMsg messages in UTM * @return A ROS message - * LocalizationUtmMsg just created + * LocalizationMsg just created + */ + LocalizationMsg LocalizationUtmCallback(); + + /** + * @brief "Callback" function when constructing + * LocalizationMsg messages in ECEF + * @return A ROS message + * LocalizationMsg just created + */ + LocalizationMsg LocalizationEcefCallback(); + + /** + * @brief function to fill twist part of LocalizationMsg + * @param[in] roll roll [rad] + * @param[in] pitch pitch [rad] + * @param[in] yaw yaw [rad] + * @param[inout] msg LocalizationMsg to be filled */ - LocalizationUtmMsg LocalizationUtmCallback(); + void fillLocalizationMsgTwist(double roll, double pitch, double yaw, + LocalizationMsg& msg); /** * @brief "Callback" function when constructing diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index e5b69a39..f0d42ff3 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -315,7 +315,14 @@ namespace parsing_utilities { * @param[in] roll Roll about the new x-axis [rad] * @return ROS message representing a quaternion */ - QuaternionMsg convertEulerToQuaternionMsg(double yaw, double pitch, double roll); + QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw); + + /** + * @brief Convert Eigen quaternion to a QuaternionMsg + * @param[in] q Eigen quaternion + * @return ROS message representing a quaternion + */ + QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q); /** * @brief Generates the quaternion to rotate from local ENU to ECEF diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 3eea92bf..011ad1e4 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -1170,7 +1170,7 @@ void io_comm_rx::Comm_IO::defineMessages() if (settings_->publish_localization || settings_->publish_tf) { handlers_.callbackmap_ = - handlers_.insert("Localization"); + handlers_.insert("Localization"); } } handlers_.callbackmap_ = diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index ca8c9194..9651e1f4 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -47,9 +47,11 @@ * computations. */ +using parsing_utilities::convertEulerToQuaternionMsg; using parsing_utilities::deg2rad; using parsing_utilities::deg2radSq; using parsing_utilities::rad2deg; +using parsing_utilities::square; PoseWithCovarianceStampedMsg io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() @@ -67,8 +69,8 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() double roll = 0.0; if (validValue(last_atteuler_.roll)) roll = last_atteuler_.roll; - msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternionMsg( - deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); + msg.pose.pose.orientation = + convertEulerToQuaternionMsg(deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); msg.pose.pose.position.z = last_pvtgeodetic_.height; @@ -102,12 +104,9 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() if ((last_insnavgeod_.sb_list & 1) != 0) { // Pos autocov - msg.pose.covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.pose.covariance[7] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.pose.covariance[14] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); + msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev); + msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev); + msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev); } else { msg.pose.covariance[0] = -1.0; @@ -126,9 +125,8 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() if (validValue(last_insnavgeod_.roll)) roll = last_insnavgeod_.roll; // Attitude - msg.pose.pose.orientation = - parsing_utilities::convertEulerToQuaternionMsg( - deg2rad(yaw), deg2rad(pitch), deg2rad(roll)); + msg.pose.pose.orientation = convertEulerToQuaternionMsg( + deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); } else { msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); @@ -140,18 +138,18 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() { // Attitude autocov if (validValue(last_insnavgeod_.roll_std_dev)) - msg.pose.covariance[21] = parsing_utilities::square( - deg2rad(last_insnavgeod_.roll_std_dev)); + msg.pose.covariance[21] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); else msg.pose.covariance[21] = -1.0; if (validValue(last_insnavgeod_.pitch_std_dev)) - msg.pose.covariance[28] = parsing_utilities::square( - deg2rad(last_insnavgeod_.pitch_std_dev)); + msg.pose.covariance[28] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); else msg.pose.covariance[28] = -1.0; if (validValue(last_insnavgeod_.heading_std_dev)) - msg.pose.covariance[35] = parsing_utilities::square( - deg2rad(last_insnavgeod_.heading_std_dev)); + msg.pose.covariance[35] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); else msg.pose.covariance[35] = -1.0; } else @@ -330,11 +328,10 @@ ImuMsg io_comm_rx::RxMessage::ImuCallback() validValue(last_insnavgeod_.pitch) && validValue(last_insnavgeod_.roll)) { - msg.orientation = - parsing_utilities::convertEulerToQuaternionMsg( - deg2rad(last_insnavgeod_.heading), - deg2rad(last_insnavgeod_.pitch), - deg2rad(last_insnavgeod_.roll)); + msg.orientation = convertEulerToQuaternionMsg( + deg2rad(last_insnavgeod_.roll), + deg2rad(last_insnavgeod_.pitch), + deg2rad(last_insnavgeod_.heading)); } else { valid_orientation = false; @@ -350,12 +347,12 @@ ImuMsg io_comm_rx::RxMessage::ImuCallback() validValue(last_insnavgeod_.pitch_std_dev) && validValue(last_insnavgeod_.heading_std_dev)) { - msg.orientation_covariance[0] = parsing_utilities::square( - deg2rad(last_insnavgeod_.roll_std_dev)); - msg.orientation_covariance[4] = parsing_utilities::square( - deg2rad(last_insnavgeod_.pitch_std_dev)); - msg.orientation_covariance[8] = parsing_utilities::square( - deg2rad(last_insnavgeod_.heading_std_dev)); + msg.orientation_covariance[0] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + msg.orientation_covariance[4] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + msg.orientation_covariance[8] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); } else { valid_orientation = false; @@ -451,69 +448,65 @@ io_comm_rx::RxMessage::TwistCallback(bool fromIns /* = false*/) ((last_insnavgeod_.sb_list & 2) != 0) && ((last_insnavgeod_.sb_list & 8) != 0)) { - Eigen::Matrix3d Cov_vel_n = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); if ((last_insnavgeod_.sb_list & 128) != 0) { // Linear velocity covariance if (validValue(last_insnavgeod_.ve_std_dev)) if (settings_->use_ros_axis_orientation) - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); + covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); else - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); + covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); else - Cov_vel_n(0, 0) = -1.0; + covVel_local(0, 0) = -1.0; if (validValue(last_insnavgeod_.vn_std_dev)) if (settings_->use_ros_axis_orientation) - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); + covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); else - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); + covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); else - Cov_vel_n(1, 1) = -1.0; + covVel_local(1, 1) = -1.0; if (validValue(last_insnavgeod_.vu_std_dev)) - Cov_vel_n(2, 2) = - parsing_utilities::square(last_insnavgeod_.vu_std_dev); + covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); else - Cov_vel_n(2, 2) = -1.0; + covVel_local(2, 2) = -1.0; if (validValue(last_insnavgeod_.ve_vn_cov)) - Cov_vel_n(0, 1) = Cov_vel_n(1, 0) = last_insnavgeod_.ve_vn_cov; + covVel_local(0, 1) = covVel_local(1, 0) = + last_insnavgeod_.ve_vn_cov; if (settings_->use_ros_axis_orientation) { if (validValue(last_insnavgeod_.ve_vu_cov)) - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = + covVel_local(0, 2) = covVel_local(2, 0) = last_insnavgeod_.ve_vu_cov; if (validValue(last_insnavgeod_.vn_vu_cov)) - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = + covVel_local(2, 1) = covVel_local(1, 2) = last_insnavgeod_.vn_vu_cov; } else { if (validValue(last_insnavgeod_.vn_vu_cov)) - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = + covVel_local(0, 2) = covVel_local(2, 0) = -last_insnavgeod_.vn_vu_cov; if (validValue(last_insnavgeod_.ve_vu_cov)) - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = + covVel_local(2, 1) = covVel_local(1, 2) = -last_insnavgeod_.ve_vu_cov; } } else { - Cov_vel_n(0, 0) = -1.0; - Cov_vel_n(1, 1) = -1.0; - Cov_vel_n(2, 2) = -1.0; + covVel_local(0, 0) = -1.0; + covVel_local(1, 1) = -1.0; + covVel_local(2, 2) = -1.0; } - msg.twist.covariance[0] = Cov_vel_n(0, 0); - msg.twist.covariance[1] = Cov_vel_n(0, 1); - msg.twist.covariance[2] = Cov_vel_n(0, 2); - msg.twist.covariance[6] = Cov_vel_n(1, 0); - msg.twist.covariance[7] = Cov_vel_n(1, 1); - msg.twist.covariance[8] = Cov_vel_n(1, 2); - msg.twist.covariance[12] = Cov_vel_n(2, 0); - msg.twist.covariance[13] = Cov_vel_n(2, 1); - msg.twist.covariance[14] = Cov_vel_n(2, 2); + msg.twist.covariance[0] = covVel_local(0, 0); + msg.twist.covariance[1] = covVel_local(0, 1); + msg.twist.covariance[2] = covVel_local(0, 2); + msg.twist.covariance[6] = covVel_local(1, 0); + msg.twist.covariance[7] = covVel_local(1, 1); + msg.twist.covariance[8] = covVel_local(1, 2); + msg.twist.covariance[12] = covVel_local(2, 0); + msg.twist.covariance[13] = covVel_local(2, 1); + msg.twist.covariance[14] = covVel_local(2, 2); } else { msg.twist.covariance[0] = -1.0; @@ -564,47 +557,51 @@ io_comm_rx::RxMessage::TwistCallback(bool fromIns /* = false*/) if (last_velcovgeodetic_.error == 0) { - Eigen::Matrix3d Cov_vel_n = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); // Linear velocity covariance in navigation frame if (validValue(last_velcovgeodetic_.cov_veve)) if (settings_->use_ros_axis_orientation) - Cov_vel_n(0, 0) = last_velcovgeodetic_.cov_veve; + covVel_local(0, 0) = last_velcovgeodetic_.cov_veve; else - Cov_vel_n(1, 1) = last_velcovgeodetic_.cov_veve; + covVel_local(1, 1) = last_velcovgeodetic_.cov_veve; else - Cov_vel_n(0, 0) = -1.0; + covVel_local(0, 0) = -1.0; if (validValue(last_velcovgeodetic_.cov_vnvn)) if (settings_->use_ros_axis_orientation) - Cov_vel_n(1, 1) = last_velcovgeodetic_.cov_vnvn; + covVel_local(1, 1) = last_velcovgeodetic_.cov_vnvn; else - Cov_vel_n(0, 0) = last_velcovgeodetic_.cov_vnvn; + covVel_local(0, 0) = last_velcovgeodetic_.cov_vnvn; else - Cov_vel_n(1, 1) = -1.0; + covVel_local(1, 1) = -1.0; if (validValue(last_velcovgeodetic_.cov_vuvu)) - Cov_vel_n(2, 2) = last_velcovgeodetic_.cov_vuvu; + covVel_local(2, 2) = last_velcovgeodetic_.cov_vuvu; else - Cov_vel_n(2, 2) = -1.0; + covVel_local(2, 2) = -1.0; - Cov_vel_n(0, 1) = Cov_vel_n(1, 0) = last_velcovgeodetic_.cov_vnve; + covVel_local(0, 1) = covVel_local(1, 0) = last_velcovgeodetic_.cov_vnve; if (settings_->use_ros_axis_orientation) { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = last_velcovgeodetic_.cov_vevu; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = last_velcovgeodetic_.cov_vnvu; + covVel_local(0, 2) = covVel_local(2, 0) = + last_velcovgeodetic_.cov_vevu; + covVel_local(2, 1) = covVel_local(1, 2) = + last_velcovgeodetic_.cov_vnvu; } else { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = -last_velcovgeodetic_.cov_vnvu; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = -last_velcovgeodetic_.cov_vevu; + covVel_local(0, 2) = covVel_local(2, 0) = + -last_velcovgeodetic_.cov_vnvu; + covVel_local(2, 1) = covVel_local(1, 2) = + -last_velcovgeodetic_.cov_vevu; } - msg.twist.covariance[0] = Cov_vel_n(0, 0); - msg.twist.covariance[1] = Cov_vel_n(0, 1); - msg.twist.covariance[2] = Cov_vel_n(0, 2); - msg.twist.covariance[6] = Cov_vel_n(1, 0); - msg.twist.covariance[7] = Cov_vel_n(1, 1); - msg.twist.covariance[8] = Cov_vel_n(1, 2); - msg.twist.covariance[12] = Cov_vel_n(2, 0); - msg.twist.covariance[13] = Cov_vel_n(2, 1); - msg.twist.covariance[14] = Cov_vel_n(2, 2); + msg.twist.covariance[0] = covVel_local(0, 0); + msg.twist.covariance[1] = covVel_local(0, 1); + msg.twist.covariance[2] = covVel_local(0, 2); + msg.twist.covariance[6] = covVel_local(1, 0); + msg.twist.covariance[7] = covVel_local(1, 1); + msg.twist.covariance[8] = covVel_local(1, 2); + msg.twist.covariance[12] = covVel_local(2, 0); + msg.twist.covariance[13] = covVel_local(2, 1); + msg.twist.covariance[14] = covVel_local(2, 2); } else { msg.twist.covariance[0] = -1.0; @@ -627,9 +624,10 @@ io_comm_rx::RxMessage::TwistCallback(bool fromIns /* = false*/) * north. Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ -LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() +LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() { - LocalizationUtmMsg msg; + LocalizationMsg msg; + msg.header.stamp = last_insnavgeod_.header.stamp; int zone; std::string zonestring; @@ -678,12 +676,9 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() if ((last_insnavgeod_.sb_list & 1) != 0) { // Position autocovariance - msg.pose.covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.pose.covariance[7] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.pose.covariance[14] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); + msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev); + msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev); + msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev); } else { msg.pose.covariance[0] = -1.0; @@ -707,12 +702,10 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() else yaw += deg2rad(gamma); - Eigen::Matrix3d R_n_b = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); if ((last_insnavgeod_.sb_list & 2) != 0) { // Attitude - msg.pose.pose.orientation = - parsing_utilities::convertEulerToQuaternionMsg(yaw, pitch, roll); + msg.pose.pose.orientation = convertEulerToQuaternionMsg(roll, pitch, yaw); } else { msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); @@ -724,18 +717,17 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() { // Attitude autocovariance if (validValue(last_insnavgeod_.roll_std_dev)) - msg.pose.covariance[21] = - parsing_utilities::square(deg2rad(last_insnavgeod_.roll_std_dev)); + msg.pose.covariance[21] = square(deg2rad(last_insnavgeod_.roll_std_dev)); else msg.pose.covariance[21] = -1.0; if (validValue(last_insnavgeod_.pitch_std_dev)) msg.pose.covariance[28] = - parsing_utilities::square(deg2rad(last_insnavgeod_.pitch_std_dev)); + square(deg2rad(last_insnavgeod_.pitch_std_dev)); else msg.pose.covariance[28] = -1.0; if (validValue(last_insnavgeod_.heading_std_dev)) msg.pose.covariance[35] = - parsing_utilities::square(deg2rad(last_insnavgeod_.heading_std_dev)); + square(deg2rad(last_insnavgeod_.heading_std_dev)); else msg.pose.covariance[35] = -1.0; } else @@ -744,6 +736,227 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() msg.pose.covariance[28] = -1.0; msg.pose.covariance[35] = -1.0; } + + if ((last_insnavgeod_.sb_list & 32) != 0) + { + // Position covariance + msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; + + if (settings_->use_ros_axis_orientation) + { + // (ENU) + msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; + msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; + } else + { + // (NED) + msg.pose.covariance[2] = -last_insnavgeod_.latitude_height_cov; + msg.pose.covariance[8] = -last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[12] = -last_insnavgeod_.latitude_height_cov; + msg.pose.covariance[13] = -last_insnavgeod_.longitude_height_cov; + } + } + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude covariancae + msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + + msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + + if (!settings_->use_ros_axis_orientation) + { + // (NED) + msg.pose.covariance[33] *= -1.0; + msg.pose.covariance[23] *= -1.0; + msg.pose.covariance[22] *= -1.0; + msg.pose.covariance[27] *= -1.0; + } + } + + fillLocalizationMsgTwist(roll, pitch, yaw, msg); + + return msg; +}; + +/** + * Localization in ECEF coordinates. Attitude is converted from local to ECEF + * Linear velocity of twist in body frame as per msg definition. Angular + * velocity not available, thus according autocovariances are set to -1.0. + */ +LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() +{ + LocalizationMsg msg; + msg.header.stamp = last_insnavcart_.header.stamp; + msg.header.frame_id = "ecef"; + if (settings_->ins_use_poi) + msg.child_frame_id = settings_->poi_frame_id; // TODO param + else + msg.child_frame_id = settings_->frame_id; + + // ECEF position + msg.pose.pose.position.x = last_insnavcart_.x; + msg.pose.pose.position.y = last_insnavcart_.y; + msg.pose.pose.position.z = last_insnavcart_.z; + + if ((last_insnavgeod_.sb_list & 1) != 0) + { + // Position autocovariance + msg.pose.covariance[0] = square(last_insnavcart_.x_std_dev); + msg.pose.covariance[7] = square(last_insnavcart_.y_std_dev); + msg.pose.covariance[14] = square(last_insnavcart_.z_std_dev); + } else + { + msg.pose.covariance[0] = -1.0; + msg.pose.covariance[7] = -1.0; + msg.pose.covariance[14] = -1.0; + } + + // Euler angles + double roll = 0.0; + if (validValue(last_insnavcart_.roll)) + roll = deg2rad(last_insnavcart_.roll); + double pitch = 0.0; + if (validValue(last_insnavcart_.pitch)) + pitch = deg2rad(last_insnavcart_.pitch); + double yaw = 0.0; + if (validValue(last_insnavcart_.heading)) + yaw = deg2rad(last_insnavcart_.heading); + + double latitude = deg2rad(last_insnavgeod_.latitude); + double longitude = deg2rad(last_insnavgeod_.longitude); + + if ((last_insnavcart_.sb_list & 2) != 0) + { + // Attitude + Eigen::Quaterniond q_local_ecef; + if (settings_->use_ros_axis_orientation) + q_local_ecef = parsing_utilities::q_enu_ecef(latitude, longitude); + else + q_local_ecef = parsing_utilities::q_ned_ecef(latitude, longitude); + Eigen::Quaterniond q_b_local = + parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw); + + Eigen::Quaterniond q_b_ecef = q_b_local * q_local_ecef; + + msg.pose.pose.orientation = + parsing_utilities::quaternionToQuaternionMsg(q_b_ecef); + } else + { + msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + } + Eigen::Matrix3d covAtt_local = Eigen::Matrix3d::Zero(); + bool covAttValid = true; + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocovariance + if (validValue(last_insnavgeod_.roll_std_dev)) + covAtt_local(0, 0) = square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + { + covAtt_local(0, 0) = -1.0; + covAttValid = false; + } + if (validValue(last_insnavgeod_.pitch_std_dev)) + covAtt_local(1, 1) = square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + { + covAtt_local(1, 1) = -1.0; + covAttValid = false; + } + if (validValue(last_insnavgeod_.heading_std_dev)) + covAtt_local(2, 2) = square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + { + covAtt_local(2, 2) = -1.0; + covAttValid = false; + } + } else + { + covAtt_local(0, 0) = -1.0; + covAtt_local(1, 1) = -1.0; + covAtt_local(2, 2) = -1.0; + covAttValid = false; + } + + if ((last_insnavcart_.sb_list & 32) != 0) + { + // Position covariance + msg.pose.covariance[1] = last_insnavcart_.xy_cov; + msg.pose.covariance[6] = last_insnavcart_.xy_cov; + msg.pose.covariance[2] = last_insnavcart_.xz_cov; + msg.pose.covariance[8] = last_insnavcart_.yz_cov; + msg.pose.covariance[12] = last_insnavcart_.xz_cov; + msg.pose.covariance[13] = last_insnavcart_.yz_cov; + } + + if (covAttValid) + { + if ((last_insnavcart_.sb_list & 64) != 0) + { + // Attitude covariancae + covAtt_local(0, 1) = deg2radSq(last_insnavcart_.pitch_roll_cov); + covAtt_local(0, 2) = deg2radSq(last_insnavcart_.heading_roll_cov); + covAtt_local(1, 0) = deg2radSq(last_insnavcart_.pitch_roll_cov); + + covAtt_local(2, 1) = deg2radSq(last_insnavcart_.heading_pitch_cov); + covAtt_local(2, 0) = deg2radSq(last_insnavcart_.heading_roll_cov); + covAtt_local(1, 2) = deg2radSq(last_insnavcart_.heading_pitch_cov); + + if (!settings_->use_ros_axis_orientation) + { + // (NED) + covAtt_local(0, 2) *= -1.0; + covAtt_local(2, 0) *= -1.0; + covAtt_local(0, 1) *= -1.0; + covAtt_local(1, 0) *= -1.0; + } + } + Eigen::Matrix3d R_local_ecef; + if (settings_->use_ros_axis_orientation) + R_local_ecef = parsing_utilities::R_enu_ecef(latitude, longitude); + else + R_local_ecef = parsing_utilities::R_ned_ecef(latitude, longitude); + // Rotate attitude covariance matrix to ecef coordinates + Eigen::Matrix3d covAtt_ecef = + R_local_ecef * covAtt_local * R_local_ecef.transpose(); + + msg.pose.covariance[21] = covAtt_ecef(0, 0); + msg.pose.covariance[28] = covAtt_ecef(1, 1); + msg.pose.covariance[35] = covAtt_ecef(2, 2); + msg.pose.covariance[22] = covAtt_ecef(0, 1); + msg.pose.covariance[23] = covAtt_ecef(0, 2); + msg.pose.covariance[27] = covAtt_ecef(1, 0); + msg.pose.covariance[29] = covAtt_ecef(2, 1); + msg.pose.covariance[33] = covAtt_ecef(2, 0); + msg.pose.covariance[34] = covAtt_ecef(1, 2); + } else + { + msg.pose.covariance[21] = -1.0; + msg.pose.covariance[28] = -1.0; + msg.pose.covariance[35] = -1.0; + } + + fillLocalizationMsgTwist(roll, pitch, yaw, msg); + + return msg; +}; + +void io_comm_rx::RxMessage::fillLocalizationMsgTwist(double roll, double pitch, + double yaw, + LocalizationMsg& msg) +{ + Eigen::Matrix3d R_local_body = + parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); if ((last_insnavgeod_.sb_list & 8) != 0) { // Linear velocity (ENU) @@ -756,18 +969,18 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() double vu = 0.0; if (validValue(last_insnavgeod_.vu)) vu = last_insnavgeod_.vu; - Eigen::Vector3d vel_enu; + Eigen::Vector3d vel_local; if (settings_->use_ros_axis_orientation) { // (ENU) - vel_enu << ve, vn, vu; + vel_local << ve, vn, vu; } else { // (NED) - vel_enu << vn, ve, -vu; + vel_local << vn, ve, -vu; } // Linear velocity, rotate to body coordinates - Eigen::Vector3d vel_body = R_n_b * vel_enu; + Eigen::Vector3d vel_body = R_local_body * vel_local; msg.twist.twist.linear.x = vel_body(0); msg.twist.twist.linear.y = vel_body(1); msg.twist.twist.linear.z = vel_body(2); @@ -777,91 +990,46 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); } - Eigen::Matrix3d Cov_vel_n = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); if ((last_insnavgeod_.sb_list & 16) != 0) { // Linear velocity autocovariance if (validValue(last_insnavgeod_.ve_std_dev)) if (settings_->use_ros_axis_orientation) - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); + covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); else - Cov_vel_n(0, 0) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); + covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); else - Cov_vel_n(0, 0) = -1.0; + covVel_local(0, 0) = -1.0; if (validValue(last_insnavgeod_.vn_std_dev)) if (settings_->use_ros_axis_orientation) - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.vn_std_dev); + covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); else - Cov_vel_n(1, 1) = - parsing_utilities::square(last_insnavgeod_.ve_std_dev); + covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); else - Cov_vel_n(1, 1) = -1.0; + covVel_local(1, 1) = -1.0; if (validValue(last_insnavgeod_.vu_std_dev)) - Cov_vel_n(2, 2) = parsing_utilities::square(last_insnavgeod_.vu_std_dev); + covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); else - Cov_vel_n(2, 2) = -1.0; + covVel_local(2, 2) = -1.0; } else { - Cov_vel_n(0, 0) = -1.0; - Cov_vel_n(1, 1) = -1.0; - Cov_vel_n(2, 2) = -1.0; - } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - // Position covariance - msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; - - if (settings_->use_ros_axis_orientation) - { - // (ENU) - msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; - } else - { - // (NED) - msg.pose.covariance[2] = -last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[8] = -last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[12] = -last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[13] = -last_insnavgeod_.longitude_height_cov; - } + covVel_local(0, 0) = -1.0; + covVel_local(1, 1) = -1.0; + covVel_local(2, 2) = -1.0; } - if ((last_insnavgeod_.sb_list & 64) != 0) - { - // Attitude covariancae - msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - if (!settings_->use_ros_axis_orientation) - { - // (NED) - msg.pose.covariance[33] *= -1.0; - msg.pose.covariance[23] *= -1.0; - msg.pose.covariance[22] *= -1.0; - msg.pose.covariance[27] *= -1.0; - } - } if ((last_insnavgeod_.sb_list & 128) != 0) { - Cov_vel_n(0, 1) = Cov_vel_n(1, 0) = last_insnavgeod_.ve_vn_cov; + covVel_local(0, 1) = covVel_local(1, 0) = last_insnavgeod_.ve_vn_cov; if (settings_->use_ros_axis_orientation) { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = last_insnavgeod_.ve_vu_cov; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = last_insnavgeod_.vn_vu_cov; + covVel_local(0, 2) = covVel_local(2, 0) = last_insnavgeod_.ve_vu_cov; + covVel_local(2, 1) = covVel_local(1, 2) = last_insnavgeod_.vn_vu_cov; } else { - Cov_vel_n(0, 2) = Cov_vel_n(2, 0) = -last_insnavgeod_.vn_vu_cov; - Cov_vel_n(2, 1) = Cov_vel_n(1, 2) = -last_insnavgeod_.ve_vu_cov; + covVel_local(0, 2) = covVel_local(2, 0) = -last_insnavgeod_.vn_vu_cov; + covVel_local(2, 1) = covVel_local(1, 2) = -last_insnavgeod_.ve_vu_cov; } } @@ -872,18 +1040,19 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() validValue(last_insnavgeod_.vn_std_dev) && validValue(last_insnavgeod_.vu_std_dev)) { - // Rotate covariance matrix to body coordinates - Eigen::Matrix3d Cov_vel_body = R_n_b * Cov_vel_n * R_n_b.transpose(); - - msg.twist.covariance[0] = Cov_vel_body(0, 0); - msg.twist.covariance[1] = Cov_vel_body(0, 1); - msg.twist.covariance[2] = Cov_vel_body(0, 2); - msg.twist.covariance[6] = Cov_vel_body(1, 0); - msg.twist.covariance[7] = Cov_vel_body(1, 1); - msg.twist.covariance[8] = Cov_vel_body(1, 2); - msg.twist.covariance[12] = Cov_vel_body(2, 0); - msg.twist.covariance[13] = Cov_vel_body(2, 1); - msg.twist.covariance[14] = Cov_vel_body(2, 2); + // Rotate velocity covariance matrix to body coordinates + Eigen::Matrix3d covVel_body = + R_local_body * covVel_local * R_local_body.transpose(); + + msg.twist.covariance[0] = covVel_body(0, 0); + msg.twist.covariance[1] = covVel_body(0, 1); + msg.twist.covariance[2] = covVel_body(0, 2); + msg.twist.covariance[6] = covVel_body(1, 0); + msg.twist.covariance[7] = covVel_body(1, 1); + msg.twist.covariance[8] = covVel_body(1, 2); + msg.twist.covariance[12] = covVel_body(2, 0); + msg.twist.covariance[13] = covVel_body(2, 1); + msg.twist.covariance[14] = covVel_body(2, 2); } else { msg.twist.covariance[0] = -1.0; @@ -894,8 +1063,7 @@ LocalizationUtmMsg io_comm_rx::RxMessage::LocalizationUtmCallback() msg.twist.covariance[21] = -1.0; msg.twist.covariance[28] = -1.0; msg.twist.covariance[35] = -1.0; - return msg; -}; +} /** * The position_covariance array is populated in row-major order, where the basis of @@ -1057,12 +1225,9 @@ NavSatFixMsg io_comm_rx::RxMessage::NavSatFixCallback() if ((last_insnavgeod_.sb_list & 1) != 0) { - msg.position_covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.position_covariance[4] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.position_covariance[8] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); + msg.position_covariance[0] = square(last_insnavgeod_.longitude_std_dev); + msg.position_covariance[4] = square(last_insnavgeod_.latitude_std_dev); + msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); } if ((last_insnavgeod_.sb_list & 32) != 0) { @@ -1277,8 +1442,8 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() msg.altitude = last_pvtgeodetic_.height; // Note that cog is of type float32 while track is of type float64. msg.track = last_pvtgeodetic_.cog; - msg.speed = std::sqrt(parsing_utilities::square(last_pvtgeodetic_.vn) + - parsing_utilities::square(last_pvtgeodetic_.ve)); + msg.speed = + std::sqrt(square(last_pvtgeodetic_.vn) + square(last_pvtgeodetic_.ve)); msg.climb = last_pvtgeodetic_.vu; msg.pitch = last_atteuler_.pitch; msg.roll = last_atteuler_.roll; @@ -1287,8 +1452,7 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() msg.gdop = -1.0; } else { - msg.gdop = std::sqrt(parsing_utilities::square(last_dop_.pdop) + - parsing_utilities::square(last_dop_.tdop)); + msg.gdop = std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); } if (last_dop_.pdop == 0.0) { @@ -1332,16 +1496,13 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() 2 * std::sqrt(static_cast(last_poscovgeodetic_.cov_hgthgt)); msg.err_track = 2 * - (std::sqrt(parsing_utilities::square( - 1.0 / (last_pvtgeodetic_.vn + - parsing_utilities::square(last_pvtgeodetic_.ve) / - last_pvtgeodetic_.vn)) * - last_poscovgeodetic_.cov_lonlon + - parsing_utilities::square( - (last_pvtgeodetic_.ve) / - (parsing_utilities::square(last_pvtgeodetic_.vn) + - parsing_utilities::square(last_pvtgeodetic_.ve))) * - last_poscovgeodetic_.cov_latlat)); + (std::sqrt( + square(1.0 / (last_pvtgeodetic_.vn + + square(last_pvtgeodetic_.ve) / last_pvtgeodetic_.vn)) * + last_poscovgeodetic_.cov_lonlon + + square((last_pvtgeodetic_.ve) / (square(last_pvtgeodetic_.vn) + + square(last_pvtgeodetic_.ve))) * + last_poscovgeodetic_.cov_latlat)); msg.err_speed = 2 * (std::sqrt(static_cast(last_velcovgeodetic_.cov_vnvn) + static_cast(last_velcovgeodetic_.cov_veve))); @@ -1420,8 +1581,8 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() } if ((last_insnavgeod_.sb_list & 8) != 0) { - msg.speed = std::sqrt(parsing_utilities::square(last_insnavgeod_.vn) + - parsing_utilities::square(last_insnavgeod_.ve)); + msg.speed = + std::sqrt(square(last_insnavgeod_.vn) + square(last_insnavgeod_.ve)); msg.climb = last_insnavgeod_.vu; } @@ -1430,8 +1591,7 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() msg.gdop = -1.0; } else { - msg.gdop = std::sqrt(parsing_utilities::square(last_dop_.pdop) + - parsing_utilities::square(last_dop_.tdop)); + msg.gdop = std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); } if (last_dop_.pdop == 0.0) { @@ -1466,64 +1626,46 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() 60 * 60); if ((last_insnavgeod_.sb_list & 1) != 0) { - msg.err = - 2 * - (std::sqrt( - parsing_utilities::square(last_insnavgeod_.latitude_std_dev) + - parsing_utilities::square(last_insnavgeod_.longitude_std_dev) + - parsing_utilities::square(last_insnavgeod_.height_std_dev))); + msg.err = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + + square(last_insnavgeod_.longitude_std_dev) + + square(last_insnavgeod_.height_std_dev))); msg.err_horz = - 2 * - (std::sqrt( - parsing_utilities::square(last_insnavgeod_.latitude_std_dev) + - parsing_utilities::square(last_insnavgeod_.longitude_std_dev))); - msg.err_vert = 2 * (std::sqrt(parsing_utilities::square( - last_insnavgeod_.height_std_dev))); + 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + + square(last_insnavgeod_.longitude_std_dev))); + msg.err_vert = 2 * (std::sqrt(square(last_insnavgeod_.height_std_dev))); } if (((last_insnavgeod_.sb_list & 8) != 0) || ((last_insnavgeod_.sb_list & 1) != 0)) { msg.err_track = - 2 * (std::sqrt( - parsing_utilities::square( - 1.0 / (last_insnavgeod_.vn + - parsing_utilities::square(last_insnavgeod_.ve) / - last_insnavgeod_.vn)) * - parsing_utilities::square( - last_insnavgeod_.longitude_std_dev) + - parsing_utilities::square( - (last_insnavgeod_.ve) / - (parsing_utilities::square(last_insnavgeod_.vn) + - parsing_utilities::square(last_insnavgeod_.ve))) * - parsing_utilities::square( - last_insnavgeod_.latitude_std_dev))); + 2 * + (std::sqrt( + square(1.0 / (last_insnavgeod_.vn + square(last_insnavgeod_.ve) / + last_insnavgeod_.vn)) * + square(last_insnavgeod_.longitude_std_dev) + + square((last_insnavgeod_.ve) / (square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve))) * + square(last_insnavgeod_.latitude_std_dev))); } if ((last_insnavgeod_.sb_list & 8) != 0) { - msg.err_speed = - 2 * (std::sqrt(parsing_utilities::square(last_insnavgeod_.vn) + - parsing_utilities::square(last_insnavgeod_.ve))); - msg.err_climb = - 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.vn)); + msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve))); + msg.err_climb = 2 * std::sqrt(square(last_insnavgeod_.vn)); } if ((last_insnavgeod_.sb_list & 2) != 0) { - msg.err_pitch = - 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.pitch)); + msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.pitch)); } if ((last_insnavgeod_.sb_list & 2) != 0) { - msg.err_pitch = - 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.roll)); + msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.roll)); } if ((last_insnavgeod_.sb_list & 1) != 0) { - msg.position_covariance[0] = - parsing_utilities::square(last_insnavgeod_.longitude_std_dev); - msg.position_covariance[4] = - parsing_utilities::square(last_insnavgeod_.latitude_std_dev); - msg.position_covariance[8] = - parsing_utilities::square(last_insnavgeod_.height_std_dev); + msg.position_covariance[0] = square(last_insnavgeod_.longitude_std_dev); + msg.position_covariance[4] = square(last_insnavgeod_.latitude_std_dev); + msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); } if ((last_insnavgeod_.sb_list & 32) != 0) { @@ -1884,7 +2026,7 @@ void io_comm_rx::RxMessage::publish(const std::string& topic, const M& msg) /** * If GNSS time is used, Publishing is only done with valid leap seconds */ -void io_comm_rx::RxMessage::publishTf(const LocalizationUtmMsg& msg) +void io_comm_rx::RxMessage::publishTf(const LocalizationMsg& msg) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -2889,7 +3031,7 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) } case evLocalization: { - LocalizationUtmMsg msg; + LocalizationMsg msg; try { msg = LocalizationUtmCallback(); @@ -2907,7 +3049,7 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) wait(time_obj); } if (settings_->publish_localization) - publish("/localization", msg); + publish("/localization", msg); if (settings_->publish_tf) publishTf(msg); break; diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index dd9246a4..5af8b88d 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -320,12 +320,10 @@ namespace parsing_utilities { cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy); } - QuaternionMsg convertEulerToQuaternionMsg(double yaw, double pitch, double roll) + QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q) { QuaternionMsg qm; - Eigen::Quaterniond q = convertEulerToQuaternion(roll, pitch, yaw); - qm.w = q.w(); qm.x = q.x(); qm.y = q.y(); @@ -334,6 +332,11 @@ namespace parsing_utilities { return qm; } + QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw) + { + return quaternionToQuaternionMsg(convertEulerToQuaternion(roll, pitch, yaw)); + } + Eigen::Quaterniond q_enu_ecef(double lat, double lon) { static double pihalf = boost::math::constants::pi() / 2.0; From d22ea21fefa1eb5fe96aca51e4c06339d8129243 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Oct 2022 17:55:55 +0200 Subject: [PATCH 003/170] Add localization ECEF publishing --- config/ins.yaml | 2 + config/rover.yaml | 2 + .../communication/callback_handlers.hpp | 42 +++++++++++----- .../communication/rx_message.hpp | 13 ++++- .../communication/settings.h | 4 ++ .../communication/callback_handlers.cpp | 40 ++++++++++++++++ .../communication/communication_core.cpp | 11 ++++- .../communication/rx_message.cpp | 48 ++++++++++++++++--- .../node/rosaic_node.cpp | 10 ++++ 9 files changed, 150 insertions(+), 22 deletions(-) diff --git a/config/ins.yaml b/config/ins.yaml index 8248caa8..95df4600 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -112,6 +112,8 @@ publish: imu: true localization: true tf: true + localization_ecef: false + tf_ecef: false # INS-Specific Parameters diff --git a/config/rover.yaml b/config/rover.yaml index 2ffe118e..f31b19f9 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -124,6 +124,8 @@ publish: imu: false localization: false tf: false + localization_ecef: false + tf_ecef: false # INS-Specific Parameters diff --git a/include/septentrio_gnss_driver/communication/callback_handlers.hpp b/include/septentrio_gnss_driver/communication/callback_handlers.hpp index 5d202f58..f953427c 100644 --- a/include/septentrio_gnss_driver/communication/callback_handlers.hpp +++ b/include/septentrio_gnss_driver/communication/callback_handlers.hpp @@ -188,11 +188,10 @@ namespace io_comm_rx { boost::shared_ptr> CallbackMap; - CallbackHandlers(ROSaicNodeBase* node, Settings* settings) : - node_(node), - rx_message_(node, settings), - settings_(settings) - {} + CallbackHandlers(ROSaicNodeBase* node, Settings* settings) : + node_(node), rx_message_(node, settings), settings_(settings) + { + } /** * @brief Adds a pair to the multimap "callbackmap_", with the message_key @@ -214,8 +213,10 @@ namespace io_comm_rx { callbackmap_.insert(std::make_pair( message_key, boost::shared_ptr(handler))); CallbackMap::key_type key = message_key; - node_->log(LogLevel::DEBUG, "Key " + message_key + " successfully inserted into multimap: " + - (((unsigned int)callbackmap_.count(key)) ? "true" : "false")); + node_->log( + LogLevel::DEBUG, + "Key " + message_key + " successfully inserted into multimap: " + + (((unsigned int)callbackmap_.count(key)) ? "true" : "false")); return callbackmap_; } @@ -228,11 +229,13 @@ namespace io_comm_rx { /** * @brief Searches for Rx messages that could potentially be * decoded/parsed/published - * @param[in] recvTimestamp Timestamp of buffer reception passed on from AsyncManager class + * @param[in] recvTimestamp Timestamp of buffer reception passed on from + * AsyncManager class * @param[in] data Buffer passed on from AsyncManager class * @param[in] size Size of the buffer */ - void readCallback(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size); + void readCallback(Timestamp recvTimestamp, const uint8_t* data, + std::size_t& size); //! Callback handlers multimap for Rx messages; it needs to be public since //! we copy-assign (did not work otherwise) new callbackmap_, after inserting @@ -261,8 +264,9 @@ namespace io_comm_rx { //! ROS message arrives last and thus launches its construction static std::string do_gpsfix_; - //! Determines which of the INS integrated SBF blocks necessary for the gps_common::GPSFix - //! ROS message arrives last and thus launches its construction + //! Determines which of the INS integrated SBF blocks necessary for the + //! gps_common::GPSFix ROS message arrives last and thus launches its + //! construction static std::string do_insgpsfix_; //! Determines which of the SBF blocks necessary for the @@ -299,6 +303,11 @@ namespace io_comm_rx { //! launches its construction static std::string do_inslocalization_; + //! Determines which of the SBF blocks necessary for the + //! nav_msgs/Odometry ROS message arrives last and thus + //! launches its construction + static std::string do_inslocalization_ecef_; + //! Shorthand for the map responsible for matching ROS message identifiers //! relevant for GPSFix to a uint32_t typedef std::unordered_map GPSFixMap; @@ -317,7 +326,8 @@ namespace io_comm_rx { //! Shorthand for the map responsible for matching ROS message identifiers //! relevant for PoseWithCovarianceStamped to a uint32_t - typedef std::unordered_map PoseWithCovarianceStampedMap; + typedef std::unordered_map + PoseWithCovarianceStampedMap; //! All instances of the CallbackHandlers class shall have access to the map //! without reinitializing it, hence static @@ -343,9 +353,17 @@ namespace io_comm_rx { //! relevant for Localization to a uint32_t typedef std::unordered_map LocalizationMap; + //! Shorthand for the map responsible for matching ROS message identifiers + //! relevant for Localization to a uint32_t + typedef std::unordered_map LocalizationEcefMap; + //! All instances of the CallbackHandlers class shall have access to the map //! without reinitializing it, hence static static LocalizationMap localization_map; + + //! All instances of the CallbackHandlers class shall have access to the map + //! without reinitializing it, hence static + static LocalizationMap localization_ecef_map; }; } // namespace io_comm_rx diff --git a/include/septentrio_gnss_driver/communication/rx_message.hpp b/include/septentrio_gnss_driver/communication/rx_message.hpp index 3c78b3e4..0d3bab88 100644 --- a/include/septentrio_gnss_driver/communication/rx_message.hpp +++ b/include/septentrio_gnss_driver/communication/rx_message.hpp @@ -186,6 +186,7 @@ enum RxID_Enum evVelCovGeodetic, evDiagnosticArray, evLocalization, + evLocalizationEcef, evReceiverStatus, evQualityInd, evReceiverTime, @@ -265,6 +266,7 @@ namespace io_comm_rx { std::make_pair("5908", evVelCovGeodetic), std::make_pair("DiagnosticArray", evDiagnosticArray), std::make_pair("Localization", evLocalization), + std::make_pair("LocalizationEcef", evLocalizationEcef), std::make_pair("4014", evReceiverStatus), std::make_pair("4082", evQualityInd), std::make_pair("5902", evReceiverSetup), @@ -432,6 +434,11 @@ namespace io_comm_rx { */ bool ins_localization_complete(uint32_t id); + /** + * @brief Wether all blocks have arrived for Localization Message + */ + bool ins_localization_ecef_complete(uint32_t id); + private: /** * @brief Pointer to the node @@ -658,9 +665,11 @@ namespace io_comm_rx { //! has arrived or not bool qualityind_has_arrived_diagnostics_ = false; - //! For Localization: Whether the INSNavGeod block of the current epoch - //! has arrived or not + //! For Localization: Whether the INSNavGeod and INSNavCart blocks of the + //! current epoch have arrived or not bool insnavgeod_has_arrived_localization_ = false; + bool insnavgeod_has_arrived_localization_ecef_ = false; + bool insnavcart_has_arrived_localization_ecef_ = false; /** * @brief "Callback" function when constructing NavSatFix messages diff --git a/include/septentrio_gnss_driver/communication/settings.h b/include/septentrio_gnss_driver/communication/settings.h index 02f50b60..26469dbf 100644 --- a/include/septentrio_gnss_driver/communication/settings.h +++ b/include/septentrio_gnss_driver/communication/settings.h @@ -245,10 +245,14 @@ struct Settings bool publish_imu; //! Whether or not to publish the LocalizationMsg message bool publish_localization; + //! Whether or not to publish the LocalizationMsg message + bool publish_localization_ecef; //! Whether or not to publish the TwistWithCovarianceStampedMsg message bool publish_twist; //! Whether or not to publish the tf of the localization bool publish_tf; + //! Whether or not to publish the tf of the localization + bool publish_tf_ecef; //! Wether local frame should be inserted into tf bool insert_local_frame = false; //! Frame id of the local frame to be inserted diff --git a/src/septentrio_gnss_driver/communication/callback_handlers.cpp b/src/septentrio_gnss_driver/communication/callback_handlers.cpp index 7f708217..8ff8904f 100644 --- a/src/septentrio_gnss_driver/communication/callback_handlers.cpp +++ b/src/septentrio_gnss_driver/communication/callback_handlers.cpp @@ -55,6 +55,8 @@ std::pair imu_pairs[] = {std::make_pair("4226", 0), std::make_pair("4050", 1)}; std::pair localization_pairs[] = {std::make_pair("4226", 0)}; +std::pair localization_ecef_pairs[] = { + std::make_pair("4225", 0), std::make_pair("4226", 1)}; namespace io_comm_rx { boost::mutex CallbackHandlers::callback_mutex_; @@ -74,6 +76,10 @@ namespace io_comm_rx { CallbackHandlers::localization_map(localization_pairs, localization_pairs + 1); + CallbackHandlers::LocalizationEcefMap + CallbackHandlers::localization_ecef_map(localization_ecef_pairs, + localization_ecef_pairs + 2); + std::string CallbackHandlers::do_gpsfix_ = "4007"; std::string CallbackHandlers::do_navsatfix_ = "4007"; std::string CallbackHandlers::do_pose_ = "4007"; @@ -84,6 +90,7 @@ namespace io_comm_rx { std::string CallbackHandlers::do_inspose_ = "4226"; std::string CallbackHandlers::do_imu_ = "4226"; std::string CallbackHandlers::do_inslocalization_ = "4226"; + std::string CallbackHandlers::do_inslocalization_ecef_ = "4226"; //! The for loop forwards to a ROS message specific handle if the latter was //! added via callbackmap_.insert at some earlier point. @@ -318,6 +325,29 @@ namespace io_comm_rx { do_inslocalization_ = std::string(); } } + if (settings_->publish_localization_ecef || settings_->publish_tf_ecef) + { + CallbackMap::key_type key = "LocalizationEcef"; + std::string ID_temp = rx_message_.messageID(); + if (ID_temp == do_inslocalization_ecef_) + // The last incoming block INSNavGeod triggers the publishing of + // PoseWithCovarianceStamped. + { + for (CallbackMap::iterator callback = + callbackmap_.lower_bound(key); + callback != callbackmap_.upper_bound(key); ++callback) + { + try + { + callback->second->handle(rx_message_, callback->first); + } catch (std::runtime_error& e) + { + throw std::runtime_error(e.what()); + } + } + do_inslocalization_ecef_ = std::string(); + } + } } // Call TimeReferenceMsg (with GPST) callback function if it was // added @@ -581,6 +611,16 @@ namespace io_comm_rx { do_inslocalization_ = ID_temp; } } + if ((settings_->publish_localization_ecef || + settings_->publish_tf_ecef) && + (ID_temp == "4226" || ID_temp == "4225")) + { + if (rx_message_.ins_localization_ecef_complete( + localization_ecef_map[ID_temp])) + { + do_inslocalization_ecef_ = ID_temp; + } + } } if (rx_message_.isNMEA()) { diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index b2acb9e2..8613e9d7 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -528,14 +528,16 @@ void io_comm_rx::Comm_IO::configureRx() // If INS then... if (settings_->septentrio_receiver_type == "ins") { - if (settings_->publish_insnavcart) + if (settings_->publish_insnavcart || + settings_->publish_localization_ecef || settings_->publish_tf_ecef) { blocks << " +INSNavCart"; } if (settings_->publish_insnavgeod || settings_->publish_navsatfix || settings_->publish_gpsfix || settings_->publish_pose || settings_->publish_imu || settings_->publish_localization || - settings_->publish_tf || settings_->publish_twist) + settings_->publish_tf || settings_->publish_twist || + settings_->publish_localization_ecef || settings_->publish_tf_ecef) { blocks << " +INSNavGeod"; } @@ -1182,6 +1184,11 @@ void io_comm_rx::Comm_IO::defineMessages() handlers_.callbackmap_ = handlers_.insert("Localization"); } + if (settings_->publish_localization_ecef || settings_->publish_tf_ecef) + { + handlers_.callbackmap_ = + handlers_.insert("LocalizationEcef"); + } } handlers_.callbackmap_ = handlers_.insert("5902"); // ReceiverSetup block diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 75ef964e..8ab69912 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -2278,31 +2278,32 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) case evINSNavCart: // Position, velocity and orientation in cartesian coordinate // frame (ENU frame) { - INSNavCartMsg msg; std::vector dvec(data_, data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, + if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), last_insnavcart_, settings_->use_ros_axis_orientation)) { + insnavgeod_has_arrived_localization_ecef_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in INSNavCart"); break; } if (settings_->ins_use_poi) { - msg.header.frame_id = settings_->poi_frame_id; + last_insnavcart_.header.frame_id = settings_->poi_frame_id; } else { - msg.header.frame_id = settings_->frame_id; + last_insnavcart_.header.frame_id = settings_->frame_id; } Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); + last_insnavcart_.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ecef_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { wait(time_obj); } - publish("/insnavcart", msg); + publish("/insnavcart", last_insnavcart_); break; } case evINSNavGeod: // Position, velocity and orientation in geodetic coordinate @@ -2317,6 +2318,7 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) insnavgeod_has_arrived_navsatfix_ = false; insnavgeod_has_arrived_pose_ = false; insnavgeod_has_arrived_localization_ = false; + insnavgeod_has_arrived_localization_ecef_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in INSNavGeod"); break; @@ -2334,6 +2336,7 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) insnavgeod_has_arrived_navsatfix_ = true; insnavgeod_has_arrived_pose_ = true; insnavgeod_has_arrived_localization_ = true; + insnavgeod_has_arrived_localization_ecef_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -3053,6 +3056,32 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) publishTf(msg); break; } + case evLocalizationEcef: + { + LocalizationMsg msg; + try + { + msg = LocalizationEcefCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); + break; + } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ecef_ = false; + insnavgeod_has_arrived_localization_ecef_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_localization_ecef) + publish("/localization_ecef", msg); + if (settings_->publish_tf_ecef) + publishTf(msg); + break; + } case evReceiverStatus: { std::vector dvec(data_, @@ -3267,6 +3296,13 @@ bool io_comm_rx::RxMessage::ins_localization_complete(uint32_t id) return allTrue(loc_vec, id); } +bool io_comm_rx::RxMessage::ins_localization_ecef_complete(uint32_t id) +{ + std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, + insnavcart_has_arrived_localization_ecef_}; + return allTrue(loc_vec, id); +} + bool io_comm_rx::RxMessage::allTrue(std::vector& vec, uint32_t id) { vec.erase(vec.begin() + id); diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 2f080c56..720ec9a0 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -177,8 +177,18 @@ bool rosaic_node::ROSaicNode::getROSParams() param("publish/extsensormeas", settings_.publish_extsensormeas, false); param("publish/imu", settings_.publish_imu, false); param("publish/localization", settings_.publish_localization, false); + param("publish/localization_ecef", settings_.publish_localization_ecef, false); param("publish/twist", settings_.publish_twist, false); param("publish/tf", settings_.publish_tf, false); + param("publish/tf_ecef", settings_.publish_tf_ecef, false); + + if (settings_.publish_tf && settings_.publish_tf_ecef) + { + this->log( + LogLevel::WARN, + "Only one of the tfs may be published at once, just activating tf in ECEF "); + settings_.publish_tf = false; + } // Datum and marker-to-ARP offset param("datum", settings_.datum, std::string("Default")); From f68107641dda6613bd98428c8c74624113400584 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Oct 2022 21:46:03 +0200 Subject: [PATCH 004/170] Fix NED to ECEF rotation matrix --- .../parsers/parsing_utilities.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index 5af8b88d..1cc18ac4 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -351,10 +351,10 @@ namespace parsing_utilities { Eigen::Quaterniond q_ned_ecef(double lat, double lon) { static double pihalf = boost::math::constants::pi() / 2.0; - double sp = sin((-lat - pihalf) / 2); - double cp = cos((-lat - pihalf) / 2); - double sy = sin(lon / 2); - double cy = cos(lon / 2); + double sp = sin((-lat - pihalf) / 2.0); + double cp = cos((-lat - pihalf) / 2.0); + double sy = sin(lon / 2.0); + double cy = cos(lon / 2.0); return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy); } @@ -390,14 +390,14 @@ namespace parsing_utilities { double sin_lon = sin(lon); double cos_lon = cos(lon); - R(0, 0) = cos_lon; - R(0, 1) = -sin_lon * sin_lat; - R(0, 2) = sin_lon * cos_lat; - R(1, 0) = -sin_lon; - R(1, 1) = -cos_lon * sin_lat; - R(1, 2) = cos_lon * cos_lat; - R(2, 0) = 0.0; - R(2, 1) = -cos_lat; + R(0, 0) = -cos_lon * sin_lat; + R(0, 1) = -sin_lon; + R(0, 2) = -cos_lon * cos_lat; + R(1, 0) = -sin_lon * sin_lat; + R(1, 1) = cos_lon; + R(1, 2) = -sin_lon * cos_lat; + R(2, 0) = cos_lat; + R(2, 1) = 0.0; R(2, 2) = -sin_lat; return R; From c30b6606a3ee96f78f1bbdffc1d92d62d7d3bf25 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Oct 2022 22:20:21 +0200 Subject: [PATCH 005/170] Update readme --- CHANGELOG.rst | 5 +++++ README.md | 10 ++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index bdb0b5c5..354b3c73 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.4 (upcoming) +------------------ +* New Features + * Output ot localization and tf in ECEF frame + 1.2.3 (upcoming) ------------------ * New Features diff --git a/README.md b/README.md index c1bb44b5..679f5d2d 100644 --- a/README.md +++ b/README.md @@ -224,7 +224,9 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo exteventinsnavgeod: false imu: false localization: false - tf: false + tf: false + localization_ecef: false + tf_ecef: false # INS-Specific Parameters @@ -597,7 +599,9 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/exteventinsnavgeod`: `true` to publish `septentrio_gnss_driver/ExtEventINSNavGeod.msgs` message into the topic`/exteventinsnavgeod` + `publish/imu`: `true` to publish `sensor_msgs/Imu.msg` message into the topic`/imu` + `publish/localization`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` - + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. + + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`. + + `publish/localization_ecef`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` related to ECEF frame. + + `publish/tf_ecef`: `true` to broadcast tf of localization related to ECEF frame. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`. ## ROS Topic Publications @@ -639,6 +643,8 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + The ROS message [`sensor_msgs/Imu.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + `/localization`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + + `/localization_ecef`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. ## Suggestions for Improvements From ce935ded37ec571891f79663e844cd13a1003fd3 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Oct 2022 22:46:50 +0200 Subject: [PATCH 006/170] Reorder localization msg filling --- .../communication/rx_message.cpp | 32 +++++++++---------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 8ab69912..ba183834 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -816,6 +816,16 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() msg.pose.covariance[7] = -1.0; msg.pose.covariance[14] = -1.0; } + if ((last_insnavcart_.sb_list & 32) != 0) + { + // Position covariance + msg.pose.covariance[1] = last_insnavcart_.xy_cov; + msg.pose.covariance[6] = last_insnavcart_.xy_cov; + msg.pose.covariance[2] = last_insnavcart_.xz_cov; + msg.pose.covariance[8] = last_insnavcart_.yz_cov; + msg.pose.covariance[12] = last_insnavcart_.xz_cov; + msg.pose.covariance[13] = last_insnavcart_.yz_cov; + } // Euler angles double roll = 0.0; @@ -842,7 +852,7 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() Eigen::Quaterniond q_b_local = parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw); - Eigen::Quaterniond q_b_ecef = q_b_local * q_local_ecef; + Eigen::Quaterniond q_b_ecef = q_local_ecef * q_b_local; msg.pose.pose.orientation = parsing_utilities::quaternionToQuaternionMsg(q_b_ecef); @@ -887,17 +897,6 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() covAttValid = false; } - if ((last_insnavcart_.sb_list & 32) != 0) - { - // Position covariance - msg.pose.covariance[1] = last_insnavcart_.xy_cov; - msg.pose.covariance[6] = last_insnavcart_.xy_cov; - msg.pose.covariance[2] = last_insnavcart_.xz_cov; - msg.pose.covariance[8] = last_insnavcart_.yz_cov; - msg.pose.covariance[12] = last_insnavcart_.xz_cov; - msg.pose.covariance[13] = last_insnavcart_.yz_cov; - } - if (covAttValid) { if ((last_insnavcart_.sb_list & 64) != 0) @@ -906,7 +905,6 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() covAtt_local(0, 1) = deg2radSq(last_insnavcart_.pitch_roll_cov); covAtt_local(0, 2) = deg2radSq(last_insnavcart_.heading_roll_cov); covAtt_local(1, 0) = deg2radSq(last_insnavcart_.pitch_roll_cov); - covAtt_local(2, 1) = deg2radSq(last_insnavcart_.heading_pitch_cov); covAtt_local(2, 0) = deg2radSq(last_insnavcart_.heading_roll_cov); covAtt_local(1, 2) = deg2radSq(last_insnavcart_.heading_pitch_cov); @@ -930,14 +928,14 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() R_local_ecef * covAtt_local * R_local_ecef.transpose(); msg.pose.covariance[21] = covAtt_ecef(0, 0); - msg.pose.covariance[28] = covAtt_ecef(1, 1); - msg.pose.covariance[35] = covAtt_ecef(2, 2); msg.pose.covariance[22] = covAtt_ecef(0, 1); msg.pose.covariance[23] = covAtt_ecef(0, 2); msg.pose.covariance[27] = covAtt_ecef(1, 0); - msg.pose.covariance[29] = covAtt_ecef(2, 1); + msg.pose.covariance[28] = covAtt_ecef(1, 1); + msg.pose.covariance[29] = covAtt_ecef(1, 2); msg.pose.covariance[33] = covAtt_ecef(2, 0); - msg.pose.covariance[34] = covAtt_ecef(1, 2); + msg.pose.covariance[34] = covAtt_ecef(2, 1); + msg.pose.covariance[35] = covAtt_ecef(2, 2); } else { msg.pose.covariance[21] = -1.0; From da432fb0dd46a50b932b3e5384e6b300740174b6 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 29 Oct 2022 23:21:28 +0200 Subject: [PATCH 007/170] Fix lat/long in rad --- .../communication/rx_message.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index ba183834..7e27343e 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -838,17 +838,16 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() if (validValue(last_insnavcart_.heading)) yaw = deg2rad(last_insnavcart_.heading); - double latitude = deg2rad(last_insnavgeod_.latitude); - double longitude = deg2rad(last_insnavgeod_.longitude); - if ((last_insnavcart_.sb_list & 2) != 0) { // Attitude Eigen::Quaterniond q_local_ecef; if (settings_->use_ros_axis_orientation) - q_local_ecef = parsing_utilities::q_enu_ecef(latitude, longitude); + q_local_ecef = parsing_utilities::q_enu_ecef(last_insnavgeod_.latitude, + last_insnavgeod_.longitude); else - q_local_ecef = parsing_utilities::q_ned_ecef(latitude, longitude); + q_local_ecef = parsing_utilities::q_ned_ecef(last_insnavgeod_.latitude, + last_insnavgeod_.longitude); Eigen::Quaterniond q_b_local = parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw); @@ -920,9 +919,11 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() } Eigen::Matrix3d R_local_ecef; if (settings_->use_ros_axis_orientation) - R_local_ecef = parsing_utilities::R_enu_ecef(latitude, longitude); + R_local_ecef = parsing_utilities::R_enu_ecef(last_insnavgeod_.latitude, + last_insnavgeod_.longitude); else - R_local_ecef = parsing_utilities::R_ned_ecef(latitude, longitude); + R_local_ecef = parsing_utilities::R_ned_ecef(last_insnavgeod_.latitude, + last_insnavgeod_.longitude); // Rotate attitude covariance matrix to ecef coordinates Eigen::Matrix3d covAtt_ecef = R_local_ecef * covAtt_local * R_local_ecef.transpose(); From 7deef267a02f5476c08dee2ab06a61db97c17a86 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 7 Dec 2022 13:14:47 +0100 Subject: [PATCH 008/170] Fix spelling errors --- CHANGELOG.rst | 2 +- include/septentrio_gnss_driver/parsers/string_utilities.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 90a10210..5e34801e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,7 +5,7 @@ Changelog for package septentrio_gnss_driver 1.2.4 (upcoming) ------------------ * New Features - * Output ot localization and tf in ECEF frame + * Output of localization and tf in ECEF frame 1.2.3 (2022-11-09) ------------------ diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.h b/include/septentrio_gnss_driver/parsers/string_utilities.h index 0ce07c7e..9340f9cf 100644 --- a/include/septentrio_gnss_driver/parsers/string_utilities.h +++ b/include/septentrio_gnss_driver/parsers/string_utilities.h @@ -137,7 +137,7 @@ namespace string_utilities { uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base = 10); /** - * @brief Trims decimal places to two + * @brief Trims decimal places to three * @param[in] num The double who shall be trimmed * @return The string */ From c1462d1b4f527920d03d9f4afdc9b19e3174f902 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 7 Dec 2022 14:27:18 +0100 Subject: [PATCH 009/170] Remove obsolete define --- include/septentrio_gnss_driver/parsers/string_utilities.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.h b/include/septentrio_gnss_driver/parsers/string_utilities.h index 9340f9cf..587f2238 100644 --- a/include/septentrio_gnss_driver/parsers/string_utilities.h +++ b/include/septentrio_gnss_driver/parsers/string_utilities.h @@ -42,10 +42,6 @@ * @date 13/08/20 */ -#ifdef __cplusplus -extern "C" { -#endif - /** * @namespace string_utilities * This namespace is for the functions that encapsulate basic string manipulation and @@ -151,8 +147,4 @@ namespace string_utilities { bool containsSpace(const std::string str); } // namespace string_utilities -#ifdef __cplusplus -} -#endif - #endif // STRING_UTILITIES_H From 0b47cc07814e51daf9e0a60e483d840c740ab77a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 18 Dec 2022 14:06:47 +0100 Subject: [PATCH 010/170] Rename meridian convergence and fix sense --- .../communication/rx_message.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 7e27343e..5661d041 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -633,21 +633,22 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() bool northernHemisphere; double easting; double northing; - double gamma = 0.0; + double meridian_convergence = 0.0; if (fixedUtmZone_) { double k; GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, northernHemisphere); - GeographicLib::UTMUPS::Forward( - rad2deg(last_insnavgeod_.latitude), rad2deg(last_insnavgeod_.longitude), - zone, northernHemisphere, easting, northing, gamma, k, zone); + GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), zone, + northernHemisphere, easting, northing, + meridian_convergence, k, zone); zonestring = *fixedUtmZone_; } else { double k; GeographicLib::UTMUPS::Forward( rad2deg(last_insnavgeod_.latitude), rad2deg(last_insnavgeod_.longitude), - zone, northernHemisphere, easting, northing, gamma, k); + zone, northernHemisphere, easting, northing, meridian_convergence, k); zonestring = GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); } if (settings_->lock_utm_zone && !fixedUtmZone_) @@ -695,11 +696,11 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() double yaw = 0.0; if (validValue(last_insnavgeod_.heading)) yaw = deg2rad(last_insnavgeod_.heading); - // gamma for conversion from true north to grid north + // meridian_convergence for conversion from true north to grid north if (settings_->use_ros_axis_orientation) - yaw -= deg2rad(gamma); + yaw += deg2rad(meridian_convergence); else - yaw += deg2rad(gamma); + yaw -= deg2rad(meridian_convergence); if ((last_insnavgeod_.sb_list & 2) != 0) { From 3f6cc00a76508b142e5e997cd8488cac42aa2104 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 18 Dec 2022 15:18:03 +0100 Subject: [PATCH 011/170] Add cov alignment from true north to grid north --- .../communication/rx_message.cpp | 55 +++++++++++++------ 1 file changed, 37 insertions(+), 18 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 5661d041..3f09c87c 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -673,17 +673,13 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() else msg.child_frame_id = settings_->frame_id; + Eigen::Matrix3d P_pos = -Eigen::Matrix3d::Identity(); if ((last_insnavgeod_.sb_list & 1) != 0) { // Position autocovariance - msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev); - msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev); - msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev); - } else - { - msg.pose.covariance[0] = -1.0; - msg.pose.covariance[7] = -1.0; - msg.pose.covariance[14] = -1.0; + P_pos(0, 0) = square(last_insnavgeod_.longitude_std_dev); + P_pos(1, 1) = square(last_insnavgeod_.latitude_std_dev); + P_pos(2, 2) = square(last_insnavgeod_.height_std_dev); } // Euler angles @@ -740,25 +736,48 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() if ((last_insnavgeod_.sb_list & 32) != 0) { // Position covariance - msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; + P_pos(0, 1) = last_insnavgeod_.latitude_longitude_cov; + P_pos(1, 0) = last_insnavgeod_.latitude_longitude_cov; if (settings_->use_ros_axis_orientation) { // (ENU) - msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; + P_pos(0, 2) = last_insnavgeod_.longitude_height_cov; + P_pos(1, 2) = last_insnavgeod_.latitude_height_cov; + P_pos(2, 0) = last_insnavgeod_.longitude_height_cov; + P_pos(2, 1) = last_insnavgeod_.latitude_height_cov; } else { // (NED) - msg.pose.covariance[2] = -last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[8] = -last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[12] = -last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[13] = -last_insnavgeod_.longitude_height_cov; + P_pos(0, 2) = -last_insnavgeod_.latitude_height_cov; + P_pos(1, 2) = -last_insnavgeod_.longitude_height_cov; + P_pos(2, 0) = -last_insnavgeod_.latitude_height_cov; + P_pos(2, 1) = -last_insnavgeod_.longitude_height_cov; } } + + if ((meridian_convergence != 0.0) && (last_insnavgeod_.sb_list & 1)) + { + double cg = std::cos(meridian_convergence); + double sg = std::sin(meridian_convergence); + Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); + R(0, 0) = cg; + R(0, 1) = -sg; + R(1, 0) = sg; + R(1, 1) = cg; + P_pos = (R * P_pos * R.transpose()).eval(); + } + + msg.pose.covariance[0] = P_pos(0, 0); + msg.pose.covariance[1] = P_pos(0, 1); + msg.pose.covariance[2] = P_pos(0, 2); + msg.pose.covariance[6] = P_pos(1, 0); + msg.pose.covariance[7] = P_pos(1, 1); + msg.pose.covariance[8] = P_pos(1, 2); + msg.pose.covariance[12] = P_pos(2, 0); + msg.pose.covariance[13] = P_pos(2, 1); + msg.pose.covariance[14] = P_pos(2, 2); + if ((last_insnavgeod_.sb_list & 64) != 0) { // Attitude covariancae From 9c69d8338674060d632bb71edbe42a63fe2796e6 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 18 Dec 2022 15:35:11 +0100 Subject: [PATCH 012/170] Fix attitude cov flipped twice --- .../communication/rx_message.cpp | 21 ++----------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 3f09c87c..12b36847 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -748,7 +748,7 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() P_pos(2, 1) = last_insnavgeod_.latitude_height_cov; } else { - // (NED) + // (NED): down = -height P_pos(0, 2) = -last_insnavgeod_.latitude_height_cov; P_pos(1, 2) = -last_insnavgeod_.longitude_height_cov; P_pos(2, 0) = -last_insnavgeod_.latitude_height_cov; @@ -788,15 +788,6 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - - if (!settings_->use_ros_axis_orientation) - { - // (NED) - msg.pose.covariance[33] *= -1.0; - msg.pose.covariance[23] *= -1.0; - msg.pose.covariance[22] *= -1.0; - msg.pose.covariance[27] *= -1.0; - } } fillLocalizationMsgTwist(roll, pitch, yaw, msg); @@ -927,16 +918,8 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() covAtt_local(2, 1) = deg2radSq(last_insnavcart_.heading_pitch_cov); covAtt_local(2, 0) = deg2radSq(last_insnavcart_.heading_roll_cov); covAtt_local(1, 2) = deg2radSq(last_insnavcart_.heading_pitch_cov); - - if (!settings_->use_ros_axis_orientation) - { - // (NED) - covAtt_local(0, 2) *= -1.0; - covAtt_local(2, 0) *= -1.0; - covAtt_local(0, 1) *= -1.0; - covAtt_local(1, 0) *= -1.0; - } } + Eigen::Matrix3d R_local_ecef; if (settings_->use_ros_axis_orientation) R_local_ecef = parsing_utilities::R_enu_ecef(last_insnavgeod_.latitude, From 629ada6586ca9c2c76397955d4d1be8fd9569098 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 9 Jan 2023 23:15:57 +0100 Subject: [PATCH 013/170] Change connection thread --- .../communication/communication_core.hpp | 2 +- .../communication/communication_core.cpp | 16 +++++++--------- .../communication/rx_message.cpp | 2 +- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 336367fc..7b5ab78a 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -249,7 +249,7 @@ namespace io_comm_rx { bool nmeaActivated_ = false; //! Connection or reading thread - std::unique_ptr connectionThread_; + boost::thread connectionThread_; //! Indicator for threads to exit std::atomic stopping_; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 8613e9d7..f7a0a576 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -185,7 +185,7 @@ io_comm_rx::Comm_IO::~Comm_IO() } stopping_ = true; - connectionThread_->join(); + connectionThread_.join(); } void io_comm_rx::Comm_IO::resetMainPort() @@ -224,16 +224,15 @@ void io_comm_rx::Comm_IO::initializeIO() tcp_port_ = match[3]; serial_ = false; - connectionThread_.reset( - new boost::thread(boost::bind(&Comm_IO::connect, this))); + connectionThread_ = boost::thread(boost::bind(&Comm_IO::connect, this)); } else if (boost::regex_match(settings_->device, match, boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) { serial_ = false; settings_->read_from_sbf_log = true; settings_->use_gnss_time = true; - connectionThread_.reset(new boost::thread( - boost::bind(&Comm_IO::prepareSBFFileReading, this, match[2]))); + connectionThread_ = boost::thread( + boost::bind(&Comm_IO::prepareSBFFileReading, this, match[2])); } else if (boost::regex_match( settings_->device, match, @@ -242,8 +241,8 @@ void io_comm_rx::Comm_IO::initializeIO() serial_ = false; settings_->read_from_pcap = true; settings_->use_gnss_time = true; - connectionThread_.reset(new boost::thread( - boost::bind(&Comm_IO::preparePCAPFileReading, this, match[2]))); + connectionThread_ = boost::thread( + boost::bind(&Comm_IO::preparePCAPFileReading, this, match[2])); } else if (boost::regex_match(settings_->device, match, boost::regex("(serial):(.+)"))) @@ -254,8 +253,7 @@ void io_comm_rx::Comm_IO::initializeIO() ss << "Searching for serial port" << proto; settings_->device = proto; node_->log(LogLevel::DEBUG, ss.str()); - connectionThread_.reset( - new boost::thread(boost::bind(&Comm_IO::connect, this))); + connectionThread_ = boost::thread(boost::bind(&Comm_IO::connect, this)); } else { std::stringstream ss; diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 12b36847..6088c747 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -311,7 +311,7 @@ ImuMsg io_comm_rx::RxMessage::ImuCallback() last_extsensmeas_.block_header.wnc, true); Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow, last_insnavgeod_.block_header.wnc, - true); // Filling in the oreintation data + true); // Filling in the orientation data static int64_t maxDt = (settings_->polling_period_pvt == 0) ? 10000000 From b2798118694009fa20d8371fce166bc14fb5eb88 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 11 Jan 2023 00:01:42 +0100 Subject: [PATCH 014/170] Add new low level interface, WIP --- .../communication/AsyncManager.hpp | 481 ++++++++++++++++++ .../communication/telegram.hpp | 92 ++++ 2 files changed, 573 insertions(+) create mode 100644 include/septentrio_gnss_driver/communication/AsyncManager.hpp create mode 100644 include/septentrio_gnss_driver/communication/telegram.hpp diff --git a/include/septentrio_gnss_driver/communication/AsyncManager.hpp b/include/septentrio_gnss_driver/communication/AsyncManager.hpp new file mode 100644 index 00000000..6284ff53 --- /dev/null +++ b/include/septentrio_gnss_driver/communication/AsyncManager.hpp @@ -0,0 +1,481 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +// ***************************************************************************** +// +// Boost Software License - Version 1.0 - August 17th, 2003 +// +// Permission is hereby granted, free of charge, to any person or organization +// obtaining a copy of the software and accompanying documentation covered by +// this license (the "Software") to use, reproduce, display, distribute, +// execute, and transmit the Software, and to prepare derivative works of the +// Software, and to permit third-parties to whom the Software is furnished to +// do so, all subject to the following: + +// The copyright notices in the Software and this entire statement, including +// the above license grant, this restriction and the following disclaimer, +// must be included in all copies of the Software, in whole or in part, and +// all derivative works of the Software, unless such copies or derivative +// works are solely in the form of machine-executable object code generated by +// a source language processor. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// ***************************************************************************** + +// Boost includes +#include +#include +#include +#include +#include +#include +#include +#include + +// ROSaic includes +#include +#include + +// local includes +#include "telegram.hpp" + +#pragma once + +/** + * @file async_manager.hpp + * @date 20/08/20 + * @brief Implements asynchronous operations for an I/O manager + * + * Such operations include reading NMEA messages and SBF blocks yet also sending + * commands to serial port or via TCP/IP. + */ + +namespace io_comm_rx { + + /** + * @class AsyncManager + * @brief This is the central interface between ROSaic and the Rx(s), managing + * I/O operations such as reading messages and sending commands.. + * + * StreamT is either boost::asio::serial_port or boost::asio::tcp::ip + */ + template + class AsyncManager + { + public: + /** + * @brief Class constructor + * @param stream data stream + * @param[in] buffer_size Size of the circular buffer in bytes + */ + AsyncManager(ROSaicNodeBase* node, boost::shared_ptr stream) : + stream_(stream) + { + receive(); + + watchdogThread_ = std::thread(std::bind(&TcpClient::runWatchdog, this)); + } + + ~AsyncManager() + { + running_ = false; + flushOutputQueue(); + close(); + node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); + ioService_.stop(); + ioThread_.join(); + watchdogThread_.join(); + node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); + } + + private: + void receive() + { + resync(); + ioThread_ = + std::thread(std::thread(std::bind(&TcpClient::runIoService, this))); + } + + void flushOutputQueue() + { + ioService_.post([this]() { write(); }); + } + + void close() + { + ioService_.post([this]() { socket_->close(); }); + } + + void runIoService() + { + ioService_.run(); + node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); + } + + void runWatchdog() + { + while (running_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + + if (running_ && ioService_.stopped()) + { + node_->log(LogLevel::DEBUG, + "AsyncManager connection lost. Trying to reconnect."); + ioService_.reset(); + ioThread_->join(); + receive(); + } + } + } + + void send(const std::string& cmd) + { + if (cmd.size() == 0) + { + node_->log( + LogLevel::ERROR, + "AsyncManager message size to be sent to the Rx would be 0"); + } + + io_service_->post(boost::bind(&AsyncManager::write, this, cmd)); + } + + void write(const std::string& cmd) + { + boost::asio::write(*stream_, + boost::asio::buffer(cmd.data(), cmd.size())); + // Prints the data that was sent + node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); + } + + void resync() + { + telegram_.reset(new Telegram); + readSync<0>(); + } + + template + void readSync() + { + static_assert(index < 3); + + boost::asio::async_read( + *stream_, boost::asio::buffer(telegram_->data.data() + index, 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + Timestamp stamp = node_->getTime(); + + if (!ec) + { + if (numBytes == 1) + { + switch (index) + { + case 0: + { + if (telegram_->data[index] == SYNC_0) + { + telegram_->stamp = stamp; + readSync<1>(); + } else + readSync<0>(); + break; + } + case 1: + { + switch (telegram_->data[index]) + { + case SBF_SYNC_BYTE_1: + { + telegram_->type = SBF; + readSbfHeader(); + break; + } + case NMEA_SYNC_BYTE_1: + { + telegram_->type = NMEA; + readSync<2>(); + break; + } + case RESPONSE_SYNC_BYTE_1: + { + stelegram_->type = RESPONSE; + readSync<2>(); + break; + } + case CONNECTION_DESCRIPTOR_BYTE_1: + { + telegram_->type = CONNECTION_DESCRIPTOR; + readSync<2>(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 1 read fault, should never come here."); + resync(); + break; + } + } + break; + } + case 2: + { + switch (telegram_->data[index]) + { + case NMEA_SYNC_BYTE_2: + { + if (telegram_->type == NMEA) + readString(); + else + resync(); + break; + } + case RESPONSE_SYNC_BYTE_1: + { + if (telegram_->type == RESPONSE) + readString(); + else + resync(); + break; + } + case CONNECTION_DESCRIPTOR_BYTE_1: + { + if (telegram_->type == CONNECTION_DESCRIPTOR) + readString(); + else + resync(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 2 read fault, should never come here."); + resync(); + break; + } + } + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync read fault, should never come here."); + resync(); + break; + } + } + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager sync read error: " + ec.message()); + resync(); + } + }); + } + + void readSbfHeader() + { + telegram_->data.resize(SBF_HEADER_SIZE); + + boost::asio::async_read( + *stream_, + boost::asio::buffer(telegram_->data.data() + 2, SBF_HEADER_SIZE - 2), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (SBF_HEADER_SIZE - 2)) + { + unit16_t length = + parsing_utilities::getLength(telegram_->data.data()); + if (length > MAX_SBF_SIZE) + { + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF header read fault, length of block exceeds " + + std::to_string(MAX_SBF_SIZE) + ": " + + std::to_string(length)); + } else + readSbf(length); + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF header read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager SBF header read error: " + + ec.message()); + resync(); + } + }); + } + + void readSbf(std::size_t length) + { + telegram_->data.resize(length); + + boost::asio::async_read( + *stream_, + boost::asio::buffer(telegram_->data.data() + SBF_HEADER_SIZE, + length - SBF_HEADER_SIZE), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (length - SBF_HEADER_SIZE)) + { + if (isValid( + telegram_->data.data())) // TODO namespace crc + { + telegram_->sbfId = + parsing_utilities::getId(telegram_->data.data()); + + telegramQueue_->push(telegram_); + } + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager SBF read error: " + ec.message()); + } + resync(); + }); + } + + void readString() + { + telegram_->data.resize(3); + readStringElements(); + } + + void readStringElements() + { + std::byte byte; + + boost::asio::async_read( + *stream_, boost::asio::buffer(byte, 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == 1) + { + switch (byte) + { + case SYNC_0: + { + telegram_.reset(new Telegram); + telegram_->data[0] = byte; + telegram_->stamp = node_->getTime(); + node_->log( + LogLevel::DEBUG, + "AsyncManager string read fault, sync 0 found.")); + readSync<1>(); + break; + } + case LF: + { + telegram_->data.push_back(byte); + if (telegram_->data[telegram_->data.size() - 2] == + CR) + telegramQueue_->push(telegram_); + resync(); + break; + } + default: + { + telegram_->data.push_back(byte); + readString(); + break; + } + } + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager string read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager string read error: " + + ec.message()); + resync(); + } + }); + } + + //! Stream, represents either interface connection or file + std::shared_ptr stream_; + //! Pointer to the node + ROSaicNodeBase* node_; + std::atomic running_; + boost::asio::io_service ioService_; + std::thread ioThread_; + std::thread watchdogThread_; + //! Timestamp of receiving buffer + Timestamp recvStamp_; + //! Telegram + std::shared_ptr telegram_; + //! TelegramQueue + TelegramQueue* telegramQueue_; + } +} // namespace io_comm_rx \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp new file mode 100644 index 00000000..8324d4d5 --- /dev/null +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -0,0 +1,92 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#pragma once + +// C++ +#include +#include + +// TBB +#include + +// ROSaic +#include + +//! 0x24 is ASCII for $ - 1st byte in each message +static const std::byte SYNC_0 = 0x24; +//! 0x40 is ASCII for @ - 2nd byte to indicate SBF block +static const std::byte SBF_SYNC_BYTE_1 = 0x40; +//! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message +static const std::byte NMEA_SYNC_BYTE_1 0x47; +//! 0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message +static const std::byte NMEA_SYNC_BYTE_2 0x50; +//! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx +static const std::byte RESPONSE_SYNC_BYTE_1 0x52; +//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the +//! command was invalid +static const std::byte RESPONSE_SYNC_BYTE_2 0x3F; +//! 0x0D is ASCII for "Carriage Return", i.e. "Enter" +static const std::byte CR 0x0D; +//! 0x0A is ASCII for "Line Feed", i.e. "New Line" +static const std::byte LF 0x0A; +//! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after +//! initiating TCP connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_1 0x49; +//! 0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after +//! initiating TCP connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_2 0x50; + +static const uint16_t SBF_HEADER_SIZE = 8; +static const uint16_t MAX_SBF_SIZE 65535; + +enum MessageType +{ + INVALID, + SBF, + NMEA, + RESPONSE, + CONNECTION_DESCRIPTOR +}; + +struct Telegram +{ + Timestamp stamp = 0; + MessageType type = INVALID; + uint16_t sbfId = 0; + std::vector data(3); + + Telegram() : stamp(0), type(INVALID), sbfId(0), data(std::vector(3)) + { + data.reserve(MAX_SBF_SIZE); + } +}; + +typedef tbb::concurrent_bounded_queue> TelegramQueue; From 7117634bf9a84df0df2f8c9531a14f1cf9f1e101 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 11 Jan 2023 17:51:05 +0100 Subject: [PATCH 015/170] Add io modules --- CHANGELOG.rst | 4 +- README.md | 2 +- .../communication/AsyncManager.hpp | 481 ----------- .../communication/async_manager.hpp | 765 ++++++++++-------- .../communication/callback_handlers.hpp | 6 +- .../communication/communication_core.hpp | 46 +- .../communication/io.hpp | 278 +++++++ .../communication/rx_message.hpp | 4 +- .../node/rosaic_node.hpp | 2 +- .../communication/callback_handlers.cpp | 4 +- .../communication/communication_core.cpp | 562 +------------ .../communication/rx_message.cpp | 80 +- .../node/rosaic_node.cpp | 2 +- 13 files changed, 796 insertions(+), 1440 deletions(-) delete mode 100644 include/septentrio_gnss_driver/communication/AsyncManager.hpp create mode 100644 include/septentrio_gnss_driver/communication/io.hpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5e34801e..a9ffcd47 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -303,8 +303,8 @@ Changelog for package septentrio_gnss_driver * Move more global settings to settings struct * Move more global settings to settings struct * Move global settings to settings struct - * Move more functions to Comm_IO - * Move settings to struct and configuration to Comm_IO + * Move more functions to CommIo + * Move settings to struct and configuration to CommIo * Merge branch 'dev/change_utc_calculation' into dev/refactor * Remove obsolete global variables * Move g_unix_time to class diff --git a/README.md b/README.md index 679f5d2d..114389d3 100644 --- a/README.md +++ b/README.md @@ -667,7 +667,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr - Both: Add a new include guard to let the compiler know about the existence of the header file (such as `septentrio_gnss_driver/PVTGeodetic.h`) that gets compiler-generated from the `.msg` file constructed in step 3. - SBF: Extend the `NMEA_ID_Enum` enumeration in the `rx_message.hpp` file with a new entry. - SBF: Extend the initialization of the `RxIDMap` map in the `rx_message.cpp` file with a new pair. - - SBF: Add a new callback function declaration, a new method, to the `io_comm_rx::RxMessage class` in the `rx_message.hpp` file. + - SBF: Add a new callback function declaration, a new method, to the `io::RxMessage class` in the `rx_message.hpp` file. - SBF: Add the latter's definition to the `rx_message.cpp` file. - SBF: Add a new C++ "case" (part of the C++ switch-case structure) in the `rx_message.hpp` file. It should be modeled on the existing `evPVTGeodetic` case, e.g. one needs a static counter variable declaration. - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `septentrio_gnss_driver/src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `septentrio_gnss_driver/include/septentrio_gnss_driver/parsers/nmea_parsers` folder. diff --git a/include/septentrio_gnss_driver/communication/AsyncManager.hpp b/include/septentrio_gnss_driver/communication/AsyncManager.hpp deleted file mode 100644 index 6284ff53..00000000 --- a/include/septentrio_gnss_driver/communication/AsyncManager.hpp +++ /dev/null @@ -1,481 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ***************************************************************************** -// -// Boost Software License - Version 1.0 - August 17th, 2003 -// -// Permission is hereby granted, free of charge, to any person or organization -// obtaining a copy of the software and accompanying documentation covered by -// this license (the "Software") to use, reproduce, display, distribute, -// execute, and transmit the Software, and to prepare derivative works of the -// Software, and to permit third-parties to whom the Software is furnished to -// do so, all subject to the following: - -// The copyright notices in the Software and this entire statement, including -// the above license grant, this restriction and the following disclaimer, -// must be included in all copies of the Software, in whole or in part, and -// all derivative works of the Software, unless such copies or derivative -// works are solely in the form of machine-executable object code generated by -// a source language processor. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT -// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE -// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, -// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. -// -// ***************************************************************************** - -// Boost includes -#include -#include -#include -#include -#include -#include -#include -#include - -// ROSaic includes -#include -#include - -// local includes -#include "telegram.hpp" - -#pragma once - -/** - * @file async_manager.hpp - * @date 20/08/20 - * @brief Implements asynchronous operations for an I/O manager - * - * Such operations include reading NMEA messages and SBF blocks yet also sending - * commands to serial port or via TCP/IP. - */ - -namespace io_comm_rx { - - /** - * @class AsyncManager - * @brief This is the central interface between ROSaic and the Rx(s), managing - * I/O operations such as reading messages and sending commands.. - * - * StreamT is either boost::asio::serial_port or boost::asio::tcp::ip - */ - template - class AsyncManager - { - public: - /** - * @brief Class constructor - * @param stream data stream - * @param[in] buffer_size Size of the circular buffer in bytes - */ - AsyncManager(ROSaicNodeBase* node, boost::shared_ptr stream) : - stream_(stream) - { - receive(); - - watchdogThread_ = std::thread(std::bind(&TcpClient::runWatchdog, this)); - } - - ~AsyncManager() - { - running_ = false; - flushOutputQueue(); - close(); - node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); - ioService_.stop(); - ioThread_.join(); - watchdogThread_.join(); - node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); - } - - private: - void receive() - { - resync(); - ioThread_ = - std::thread(std::thread(std::bind(&TcpClient::runIoService, this))); - } - - void flushOutputQueue() - { - ioService_.post([this]() { write(); }); - } - - void close() - { - ioService_.post([this]() { socket_->close(); }); - } - - void runIoService() - { - ioService_.run(); - node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); - } - - void runWatchdog() - { - while (running_) - { - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - - if (running_ && ioService_.stopped()) - { - node_->log(LogLevel::DEBUG, - "AsyncManager connection lost. Trying to reconnect."); - ioService_.reset(); - ioThread_->join(); - receive(); - } - } - } - - void send(const std::string& cmd) - { - if (cmd.size() == 0) - { - node_->log( - LogLevel::ERROR, - "AsyncManager message size to be sent to the Rx would be 0"); - } - - io_service_->post(boost::bind(&AsyncManager::write, this, cmd)); - } - - void write(const std::string& cmd) - { - boost::asio::write(*stream_, - boost::asio::buffer(cmd.data(), cmd.size())); - // Prints the data that was sent - node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + - std::to_string(cmd.size()) + - " bytes to the Rx: " + cmd); - } - - void resync() - { - telegram_.reset(new Telegram); - readSync<0>(); - } - - template - void readSync() - { - static_assert(index < 3); - - boost::asio::async_read( - *stream_, boost::asio::buffer(telegram_->data.data() + index, 1), - [this](boost::system::error_code ec, std::size_t numBytes) { - Timestamp stamp = node_->getTime(); - - if (!ec) - { - if (numBytes == 1) - { - switch (index) - { - case 0: - { - if (telegram_->data[index] == SYNC_0) - { - telegram_->stamp = stamp; - readSync<1>(); - } else - readSync<0>(); - break; - } - case 1: - { - switch (telegram_->data[index]) - { - case SBF_SYNC_BYTE_1: - { - telegram_->type = SBF; - readSbfHeader(); - break; - } - case NMEA_SYNC_BYTE_1: - { - telegram_->type = NMEA; - readSync<2>(); - break; - } - case RESPONSE_SYNC_BYTE_1: - { - stelegram_->type = RESPONSE; - readSync<2>(); - break; - } - case CONNECTION_DESCRIPTOR_BYTE_1: - { - telegram_->type = CONNECTION_DESCRIPTOR; - readSync<2>(); - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync 1 read fault, should never come here."); - resync(); - break; - } - } - break; - } - case 2: - { - switch (telegram_->data[index]) - { - case NMEA_SYNC_BYTE_2: - { - if (telegram_->type == NMEA) - readString(); - else - resync(); - break; - } - case RESPONSE_SYNC_BYTE_1: - { - if (telegram_->type == RESPONSE) - readString(); - else - resync(); - break; - } - case CONNECTION_DESCRIPTOR_BYTE_1: - { - if (telegram_->type == CONNECTION_DESCRIPTOR) - readString(); - else - resync(); - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync 2 read fault, should never come here."); - resync(); - break; - } - } - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync read fault, should never come here."); - resync(); - break; - } - } - } else - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync read fault, wrong number of bytes read: " + - std::to_string(numBytes)); - resync(); - } - } else - { - node_->log(LogLevel::DEBUG, - "AsyncManager sync read error: " + ec.message()); - resync(); - } - }); - } - - void readSbfHeader() - { - telegram_->data.resize(SBF_HEADER_SIZE); - - boost::asio::async_read( - *stream_, - boost::asio::buffer(telegram_->data.data() + 2, SBF_HEADER_SIZE - 2), - [this](boost::system::error_code ec, std::size_t numBytes) { - if (!ec) - { - if (numBytes == (SBF_HEADER_SIZE - 2)) - { - unit16_t length = - parsing_utilities::getLength(telegram_->data.data()); - if (length > MAX_SBF_SIZE) - { - node_->log( - LogLevel::DEBUG, - "AsyncManager SBF header read fault, length of block exceeds " + - std::to_string(MAX_SBF_SIZE) + ": " + - std::to_string(length)); - } else - readSbf(length); - } else - { - node_->log( - LogLevel::DEBUG, - "AsyncManager SBF header read fault, wrong number of bytes read: " + - std::to_string(numBytes)); - resync(); - } - } else - { - node_->log(LogLevel::DEBUG, - "AsyncManager SBF header read error: " + - ec.message()); - resync(); - } - }); - } - - void readSbf(std::size_t length) - { - telegram_->data.resize(length); - - boost::asio::async_read( - *stream_, - boost::asio::buffer(telegram_->data.data() + SBF_HEADER_SIZE, - length - SBF_HEADER_SIZE), - [this](boost::system::error_code ec, std::size_t numBytes) { - if (!ec) - { - if (numBytes == (length - SBF_HEADER_SIZE)) - { - if (isValid( - telegram_->data.data())) // TODO namespace crc - { - telegram_->sbfId = - parsing_utilities::getId(telegram_->data.data()); - - telegramQueue_->push(telegram_); - } - } else - { - node_->log( - LogLevel::DEBUG, - "AsyncManager SBF read fault, wrong number of bytes read: " + - std::to_string(numBytes)); - } - } else - { - node_->log(LogLevel::DEBUG, - "AsyncManager SBF read error: " + ec.message()); - } - resync(); - }); - } - - void readString() - { - telegram_->data.resize(3); - readStringElements(); - } - - void readStringElements() - { - std::byte byte; - - boost::asio::async_read( - *stream_, boost::asio::buffer(byte, 1), - [this](boost::system::error_code ec, std::size_t numBytes) { - if (!ec) - { - if (numBytes == 1) - { - switch (byte) - { - case SYNC_0: - { - telegram_.reset(new Telegram); - telegram_->data[0] = byte; - telegram_->stamp = node_->getTime(); - node_->log( - LogLevel::DEBUG, - "AsyncManager string read fault, sync 0 found.")); - readSync<1>(); - break; - } - case LF: - { - telegram_->data.push_back(byte); - if (telegram_->data[telegram_->data.size() - 2] == - CR) - telegramQueue_->push(telegram_); - resync(); - break; - } - default: - { - telegram_->data.push_back(byte); - readString(); - break; - } - } - } else - { - node_->log( - LogLevel::DEBUG, - "AsyncManager string read fault, wrong number of bytes read: " + - std::to_string(numBytes)); - resync(); - } - } else - { - node_->log(LogLevel::DEBUG, - "AsyncManager string read error: " + - ec.message()); - resync(); - } - }); - } - - //! Stream, represents either interface connection or file - std::shared_ptr stream_; - //! Pointer to the node - ROSaicNodeBase* node_; - std::atomic running_; - boost::asio::io_service ioService_; - std::thread ioThread_; - std::thread watchdogThread_; - //! Timestamp of receiving buffer - Timestamp recvStamp_; - //! Telegram - std::shared_ptr telegram_; - //! TelegramQueue - TelegramQueue* telegramQueue_; - } -} // namespace io_comm_rx \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index dab22a34..81ae6a16 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -57,20 +57,19 @@ // ***************************************************************************** // Boost includes -#include #include #include -#include -#include -#include -#include -#include +#include // ROSaic includes -#include +#include +#include -#ifndef ASYNC_MANAGER_HPP -#define ASYNC_MANAGER_HPP +// local includes +#include "io.hpp" +#include "telegram.hpp" + +#pragma once /** * @file async_manager.hpp @@ -81,28 +80,19 @@ * commands to serial port or via TCP/IP. */ -namespace io_comm_rx { +namespace io { /** - * @class Manager + * @class AsyncManagerBase * @brief Interface (in C++ terms), that could be used for any I/O manager, * synchronous and asynchronous alike */ - class Manager + class AsyncManagerBase { public: - typedef boost::function - Callback; - virtual ~Manager() {} - //! Sets the callback function - virtual void setCallback(const Callback& callback) = 0; + virtual ~AsyncManagerBase() {} //! Sends commands to the receiver virtual bool send(const std::string& cmd) = 0; - //! Waits count seconds before throwing ROS_INFO message in case no message - //! from the receiver arrived - virtual void wait(uint16_t* count) = 0; - //! Determines whether or not the connection is open - virtual bool isOpen() const = 0; }; /** @@ -113,357 +103,464 @@ namespace io_comm_rx { * StreamT is either boost::asio::serial_port or boost::asio::tcp::ip */ template - class AsyncManager : public Manager + class AsyncManager : public AsyncManagerBase { public: /** * @brief Class constructor - * @param stream Whether TCP/IP or serial communication, either - * boost::asio::serial_port or boost::asio::tcp::ip - * @param io_service The io_context object. The io_context represents your - * program's link to the operating system's I/O services + * @param stream data stream * @param[in] buffer_size Size of the circular buffer in bytes */ - AsyncManager(ROSaicNodeBase* node, boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size = 16384); - virtual ~AsyncManager(); - - /** - * @brief Allows to connect to the CallbackHandlers class - * @param callback The function that becomes our callback, typically the - * readCallback() method of CallbackHandlers - */ - void setCallback(const Callback& callback) { read_callback_ = callback; } - - void wait(uint16_t* count); - - /** - * @brief Sends commands via the I/O stream. - * @param cmd The command to be sent - */ - bool send(const std::string& cmd); - - bool isOpen() const { return stream_->is_open(); } - - private: - //! Pointer to the node - ROSaicNodeBase* node_; - - protected: - //! Reads in via async_read_some and hands certain number of bytes - //! (bytes_transferred) over to async_read_some_handler - void read(); - - //! Handler for async_read_some (Boost library).. - void asyncReadSomeHandler(const boost::system::error_code& error, - std::size_t bytes_transferred); - - //! Sends command "cmd" to the Rx - void write(const std::string& cmd); - - //! Closes stream "stream_" - void close(); - - //! Tries parsing SBF/NMEA whenever the boolean class variable "try_parsing" - //! is true - void tryParsing(); - - //! Mutex to control changes of class variable "try_parsing" - boost::mutex parse_mutex_; - - //! Determines when the tryParsing() method will attempt parsing SBF/NMEA - bool try_parsing_; - - //! Determines when the asyncReadSomeHandler() method should write SBF/NMEA - //! into the circular buffer - bool allow_writing_; - - //! Condition variable complementing "parse_mutex" - boost::condition_variable parsing_condition_; - - //! Stream, represents either serial or TCP/IP connection - boost::shared_ptr stream_; - - //! io_context object - boost::shared_ptr io_service_; - - //! Buffer for async_read_some() to read continuous SBF/NMEA stream - std::vector in_; + AsyncManager(ROSaicNodeBase* node, std::unique_ptr stream, + TelegramQueue* telegramQueue) : + node_(node), + stream_(std::move(stream)), telegramQueue_(telegramQueue) + { + } - //! Circular buffer to avoid unsuccessful SBF/NMEA parsing due to incomplete - //! messages - CircularBuffer circular_buffer_; + ~AsyncManager() + { + running_ = false; + flushOutputQueue(); + close(); + node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); + ioService_.stop(); + ioThread_.join(); + watchdogThread_.join(); + node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); + } - //! New thread for receiving incoming messages - boost::shared_ptr async_background_thread_; + [[nodicard]] bool connect() + { + if (!ioSocket_) + { + if (!initIo()) + return false; + } - //! New thread for receiving incoming messages - boost::shared_ptr parsing_thread_; + if (ioSocket_->connect()) + { + return false; + } + receive(); - //! New thread for receiving incoming messages - boost::shared_ptr waiting_thread_; + watchdogThread_ = std::thread(std::bind(&TcpClient::runWatchdog, this)); + return true; + } - //! Callback to be called once message arrives - Callback read_callback_; + void send(const std::string& cmd) + { + if (cmd.size() == 0) + { + node_->log( + LogLevel::ERROR, + "AsyncManager message size to be sent to the Rx would be 0"); + } - //! Whether or not we want to sever the connection to the Rx - bool stopping_; + ioService_.post(boost::bind(&AsyncManager::write, this, cmd)); + } - /// Size of in_ buffers - const std::size_t buffer_size_; + private: + [[nodiscard]] initIo() + { + node_->log(LogLevel::DEBUG, "Called initializeIo() method"); + boost::smatch match; + // In fact: smatch is a typedef of match_results + if (boost::regex_match(node_->settings_.device, match, + boost::regex("(tcp)://(.+):(\\d+)"))) + // C++ needs \\d instead of \d: Both mean decimal. + // Note that regex_match can be used with a smatch object to store + // results, or without. In any case, true is returned if and only if it + // matches the !complete! string. + { + // The first sub_match (index 0) contained in a match_result always + // represents the full match within a target sequence made by a + // regex, and subsequent sub_matches represent sub-expression matches + // corresponding in sequence to the left parenthesis delimiting the + // sub-expression in the regex, i.e. $n Perl is equivalent to m[n] in + // boost regex. + + ioSocket.reset(new TcpIo(node_, &ioService_, match[2], match[3])) + } else if (boost::regex_match( + node_->settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) + { + node_->settings_.read_from_sbf_log = true; + node_->settings_.use_gnss_time = true; + // connectionThread_ = boost::thread( + // boost::bind(&CommIo::prepareSBFFileReading, this, match[2])); + + } else if (boost::regex_match( + node_->settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) + { + node_->settings_.read_from_pcap = true; + node_->settings_.use_gnss_time = true; + // connectionThread_ = boost::thread( + // boost::bind(&CommIo::preparePCAPFileReading, this, match[2])); - //! Boost timer for throwing ROS_INFO message once timed out due to lack of - //! incoming messages - boost::asio::deadline_timer timer_; + } else if (boost::regex_match(node_->settings_.device, match, + boost::regex("(serial):(.+)"))) + { + std::string proto(match[2]); + std::stringstream ss; + ss << "Searching for serial port" << proto; + node_->log(LogLevel::DEBUG, ss.str()); + node_->settings_.device = proto; + ioSocket.reset(new SerialIo(node_, &ioService_, settings_->device, + settings_->baudrate, + settings_->hw_flow_control)) + } else + { + std::stringstream ss; + ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; + node_->log(LogLevel::ERROR, ss.str()); + } + node_->log(LogLevel::DEBUG, "Leaving initializeIo() method"); + } - //! Number of seconds before ROS_INFO message is thrown (if no incoming - //! message) - const uint16_t count_max_; + void receive() + { + resync(); + ioThread_ = std::thread( + std::thread(std::bind(&AsyncManager::runIoService, this))); + } - //! Handles the ROS_INFO throwing (if no incoming message) - void callAsyncWait(uint16_t* count); + void flushOutputQueue() + { + ioService_.post([this]() { write(); }); + } - //! Number of times the DoRead() method has been called (only counts - //! initially) - uint16_t do_read_count_; + void close() + { + ioService_.post([this]() { socket_->close(); }); + } - //! Timestamp of receiving buffer - Timestamp recvTime_; - }; + void runIoService() + { + ioService_.run(); + node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); + } - template - void AsyncManager::tryParsing() - { - uint8_t* to_be_parsed = new uint8_t[buffer_size_ * 16]; - uint8_t* to_be_parsed_index = to_be_parsed; - bool timed_out = false; - std::size_t shift_bytes = 0; - std::size_t arg_for_read_callback = 0; - - while (!timed_out && - !stopping_) // Loop will stop if condition variable timed out + void runWatchdog() { - boost::mutex::scoped_lock lock(parse_mutex_); - parsing_condition_.wait_for(lock, boost::chrono::seconds(10), - [this]() { return try_parsing_; }); - bool timed_out = !try_parsing_; - if (timed_out) - break; - try_parsing_ = false; - allow_writing_ = true; - std::size_t current_buffer_size = circular_buffer_.size(); - arg_for_read_callback += current_buffer_size; - circular_buffer_.read(to_be_parsed + shift_bytes, current_buffer_size); - Timestamp revcTime = recvTime_; - lock.unlock(); - parsing_condition_.notify_one(); - - try + while (running_) { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - std::to_string(arg_for_read_callback)); - read_callback_(revcTime, to_be_parsed_index, arg_for_read_callback); - } catch (std::size_t& parsing_failed_here) - { - to_be_parsed_index += parsing_failed_here; - arg_for_read_callback -= parsing_failed_here; - node_->log(LogLevel::DEBUG, "Current buffer size is " + - std::to_string(current_buffer_size) + - " and parsing_failed_here is " + - std::to_string(parsing_failed_here)); - if (arg_for_read_callback < 0) // In case some parsing error was not - // caught, which should never happen.. + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + + if (running_ && ioService_.stopped()) { - to_be_parsed_index = to_be_parsed; - shift_bytes = 0; - arg_for_read_callback = 0; - continue; + node_->log(LogLevel::DEBUG, + "AsyncManager connection lost. Trying to reconnect."); + ioService_.reset(); + ioThread_->join(); + receive(); } - shift_bytes += current_buffer_size; - continue; } - to_be_parsed_index = to_be_parsed; - shift_bytes = 0; - arg_for_read_callback = 0; } - node_->log( - LogLevel::INFO, - "TryParsing() method finished since it did not receive anything to parse for 10 seconds.."); - delete[] to_be_parsed; // Freeing memory - } - template - bool AsyncManager::send(const std::string& cmd) - { - if (cmd.size() == 0) + void write(const std::string& cmd) { - node_->log(LogLevel::ERROR, - "Message size to be sent to the Rx would be 0"); - return true; + boost::asio::write(*stream_, + boost::asio::buffer(cmd.data(), cmd.size())); + // Prints the data that was sent + node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); } - io_service_->post(boost::bind(&AsyncManager::write, this, cmd)); - return true; - } - - template - void AsyncManager::write(const std::string& cmd) - { - boost::asio::write(*stream_, boost::asio::buffer(cmd.data(), cmd.size())); - // Prints the data that was sent - node_->log(LogLevel::DEBUG, "Sent the following " + - std::to_string(cmd.size()) + - " bytes to the Rx: \n" + cmd); - } - - template - void AsyncManager::callAsyncWait(uint16_t* count) - { - timer_.async_wait(boost::bind(&AsyncManager::wait, this, count)); - } - - template - AsyncManager::AsyncManager( - ROSaicNodeBase* node, boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size) : - node_(node), - timer_(*(io_service.get()), boost::posix_time::seconds(1)), stopping_(false), - try_parsing_(false), allow_writing_(true), do_read_count_(0), - buffer_size_(buffer_size), count_max_(6), circular_buffer_(node, buffer_size) - // Since buffer_size = 16384 in declaration, no need in definition anymore (even - // yields error message, due to "overwrite"). - { - node_->log( - LogLevel::DEBUG, - "Setting the private stream variable of the AsyncManager instance."); - stream_ = stream; - io_service_ = io_service; - in_.resize(buffer_size_); - - io_service_->post(boost::bind(&AsyncManager::read, this)); - // This function is used to ask the io_service to execute the given handler, - // but without allowing the io_service to call the handler from inside this - // function. The function signature of the handler must be: void handler(); - // The io_service guarantees that the handler (given as parameter) will only - // be called in a thread in which the run(), run_one(), poll() or poll_one() - // member functions is currently being invoked. So the fundamental difference - // is that dispatch will execute the work right away if it can and queue it - // otherwise while post queues the work no matter what. - async_background_thread_.reset(new boost::thread( - boost::bind(&boost::asio::io_service::run, io_service_))); - // Note that io_service_ is already pointer, hence need dereferencing - // operator & (ampersand). If the value of the pointer for the current thread - // is changed using reset(), then the previous value is destroyed by calling - // the cleanup routine. Alternatively, the stored value can be reset to NULL - // and the prior value returned by calling the release() member function, - // allowing the application to take back responsibility for destroying the - // object. - uint16_t count = 0; - waiting_thread_.reset(new boost::thread( - boost::bind(&AsyncManager::callAsyncWait, this, &count))); - - node_->log(LogLevel::DEBUG, "Launching tryParsing() thread.."); - parsing_thread_.reset( - new boost::thread(boost::bind(&AsyncManager::tryParsing, this))); - } // Calls std::terminate() on thread just created - - template - AsyncManager::~AsyncManager() - { - close(); - io_service_->stop(); - try_parsing_ = true; - parsing_condition_.notify_one(); - parsing_thread_->join(); - waiting_thread_->join(); - async_background_thread_->join(); - } - - template - void AsyncManager::read() - { - stream_->async_read_some( - boost::asio::buffer(in_.data(), in_.size()), - boost::bind(&AsyncManager::asyncReadSomeHandler, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); - // The handler is async_read_some_handler, whose call is postponed to - // when async_read_some completes. - if (do_read_count_ < 5) - ++do_read_count_; - } - - template - void AsyncManager::asyncReadSomeHandler( - const boost::system::error_code& error, std::size_t bytes_transferred) - { - if (error) + void resync() { - node_->log(LogLevel::ERROR, - "Rx ASIO input buffer read error: " + error.message() + ", " + - std::to_string(bytes_transferred)); - } else if (bytes_transferred > 0) + telegram_.reset(new Telegram); + readSync<0>(); + } + + template + void readSync() { - Timestamp inTime = node_->getTime(); - if (read_callback_ && - !stopping_) // Will be false in InitializeSerial (first call) - // since read_callback_ not added yet.. - { - boost::mutex::scoped_lock lock(parse_mutex_); - parsing_condition_.wait(lock, [this]() { return allow_writing_; }); - circular_buffer_.write(in_.data(), bytes_transferred); - allow_writing_ = false; - try_parsing_ = true; - recvTime_ = inTime; - lock.unlock(); - parsing_condition_.notify_one(); - } + static_assert(index < 3); + + boost::asio::async_read( + *stream_, boost::asio::buffer(telegram_->data.data() + index, 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + Timestamp stamp = node_->getTime(); + + if (!ec) + { + if (numBytes == 1) + { + switch (index) + { + case 0: + { + if (telegram_->data[index] == SYNC_0) + { + telegram_->stamp = stamp; + readSync<1>(); + } else + readSync<0>(); + break; + } + case 1: + { + switch (telegram_->data[index]) + { + case SBF_SYNC_BYTE_1: + { + telegram_->type = SBF; + readSbfHeader(); + break; + } + case NMEA_SYNC_BYTE_1: + { + telegram_->type = NMEA; + readSync<2>(); + break; + } + case RESPONSE_SYNC_BYTE_1: + { + stelegram_->type = RESPONSE; + readSync<2>(); + break; + } + case CONNECTION_DESCRIPTOR_BYTE_1: + { + telegram_->type = CONNECTION_DESCRIPTOR; + readSync<2>(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 1 read fault, should never come here."); + resync(); + break; + } + } + break; + } + case 2: + { + switch (telegram_->data[index]) + { + case NMEA_SYNC_BYTE_2: + { + if (telegram_->type == NMEA) + readString(); + else + resync(); + break; + } + case RESPONSE_SYNC_BYTE_1: + { + if (telegram_->type == RESPONSE) + readString(); + else + resync(); + break; + } + case CONNECTION_DESCRIPTOR_BYTE_1: + { + if (telegram_->type == CONNECTION_DESCRIPTOR) + readString(); + else + resync(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 2 read fault, should never come here."); + resync(); + break; + } + } + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync read fault, should never come here."); + resync(); + break; + } + } + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager sync read error: " + ec.message()); + resync(); + } + }); } - if (!stopping_) - io_service_->post(boost::bind(&AsyncManager::read, this)); - } + void readSbfHeader() + { + telegram_->data.resize(SBF_HEADER_SIZE); + + boost::asio::async_read( + *stream_, + boost::asio::buffer(telegram_->data.data() + 2, SBF_HEADER_SIZE - 2), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (SBF_HEADER_SIZE - 2)) + { + unit16_t length = + parsing_utilities::getLength(telegram_->data.data()); + if (length > MAX_SBF_SIZE) + { + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF header read fault, length of block exceeds " + + std::to_string(MAX_SBF_SIZE) + ": " + + std::to_string(length)); + } else + readSbf(length); + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF header read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager SBF header read error: " + + ec.message()); + resync(); + } + }); + } - template - void AsyncManager::close() - { - stopping_ = true; - boost::system::error_code error; - stream_->close(error); - if (error) + void readSbf(std::size_t length) { - node_->log(LogLevel::ERROR, - "Error while closing the AsyncManager: " + error.message()); + telegram_->data.resize(length); + + boost::asio::async_read( + *stream_, + boost::asio::buffer(telegram_->data.data() + SBF_HEADER_SIZE, + length - SBF_HEADER_SIZE), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (length - SBF_HEADER_SIZE)) + { + if (isValid( + telegram_->data.data())) // TODO namespace crc + { + telegram_->sbfId = + parsing_utilities::getId(telegram_->data.data()); + + telegramQueue_->push(telegram_); + } + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager SBF read error: " + ec.message()); + } + resync(); + }); } - } - template - void AsyncManager::wait(uint16_t* count) - { - if (*count < count_max_) + void readString() { - ++(*count); - timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1)); - if (!(*count == count_max_)) - { - timer_.async_wait(boost::bind(&AsyncManager::wait, this, count)); - } + telegram_->data.resize(3); + readStringElements(); } - if ((*count == count_max_) && (do_read_count_ < 3)) - // Why 3? Even if there are no incoming messages, read() is called once. - // It will be called a second time in TCP/IP mode since (just example) - // "IP10<" is transmitted. + + void readStringElements() { - node_->log( - LogLevel::INFO, - "No incoming messages, driver stopped, ros::spin() will spin forever unless you hit Ctrl+C."); - async_background_thread_->interrupt(); + std::byte byte; + + boost::asio::async_read( + *stream_, boost::asio::buffer(byte, 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == 1) + { + switch (byte) + { + case SYNC_0: + { + telegram_.reset(new Telegram); + telegram_->data[0] = byte; + telegram_->stamp = node_->getTime(); + node_->log( + LogLevel::DEBUG, + "AsyncManager string read fault, sync 0 found.")); + readSync<1>(); + break; + } + case LF: + { + telegram_->data.push_back(byte); + if (telegram_->data[telegram_->data.size() - 2] == + CR) + telegramQueue_->push(telegram_); + resync(); + break; + } + default: + { + telegram_->data.push_back(byte); + readString(); + break; + } + } + } else + { + node_->log( + LogLevel::DEBUG, + "AsyncManager string read fault, wrong number of bytes read: " + + std::to_string(numBytes)); + resync(); + } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager string read error: " + + ec.message()); + resync(); + } + }); } - } -} // namespace io_comm_rx -#endif // for ASYNC_MANAGER_HPP + //! Stream, represents either interface connection or file + std::unique_ptr stream_; + //! Pointer to the node + ROSaicNodeBase* node_; + std::unique_ptr ioSocket_; + std::atomic running_; + boost::asio::io_service ioService_; + std::thread ioThread_; + std::thread watchdogThread_; + //! Timestamp of receiving buffer + Timestamp recvStamp_; + //! Telegram + std::shared_ptr telegram_; + //! TelegramQueue + TelegramQueue* telegramQueue_; + }; +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/callback_handlers.hpp b/include/septentrio_gnss_driver/communication/callback_handlers.hpp index f953427c..b7b9ab62 100644 --- a/include/septentrio_gnss_driver/communication/callback_handlers.hpp +++ b/include/septentrio_gnss_driver/communication/callback_handlers.hpp @@ -111,7 +111,7 @@ extern boost::condition_variable g_cd_condition; extern uint32_t g_cd_count; extern std::string g_rx_tcp_port; -namespace io_comm_rx { +namespace io { /** * @class CallbackHandler * @brief Abstract class representing a generic callback handler, includes @@ -257,7 +257,7 @@ namespace io_comm_rx { //! mutex by making it available throughout the code unit. The mutex //! constructor list contains "mutex (const mutex&) = delete", hence //! construct-by-copying a mutex is explicitly prohibited. The get_handlers() - //! method of the Comm_IO class hence forces us to make this mutex static. + //! method of the CommIo class hence forces us to make this mutex static. static boost::mutex callback_mutex_; //! Determines which of the SBF blocks necessary for the gps_common::GPSFix @@ -366,6 +366,6 @@ namespace io_comm_rx { static LocalizationMap localization_ecef_map; }; -} // namespace io_comm_rx +} // namespace io #endif \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 7b5ab78a..e03d45fd 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -56,12 +56,7 @@ // // ***************************************************************************** -#ifndef COMMUNICATION_CORE_HPP // This block is called a conditional group. The - // controlled text will get included in the - // preprocessor output iff the macroname is not - // defined. -#define COMMUNICATION_CORE_HPP // Include guards help to avoid the double inclusion - // of header files, by defining a token = macro. +#pragma once // Boost includes #include @@ -77,7 +72,6 @@ #include // for usleep() // ROSaic includes #include -#include /** * @file communication_core.hpp @@ -86,11 +80,11 @@ */ /** - * @namespace io_comm_rx + * @namespace io * This namespace is for the communication interface, handling all aspects related to * serial and TCP/IP communication.. */ -namespace io_comm_rx { +namespace io { //! Possible baudrates for the Rx const static uint32_t BAUDRATES[] = { @@ -99,27 +93,27 @@ namespace io_comm_rx { 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; /** - * @class Comm_IO + * @class CommIo * @brief Handles communication with and configuration of the mosaic (and beyond) * receiver(s) */ - class Comm_IO + class CommIo { public: /** - * @brief Constructor of the class Comm_IO + * @brief Constructor of the class CommIo * @param[in] node Pointer to node */ - Comm_IO(ROSaicNodeBase* node, Settings* settings); + CommIo(ROSaicNodeBase* node, Settings* settings); /** - * @brief Default destructor of the class Comm_IO + * @brief Default destructor of the class CommIo */ - ~Comm_IO(); + ~CommIo(); /** * @brief Initializes the I/O handling */ - void initializeIO(); + void initializeIo(); /** * @brief Configures Rx: Which SBF/NMEA messages it should output and later @@ -128,12 +122,6 @@ namespace io_comm_rx { * */ void configureRx(); - /** - * @brief Defines which Rx messages to read and which ROS messages to publish - * @param[in] settings The device's settings - * */ - void defineMessages(); - /** * @brief Hands over NMEA velocity message over to the send() method of * manager_ @@ -208,7 +196,7 @@ namespace io_comm_rx { * @brief Set the I/O manager * @param[in] manager An I/O handler */ - void setManager(const boost::shared_ptr& manager); + void setManager(const boost::shared_ptr& manager); /** * @brief Hands over to the send() method of manager_ @@ -227,11 +215,11 @@ namespace io_comm_rx { //! Since the configureRx() method should only be called once the connection //! was established, we need the threads to communicate this to each other. //! Associated mutex.. - boost::mutex connection_mutex_; + std::mutex connection_mutex_; //! Since the configureRx() method should only be called once the connection //! was established, we need the threads to communicate this to each other. //! Associated condition variable.. - boost::condition_variable connection_condition_; + std::condition_variable connection_condition_; //! Host name of TCP server std::string tcp_host_; //! TCP port number @@ -242,14 +230,14 @@ namespace io_comm_rx { std::string serial_port_; //! Processes I/O stream data //! This declaration is deliberately stream-independent (Serial or TCP). - boost::shared_ptr manager_; + std::unique_ptr manager_; //! Baudrate at the moment, unless InitializeSerial or ResetSerial fail uint32_t baudrate_; bool nmeaActivated_ = false; //! Connection or reading thread - boost::thread connectionThread_; + std::thread connectionThread_; //! Indicator for threads to exit std::atomic stopping_; @@ -268,6 +256,4 @@ namespace io_comm_rx { //! increments) const static unsigned int SET_BAUDRATE_SLEEP_ = 500000; }; -} // namespace io_comm_rx - -#endif // for COMMUNICATION_CORE_HPP +} // namespace io diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp new file mode 100644 index 00000000..7af37b2d --- /dev/null +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -0,0 +1,278 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#pragma once + +// C++ +#include + +// Linux +#include + +// Boost +#include + +// ROSaic +#include + +//! Possible baudrates for the Rx +const static std::array baudrates = { + 1200, 2400, 4800, 9600, 19200, 38400, 57600, + 115200, 230400, 460800, 500000, 576000, 921600, 1000000, + 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; + +namespace io { + class IoBase + { + public: + IoBase(ROSaicNodeBase* node) : node_(node) {} + + virtual ~IoBase(); + + virtual [[nodiscard]] bool connect() = 0; + + private: + ROSaicNodeBase* node_; + }; + + class TcpIo : public IoBase + { + public: + TcpIo(ROSaicNodeBase* node, boost::asio::io_service* ioService, + const std::string& ip, const std::string& port) : + IoBase(node), + ioService_(ioService), socket_(*ioService_) + { + boost::asio::ip::tcp::resolver resolver(*ioService_); + boost::asio::ip::tcp::resolver::query query(ip, port); + endpointIterator_ = resolver.resolve(query); + } + + ~TcpIo() { socket_.close(); } + + [[nodiscard]] bool connect() + { + socket_.reset(new boost::asio::ip::tcp::socket(*ioService_)); + + socket_.connect(*endpointIterator_); + + socket_.set_option(boost::asio::ip::tcp::no_delay(true)); + } + + private: + boost::asio::ip::tcp::resolver::iterator endpointIterator_; + boost::asio::io_service* ioService_; + boost::asio::ip::tcp::socket socket_; + }; + + class SerialIo : public IoBase + { + public: + SerialIo(ROSaicNodeBase* node, boost::asio::io_service* ioService, + std::string serialPort, uint32_t baudrate, bool flowcontrol) : + IoBase(node), + flowcontrol_(flowcontrol), baudrate_(baudrate), serialPort_(ioService_) + { + } + + ~SerialIo() { serialPort_.close(); } + + [[nodiscard]] bool connect() + { + if (serialPort_.is_open()) + { + serialPort_.close(); + } + + bool opened = false; + + while (!opened) + { + try + { + serialPort_.open(port_); + opened = true; + } catch (const boost::system::system_error& err) + { + node_->log(LogLevel::ERROR, + "SerialCoket: Could not open serial port " + + std::to_string(port_) + ". Error: " + err.what() + + ". Will retry every second."); + + using namespace std::chrono_literals; + std::this_thread::sleep_for(1s); + } + } + + // No Parity, 8bits data, 1 stop Bit + serialPort_.set_option( + boost::asio::serial_port_base::baud_rate(baudrate_)); + serialPort_.set_option(boost::asio::serial_port_base::parity( + boost::asio::serial_port_base::parity::none)); + serialPort_.set_option(boost::asio::serial_port_base::character_size(8)); + serialPort_.set_option(boost::asio::serial_port_base::stop_bits( + boost::asio::serial_port_base::stop_bits::one)); + serialPort_.set_option(boost::asio::serial_port_base::flow_control( + boost::asio::serial_port_base::flow_control::none)); + + int fd = serialPort_.native_handle(); + termios tio; + // Get terminal attribute, follows the syntax + // int tcgetattr(int fd, struct termios *termios_p); + tcgetattr(fd, &tio); + + // Hardware flow control settings_->. + if (flowcontrol_) + { + tio.c_iflag &= ~(IXOFF | IXON); + tio.c_cflag |= CRTSCTS; + } else + { + tio.c_iflag &= ~(IXOFF | IXON); + tio.c_cflag &= ~CRTSCTS; + } + // Setting serial port to "raw" mode to prevent EOF exit.. + cfmakeraw(&tio); + + // Commit settings, syntax is + // int tcsetattr(int fd, int optional_actions, const struct termios + // *termios_p); + tcsetattr(fd, TCSANOW, &tio); + // Set low latency + struct serial_struct serialInfo; + + ioctl(fd, TIOCGSERIAL, &serialInfo); + serialInfo.flags |= ASYNC_LOW_LATENCY; + ioctl(fd, TIOCSSERIAL, &serialInfo); + + return setBaudrate(); + } + + [[nodiscard]] bool setBaurate() + { + // Setting the baudrate, incrementally.. + node_->log(LogLevel::DEBUG, + "Gradually increasing the baudrate to the desired value..."); + boost::asio::serial_port_base::baud_rate current_baudrate; + node_->log(LogLevel::DEBUG, "Initiated current_baudrate object..."); + try + { + serialPort_.get_option( + current_baudrate); // Note that this sets + // current_baudrate.value() often to 115200, + // since by default, all Rx COM ports, + // at least for mosaic Rxs, are set to a baudrate of 115200 baud, + // using 8 data-bits, no parity and 1 stop-bit. + } catch (boost::system::system_error& e) + { + + node_->log(LogLevel::ERROR, + "get_option failed due to " + std::string(e.what())); + node_->log(LogLevel::INFO, "Additional info about error is " + + boost::diagnostic_information(e)); + /* + boost::system::error_code e_loop; + do // Caution: Might cause infinite loop.. + { + serialPort_.get_option(current_baudrate, e_loop); + } while(e_loop); + */ + return false; + } + // Gradually increase the baudrate to the desired value + // The desired baudrate can be lower or larger than the + // current baudrate; the for loop takes care of both scenarios. + node_->log(LogLevel::DEBUG, + "Current baudrate is " + + std::to_string(current_baudrate.value())); + for (uint8_t i = 0; i < baudrates.size(); i++) + { + if (current_baudrate.value() == baudrate_) + { + break; // Break if the desired baudrate has been reached. + } + if (current_baudrate.value() >= baudrates[i] && + baudrate_ > baudrates[i]) + { + continue; + } + // Increment until Baudrate[i] matches current_baudrate. + try + { + serialPort_.set_option( + boost::asio::serial_port_base::baud_rate(baudrates[i])); + } catch (boost::system::system_error& e) + { + + node_->log(LogLevel::ERROR, + "set_option failed due to " + std::string(e.what())); + node_->log(LogLevel::INFO, "Additional info about error is " + + boost::diagnostic_information(e)); + return false; + } + using namespace std::chrono_literals; + std::this_thread::sleep_for(500ms); + + try + { + serialPort_.get_option(current_baudrate); + } catch (boost::system::system_error& e) + { + + node_->log(LogLevel::ERROR, + "get_option failed due to " + std::string(e.what())); + node_->log(LogLevel::INFO, "Additional info about error is " + + boost::diagnostic_information(e)); + /* + boost::system::error_code e_loop; + do // Caution: Might cause infinite loop.. + { + serialPort_.get_option(current_baudrate, e_loop); + } while(e_loop); + */ + return false; + } + node_->log(LogLevel::DEBUG, + "Set ASIO baudrate to " + + std::to_string(current_baudrate.value())); + } + node_->log(LogLevel::INFO, "Set ASIO baudrate to " + + std::to_string(current_baudrate.value()) + + ", leaving InitializeSerial() method"); + } + + private: + bool flowcontrol_; + std::string port_; + uint32_t baudrate_; + boost::asio::io_service* ioService_; + boost::asio::serial_port serialPort_; + }; +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/rx_message.hpp b/include/septentrio_gnss_driver/communication/rx_message.hpp index 0d3bab88..bf2f1448 100644 --- a/include/septentrio_gnss_driver/communication/rx_message.hpp +++ b/include/septentrio_gnss_driver/communication/rx_message.hpp @@ -193,7 +193,7 @@ enum RxID_Enum evReceiverSetup }; -namespace io_comm_rx { +namespace io { /** * @class RxMessage @@ -789,5 +789,5 @@ namespace io_comm_rx { */ Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time); }; -} // namespace io_comm_rx +} // namespace io #endif // for RX_MESSAGE_HPP \ No newline at end of file diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp index b232c51f..caccb06e 100644 --- a/include/septentrio_gnss_driver/node/rosaic_node.hpp +++ b/include/septentrio_gnss_driver/node/rosaic_node.hpp @@ -128,7 +128,7 @@ namespace rosaic_node { void sendVelocity(const std::string& velNmea); //! Handles communication with the Rx - io_comm_rx::Comm_IO IO_; + io::CommIo IO_; //! tf2 buffer and listener tf2_ros::Buffer tfBuffer_; std::unique_ptr tfListener_; diff --git a/src/septentrio_gnss_driver/communication/callback_handlers.cpp b/src/septentrio_gnss_driver/communication/callback_handlers.cpp index 8ff8904f..de079e24 100644 --- a/src/septentrio_gnss_driver/communication/callback_handlers.cpp +++ b/src/septentrio_gnss_driver/communication/callback_handlers.cpp @@ -58,7 +58,7 @@ std::pair localization_pairs[] = {std::make_pair("4226", std::pair localization_ecef_pairs[] = { std::make_pair("4225", 0), std::make_pair("4226", 1)}; -namespace io_comm_rx { +namespace io { boost::mutex CallbackHandlers::callback_mutex_; CallbackHandlers::GPSFixMap CallbackHandlers::gpsfix_map(gpsfix_pairs, @@ -698,4 +698,4 @@ namespace io_comm_rx { } } } -} // namespace io_comm_rx \ No newline at end of file +} // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index f7a0a576..18bd6fbc 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -119,7 +119,7 @@ std::string g_rx_tcp_port; //! to count the connection descriptors uint32_t g_cd_count; -io_comm_rx::Comm_IO::Comm_IO(ROSaicNodeBase* node, Settings* settings) : +io::CommIo::CommIo(ROSaicNodeBase* node, Settings* settings) : node_(node), handlers_(node, settings), settings_(settings), stopping_(false) { g_response_received = false; @@ -128,7 +128,7 @@ io_comm_rx::Comm_IO::Comm_IO(ROSaicNodeBase* node, Settings* settings) : g_cd_count = 0; } -io_comm_rx::Comm_IO::~Comm_IO() +io::CommIo::~CommIo() { if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { @@ -188,7 +188,7 @@ io_comm_rx::Comm_IO::~Comm_IO() connectionThread_.join(); } -void io_comm_rx::Comm_IO::resetMainPort() +void io::CommIo::resetMainPort() { // It is imperative to hold a lock on the mutex "g_cd_mutex" while // modifying the variable and "g_cd_received". @@ -203,9 +203,9 @@ void io_comm_rx::Comm_IO::resetMainPort() g_cd_received = false; } -void io_comm_rx::Comm_IO::initializeIO() +void io::CommIo::initializeIo() { - node_->log(LogLevel::DEBUG, "Called initializeIO() method"); + node_->log(LogLevel::DEBUG, "Called initializeIo() method"); boost::smatch match; // In fact: smatch is a typedef of match_results if (boost::regex_match(settings_->device, match, @@ -224,7 +224,7 @@ void io_comm_rx::Comm_IO::initializeIO() tcp_port_ = match[3]; serial_ = false; - connectionThread_ = boost::thread(boost::bind(&Comm_IO::connect, this)); + connectionThread_ = boost::thread(boost::bind(&CommIo::connect, this)); } else if (boost::regex_match(settings_->device, match, boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) { @@ -232,7 +232,7 @@ void io_comm_rx::Comm_IO::initializeIO() settings_->read_from_sbf_log = true; settings_->use_gnss_time = true; connectionThread_ = boost::thread( - boost::bind(&Comm_IO::prepareSBFFileReading, this, match[2])); + boost::bind(&CommIo::prepareSBFFileReading, this, match[2])); } else if (boost::regex_match( settings_->device, match, @@ -242,7 +242,7 @@ void io_comm_rx::Comm_IO::initializeIO() settings_->read_from_pcap = true; settings_->use_gnss_time = true; connectionThread_ = boost::thread( - boost::bind(&Comm_IO::preparePCAPFileReading, this, match[2])); + boost::bind(&CommIo::preparePCAPFileReading, this, match[2])); } else if (boost::regex_match(settings_->device, match, boost::regex("(serial):(.+)"))) @@ -253,17 +253,17 @@ void io_comm_rx::Comm_IO::initializeIO() ss << "Searching for serial port" << proto; settings_->device = proto; node_->log(LogLevel::DEBUG, ss.str()); - connectionThread_ = boost::thread(boost::bind(&Comm_IO::connect, this)); + connectionThread_ = boost::thread(boost::bind(&CommIo::connect, this)); } else { std::stringstream ss; ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; node_->log(LogLevel::ERROR, ss.str()); } - node_->log(LogLevel::DEBUG, "Leaving initializeIO() method"); + node_->log(LogLevel::DEBUG, "Leaving initializeIo() method"); } -void io_comm_rx::Comm_IO::prepareSBFFileReading(std::string file_name) +void io::CommIo::prepareSBFFileReading(std::string file_name) { try { @@ -274,13 +274,13 @@ void io_comm_rx::Comm_IO::prepareSBFFileReading(std::string file_name) } catch (std::runtime_error& e) { std::stringstream ss; - ss << "Comm_IO::initializeSBFFileReading() failed for SBF File" << file_name + ss << "CommIo::initializeSBFFileReading() failed for SBF File" << file_name << " due to: " << e.what(); node_->log(LogLevel::ERROR, ss.str()); } } -void io_comm_rx::Comm_IO::preparePCAPFileReading(std::string file_name) +void io::CommIo::preparePCAPFileReading(std::string file_name) { try { @@ -297,7 +297,7 @@ void io_comm_rx::Comm_IO::preparePCAPFileReading(std::string file_name) } } -void io_comm_rx::Comm_IO::connect() +void io::CommIo::connect() { node_->log(LogLevel::DEBUG, "Called connect() method"); node_->log( @@ -316,67 +316,6 @@ void io_comm_rx::Comm_IO::connect() node_->log(LogLevel::DEBUG, "Successully connected. Leaving connect() method"); } -//! In serial mode (not USB, since the Rx port is then called USB1 or USB2), please -//! ensure that you are connected to the Rx's COM1, COM2 or COM3 port, !if! you -//! employ UART hardware flow control. -void io_comm_rx::Comm_IO::reconnect() -{ - node_->log(LogLevel::DEBUG, "Called reconnect() method"); - if (serial_) - { - bool initialize_serial_return = false; - try - { - node_->log( - LogLevel::INFO, - "Connecting serially to device" + settings_->device + - ", targeted baudrate: " + std::to_string(settings_->baudrate)); - initialize_serial_return = initializeSerial( - settings_->device, settings_->baudrate, settings_->hw_flow_control); - } catch (std::runtime_error& e) - { - { - std::stringstream ss; - ss << "initializeSerial() failed for device " << settings_->device - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } - } - if (initialize_serial_return) - { - boost::mutex::scoped_lock lock(connection_mutex_); - connected_ = true; - lock.unlock(); - connection_condition_.notify_one(); - } - } else - { - bool initialize_tcp_return = false; - try - { - node_->log(LogLevel::INFO, - "Connecting to tcp://" + tcp_host_ + ":" + tcp_port_ + "..."); - initialize_tcp_return = initializeTCP(tcp_host_, tcp_port_); - } catch (std::runtime_error& e) - { - { - std::stringstream ss; - ss << "initializeTCP() failed for host " << tcp_host_ << " on port " - << tcp_port_ << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } - } - if (initialize_tcp_return) - { - boost::mutex::scoped_lock lock(connection_mutex_); - connected_ = true; - lock.unlock(); - connection_condition_.notify_one(); - } - } - node_->log(LogLevel::DEBUG, "Leaving reconnect() method"); -} - //! The send() method of AsyncManager class is paramount for this purpose. //! Note that std::to_string() is from C++11 onwards only. //! Since ROSaic can be launched before booting the Rx, we have to watch out for @@ -384,7 +323,7 @@ void io_comm_rx::Comm_IO::reconnect() //! Those characters would then be mingled with the first command we send to it in //! this method and could result in an invalid command. Hence we first enter command //! mode via "SSSSSSSSSS". -void io_comm_rx::Comm_IO::configureRx() +void io::CommIo::configureRx() { node_->log(LogLevel::DEBUG, "Called configureRx() method"); { @@ -977,224 +916,7 @@ void io_comm_rx::Comm_IO::configureRx() node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); } -//! initializeSerial is not self-contained: The for loop in Callbackhandlers' handle -//! method would never open a specific handler unless the handler is added -//! (=inserted) to the C++ map via this function. This way, the specific handler can -//! be called, in which in turn RxMessage's read() method is called, which publishes -//! the ROS message. -void io_comm_rx::Comm_IO::defineMessages() -{ - node_->log(LogLevel::DEBUG, "Called defineMessages() method"); - - if (settings_->use_gnss_time || settings_->publish_gpst) - { - handlers_.callbackmap_ = handlers_.insert("5914"); - } - if (settings_->publish_gpgga) - { - handlers_.callbackmap_ = handlers_.insert("$GPGGA"); - } - if (settings_->publish_gprmc) - { - handlers_.callbackmap_ = handlers_.insert("$GPRMC"); - } - if (settings_->publish_gpgsa) - { - handlers_.callbackmap_ = handlers_.insert("$GPGSA"); - } - if (settings_->publish_gpgsv) - { - handlers_.callbackmap_ = handlers_.insert("$GPGSV"); - handlers_.callbackmap_ = handlers_.insert("$GLGSV"); - handlers_.callbackmap_ = handlers_.insert("$GAGSV"); - handlers_.callbackmap_ = handlers_.insert("$GBGSV"); - } - if (settings_->publish_pvtcartesian) - { - handlers_.callbackmap_ = handlers_.insert("4006"); - } - if (settings_->publish_pvtgeodetic || settings_->publish_twist || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("4007"); - } - if (settings_->publish_basevectorcart) - { - handlers_.callbackmap_ = handlers_.insert("4043"); - } - if (settings_->publish_basevectorgeod) - { - handlers_.callbackmap_ = handlers_.insert("4028"); - } - if (settings_->publish_poscovcartesian) - { - handlers_.callbackmap_ = handlers_.insert("5905"); - } - if (settings_->publish_poscovgeodetic || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5906"); - } - if (settings_->publish_velcovgeodetic || settings_->publish_twist || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5908"); - } - if (settings_->publish_atteuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5938"); - } - if (settings_->publish_attcoveuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && (settings_->septentrio_receiver_type == "gnss"))) - { - handlers_.callbackmap_ = handlers_.insert("5939"); - } - if (settings_->publish_measepoch || settings_->publish_gpsfix) - { - handlers_.callbackmap_ = - handlers_.insert("4027"); // MeasEpoch block - } - - // INS-related SBF blocks - if (settings_->publish_insnavcart) - { - handlers_.callbackmap_ = handlers_.insert("4225"); - } - if (settings_->publish_insnavgeod || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_imu && (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_localization && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_twist && - (settings_->septentrio_receiver_type == "ins")) || - (settings_->publish_tf && (settings_->septentrio_receiver_type == "ins"))) - { - handlers_.callbackmap_ = handlers_.insert("4226"); - } - if (settings_->publish_imusetup) - { - handlers_.callbackmap_ = handlers_.insert("4224"); - } - if (settings_->publish_extsensormeas || settings_->publish_imu) - { - handlers_.callbackmap_ = handlers_.insert("4050"); - } - if (settings_->publish_exteventinsnavgeod) - { - handlers_.callbackmap_ = handlers_.insert("4230"); - } - if (settings_->publish_velsensorsetup) - { - handlers_.callbackmap_ = handlers_.insert("4244"); - } - if (settings_->publish_exteventinsnavcart) - { - handlers_.callbackmap_ = handlers_.insert("4229"); - } - if (settings_->publish_gpst) - { - handlers_.callbackmap_ = handlers_.insert("GPST"); - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_navsatfix) - { - handlers_.callbackmap_ = handlers_.insert("NavSatFix"); - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_navsatfix) - { - handlers_.callbackmap_ = handlers_.insert("INSNavSatFix"); - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpsfix) - { - handlers_.callbackmap_ = handlers_.insert("GPSFix"); - // The following blocks are never published, yet are needed for the - // construction of the GPSFix message, hence we have empty callbacks. - handlers_.callbackmap_ = - handlers_.insert("4013"); // ChannelStatus block - handlers_.callbackmap_ = handlers_.insert("4001"); // DOP block - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpsfix) - { - handlers_.callbackmap_ = handlers_.insert("INSGPSFix"); - handlers_.callbackmap_ = - handlers_.insert("4013"); // ChannelStatus block - handlers_.callbackmap_ = handlers_.insert("4001"); // DOP block - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_pose) - { - handlers_.callbackmap_ = handlers_.insert( - "PoseWithCovarianceStamped"); - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_pose) - { - handlers_.callbackmap_ = handlers_.insert( - "INSPoseWithCovarianceStamped"); - } - } - if (settings_->publish_diagnostics) - { - handlers_.callbackmap_ = - handlers_.insert("DiagnosticArray"); - handlers_.callbackmap_ = - handlers_.insert("4014"); // ReceiverStatus block - handlers_.callbackmap_ = - handlers_.insert("4082"); // QualityInd block - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_localization || settings_->publish_tf) - { - handlers_.callbackmap_ = - handlers_.insert("Localization"); - } - if (settings_->publish_localization_ecef || settings_->publish_tf_ecef) - { - handlers_.callbackmap_ = - handlers_.insert("LocalizationEcef"); - } - } - handlers_.callbackmap_ = - handlers_.insert("5902"); // ReceiverSetup block - // so on and so forth... - node_->log(LogLevel::DEBUG, "Leaving defineMessages() method"); -} - -void io_comm_rx::Comm_IO::send(const std::string& cmd) +void io::CommIo::send(const std::string& cmd) { // It is imperative to hold a lock on the mutex "g_response_mutex" while // modifying the variable "g_response_received". @@ -1205,76 +927,13 @@ void io_comm_rx::Comm_IO::send(const std::string& cmd) g_response_received = false; } -void io_comm_rx::Comm_IO::sendVelocity(const std::string& velNmea) +void io::CommIo::sendVelocity(const std::string& velNmea) { if (nmeaActivated_) manager_.get()->send(velNmea); } -bool io_comm_rx::Comm_IO::initializeTCP(std::string host, std::string port) -{ - node_->log(LogLevel::DEBUG, "Calling initializeTCP() method.."); - host_ = host; - port_ = port; - // The io_context, of which io_service is a typedef of; it represents your - // program's link to the operating system's I/O services. - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::asio::ip::tcp::resolver::iterator endpoint; - - try - { - boost::asio::ip::tcp::resolver resolver(*io_service); - // Note that tcp::resolver::query takes the host to resolve or the IP as the - // first parameter and the name of the service (as defined e.g. in - // /etc/services on Unix hosts) as second parameter. For the latter, one can - // also use a numeric service identifier (aka port number). In any case, it - // returns a list of possible endpoints, as there might be several entries - // for a single host. - endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query( - host, port)); // Resolves query object.. - } catch (std::runtime_error& e) - { - throw std::runtime_error("Could not resolve " + host + " on port " + port + - ": " + e.what()); - return false; - } - - boost::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); - - try - { - // The list of endpoints obtained above may contain both IPv4 and IPv6 - // endpoints, so we need to try each of them until we find one that works. - // This keeps the client program independent of a specific IP version. The - // boost::asio::connect() function does this for us automatically. - socket->connect(*endpoint); - socket->set_option(boost::asio::ip::tcp::no_delay(true)); - } catch (std::runtime_error& e) - { - throw std::runtime_error("Could not connect to " + endpoint->host_name() + - ": " + endpoint->service_name() + ": " + e.what()); - return false; - } - - node_->log(LogLevel::INFO, "Connected to " + endpoint->host_name() + ":" + - endpoint->service_name() + "."); - - if (manager_) - { - node_->log( - LogLevel::ERROR, - "You have called the InitializeTCP() method though an AsyncManager object is already available! Start all anew.."); - return false; - } - setManager(boost::shared_ptr( - new AsyncManager(node_, socket, io_service))); - node_->log(LogLevel::DEBUG, "Leaving initializeTCP() method.."); - return true; -} - -void io_comm_rx::Comm_IO::initializeSBFFileReading(std::string file_name) +void io::CommIo::initializeSBFFileReading(std::string file_name) { node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); std::size_t buffer_size = 8192; @@ -1329,7 +988,7 @@ void io_comm_rx::Comm_IO::initializeSBFFileReading(std::string file_name) node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method.."); } -void io_comm_rx::Comm_IO::initializePCAPFileReading(std::string file_name) +void io::CommIo::initializePCAPFileReading(std::string file_name) { node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() method.."); pcapReader::buffer_t vec_buf; @@ -1380,187 +1039,4 @@ void io_comm_rx::Comm_IO::initializePCAPFileReading(std::string file_name) to_be_parsed = to_be_parsed + buffer_size; } node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method.."); -} - -bool io_comm_rx::Comm_IO::initializeSerial(std::string port, uint32_t baudrate, - std::string flowcontrol) -{ - node_->log(LogLevel::DEBUG, "Calling initializeSerial() method.."); - serial_port_ = port; - baudrate_ = baudrate; - // The io_context, of which io_service is a typedef of; it represents your - // program's link to the operating system's I/O services. - boost::shared_ptr io_service( - new boost::asio::io_service); - // To perform I/O operations the program needs an I/O object, here "serial". - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); - - // We attempt the opening of the serial port.. - try - { - serial->open(serial_port_); - } catch (std::runtime_error& e) - { - // and return an error message in case it fails. - throw std::runtime_error("Could not open serial port : " + serial_port_ + - ": " + e.what()); - return false; - } - - node_->log(LogLevel::INFO, "Opened serial port " + serial_port_); - node_->log(LogLevel::DEBUG, - "Our boost version is " + std::to_string(BOOST_VERSION) + "."); - if (BOOST_VERSION < 106600) // E.g. for ROS melodic (i.e. Ubuntu 18.04), the - // version is 106501, standing for 1.65.1. - { - // Workaround to set some options for the port manually, - // cf. https://github.com/mavlink/mavros/pull/971/files - // This function native_handle() may be used to obtain - // the underlying representation of the serial port. - // Conversion from type native_handle_type to int is done implicitly. - int fd = serial->native_handle(); - termios tio; - // Get terminal attribute, follows the syntax - // int tcgetattr(int fd, struct termios *termios_p); - tcgetattr(fd, &tio); - - // Hardware flow control settings_->. - if (flowcontrol == "RTS|CTS") - { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag |= CRTSCTS; - } else - { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag &= ~CRTSCTS; - } - // Setting serial port to "raw" mode to prevent EOF exit.. - cfmakeraw(&tio); - - // Commit settings, syntax is - // int tcsetattr(int fd, int optional_actions, const struct termios - // *termios_p); - tcsetattr(fd, TCSANOW, &tio); - - // Set low latency - struct serial_struct serialInfo; - - ioctl(fd, TIOCGSERIAL, &serialInfo); - serialInfo.flags |= ASYNC_LOW_LATENCY; - ioctl(fd, TIOCSSERIAL, &serialInfo); - } - - // Set the I/O manager - if (manager_) - { - node_->log( - LogLevel::ERROR, - "You have called the initializeSerial() method though an AsyncManager object is already available! Start all anew.."); - return false; - } - node_->log(LogLevel::DEBUG, "Creating new Async-Manager object.."); - setManager(boost::shared_ptr( - new AsyncManager(node_, serial, io_service))); - - // Setting the baudrate, incrementally.. - node_->log(LogLevel::DEBUG, - "Gradually increasing the baudrate to the desired value..."); - boost::asio::serial_port_base::baud_rate current_baudrate; - node_->log(LogLevel::DEBUG, "Initiated current_baudrate object..."); - try - { - serial->get_option( - current_baudrate); // Note that this sets current_baudrate.value() often - // to 115200, since by default, all Rx COM ports, - // at least for mosaic Rxs, are set to a baudrate of 115200 baud, using 8 - // data-bits, no parity and 1 stop-bit. - } catch (boost::system::system_error& e) - { - - node_->log(LogLevel::ERROR, - "get_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); - /* - boost::system::error_code e_loop; - do // Caution: Might cause infinite loop.. - { - serial->get_option(current_baudrate, e_loop); - } while(e_loop); - */ - return false; - } - // Gradually increase the baudrate to the desired value - // The desired baudrate can be lower or larger than the - // current baudrate; the for loop takes care of both scenarios. - node_->log(LogLevel::DEBUG, - "Current baudrate is " + std::to_string(current_baudrate.value())); - for (uint8_t i = 0; i < sizeof(BAUDRATES) / sizeof(BAUDRATES[0]); i++) - { - if (current_baudrate.value() == baudrate_) - { - break; // Break if the desired baudrate has been reached. - } - if (current_baudrate.value() >= BAUDRATES[i] && baudrate_ > BAUDRATES[i]) - { - continue; - } - // Increment until Baudrate[i] matches current_baudrate. - try - { - serial->set_option( - boost::asio::serial_port_base::baud_rate(BAUDRATES[i])); - } catch (boost::system::system_error& e) - { - - node_->log(LogLevel::ERROR, - "set_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); - return false; - } - usleep(SET_BAUDRATE_SLEEP_); - // boost::this_thread::sleep(boost::posix_time::milliseconds(SET_BAUDRATE_SLEEP_*1000)); - // Boost's sleep would yield an error message with exit code -7 the second - // time it is called, hence we use sleep() or usleep(). - try - { - serial->get_option(current_baudrate); - } catch (boost::system::system_error& e) - { - - node_->log(LogLevel::ERROR, - "get_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); - /* - boost::system::error_code e_loop; - do // Caution: Might cause infinite loop.. - { - serial->get_option(current_baudrate, e_loop); - } while(e_loop); - */ - return false; - } - node_->log(LogLevel::DEBUG, "Set ASIO baudrate to " + - std::to_string(current_baudrate.value())); - } - node_->log(LogLevel::INFO, "Set ASIO baudrate to " + - std::to_string(current_baudrate.value()) + - ", leaving InitializeSerial() method"); - return true; -} - -void io_comm_rx::Comm_IO::setManager(const boost::shared_ptr& manager) -{ - namespace bp = boost::placeholders; - - node_->log(LogLevel::DEBUG, "Called setManager() method"); - if (manager_) - return; - manager_ = manager; - manager_->setCallback(boost::bind(&CallbackHandlers::readCallback, &handlers_, - bp::_1, bp::_2, bp::_3)); - node_->log(LogLevel::DEBUG, "Leaving setManager() method"); } \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp index 6088c747..38f09b6d 100644 --- a/src/septentrio_gnss_driver/communication/rx_message.cpp +++ b/src/septentrio_gnss_driver/communication/rx_message.cpp @@ -54,7 +54,7 @@ using parsing_utilities::rad2deg; using parsing_utilities::square; PoseWithCovarianceStampedMsg -io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() +io::RxMessage::PoseWithCovarianceStampedCallback() { PoseWithCovarianceStampedMsg msg; if (settings_->septentrio_receiver_type == "gnss") @@ -183,7 +183,7 @@ io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback() return msg; }; -DiagnosticArrayMsg io_comm_rx::RxMessage::DiagnosticArrayCallback() +DiagnosticArrayMsg io::RxMessage::DiagnosticArrayCallback() { DiagnosticArrayMsg msg; std::string serialnumber(last_receiversetup_.rx_serial_number); @@ -290,7 +290,7 @@ DiagnosticArrayMsg io_comm_rx::RxMessage::DiagnosticArrayCallback() return msg; }; -ImuMsg io_comm_rx::RxMessage::ImuCallback() +ImuMsg io::RxMessage::ImuCallback() { ImuMsg msg; @@ -403,7 +403,7 @@ ImuMsg io_comm_rx::RxMessage::ImuCallback() }; TwistWithCovarianceStampedMsg -io_comm_rx::RxMessage::TwistCallback(bool fromIns /* = false*/) +io::RxMessage::TwistCallback(bool fromIns /* = false*/) { TwistWithCovarianceStampedMsg msg; @@ -624,7 +624,7 @@ io_comm_rx::RxMessage::TwistCallback(bool fromIns /* = false*/) * north. Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ -LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() +LocalizationMsg io::RxMessage::LocalizationUtmCallback() { LocalizationMsg msg; @@ -800,7 +800,7 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationUtmCallback() * Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ -LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() +LocalizationMsg io::RxMessage::LocalizationEcefCallback() { LocalizationMsg msg; @@ -952,7 +952,7 @@ LocalizationMsg io_comm_rx::RxMessage::LocalizationEcefCallback() return msg; }; -void io_comm_rx::RxMessage::fillLocalizationMsgTwist(double roll, double pitch, +void io::RxMessage::fillLocalizationMsgTwist(double roll, double pitch, double yaw, LocalizationMsg& msg) { @@ -1073,7 +1073,7 @@ void io_comm_rx::RxMessage::fillLocalizationMsgTwist(double roll, double pitch, * of the PVTGeodetic block does not disclose it. For that, one would need to go to * the ObsInfo field of the MeasEpochChannelType1 sub-block. */ -NavSatFixMsg io_comm_rx::RxMessage::NavSatFixCallback() +NavSatFixMsg io::RxMessage::NavSatFixCallback() { NavSatFixMsg msg; uint16_t mask = 15; // We extract the first four bits using this mask. @@ -1265,7 +1265,7 @@ NavSatFixMsg io_comm_rx::RxMessage::NavSatFixCallback() * values appear unphysical, please consult the firmware, since those most likely * refer to Do-Not-Use values. */ -GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() +GPSFixMsg io::RxMessage::GPSFixCallback() { GPSFixMsg msg; msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); @@ -1682,7 +1682,7 @@ GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback() return msg; }; -Timestamp io_comm_rx::RxMessage::timestampSBF(const uint8_t* data, +Timestamp io::RxMessage::timestampSBF(const uint8_t* data, bool use_gnss_time) { uint32_t tow = parsing_utilities::getTow(data); @@ -1696,7 +1696,7 @@ Timestamp io_comm_rx::RxMessage::timestampSBF(const uint8_t* data, /// the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the /// settings_->leap_seconds ROSaic parameter accordingly as soon as the next leap /// second is inserted into the UTC time. -Timestamp io_comm_rx::RxMessage::timestampSBF(uint32_t tow, uint16_t wnc, +Timestamp io::RxMessage::timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time) { Timestamp time_obj; @@ -1723,7 +1723,7 @@ Timestamp io_comm_rx::RxMessage::timestampSBF(uint32_t tow, uint16_t wnc, return time_obj; } -bool io_comm_rx::RxMessage::found() +bool io::RxMessage::found() { if (found_) return true; @@ -1739,7 +1739,7 @@ bool io_comm_rx::RxMessage::found() return true; } -const uint8_t* io_comm_rx::RxMessage::search() +const uint8_t* io::RxMessage::search() { if (found_) { @@ -1758,7 +1758,7 @@ const uint8_t* io_comm_rx::RxMessage::search() return data_; } -std::size_t io_comm_rx::RxMessage::messageSize() +std::size_t io::RxMessage::messageSize() { uint16_t pos = 0; message_size_ = 0; @@ -1797,7 +1797,7 @@ std::size_t io_comm_rx::RxMessage::messageSize() return message_size_; } -bool io_comm_rx::RxMessage::isMessage(const uint16_t id) +bool io::RxMessage::isMessage(const uint16_t id) { if (this->isSBF()) { @@ -1808,7 +1808,7 @@ bool io_comm_rx::RxMessage::isMessage(const uint16_t id) } } -bool io_comm_rx::RxMessage::isMessage(std::string id) +bool io::RxMessage::isMessage(std::string id) { if (this->isNMEA()) { @@ -1830,7 +1830,7 @@ bool io_comm_rx::RxMessage::isMessage(std::string id) } } -bool io_comm_rx::RxMessage::isSBF() +bool io::RxMessage::isSBF() { if (count_ >= 2) { @@ -1847,7 +1847,7 @@ bool io_comm_rx::RxMessage::isSBF() } } -bool io_comm_rx::RxMessage::isNMEA() +bool io::RxMessage::isNMEA() { if (count_ >= 2) { @@ -1865,7 +1865,7 @@ bool io_comm_rx::RxMessage::isNMEA() } } -bool io_comm_rx::RxMessage::isResponse() +bool io::RxMessage::isResponse() { if (count_ >= 2) { @@ -1882,7 +1882,7 @@ bool io_comm_rx::RxMessage::isResponse() } } -bool io_comm_rx::RxMessage::isConnectionDescriptor() +bool io::RxMessage::isConnectionDescriptor() { if (count_ >= 2) { @@ -1900,7 +1900,7 @@ bool io_comm_rx::RxMessage::isConnectionDescriptor() } } -bool io_comm_rx::RxMessage::isErrorMessage() +bool io::RxMessage::isErrorMessage() { if (count_ >= 3) { @@ -1918,7 +1918,7 @@ bool io_comm_rx::RxMessage::isErrorMessage() } } -std::string io_comm_rx::RxMessage::messageID() +std::string io::RxMessage::messageID() { if (this->isSBF()) { @@ -1938,11 +1938,11 @@ std::string io_comm_rx::RxMessage::messageID() return std::string(); // less CPU work than return ""; } -const uint8_t* io_comm_rx::RxMessage::getPosBuffer() { return data_; } +const uint8_t* io::RxMessage::getPosBuffer() { return data_; } -const uint8_t* io_comm_rx::RxMessage::getEndBuffer() { return data_ + count_; } +const uint8_t* io::RxMessage::getEndBuffer() { return data_ + count_; } -uint16_t io_comm_rx::RxMessage::getBlockLength() +uint16_t io::RxMessage::getBlockLength() { if (this->isSBF()) { @@ -1962,7 +1962,7 @@ uint16_t io_comm_rx::RxMessage::getBlockLength() * NMEA message or a command reply. In that case, search() will check the bytes one * by one for the new message's sync bytes ($P, $G or $R). */ -void io_comm_rx::RxMessage::next() +void io::RxMessage::next() { std::size_t jump_size; if (found()) @@ -2005,7 +2005,7 @@ void io_comm_rx::RxMessage::next() * If GNSS time is used, Publishing is only done with valid leap seconds */ template -void io_comm_rx::RxMessage::publish(const std::string& topic, const M& msg) +void io::RxMessage::publish(const std::string& topic, const M& msg) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -2027,7 +2027,7 @@ void io_comm_rx::RxMessage::publish(const std::string& topic, const M& msg) /** * If GNSS time is used, Publishing is only done with valid leap seconds */ -void io_comm_rx::RxMessage::publishTf(const LocalizationMsg& msg) +void io::RxMessage::publishTf(const LocalizationMsg& msg) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -2058,7 +2058,7 @@ void io_comm_rx::RxMessage::publishTf(const LocalizationMsg& msg) * seems to be 89 on a mosaic-x5. Luckily, when parsing we do not care since we just * search for \\. */ -bool io_comm_rx::RxMessage::read(std::string message_key, bool search) +bool io::RxMessage::read(std::string message_key, bool search) { if (search) this->search(); @@ -3213,7 +3213,7 @@ bool io_comm_rx::RxMessage::read(std::string message_key, bool search) return true; } -void io_comm_rx::RxMessage::wait(Timestamp time_obj) +void io::RxMessage::wait(Timestamp time_obj) { Timestamp unix_old = unix_time_; unix_time_ = time_obj; @@ -3237,7 +3237,7 @@ void io_comm_rx::RxMessage::wait(Timestamp time_obj) current_leap_seconds_ = settings_->leap_seconds; } -bool io_comm_rx::RxMessage::gnss_gpsfix_complete(uint32_t id) +bool io::RxMessage::gnss_gpsfix_complete(uint32_t id) { std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, @@ -3250,7 +3250,7 @@ bool io_comm_rx::RxMessage::gnss_gpsfix_complete(uint32_t id) return allTrue(gpsfix_vec, id); } -bool io_comm_rx::RxMessage::ins_gpsfix_complete(uint32_t id) +bool io::RxMessage::ins_gpsfix_complete(uint32_t id) { std::vector gpsfix_vec = { channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, @@ -3258,20 +3258,20 @@ bool io_comm_rx::RxMessage::ins_gpsfix_complete(uint32_t id) return allTrue(gpsfix_vec, id); } -bool io_comm_rx::RxMessage::gnss_navsatfix_complete(uint32_t id) +bool io::RxMessage::gnss_navsatfix_complete(uint32_t id) { std::vector navsatfix_vec = {pvtgeodetic_has_arrived_navsatfix_, poscovgeodetic_has_arrived_navsatfix_}; return allTrue(navsatfix_vec, id); } -bool io_comm_rx::RxMessage::ins_navsatfix_complete(uint32_t id) +bool io::RxMessage::ins_navsatfix_complete(uint32_t id) { std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; return allTrue(navsatfix_vec, id); } -bool io_comm_rx::RxMessage::gnss_pose_complete(uint32_t id) +bool io::RxMessage::gnss_pose_complete(uint32_t id) { std::vector pose_vec = { pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, @@ -3279,33 +3279,33 @@ bool io_comm_rx::RxMessage::gnss_pose_complete(uint32_t id) return allTrue(pose_vec, id); } -bool io_comm_rx::RxMessage::ins_pose_complete(uint32_t id) +bool io::RxMessage::ins_pose_complete(uint32_t id) { std::vector pose_vec = {insnavgeod_has_arrived_pose_}; return allTrue(pose_vec, id); } -bool io_comm_rx::RxMessage::diagnostics_complete(uint32_t id) +bool io::RxMessage::diagnostics_complete(uint32_t id) { std::vector diagnostics_vec = {receiverstatus_has_arrived_diagnostics_, qualityind_has_arrived_diagnostics_}; return allTrue(diagnostics_vec, id); } -bool io_comm_rx::RxMessage::ins_localization_complete(uint32_t id) +bool io::RxMessage::ins_localization_complete(uint32_t id) { std::vector loc_vec = {insnavgeod_has_arrived_localization_}; return allTrue(loc_vec, id); } -bool io_comm_rx::RxMessage::ins_localization_ecef_complete(uint32_t id) +bool io::RxMessage::ins_localization_ecef_complete(uint32_t id) { std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, insnavcart_has_arrived_localization_ecef_}; return allTrue(loc_vec, id); } -bool io_comm_rx::RxMessage::allTrue(std::vector& vec, uint32_t id) +bool io::RxMessage::allTrue(std::vector& vec, uint32_t id) { vec.erase(vec.begin() + id); // Checks whether all entries in vec are true diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 720ec9a0..2190aea8 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -59,7 +59,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_) return; // Initializes Connection - IO_.initializeIO(); + IO_.initializeIo(); // Subscribes to all requested Rx messages by adding entries to the C++ multimap // storing the callback handlers and publishes ROS messages From e134c61075337b1a77a3609127a905bb8afbe2bc Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 11 Jan 2023 22:43:04 +0100 Subject: [PATCH 016/170] Add message handler --- .../communication/async_manager.hpp | 76 +- .../communication/callback_handlers.hpp | 371 -------- .../communication/circular_buffer.hpp | 84 -- .../communication/communication_core.hpp | 44 +- .../{telegram.hpp => message.hpp} | 6 +- .../communication/message_handler.hpp | 156 ++++ .../communication/message_parser.hpp | 793 ++++++++++++++++++ .../communication/callback_handlers.cpp | 701 ---------------- .../communication/circular_buffer.cpp | 128 --- .../communication/communication_core.cpp | 199 ++--- .../communication/message_handler.cpp | 105 +++ .../{rx_message.cpp => message_parser.cpp} | 0 12 files changed, 1169 insertions(+), 1494 deletions(-) delete mode 100644 include/septentrio_gnss_driver/communication/callback_handlers.hpp delete mode 100644 include/septentrio_gnss_driver/communication/circular_buffer.hpp rename include/septentrio_gnss_driver/communication/{telegram.hpp => message.hpp} (95%) create mode 100644 include/septentrio_gnss_driver/communication/message_handler.hpp create mode 100644 include/septentrio_gnss_driver/communication/message_parser.hpp delete mode 100644 src/septentrio_gnss_driver/communication/callback_handlers.cpp delete mode 100644 src/septentrio_gnss_driver/communication/circular_buffer.cpp create mode 100644 src/septentrio_gnss_driver/communication/message_handler.cpp rename src/septentrio_gnss_driver/communication/{rx_message.cpp => message_parser.cpp} (100%) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 81ae6a16..601715dd 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -67,7 +67,7 @@ // local includes #include "io.hpp" -#include "telegram.hpp" +#include "message.hpp" #pragma once @@ -112,9 +112,9 @@ namespace io { * @param[in] buffer_size Size of the circular buffer in bytes */ AsyncManager(ROSaicNodeBase* node, std::unique_ptr stream, - TelegramQueue* telegramQueue) : + MessageQueue* messageQueue) : node_(node), - stream_(std::move(stream)), telegramQueue_(telegramQueue) + stream_(std::move(stream)), messageQueue_(messageQueue) { } @@ -271,7 +271,7 @@ namespace io { void resync() { - telegram_.reset(new Telegram); + message_.reset(new Message); readSync<0>(); } @@ -281,7 +281,7 @@ namespace io { static_assert(index < 3); boost::asio::async_read( - *stream_, boost::asio::buffer(telegram_->data.data() + index, 1), + *stream_, boost::asio::buffer(message_->data.data() + index, 1), [this](boost::system::error_code ec, std::size_t numBytes) { Timestamp stamp = node_->getTime(); @@ -293,9 +293,9 @@ namespace io { { case 0: { - if (telegram_->data[index] == SYNC_0) + if (message_->data[index] == SYNC_0) { - telegram_->stamp = stamp; + message_->stamp = stamp; readSync<1>(); } else readSync<0>(); @@ -303,29 +303,29 @@ namespace io { } case 1: { - switch (telegram_->data[index]) + switch (message_->data[index]) { case SBF_SYNC_BYTE_1: { - telegram_->type = SBF; + message_->type = SBF; readSbfHeader(); break; } case NMEA_SYNC_BYTE_1: { - telegram_->type = NMEA; + message_->type = NMEA; readSync<2>(); break; } case RESPONSE_SYNC_BYTE_1: { - stelegram_->type = RESPONSE; + smessage_->type = RESPONSE; readSync<2>(); break; } case CONNECTION_DESCRIPTOR_BYTE_1: { - telegram_->type = CONNECTION_DESCRIPTOR; + message_->type = CONNECTION_DESCRIPTOR; readSync<2>(); break; } @@ -342,11 +342,11 @@ namespace io { } case 2: { - switch (telegram_->data[index]) + switch (message_->data[index]) { case NMEA_SYNC_BYTE_2: { - if (telegram_->type == NMEA) + if (message_->type == NMEA) readString(); else resync(); @@ -354,7 +354,7 @@ namespace io { } case RESPONSE_SYNC_BYTE_1: { - if (telegram_->type == RESPONSE) + if (message_->type == RESPONSE) readString(); else resync(); @@ -362,7 +362,7 @@ namespace io { } case CONNECTION_DESCRIPTOR_BYTE_1: { - if (telegram_->type == CONNECTION_DESCRIPTOR) + if (message_->type == CONNECTION_DESCRIPTOR) readString(); else resync(); @@ -407,18 +407,18 @@ namespace io { void readSbfHeader() { - telegram_->data.resize(SBF_HEADER_SIZE); + message_->data.resize(SBF_HEADER_SIZE); boost::asio::async_read( *stream_, - boost::asio::buffer(telegram_->data.data() + 2, SBF_HEADER_SIZE - 2), + boost::asio::buffer(message_->data.data() + 2, SBF_HEADER_SIZE - 2), [this](boost::system::error_code ec, std::size_t numBytes) { if (!ec) { if (numBytes == (SBF_HEADER_SIZE - 2)) { unit16_t length = - parsing_utilities::getLength(telegram_->data.data()); + parsing_utilities::getLength(message_->data.data()); if (length > MAX_SBF_SIZE) { node_->log( @@ -448,24 +448,23 @@ namespace io { void readSbf(std::size_t length) { - telegram_->data.resize(length); + message_->data.resize(length); boost::asio::async_read( *stream_, - boost::asio::buffer(telegram_->data.data() + SBF_HEADER_SIZE, + boost::asio::buffer(message_->data.data() + SBF_HEADER_SIZE, length - SBF_HEADER_SIZE), [this](boost::system::error_code ec, std::size_t numBytes) { if (!ec) { if (numBytes == (length - SBF_HEADER_SIZE)) { - if (isValid( - telegram_->data.data())) // TODO namespace crc + if (isValid(message_->data.data())) // TODO namespace crc { - telegram_->sbfId = - parsing_utilities::getId(telegram_->data.data()); + message_->sbfId = + parsing_utilities::getId(message_->data.data()); - telegramQueue_->push(telegram_); + messageQueue_->push(message_); } } else { @@ -485,7 +484,7 @@ namespace io { void readString() { - telegram_->data.resize(3); + message_->data.resize(3); readStringElements(); } @@ -504,9 +503,9 @@ namespace io { { case SYNC_0: { - telegram_.reset(new Telegram); - telegram_->data[0] = byte; - telegram_->stamp = node_->getTime(); + message_.reset(new Message); + message_->data[0] = byte; + message_->stamp = node_->getTime(); node_->log( LogLevel::DEBUG, "AsyncManager string read fault, sync 0 found.")); @@ -515,16 +514,15 @@ namespace io { } case LF: { - telegram_->data.push_back(byte); - if (telegram_->data[telegram_->data.size() - 2] == - CR) - telegramQueue_->push(telegram_); + message_->data.push_back(byte); + if (message_->data[message_->data.size() - 2] == CR) + messageQueue_->push(message_); resync(); break; } default: { - telegram_->data.push_back(byte); + message_->data.push_back(byte); readString(); break; } @@ -558,9 +556,9 @@ namespace io { std::thread watchdogThread_; //! Timestamp of receiving buffer Timestamp recvStamp_; - //! Telegram - std::shared_ptr telegram_; - //! TelegramQueue - TelegramQueue* telegramQueue_; + //! Message + std::shared_ptr message_; + //! MessageQueue + MessageQueue* messageQueue_; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/callback_handlers.hpp b/include/septentrio_gnss_driver/communication/callback_handlers.hpp deleted file mode 100644 index b7b9ab62..00000000 --- a/include/septentrio_gnss_driver/communication/callback_handlers.hpp +++ /dev/null @@ -1,371 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ***************************************************************************** -// -// Boost Software License - Version 1.0 - August 17th, 2003 -// -// Permission is hereby granted, free of charge, to any person or organization -// obtaining a copy of the software and accompanying documentation covered by -// this license (the "Software") to use, reproduce, display, distribute, -// execute, and transmit the Software, and to prepare derivative works of the -// Software, and to permit third-parties to whom the Software is furnished to -// do so, all subject to the following: - -// The copyright notices in the Software and this entire statement, including -// the above license grant, this restriction and the following disclaimer, -// must be included in all copies of the Software, in whole or in part, and -// all derivative works of the Software, unless such copies or derivative -// works are solely in the form of machine-executable object code generated by -// a source language processor. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT -// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE -// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, -// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. -// -// ***************************************************************************** - -#ifndef CALLBACK_HANDLERS_HPP -#define CALLBACK_HANDLERS_HPP - -// Boost includes -#include -// In C++, writing a loop that iterates over a sequence is tedious --> -// BOOST_FOREACH(char ch, "Hello World") -#include -// E.g. boost::function f = std::atoi;defines a pointer f that can -// point to functions that expect a parameter of type const char* and return a value -// of type int Generally, any place in which a function pointer would be used to -// defer a call or make a callback, Boost.Function can be used instead to allow the -// user greater flexibility in the implementation of the target. -#include -// Boost's thread enables the use of multiple threads of execution with shared data -// in portable C++ code. It provides classes and functions for managing the threads -// themselves, along with others for synchronizing data between the threads or -// providing separate copies of data specific to individual threads. -#include -#include -// The tokenizer class provides a container view of a series of tokens contained in a -// sequence, e.g. if you are not interested in non-words... -#include -// Join algorithm is a counterpart to split algorithms. It joins strings from a -// 'list' by adding user defined separator. -#include -// The class boost::posix_time::ptime that we will use defines a location-independent -// time. It uses the type boost::gregorian::date, yet also stores a time. -#include -// Boost.Asio may be used to perform both synchronous and asynchronous operations on -// I/O objects such as sockets. -#include -#include -#include -#include - -// ROSaic and C++ includes -#include -#include - -/** - * @file callback_handlers.hpp - * @date 22/08/20 - * @brief Handles callbacks when reading NMEA/SBF messages - */ - -extern bool g_response_received; -extern boost::mutex g_response_mutex; -extern boost::condition_variable g_response_condition; -extern bool g_cd_received; -extern boost::mutex g_cd_mutex; -extern boost::condition_variable g_cd_condition; -extern uint32_t g_cd_count; -extern std::string g_rx_tcp_port; - -namespace io { - /** - * @class CallbackHandler - * @brief Abstract class representing a generic callback handler, includes - * high-level functionality such as wait - */ - class AbstractCallbackHandler - { - public: - virtual void handle(RxMessage& rx_message, std::string message_key) = 0; - - bool Wait(const boost::posix_time::time_duration& timeout) - { - boost::mutex::scoped_lock lock(mutex_); - return condition_.timed_wait(lock, timeout); - } - - protected: - boost::mutex mutex_; - boost::condition_variable condition_; - }; - - /** - * @class CallbackHandler - * @brief Derived class operating on a ROS message level - */ - template - class CallbackHandler : public AbstractCallbackHandler - { - public: - virtual const T& Get() { return message_; } - - void handle(RxMessage& rx_message, std::string message_key) - { - boost::mutex::scoped_lock lock(mutex_); - try - { - if (!rx_message.read(message_key)) - { - std::ostringstream ss; - ss << "Rx decoder error for message with ID (empty field if non-determinable)" - << rx_message.messageID() << ". Reason unknown."; - throw std::runtime_error(ss.str()); - return; - } - } catch (std::runtime_error& e) - { - std::ostringstream ss; - ss << "Rx decoder error for message with ID " - << rx_message.messageID() << ".\n" - << e.what(); - throw std::runtime_error(ss.str()); - return; - } - - condition_.notify_all(); - } - - private: - T message_; - }; - - /** - * @class CallbackHandlers - * @brief Represents ensemble of (to be constructed) ROS messages, to be handled - * at once by this class - */ - class CallbackHandlers - { - - public: - //! Key is std::string and represents the ROS message key, value is - //! boost::shared_ptr - typedef std::multimap> - CallbackMap; - - CallbackHandlers(ROSaicNodeBase* node, Settings* settings) : - node_(node), rx_message_(node, settings), settings_(settings) - { - } - - /** - * @brief Adds a pair to the multimap "callbackmap_", with the message_key - * being the key - * - * This method is called by "handlers_" in rosaic_node.cpp. - * T would be a (custom or not) ROS message, e.g. - * PVTGeodeticMsg, or nmea_msgs::GPGGA. Note that - * "typename" could be omitted in the argument. - * @param message_key The pair's key - * @return The modified multimap "callbackmap_" - */ - template - CallbackMap insert(std::string message_key) - { - boost::mutex::scoped_lock lock(callback_mutex_); - // Adding typename might be cleaner, but is optional again - CallbackHandler* handler = new CallbackHandler(); - callbackmap_.insert(std::make_pair( - message_key, boost::shared_ptr(handler))); - CallbackMap::key_type key = message_key; - node_->log( - LogLevel::DEBUG, - "Key " + message_key + " successfully inserted into multimap: " + - (((unsigned int)callbackmap_.count(key)) ? "true" : "false")); - return callbackmap_; - } - - /** - * @brief Called every time rx_message is found to contain some potentially - * useful message - */ - void handle(); - - /** - * @brief Searches for Rx messages that could potentially be - * decoded/parsed/published - * @param[in] recvTimestamp Timestamp of buffer reception passed on from - * AsyncManager class - * @param[in] data Buffer passed on from AsyncManager class - * @param[in] size Size of the buffer - */ - void readCallback(Timestamp recvTimestamp, const uint8_t* data, - std::size_t& size); - - //! Callback handlers multimap for Rx messages; it needs to be public since - //! we copy-assign (did not work otherwise) new callbackmap_, after inserting - //! a pair to the multimap within the DefineMessages() method of the - //! ROSaicNode class, onto its old version. - CallbackMap callbackmap_; - - private: - //! Pointer to Node - ROSaicNodeBase* node_; - - //! RxMessage parser - RxMessage rx_message_; - - //! Settings - Settings* settings_; - - //! The "static" keyword resolves construct-by-copying issues related to this - //! mutex by making it available throughout the code unit. The mutex - //! constructor list contains "mutex (const mutex&) = delete", hence - //! construct-by-copying a mutex is explicitly prohibited. The get_handlers() - //! method of the CommIo class hence forces us to make this mutex static. - static boost::mutex callback_mutex_; - - //! Determines which of the SBF blocks necessary for the gps_common::GPSFix - //! ROS message arrives last and thus launches its construction - static std::string do_gpsfix_; - - //! Determines which of the INS integrated SBF blocks necessary for the - //! gps_common::GPSFix ROS message arrives last and thus launches its - //! construction - static std::string do_insgpsfix_; - - //! Determines which of the SBF blocks necessary for the - //! NavSatFixMsg ROS message arrives last and thus launches its - //! construction - static std::string do_navsatfix_; - - //! Determines which of the INS integrated SBF blocks necessary for the - //! NavSatFixMsg ROS message arrives last and thus launches its construction - static std::string do_insnavsatfix_; - - //! Determines which of the SBF blocks necessary for the - //! geometry_msgs/PoseWithCovarianceStamped ROS message arrives last and thus - //! launches its construction - static std::string do_pose_; - - //! Determines which of the INS integrated SBF blocks necessary for the - //! geometry_msgs/PoseWithCovarianceStamped ROS message arrives last and thus - //! launches its construction - static std::string do_inspose_; - - //! Determines which of the SBF blocks necessary for the - //! diagnostic_msgs/DiagnosticArray ROS message arrives last and thus - //! launches its construction - static std::string do_diagnostics_; - - //! Determines which of the SBF blocks necessary for the - //! sensor_msgs/Imu ROS message arrives last and thus - //! launches its construction - static std::string do_imu_; - - //! Determines which of the SBF blocks necessary for the - //! nav_msgs/Odometry ROS message arrives last and thus - //! launches its construction - static std::string do_inslocalization_; - - //! Determines which of the SBF blocks necessary for the - //! nav_msgs/Odometry ROS message arrives last and thus - //! launches its construction - static std::string do_inslocalization_ecef_; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for GPSFix to a uint32_t - typedef std::unordered_map GPSFixMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static GPSFixMap gpsfix_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for NavSatFix to a uint32_t - typedef std::unordered_map NavSatFixMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static NavSatFixMap navsatfix_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for PoseWithCovarianceStamped to a uint32_t - typedef std::unordered_map - PoseWithCovarianceStampedMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static PoseWithCovarianceStampedMap pose_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for DiagnosticArray to a uint32_t - typedef std::unordered_map DiagnosticArrayMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static DiagnosticArrayMap diagnosticarray_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for Imu to a uint32_t - typedef std::unordered_map ImuMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static ImuMap imu_map; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for Localization to a uint32_t - typedef std::unordered_map LocalizationMap; - - //! Shorthand for the map responsible for matching ROS message identifiers - //! relevant for Localization to a uint32_t - typedef std::unordered_map LocalizationEcefMap; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static LocalizationMap localization_map; - - //! All instances of the CallbackHandlers class shall have access to the map - //! without reinitializing it, hence static - static LocalizationMap localization_ecef_map; - }; - -} // namespace io - -#endif \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/circular_buffer.hpp b/include/septentrio_gnss_driver/communication/circular_buffer.hpp deleted file mode 100644 index 188906de..00000000 --- a/include/septentrio_gnss_driver/communication/circular_buffer.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ROS includes -#include - -// C++ library includes -#include -#include - -#ifndef CIRCULAR_BUFFER_HPP -#define CIRCULAR_BUFFER_HPP - -/** - * @file circular_buffer.hpp - * @brief Declares a class for creating, writing to and reading from a circular - * bufffer - * @date 25/09/20 - */ - -/** - * @class CircularBuffer - * @brief Class for creating, writing to and reading from a circular buffer - */ -class CircularBuffer -{ -public: - //! Constructor of CircularBuffer - explicit CircularBuffer(ROSaicNodeBase* node, std::size_t capacity); - //! Destructor of CircularBuffer - ~CircularBuffer(); - //! Returns size_ - std::size_t size() const { return size_; } - //! Returns capacity_ - std::size_t capacity() const { return capacity_; } - //! Returns number of bytes written. - std::size_t write(const uint8_t* data, std::size_t bytes); - //! Returns number of bytes read. - std::size_t read(uint8_t* data, std::size_t bytes); - -private: - //! Pointer to the node - ROSaicNodeBase* node_; - //! Specifies where we start writing - std::size_t head_; - //! Specifies where we start reading - std::size_t tail_; - //! Number of bytes that have been written but not yet read - std::size_t size_; - //! Capacity of the circular buffer - std::size_t capacity_; - //! Pointer that always points to the same memory address, hence could be const - //! pointer - uint8_t* data_; -}; - -#endif // for CIRCULAR_BUFFER_HPP \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index e03d45fd..89c204c9 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -147,36 +147,6 @@ namespace io { */ void preparePCAPFileReading(std::string file_name); - /** - * @brief Attempts to (re)connect every reconnect_delay_s_ seconds - */ - void reconnect(); - - /** - * @brief Calls the reconnect() method - */ - void connect(); - - /** - * @brief Initializes the serial port - * @param[in] port The device's port address - * @param[in] baudrate The chosen baud rate of the port - * @param[in] flowcontrol Default is "None", set variable (not yet checked) - * to "RTS|CTS" to activate hardware flow control (only for serial ports - * COM1, COM2 and COM3 (for mosaic)) - * @return True if connection could be established, false otherwise - */ - bool initializeSerial(std::string port, uint32_t baudrate = 115200, - std::string flowcontrol = "None"); - - /** - * @brief Initializes the TCP I/O - * @param[in] host The TCP host - * @param[in] port The TCP port - * @return True if connection could be established, false otherwise - */ - bool initializeTCP(std::string host, std::string port); - /** * @brief Initializes SBF file reading and reads SBF file by repeatedly * calling read_callback_() @@ -192,12 +162,6 @@ namespace io { */ void initializePCAPFileReading(std::string file_name); - /** - * @brief Set the I/O manager - * @param[in] manager An I/O handler - */ - void setManager(const boost::shared_ptr& manager); - /** * @brief Hands over to the send() method of manager_ * @param cmd The command to hand over @@ -206,10 +170,12 @@ namespace io { //! Pointer to Node ROSaicNodeBase* node_; - //! Callback handlers for the inwards streaming messages - CallbackHandlers handlers_; //! Settings Settings* settings_; + //! MessageQueue + MessageQueue* messageQueue_; + //! Message handlers for the inwards streaming messages + MessageHandler messageHandler_; //! Whether connecting to Rx was successful bool connected_ = false; //! Since the configureRx() method should only be called once the connection @@ -236,8 +202,6 @@ namespace io { bool nmeaActivated_ = false; - //! Connection or reading thread - std::thread connectionThread_; //! Indicator for threads to exit std::atomic stopping_; diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/message.hpp similarity index 95% rename from include/septentrio_gnss_driver/communication/telegram.hpp rename to include/septentrio_gnss_driver/communication/message.hpp index 8324d4d5..8d571bab 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/message.hpp @@ -76,17 +76,17 @@ enum MessageType CONNECTION_DESCRIPTOR }; -struct Telegram +struct Message { Timestamp stamp = 0; MessageType type = INVALID; uint16_t sbfId = 0; std::vector data(3); - Telegram() : stamp(0), type(INVALID), sbfId(0), data(std::vector(3)) + Message() : stamp(0), type(INVALID), sbfId(0), data(std::vector(3)) { data.reserve(MAX_SBF_SIZE); } }; -typedef tbb::concurrent_bounded_queue> TelegramQueue; +typedef tbb::concurrent_bounded_queue> MessageQueue; diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp new file mode 100644 index 00000000..ac4d5b0b --- /dev/null +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -0,0 +1,156 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +// ***************************************************************************** +// +// Boost Software License - Version 1.0 - August 17th, 2003 +// +// Permission is hereby granted, free of charge, to any person or organization +// obtaining a copy of the software and accompanying documentation covered by +// this license (the "Software") to use, reproduce, display, distribute, +// execute, and transmit the Software, and to prepare derivative works of the +// Software, and to permit third-parties to whom the Software is furnished to +// do so, all subject to the following: + +// The copyright notices in the Software and this entire statement, including +// the above license grant, this restriction and the following disclaimer, +// must be included in all copies of the Software, in whole or in part, and +// all derivative works of the Software, unless such copies or derivative +// works are solely in the form of machine-executable object code generated by +// a source language processor. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// ***************************************************************************** + +#pragma once + +// Boost includes +#include +// In C++, writing a loop that iterates over a sequence is tedious --> +// BOOST_FOREACH(char ch, "Hello World") +#include +// E.g. boost::function f = std::atoi;defines a pointer f that can +// point to functions that expect a parameter of type const char* and return a value +// of type int Generally, any place in which a function pointer would be used to +// defer a call or make a callback, Boost.Function can be used instead to allow the +// user greater flexibility in the implementation of the target. +#include +// Boost's thread enables the use of multiple threads of execution with shared data +// in portable C++ code. It provides classes and functions for managing the threads +// themselves, along with others for synchronizing data between the threads or +// providing separate copies of data specific to individual threads. +#include +#include +// The tokenizer class provides a container view of a series of tokens contained in a +// sequence, e.g. if you are not interested in non-words... +#include +// Join algorithm is a counterpart to split algorithms. It joins strings from a +// 'list' by adding user defined separator. +#include +// The class boost::posix_time::ptime that we will use defines a location-independent +// time. It uses the type boost::gregorian::date, yet also stores a time. +#include +// Boost.Asio may be used to perform both synchronous and asynchronous operations on +// I/O objects such as sockets. +#include +#include +#include +#include + +// ROSaic and C++ includes +#include +#include + +/** + * @file message_handler.hpp + * @date 22/08/20 + * @brief Handles messages when reading NMEA/SBF/response messages + */ + +struct CommSync +{ + bool received = false; + std::mutex mutex; + std::condition_variable condition; +}; + +namespace io { + /** + * @class MessageHandler + * @brief Represents ensemble of (to be constructed) ROS messages, to be handled + * at once by this class + */ + class MessageHandler + { + + public: + MessageHandler(ROSaicNodeBase* node, Settings* settings) : + node_(node), rx_message_(node, settings), settings_(settings) + { + } + + /** + * @brief Called every time a message is received + */ + void handleMessage(const Message& message); + + CommSync* getResponseSync() { return &response_sync; } + + CommSync* getCdSync() { return &cd_sync; } + + uint8_t getCdCount() { return cd_count; } + + std::string getRxTcpPort() { return rx_tcp_port; } + + private: + void handleSbf(const Message& message); + void handleNmea(const Message& message); + void handleResponse(const Message& message); + void handleCd(const Message& message); + //! Pointer to Node + ROSaicNodeBase* node_; + + //! RxMessage parser + RxMessage rx_message_; + + CommSync response_sync; + CommSync cd_sync; + uint8_t cd_count; + std::string rx_tcp_port; + }; + +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/message_parser.hpp b/include/septentrio_gnss_driver/communication/message_parser.hpp new file mode 100644 index 00000000..bf2f1448 --- /dev/null +++ b/include/septentrio_gnss_driver/communication/message_parser.hpp @@ -0,0 +1,793 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +// ***************************************************************************** +// +// Boost Software License - Version 1.0 - August 17th, 2003 +// +// Permission is hereby granted, free of charge, to any person or organization +// obtaining a copy of the software and accompanying documentation covered by +// this license (the "Software") to use, reproduce, display, distribute, +// execute, and transmit the Software, and to prepare derivative works of the +// Software, and to permit third-parties to whom the Software is furnished to +// do so, all subject to the following: + +// The copyright notices in the Software and this entire statement, including +// the above license grant, this restriction and the following disclaimer, +// must be included in all copies of the Software, in whole or in part, and +// all derivative works of the Software, unless such copies or derivative +// works are solely in the form of machine-executable object code generated by +// a source language processor. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// ***************************************************************************** + +//! 0x24 is ASCII for $ - 1st byte in each message +#ifndef NMEA_SYNC_BYTE_1 +#define NMEA_SYNC_BYTE_1 0x24 +#endif +//! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message +#ifndef NMEA_SYNC_BYTE_2_1 +#define NMEA_SYNC_BYTE_2_1 0x47 +#endif +//! 0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message +#ifndef NMEA_SYNC_BYTE_2_2 +#define NMEA_SYNC_BYTE_2_2 0x50 +#endif +//! 0x24 is ASCII for $ - 1st byte in each response from the Rx +#ifndef RESPONSE_SYNC_BYTE_1 +#define RESPONSE_SYNC_BYTE_1 0x24 +#endif +//! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx +#ifndef RESPONSE_SYNC_BYTE_2 +#define RESPONSE_SYNC_BYTE_2 0x52 +#endif +//! 0x0D is ASCII for "Carriage Return", i.e. "Enter" +#ifndef CARRIAGE_RETURN +#define CARRIAGE_RETURN 0x0D +#endif +//! 0x0A is ASCII for "Line Feed", i.e. "New Line" +#ifndef LINE_FEED +#define LINE_FEED 0x0A +#endif +//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the +//! command was invalid +#ifndef RESPONSE_SYNC_BYTE_3 +#define RESPONSE_SYNC_BYTE_3 0x3F +#endif +//! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after +//! initiating TCP connection +#ifndef CONNECTION_DESCRIPTOR_BYTE_1 +#define CONNECTION_DESCRIPTOR_BYTE_1 0x49 +#endif +//! 0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after +//! initiating TCP connection +#ifndef CONNECTION_DESCRIPTOR_BYTE_2 +#define CONNECTION_DESCRIPTOR_BYTE_2 0x50 +#endif + +// C++ libraries +#include // for assert +#include +#include +#include +// Boost includes +#include +#include +#include +#include +// ROSaic includes +#include +#include +#include +#include +#include +#include +#include + +#ifndef RX_MESSAGE_HPP +#define RX_MESSAGE_HPP + +/** + * @file rx_message.hpp + * @date 20/08/20 + * @brief Defines a class that reads messages handed over from the circular buffer + */ + +extern bool g_read_cd; +extern uint32_t g_cd_count; + +//! Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's +//! Mode field +enum TypeOfPVT_Enum +{ + evNoPVT, + evStandAlone, + evDGPS, + evFixed, + evRTKFixed, + evRTKFloat, + evSBAS, + evMovingBaseRTKFixed, + evMovingBaseRTKFloat, + evPPP +}; + +//! Since switch only works with int (yet NMEA message IDs are strings), we need +//! enum. Note drawbacks: No variable can have a name which is already in some +//! enumeration, enums are not type safe etc.. +enum RxID_Enum +{ + evNavSatFix, + evINSNavSatFix, + evGPSFix, + evINSGPSFix, + evPoseWithCovarianceStamped, + evINSPoseWithCovarianceStamped, + evGPGGA, + evGPRMC, + evGPGSA, + evGPGSV, + evGLGSV, + evGAGSV, + evPVTCartesian, + evPVTGeodetic, + evBaseVectorCart, + evBaseVectorGeod, + evPosCovCartesian, + evPosCovGeodetic, + evAttEuler, + evAttCovEuler, + evINSNavCart, + evINSNavGeod, + evIMUSetup, + evVelSensorSetup, + evExtEventINSNavGeod, + evExtEventINSNavCart, + evExtSensorMeas, + evGPST, + evChannelStatus, + evMeasEpoch, + evDOP, + evVelCovGeodetic, + evDiagnosticArray, + evLocalization, + evLocalizationEcef, + evReceiverStatus, + evQualityInd, + evReceiverTime, + evReceiverSetup +}; + +namespace io { + + /** + * @class RxMessage + * @brief Can search buffer for messages, read/parse them, and so on + */ + class RxMessage + { + public: + /** + * @brief Constructor of the RxMessage class + * + * One can always provide a non-const value where a const one was expected. + * The const-ness of the argument just means the function promises not to + * change it.. Recall: static_cast by the way can remove or add const-ness, + * no other C++ cast is capable of removing it (not even reinterpret_cast) + * @param[in] data Pointer to the buffer that is about to be analyzed + * @param[in] size Size of the buffer (as handed over by async_read_some) + */ + RxMessage(ROSaicNodeBase* node, Settings* settings) : + node_(node), settings_(settings), unix_time_(0) + { + found_ = false; + crc_check_ = false; + message_size_ = 0; + + //! Pair of iterators to facilitate initialization of the map + std::pair type_of_pvt_pairs[] = { + std::make_pair(static_cast(0), evNoPVT), + std::make_pair(static_cast(1), evStandAlone), + std::make_pair(static_cast(2), evDGPS), + std::make_pair(static_cast(3), evFixed), + std::make_pair(static_cast(4), evRTKFixed), + std::make_pair(static_cast(5), evRTKFloat), + std::make_pair(static_cast(6), evSBAS), + std::make_pair(static_cast(7), evMovingBaseRTKFixed), + std::make_pair(static_cast(8), evMovingBaseRTKFloat), + std::make_pair(static_cast(10), evPPP)}; + + type_of_pvt_map = + TypeOfPVTMap(type_of_pvt_pairs, type_of_pvt_pairs + evPPP + 1); + + //! Pair of iterators to facilitate initialization of the map + std::pair rx_id_pairs[] = { + std::make_pair("NavSatFix", evNavSatFix), + std::make_pair("INSNavSatFix", evINSNavSatFix), + std::make_pair("GPSFix", evGPSFix), + std::make_pair("INSGPSFix", evINSGPSFix), + std::make_pair("PoseWithCovarianceStamped", + evPoseWithCovarianceStamped), + std::make_pair("INSPoseWithCovarianceStamped", + evINSPoseWithCovarianceStamped), + std::make_pair("$GPGGA", evGPGGA), + std::make_pair("$GPRMC", evGPRMC), + std::make_pair("$GPGSA", evGPGSA), + std::make_pair("$GPGSV", evGPGSV), + std::make_pair("$GLGSV", evGLGSV), + std::make_pair("$GAGSV", evGAGSV), + std::make_pair("4006", evPVTCartesian), + std::make_pair("4007", evPVTGeodetic), + std::make_pair("4043", evBaseVectorCart), + std::make_pair("4028", evBaseVectorGeod), + std::make_pair("5905", evPosCovCartesian), + std::make_pair("5906", evPosCovGeodetic), + std::make_pair("5938", evAttEuler), + std::make_pair("5939", evAttCovEuler), + std::make_pair("GPST", evGPST), + std::make_pair("4013", evChannelStatus), + std::make_pair("4027", evMeasEpoch), + std::make_pair("4001", evDOP), + std::make_pair("5908", evVelCovGeodetic), + std::make_pair("DiagnosticArray", evDiagnosticArray), + std::make_pair("Localization", evLocalization), + std::make_pair("LocalizationEcef", evLocalizationEcef), + std::make_pair("4014", evReceiverStatus), + std::make_pair("4082", evQualityInd), + std::make_pair("5902", evReceiverSetup), + std::make_pair("4225", evINSNavCart), + std::make_pair("4226", evINSNavGeod), + std::make_pair("4230", evExtEventINSNavGeod), + std::make_pair("4229", evExtEventINSNavCart), + std::make_pair("4224", evIMUSetup), + std::make_pair("4244", evVelSensorSetup), + std::make_pair("4050", evExtSensorMeas), + std::make_pair("5914", evReceiverTime)}; + + rx_id_map = RxIDMap(rx_id_pairs, rx_id_pairs + evReceiverSetup + 1); + } + + /** + * @brief Put new data + * @param[in] recvTimestamp Timestamp of receiving buffer + * @param[in] data Pointer to the buffer that is about to be analyzed + * @param[in] size Size of the buffer (as handed over by async_read_some) + */ + void newData(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size) + { + recvTimestamp_ = recvTimestamp; + data_ = data; + count_ = size; + found_ = false; + crc_check_ = false; + message_size_ = 0; + } + + //! Determines whether data_ points to the SBF block with ID "ID", e.g. 5003 + bool isMessage(const uint16_t ID); + //! Determines whether data_ points to the NMEA message with ID "ID", e.g. + //! "$GPGGA" + bool isMessage(std::string ID); + //! Determines whether data_ currently points to an SBF block + bool isSBF(); + //! Determines whether data_ currently points to an NMEA message + bool isNMEA(); + //! Determines whether data_ currently points to an NMEA message + bool isResponse(); + //! Determines whether data_ currently points to a connection descriptor + //! (right after initiating a TCP connection) + bool isConnectionDescriptor(); + //! Determines whether data_ currently points to an error message reply from + //! the Rx + bool isErrorMessage(); + //! Determines size of the message (also command reply) that data_ is + //! currently pointing at + std::size_t messageSize(); + //! Returns the message ID of the message where data_ is pointing at at the + //! moment, SBF identifiers embellished with inverted commas, e.g. "5003" + std::string messageID(); + + /** + * @brief Returns the count_ variable + * @return The variable count_ + */ + std::size_t getCount() { return count_; }; + /** + * @brief Searches the buffer for the beginning of the next message (NMEA or + * SBF) + * @return A pointer to the start of the next message. + */ + const uint8_t* search(); + + /** + * @brief Gets the length of the SBF block + * + * It determines the length from the header of the SBF block. The block + * length thus includes the header length. + * @return The length of the SBF block + */ + uint16_t getBlockLength(); + + /** + * @brief Gets the current position in the read buffer + * @return The current position of the read buffer + */ + const uint8_t* getPosBuffer(); + + /** + * @brief Gets the end position in the read buffer + * @return A pointer pointing to just after the read buffer (matches + * search()'s final pointer in case no valid message is found) + */ + const uint8_t* getEndBuffer(); + + /** + * @brief Has an NMEA message, SBF block or command reply been found in the + * buffer? + * @returns True if a message with the correct header & length has been found + */ + bool found(); + + /** + * @brief Goes to the start of the next message based on the calculated + * length of current message + */ + void next(); + + /** + * @brief Publishing function + * @param[in] topic String of topic + * @param[in] msg ROS message to be published + */ + template + void publish(const std::string& topic, const M& msg); + + /** + * @brief Publishing function + * @param[in] msg Localization message + */ + void publishTf(const LocalizationMsg& msg); + + /** + * @brief Performs the CRC check (if SBF) and publishes ROS messages + * @return True if read was successful, false otherwise + */ + bool read(std::string message_key, bool search = false); + + /** + * @brief Whether or not a message has been found + */ + bool found_; + + /** + * @brief Wether all blocks from GNSS have arrived for GpsFix Message + */ + bool gnss_gpsfix_complete(uint32_t id); + + /** + * @brief Wether all blocks from INS have arrived for GpsFix Message + */ + bool ins_gpsfix_complete(uint32_t id); + + /** + * @brief Wether all blocks from GNSS have arrived for NavSatFix Message + */ + bool gnss_navsatfix_complete(uint32_t id); + + /** + * @brief Wether all blocks from INS have arrived for NavSatFix Message + */ + bool ins_navsatfix_complete(uint32_t id); + + /** + * @brief Wether all blocks from GNSS have arrived for Pose Message + */ + bool gnss_pose_complete(uint32_t id); + + /** + * @brief Wether all blocks from INS have arrived for Pose Message + */ + bool ins_pose_complete(uint32_t id); + + /** + * @brief Wether all blocks have arrived for Diagnostics Message + */ + bool diagnostics_complete(uint32_t id); + + /** + * @brief Wether all blocks have arrived for Localization Message + */ + bool ins_localization_complete(uint32_t id); + + /** + * @brief Wether all blocks have arrived for Localization Message + */ + bool ins_localization_ecef_complete(uint32_t id); + + private: + /** + * @brief Pointer to the node + */ + ROSaicNodeBase* node_; + + /** + * @brief Timestamp of receiving buffer + */ + Timestamp recvTimestamp_; + + /** + * @brief Pointer to the buffer of messages + */ + const uint8_t* data_; + + /** + * @brief Number of unread bytes in the buffer + */ + std::size_t count_; + + /** + * @brief Whether the CRC check as evaluated in the read() method was + * successful or not is stored here + */ + bool crc_check_; + + /** + * @brief Helps to determine size of response message / NMEA message / SBF + * block + */ + std::size_t message_size_; + + /** + * @brief Number of times the GPSFixMsg message has been published + */ + uint32_t count_gpsfix_ = 0; + + /** + * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks + * need to be stored + */ + PVTGeodeticMsg last_pvtgeodetic_; + + /** + * @brief Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic + * blocks need to be stored + */ + PosCovGeodeticMsg last_poscovgeodetic_; + + /** + * @brief Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to + * be stored + */ + AttEulerMsg last_atteuler_; + + /** + * @brief Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks + * need to be stored + */ + AttCovEulerMsg last_attcoveuler_; + + /** + * @brief Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming + * INSNavGeod blocks need to be stored + */ + INSNavGeodMsg last_insnavgeod_; + + /** + * @brief Since LoclaizationEcef needs INSNavCart, incoming + * INSNavCart blocks need to be stored + */ + INSNavCartMsg last_insnavcart_; + + /** + * @brief Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks + * need to be stored + */ + ExtSensorMeasMsg last_extsensmeas_; + + /** + * @brief Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks + * need to be stored + */ + ChannelStatus last_channelstatus_; + + /** + * @brief Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks + * need to be stored + */ + MeasEpochMsg last_measepoch_; + + /** + * @brief Since GPSFix needs DOP, incoming DOP blocks need to be stored + */ + DOP last_dop_; + + /** + * @brief Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks + * need to be stored + */ + VelCovGeodeticMsg last_velcovgeodetic_; + + /** + * @brief Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus + * blocks need to be stored + */ + ReceiverStatus last_receiverstatus_; + + /** + * @brief Since DiagnosticArray needs QualityInd, incoming QualityInd blocks + * need to be stored + */ + QualityInd last_qualityind_; + + /** + * @brief Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup + * blocks need to be stored + */ + ReceiverSetup last_receiversetup_; + + //! Shorthand for the map responsible for matching PVTGeodetic's Mode field + //! to an enum value + typedef std::unordered_map TypeOfPVTMap; + + /** + * @brief All instances of the RxMessage class shall have access to the map + * without reinitializing it, hence static + */ + TypeOfPVTMap type_of_pvt_map; + + //! Shorthand for the map responsible for matching ROS message identifiers to + //! an enum value + typedef std::unordered_map RxIDMap; + + /** + * @brief All instances of the RxMessage class shall have access to the map + * without reinitializing it, hence static + * + * This map is for mapping ROS message, SBF and NMEA identifiers to an + * enumeration and makes the switch-case formalism in rx_message.hpp more + * explicit. + */ + RxIDMap rx_id_map; + + //! When reading from an SBF file, the ROS publishing frequency is governed + //! by the time stamps found in the SBF blocks therein. + Timestamp unix_time_; + + //! Current leap seconds as received, do not use value is -128 + int8_t current_leap_seconds_ = -128; + + //! For GPSFix: Whether the ChannelStatus block of the current epoch has + //! arrived or not + bool channelstatus_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the MeasEpoch block of the current epoch has arrived + //! or not + bool measepoch_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the DOP block of the current epoch has arrived or not + bool dop_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the PVTGeodetic block of the current epoch has + //! arrived or not + bool pvtgeodetic_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the PosCovGeodetic block of the current epoch has + //! arrived or not + bool poscovgeodetic_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the VelCovGeodetic block of the current epoch has + //! arrived or not + bool velcovgeodetic_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the AttEuler block of the current epoch has arrived + //! or not + bool atteuler_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the AttCovEuler block of the current epoch has + //! arrived or not + bool attcoveuler_has_arrived_gpsfix_ = false; + + //! For GPSFix: Whether the INSNavGeod block of the current epoch has arrived + //! or not + bool insnavgeod_has_arrived_gpsfix_ = false; + + //! For NavSatFix: Whether the PVTGeodetic block of the current epoch has + //! arrived or not + bool pvtgeodetic_has_arrived_navsatfix_ = false; + + //! For NavSatFix: Whether the PosCovGeodetic block of the current epoch has + //! arrived or not + bool poscovgeodetic_has_arrived_navsatfix_ = false; + + //! For NavSatFix: Whether the INSNavGeod block of the current epoch has + //! arrived or not + bool insnavgeod_has_arrived_navsatfix_ = false; + //! For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the + //! current epoch has arrived or not + bool pvtgeodetic_has_arrived_pose_ = false; + + //! For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the + //! current epoch has arrived or not + bool poscovgeodetic_has_arrived_pose_ = false; + + //! For PoseWithCovarianceStamped: Whether the AttEuler block of the current + //! epoch has arrived or not + bool atteuler_has_arrived_pose_ = false; + + //! For PoseWithCovarianceStamped: Whether the AttCovEuler block of the + //! current epoch has arrived or not + bool attcoveuler_has_arrived_pose_ = false; + + //! For PoseWithCovarianceStamped: Whether the INSNavGeod block of the + //! current epoch has arrived or not + bool insnavgeod_has_arrived_pose_ = false; + + //! For DiagnosticArray: Whether the ReceiverStatus block of the current + //! epoch has arrived or not + bool receiverstatus_has_arrived_diagnostics_ = false; + + //! For DiagnosticArray: Whether the QualityInd block of the current epoch + //! has arrived or not + bool qualityind_has_arrived_diagnostics_ = false; + + //! For Localization: Whether the INSNavGeod and INSNavCart blocks of the + //! current epoch have arrived or not + bool insnavgeod_has_arrived_localization_ = false; + bool insnavgeod_has_arrived_localization_ecef_ = false; + bool insnavcart_has_arrived_localization_ecef_ = false; + + /** + * @brief "Callback" function when constructing NavSatFix messages + * @return A smart pointer to the ROS message NavSatFix just created + */ + NavSatFixMsg NavSatFixCallback(); + + /** + * @brief "Callback" function when constructing GPSFix messages + * @return A smart pointer to the ROS message GPSFix just created + */ + GPSFixMsg GPSFixCallback(); + + /** + * @brief "Callback" function when constructing PoseWithCovarianceStamped + * messages + * @return A smart pointer to the ROS message PoseWithCovarianceStamped just + * created + */ + PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback(); + + /** + * @brief "Callback" function when constructing + * DiagnosticArrayMsg messages + * @return A ROS message + * DiagnosticArrayMsg just created + */ + DiagnosticArrayMsg DiagnosticArrayCallback(); + + /** + * @brief "Callback" function when constructing + * ImuMsg messages + * @return A ROS message + * ImuMsg just created + */ + ImuMsg ImuCallback(); + + /** + * @brief "Callback" function when constructing + * LocalizationMsg messages in UTM + * @return A ROS message + * LocalizationMsg just created + */ + LocalizationMsg LocalizationUtmCallback(); + + /** + * @brief "Callback" function when constructing + * LocalizationMsg messages in ECEF + * @return A ROS message + * LocalizationMsg just created + */ + LocalizationMsg LocalizationEcefCallback(); + + /** + * @brief function to fill twist part of LocalizationMsg + * @param[in] roll roll [rad] + * @param[in] pitch pitch [rad] + * @param[in] yaw yaw [rad] + * @param[inout] msg LocalizationMsg to be filled + */ + void fillLocalizationMsgTwist(double roll, double pitch, double yaw, + LocalizationMsg& msg); + + /** + * @brief "Callback" function when constructing + * TwistWithCovarianceStampedMsg messages + * @param[in] fromIns Wether to contruct message from INS data + * @return A ROS message + * TwistWithCovarianceStampedMsg just created + */ + TwistWithCovarianceStampedMsg TwistCallback(bool fromIns = false); + + /** + * @brief Waits according to time when reading from file + */ + void wait(Timestamp time_obj); + + /** + * @brief Wether all elements are true + */ + bool allTrue(std::vector& vec, uint32_t id); + + /** + * @brief Settings struct + */ + Settings* settings_; + + /** + * @brief Fixed UTM zone + */ + std::shared_ptr fixedUtmZone_; + + /** + * @brief Calculates the timestamp, in the Unix Epoch time format + * This is either done using the TOW as transmitted with the SBF block (if + * "use_gnss" is true), or using the current time. + * @param[in] data Pointer to the buffer + * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is + * used, otherwise the current time + * @return Timestamp object containing seconds and nanoseconds since last + * epoch + */ + Timestamp timestampSBF(const uint8_t* data, bool use_gnss_time); + + /** + * @brief Calculates the timestamp, in the Unix Epoch time format + * This is either done using the TOW as transmitted with the SBF block (if + * "use_gnss" is true), or using the current time. + * @param[in] tow (Time of Week) Number of milliseconds that elapsed since + * the beginning of the current GPS week as transmitted by the SBF block + * @param[in] wnc (Week Number Counter) counts the number of complete weeks + * elapsed since January 6, 1980 + * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is + * used, otherwise the current time + * @return Timestamp object containing seconds and nanoseconds since last + * epoch + */ + Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time); + }; +} // namespace io +#endif // for RX_MESSAGE_HPP \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/callback_handlers.cpp b/src/septentrio_gnss_driver/communication/callback_handlers.cpp deleted file mode 100644 index de079e24..00000000 --- a/src/septentrio_gnss_driver/communication/callback_handlers.cpp +++ /dev/null @@ -1,701 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include - -/** - * @file callback_handlers.cpp - * @date 22/08/20 - * @brief Handles callbacks when reading NMEA/SBF messages - */ - -std::pair gpsfix_pairs[] = { - std::make_pair("4013", 0), std::make_pair("4027", 1), std::make_pair("4001", 2), - std::make_pair("4007", 3), std::make_pair("5906", 4), std::make_pair("5908", 5), - std::make_pair("5938", 6), std::make_pair("5939", 7), std::make_pair("4226", 8)}; - -std::pair navsatfix_pairs[] = { - std::make_pair("4007", 0), std::make_pair("5906", 1), std::make_pair("4226", 2)}; - -std::pair pose_pairs[] = { - std::make_pair("4007", 0), std::make_pair("5906", 1), std::make_pair("5938", 2), - std::make_pair("5939", 3), std::make_pair("4226", 4)}; - -std::pair diagnosticarray_pairs[] = { - std::make_pair("4014", 0), std::make_pair("4082", 1)}; - -std::pair imu_pairs[] = {std::make_pair("4226", 0), - std::make_pair("4050", 1)}; - -std::pair localization_pairs[] = {std::make_pair("4226", 0)}; -std::pair localization_ecef_pairs[] = { - std::make_pair("4225", 0), std::make_pair("4226", 1)}; - -namespace io { - boost::mutex CallbackHandlers::callback_mutex_; - - CallbackHandlers::GPSFixMap CallbackHandlers::gpsfix_map(gpsfix_pairs, - gpsfix_pairs + 9); - CallbackHandlers::NavSatFixMap - CallbackHandlers::navsatfix_map(navsatfix_pairs, navsatfix_pairs + 3); - CallbackHandlers::PoseWithCovarianceStampedMap - CallbackHandlers::pose_map(pose_pairs, pose_pairs + 5); - CallbackHandlers::DiagnosticArrayMap - CallbackHandlers::diagnosticarray_map(diagnosticarray_pairs, - diagnosticarray_pairs + 2); - CallbackHandlers::ImuMap CallbackHandlers::imu_map(imu_pairs, imu_pairs + 2); - - CallbackHandlers::LocalizationMap - CallbackHandlers::localization_map(localization_pairs, - localization_pairs + 1); - - CallbackHandlers::LocalizationEcefMap - CallbackHandlers::localization_ecef_map(localization_ecef_pairs, - localization_ecef_pairs + 2); - - std::string CallbackHandlers::do_gpsfix_ = "4007"; - std::string CallbackHandlers::do_navsatfix_ = "4007"; - std::string CallbackHandlers::do_pose_ = "4007"; - std::string CallbackHandlers::do_diagnostics_ = "4014"; - - std::string CallbackHandlers::do_insgpsfix_ = "4226"; - std::string CallbackHandlers::do_insnavsatfix_ = "4226"; - std::string CallbackHandlers::do_inspose_ = "4226"; - std::string CallbackHandlers::do_imu_ = "4226"; - std::string CallbackHandlers::do_inslocalization_ = "4226"; - std::string CallbackHandlers::do_inslocalization_ecef_ = "4226"; - - //! The for loop forwards to a ROS message specific handle if the latter was - //! added via callbackmap_.insert at some earlier point. - void CallbackHandlers::handle() - { - // Find the ROS message callback handler for the equivalent Rx message - // (SBF/NMEA) at hand & call it - boost::mutex::scoped_lock lock(callback_mutex_); - CallbackMap::key_type key = rx_message_.messageID(); - std::string ID_temp = rx_message_.messageID(); - if (!(ID_temp == "4013" || ID_temp == "4001" || ID_temp == "4014" || - ID_temp == "4082")) - // We only want to handle ChannelStatus, MeasEpoch, DOP, ReceiverStatus, - // QualityInd blocks in case GPSFix and DiagnosticArray - // messages are to be published, respectively, see few lines below. - { - for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - - // Call NavSatFix callback function if it was added for GNSS - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_navsatfix) - { - CallbackMap::key_type key = "NavSatFix"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_navsatfix_) - // The last incoming block PVTGeodetic triggers - // the publishing of NavSatFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_navsatfix_ = std::string(); - } - } - } - // Call NavSatFix callback function if it was added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_navsatfix) - { - CallbackMap::key_type key = "INSNavSatFix"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_insnavsatfix_) - // The last incoming block INSNavGeod triggers - // the publishing of NavSatFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_insnavsatfix_ = std::string(); - } - } - } - // Call PoseWithCovarianceStampedMsg callback function if it was - // added for GNSS - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_pose) - { - CallbackMap::key_type key = "PoseWithCovarianceStamped"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_pose_) - // The last incoming block among PVTGeodetic, PosCovGeodetic, - // AttEuler and AttCovEuler triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_pose_ = std::string(); - } - } - } - // Call PoseWithCovarianceStampedMsg callback function if it was - // added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_pose) - { - CallbackMap::key_type key = "INSPoseWithCovarianceStamped"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_inspose_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_inspose_ = std::string(); - } - } - } - // Call DiagnosticArrayMsg callback function if it was added - // for the both type of receivers - if (settings_->publish_diagnostics) - { - CallbackMap::key_type key1 = rx_message_.messageID(); - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == "4014" || ID_temp == "4082" || ID_temp == "5902") - { - for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - CallbackMap::key_type key2 = "DiagnosticArray"; - if (ID_temp == do_diagnostics_) - // The last incoming block among ReceiverStatus, QualityInd and - // ReceiverSetup triggers the publishing of DiagnosticArray. - { - for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2); - callback != callbackmap_.upper_bound(key2); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_diagnostics_ = std::string(); - } - } - // Call ImuMsg callback function if it was - // added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_imu) - { - CallbackMap::key_type key = "Imu"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_imu_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_imu_ = std::string(); - } - } - } - // Call LocalizationMsg callback function if it was - // added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_localization || settings_->publish_tf) - { - CallbackMap::key_type key = "Localization"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_inslocalization_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_inslocalization_ = std::string(); - } - } - if (settings_->publish_localization_ecef || settings_->publish_tf_ecef) - { - CallbackMap::key_type key = "LocalizationEcef"; - std::string ID_temp = rx_message_.messageID(); - if (ID_temp == do_inslocalization_ecef_) - // The last incoming block INSNavGeod triggers the publishing of - // PoseWithCovarianceStamped. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key); - callback != callbackmap_.upper_bound(key); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_inslocalization_ecef_ = std::string(); - } - } - } - // Call TimeReferenceMsg (with GPST) callback function if it was - // added - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpst) - { - CallbackMap::key_type key1 = "GPST"; - std::string ID_temp = rx_message_.messageID(); - // If no new PVTGeodetic block is coming in, there is no need to - // publish TimeReferenceMsg (with GPST) anew. - if (ID_temp == "4007") - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpst) - { - CallbackMap::key_type key1 = "GPST"; - std::string ID_temp = rx_message_.messageID(); - // If no new INSNavGeod block is coming in, there is no need to - // publish TimeReferenceMsg (with GPST) anew. - if (ID_temp == "4226") - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - } - } - // Call GPSFix callback function if it was added for GNSS - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpsfix) - { - std::string ID_temp = rx_message_.messageID(); - CallbackMap::key_type key1 = rx_message_.messageID(); - if (ID_temp == "4013" || ID_temp == "4001") - // Even though we are not interested in publishing ChannelStatus - // (4013) and DOP (4001) ROS messages, we have to save some contents - // of these incoming blocks in order to publish the GPSFix message. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - CallbackMap::key_type key2 = "GPSFix"; - if (ID_temp == do_gpsfix_) - // The last incoming block among ChannelStatus (4013), MeasEpoch - // (4027), and DOP (4001) triggers the publishing of GPSFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key2); - callback != callbackmap_.upper_bound(key2); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_gpsfix_ = std::string(); - } - } - } - // Call GPSFix callback function if it was added for INS - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpsfix) - { - std::string ID_temp = rx_message_.messageID(); - CallbackMap::key_type key1 = rx_message_.messageID(); - if (ID_temp == "4013" || ID_temp == "4001") - // Even though we are not interested in publishing ChannelStatus - // (4013) and DOP (4001) ROS messages, we have to save some contents - // of these incoming blocks in order to publish the GPSFix message. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key1); - callback != callbackmap_.upper_bound(key1); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - } - CallbackMap::key_type key2 = "INSGPSFix"; - if (ID_temp == do_insgpsfix_) - // The last incoming block among ChannelStatus (4013), MeasEpoch - // (4027) and DOP (4001) triggers the publishing of INSGPSFix. - { - for (CallbackMap::iterator callback = - callbackmap_.lower_bound(key2); - callback != callbackmap_.upper_bound(key2); ++callback) - { - try - { - callback->second->handle(rx_message_, callback->first); - } catch (std::runtime_error& e) - { - throw std::runtime_error(e.what()); - } - } - do_insgpsfix_ = std::string(); - } - } - } - } - - void CallbackHandlers::readCallback(Timestamp recvTimestamp, const uint8_t* data, - std::size_t& size) - { - rx_message_.newData(recvTimestamp, data, size); - // Read !all! (there might be many) messages in the buffer - while (rx_message_.search() != rx_message_.getEndBuffer() && - rx_message_.found()) - { - // Print the found message (if NMEA) or just show messageID (if SBF).. - if (rx_message_.isSBF()) - { - std::size_t sbf_block_length; - std::string ID_temp = rx_message_.messageID(); - sbf_block_length = - static_cast(rx_message_.getBlockLength()); - node_->log(LogLevel::DEBUG, - "ROSaic reading SBF block " + ID_temp + " made up of " + - std::to_string(sbf_block_length) + " bytes..."); - // If full message did not yet arrive, throw an error message. - if (sbf_block_length > rx_message_.getCount()) - { - node_->log( - LogLevel::DEBUG, - "Not a valid SBF block, parts of the SBF block are yet to be received. Ignore.."); - throw( - static_cast(rx_message_.getPosBuffer() - data)); - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_gpsfix == true && - (ID_temp == "4013" || ID_temp == "4027" || - ID_temp == "4001" || ID_temp == "4007" || - ID_temp == "5906" || ID_temp == "5908" || - ID_temp == "5938" || ID_temp == "5939")) - { - if (rx_message_.gnss_gpsfix_complete(gpsfix_map[ID_temp])) - { - do_gpsfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_gpsfix == true && - (ID_temp == "4013" || ID_temp == "4027" || - ID_temp == "4001" || ID_temp == "4226")) - { - if (rx_message_.ins_gpsfix_complete(gpsfix_map[ID_temp])) - { - do_insgpsfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_navsatfix == true && - (ID_temp == "4007" || ID_temp == "5906")) - { - if (rx_message_.gnss_navsatfix_complete( - navsatfix_map[ID_temp])) - { - do_navsatfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_navsatfix == true && (ID_temp == "4226")) - { - if (rx_message_.ins_navsatfix_complete( - navsatfix_map[ID_temp])) - { - do_insnavsatfix_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - if (settings_->publish_pose == true && - (ID_temp == "4007" || ID_temp == "5906" || - ID_temp == "5938" || ID_temp == "5939")) - { - if (rx_message_.gnss_pose_complete(pose_map[ID_temp])) - { - do_pose_ = ID_temp; - } - } - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_pose == true && (ID_temp == "4226")) - { - if (rx_message_.ins_pose_complete(pose_map[ID_temp])) - { - do_inspose_ = ID_temp; - } - } - } - if (settings_->publish_diagnostics == true && - (ID_temp == "4014" || ID_temp == "4082")) - { - if (rx_message_.diagnostics_complete( - diagnosticarray_map[ID_temp])) - { - do_diagnostics_ = ID_temp; - } - } - if ((settings_->publish_localization || settings_->publish_tf) && - (ID_temp == "4226")) - { - if (rx_message_.ins_localization_complete( - localization_map[ID_temp])) - { - do_inslocalization_ = ID_temp; - } - } - if ((settings_->publish_localization_ecef || - settings_->publish_tf_ecef) && - (ID_temp == "4226" || ID_temp == "4225")) - { - if (rx_message_.ins_localization_ecef_complete( - localization_ecef_map[ID_temp])) - { - do_inslocalization_ecef_ = ID_temp; - } - } - } - if (rx_message_.isNMEA()) - { - boost::char_separator sep("\r"); // Carriage Return (CR) - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = rx_message_.messageSize(); - // Syntax: new_string_name (const char* s, size_t n); size_t is - // either 2 or 8 bytes, depending on your system - std::string block_in_string( - reinterpret_cast(rx_message_.getPosBuffer()), - nmea_size); - tokenizer tokens(block_in_string, sep); - node_->log(LogLevel::DEBUG, - "The NMEA message contains " + std::to_string(nmea_size) + - " bytes and is ready to be parsed. It reads: " + - *tokens.begin()); - } - if (rx_message_.isResponse()) // If the response is not sent at once, - // only first part is ROS_DEBUG-printed - { - std::size_t response_size = rx_message_.messageSize(); - std::string block_in_string( - reinterpret_cast(rx_message_.getPosBuffer()), - response_size); - node_->log(LogLevel::DEBUG, "The Rx's response contains " + - std::to_string(response_size) + - " bytes and reads:\n " + - block_in_string); - { - boost::mutex::scoped_lock lock(g_response_mutex); - g_response_received = true; - lock.unlock(); - g_response_condition.notify_one(); - } - if (rx_message_.isErrorMessage()) - { - node_->log( - LogLevel::ERROR, - "Invalid command just sent to the Rx! The Rx's response contains " + - std::to_string(response_size) + " bytes and reads:\n " + - block_in_string); - } - continue; - } - if (rx_message_.isConnectionDescriptor()) - { - std::string cd( - reinterpret_cast(rx_message_.getPosBuffer()), 4); - g_rx_tcp_port = cd; - if (g_cd_count == 0) - { - node_->log( - LogLevel::INFO, - "The connection descriptor for the TCP connection is " + cd); - } - if (g_cd_count < 3) - ++g_cd_count; - if (g_cd_count == 2) - { - boost::mutex::scoped_lock lock(g_cd_mutex); - g_cd_received = true; - lock.unlock(); - g_cd_condition.notify_one(); - } - continue; - } - try - { - handle(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "Incomplete message: " + std::string(e.what())); - throw(static_cast(rx_message_.getPosBuffer() - data)); - } - } - } -} // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/circular_buffer.cpp b/src/septentrio_gnss_driver/communication/circular_buffer.cpp deleted file mode 100644 index 2699fbb2..00000000 --- a/src/septentrio_gnss_driver/communication/circular_buffer.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include - -/** - * @file circular_buffer.cpp - * @brief Defines a class for creating, writing and reading from a circular bufffer - * @date 25/09/20 - */ - -CircularBuffer::CircularBuffer(ROSaicNodeBase* node, std::size_t capacity) : - node_(node), head_(0), tail_(0), size_(0), capacity_(capacity) -{ - data_ = new uint8_t[capacity]; -} - -//! The destructor frees memory (first line) and points the dangling pointer to NULL -//! (second line). -CircularBuffer::~CircularBuffer() -{ - delete[] data_; - data_ = NULL; -} - -std::size_t CircularBuffer::write(const uint8_t* data, std::size_t bytes) -{ - if (bytes == 0) - return 0; - - std::size_t capacity = capacity_; - std::size_t bytes_to_write = std::min(bytes, capacity - size_); - if (bytes_to_write != bytes) - { - node_->log( - LogLevel::ERROR, - "You are trying to overwrite parts of the circular buffer that have not yet been read!"); - } - - // Writes in a single step - if (bytes_to_write <= capacity - head_) - { - - memcpy(data_ + head_, data, bytes_to_write); - head_ += bytes_to_write; - if (head_ == capacity) - head_ = 0; - } - // Writes in two steps. Here the circular nature comes to the surface - else - { - std::size_t size_1 = capacity - head_; - memcpy(data_ + head_, data, size_1); - std::size_t size_2 = bytes_to_write - size_1; - memcpy(data_, data + size_1, size_2); - head_ = - size_2; // Hence setting head_ = 0 three lines above was not necessary. - } - size_ += bytes_to_write; - return bytes_to_write; -} - -std::size_t CircularBuffer::read(uint8_t* data, std::size_t bytes) -{ - if (bytes == 0) - return 0; - std::size_t capacity = capacity_; - std::size_t bytes_to_read = std::min(bytes, size_); - if (bytes_to_read != bytes) - { - node_->log( - LogLevel::ERROR, - "You are trying to read parts of the circular buffer that have not yet been written!"); - } - - // Read in a single step - if (bytes_to_read <= - capacity - tail_) // Note that it is not size_ - tail_: - // If write() hasn't written something into all of capacity - // yet (first round of writing), we would still read those - // unknown bytes.. - { - memcpy(data, data_ + tail_, bytes_to_read); - tail_ += bytes_to_read; - if (tail_ == capacity) - tail_ = 0; // Same here? - } - - // Read in two steps - else - { - std::size_t size_1 = capacity - tail_; - memcpy(data, data_ + tail_, size_1); - std::size_t size_2 = bytes_to_read - size_1; - memcpy(data + size_1, data_, size_2); - tail_ = size_2; - } - - size_ -= bytes_to_read; - return bytes_to_read; -} \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 18bd6fbc..6fb69f3f 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -99,13 +99,13 @@ */ //! Mutex to control changes of global variable "g_response_received" -boost::mutex g_response_mutex; +std::mutex g_response_mutex; //! Determines whether a command reply was received from the Rx bool g_response_received; //! Condition variable complementing "g_response_mutex" boost::condition_variable g_response_condition; //! Mutex to control changes of global variable "g_cd_received" -boost::mutex g_cd_mutex; +std::mutex g_cd_mutex; //! Determines whether the connection descriptor was received from the Rx bool g_cd_received; //! Condition variable complementing "g_cd_mutex" @@ -120,7 +120,7 @@ std::string g_rx_tcp_port; uint32_t g_cd_count; io::CommIo::CommIo(ROSaicNodeBase* node, Settings* settings) : - node_(node), handlers_(node, settings), settings_(settings), stopping_(false) + node_(node), handlers_(node, settings), settings_(settings), running_(true) { g_response_received = false; g_cd_received = false; @@ -184,120 +184,11 @@ io::CommIo::~CommIo() send("logout \x0D"); } - stopping_ = true; + running_ = false; connectionThread_.join(); } -void io::CommIo::resetMainPort() -{ - // It is imperative to hold a lock on the mutex "g_cd_mutex" while - // modifying the variable and "g_cd_received". - boost::mutex::scoped_lock lock_cd(g_cd_mutex); - // Escape sequence (escape from correction mode), ensuring that we can send - // our real commands afterwards... - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); - manager_.get()->send(cmd); - // We wait for the connection descriptor before we send another command, - // otherwise the latter would not be processed. - g_cd_condition.wait(lock_cd, []() { return g_cd_received; }); - g_cd_received = false; -} - void io::CommIo::initializeIo() -{ - node_->log(LogLevel::DEBUG, "Called initializeIo() method"); - boost::smatch match; - // In fact: smatch is a typedef of match_results - if (boost::regex_match(settings_->device, match, - boost::regex("(tcp)://(.+):(\\d+)"))) - // C++ needs \\d instead of \d: Both mean decimal. - // Note that regex_match can be used with a smatch object to store results, or - // without. In any case, true is returned if and only if it matches the - // !complete! string. - { - // The first sub_match (index 0) contained in a match_result always - // represents the full match within a target sequence made by a regex, and - // subsequent sub_matches represent sub-expression matches corresponding in - // sequence to the left parenthesis delimiting the sub-expression in the - // regex, i.e. $n Perl is equivalent to m[n] in boost regex. - tcp_host_ = match[2]; - tcp_port_ = match[3]; - - serial_ = false; - connectionThread_ = boost::thread(boost::bind(&CommIo::connect, this)); - } else if (boost::regex_match(settings_->device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) - { - serial_ = false; - settings_->read_from_sbf_log = true; - settings_->use_gnss_time = true; - connectionThread_ = boost::thread( - boost::bind(&CommIo::prepareSBFFileReading, this, match[2])); - - } else if (boost::regex_match( - settings_->device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) - { - serial_ = false; - settings_->read_from_pcap = true; - settings_->use_gnss_time = true; - connectionThread_ = boost::thread( - boost::bind(&CommIo::preparePCAPFileReading, this, match[2])); - - } else if (boost::regex_match(settings_->device, match, - boost::regex("(serial):(.+)"))) - { - serial_ = true; - std::string proto(match[2]); - std::stringstream ss; - ss << "Searching for serial port" << proto; - settings_->device = proto; - node_->log(LogLevel::DEBUG, ss.str()); - connectionThread_ = boost::thread(boost::bind(&CommIo::connect, this)); - } else - { - std::stringstream ss; - ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; - node_->log(LogLevel::ERROR, ss.str()); - } - node_->log(LogLevel::DEBUG, "Leaving initializeIo() method"); -} - -void io::CommIo::prepareSBFFileReading(std::string file_name) -{ - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from" << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializeSBFFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "CommIo::initializeSBFFileReading() failed for SBF File" << file_name - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } -} - -void io::CommIo::preparePCAPFileReading(std::string file_name) -{ - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializePCAPFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "CommIO::initializePCAPFileReading() failed for SBF File " << file_name - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } -} - -void io::CommIo::connect() { node_->log(LogLevel::DEBUG, "Called connect() method"); node_->log( @@ -307,10 +198,11 @@ void io::CommIo::connect() boost::asio::io_service io; boost::posix_time::millisec wait_ms( static_cast(settings_->reconnect_delay_s * 1000)); - while (!connected_ && !stopping_) + while (running_) { boost::asio::deadline_timer t(io, wait_ms); - reconnect(); + if (manager_->connect()) + break; t.wait(); } node_->log(LogLevel::DEBUG, "Successully connected. Leaving connect() method"); @@ -328,7 +220,7 @@ void io::CommIo::configureRx() node_->log(LogLevel::DEBUG, "Called configureRx() method"); { // wait for connection - boost::mutex::scoped_lock lock(connection_mutex_); + std::mutex::scoped_lock lock(connection_mutex_); connection_condition_.wait(lock, [this]() { return connected_; }); } @@ -916,21 +808,60 @@ void io::CommIo::configureRx() node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); } -void io::CommIo::send(const std::string& cmd) +void io::CommIo::sendVelocity(const std::string& velNmea) { - // It is imperative to hold a lock on the mutex "g_response_mutex" while - // modifying the variable "g_response_received". - boost::mutex::scoped_lock lock(g_response_mutex); - // Determine byte size of cmd and hand over to send() method of manager_ + if (nmeaActivated_) + manager_.get()->send(velNmea); +} + +void io::CommIo::resetMainPort() +{ + CommSync* cdSync = messageHandler.getCdSync(); + // It is imperative to hold a lock on the mutex "g_cd_mutex" while + // modifying the variable and "g_cd_received". + std::mutex::scoped_lock lock_cd(cdSync.mutex); + // Escape sequence (escape from correction mode), ensuring that we can send + // our real commands afterwards... + std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); manager_.get()->send(cmd); - g_response_condition.wait(lock, []() { return g_response_received; }); - g_response_received = false; + // We wait for the connection descriptor before we send another command, + // otherwise the latter would not be processed. + cdSync.condition.wait(lock_cd, [cdSync]() { return cdSync.received; }); + cdSync.received = false; } -void io::CommIo::sendVelocity(const std::string& velNmea) +void io::CommIo::prepareSBFFileReading(std::string file_name) { - if (nmeaActivated_) - manager_.get()->send(velNmea); + try + { + std::stringstream ss; + ss << "Setting up everything needed to read from" << file_name; + node_->log(LogLevel::DEBUG, ss.str()); + initializeSBFFileReading(file_name); + } catch (std::runtime_error& e) + { + std::stringstream ss; + ss << "CommIo::initializeSBFFileReading() failed for SBF File" << file_name + << " due to: " << e.what(); + node_->log(LogLevel::ERROR, ss.str()); + } +} + +void io::CommIo::preparePCAPFileReading(std::string file_name) +{ + try + { + std::stringstream ss; + ss << "Setting up everything needed to read from " << file_name; + node_->log(LogLevel::DEBUG, ss.str()); + initializePCAPFileReading(file_name); + } catch (std::runtime_error& e) + { + std::stringstream ss; + ss << "CommIO::initializePCAPFileReading() failed for SBF File " << file_name + << " due to: " << e.what(); + node_->log(LogLevel::ERROR, ss.str()); + } } void io::CommIo::initializeSBFFileReading(std::string file_name) @@ -959,7 +890,7 @@ void io::CommIo::initializeSBFFileReading(std::string file_name) ss << "Opened and copied over from " << file_name; node_->log(LogLevel::DEBUG, ss.str()); - while (!stopping_) // Loop will stop if we are done reading the SBF file + while (running_) // Loop will stop if we are done reading the SBF file { try { @@ -1009,7 +940,7 @@ void io::CommIo::initializePCAPFileReading(std::string file_name) uint8_t* to_be_parsed = new uint8_t[buffer_size]; to_be_parsed = vec_buf.data(); - while (!stopping_) // Loop will stop if we are done reading the SBF file + while (running_) // Loop will stop if we are done reading the SBF file { try { @@ -1039,4 +970,16 @@ void io::CommIo::initializePCAPFileReading(std::string file_name) to_be_parsed = to_be_parsed + buffer_size; } node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method.."); +} + +void io::CommIo::send(const std::string& cmd) +{ + CommSync* responseSync = messageHandler.getResponseSync(); + // It is imperative to hold a lock on the mutex "g_response_mutex" while + // modifying the variable "g_response_received". + std::mutex::scoped_lock lock(responseSync.mutex); + // Determine byte size of cmd and hand over to send() method of manager_ + manager_.get()->send(cmd); + responseSync.condition.wait(lock, []() { return responseSync.received; }); + responseSync.received = false; } \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp new file mode 100644 index 00000000..27ac171a --- /dev/null +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -0,0 +1,105 @@ +// ***************************************************************************** +// +// © Copyright 2020, Septentrio NV/SA. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +/** + * @file message_handler.cpp + * @date 22/08/20 + * @brief Handles callbacks when reading NMEA/SBF messages + */ + +namespace io { + + void MessageHandler::handle() {} + + void MessageHandler::handleSbf(const Message& message) {} + + void MessageHandler::handleNmea(const Message& message) + { + boost::char_separator sep("\r"); // Carriage Return (CR) + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = rx_message_.messageSize(); + // Syntax: new_string_name (const char* s, size_t n); size_t is + // either 2 or 8 bytes, depending on your system + std::string block_in_string( + reinterpret_cast(rx_message_.getPosBuffer()), nmea_size); + tokenizer tokens(block_in_string, sep); + node_->log( + LogLevel::DEBUG, + "The NMEA message contains " + std::to_string(nmea_size) + + " bytes and is ready to be parsed. It reads: " + *tokens.begin()); + } + + void MessageHandler::handleResponse(const Message& message) + { + std::size_t response_size = rx_message_.messageSize(); + std::string block_in_string( + reinterpret_cast(rx_message_.getPosBuffer()), + response_size); + node_->log(LogLevel::DEBUG, "The Rx's response contains " + + std::to_string(response_size) + + " bytes and reads:\n " + block_in_string); + { + std::mutex::scoped_lock lock(response_sync.mutex); + response_sync.received = true; + lock.unlock(); + response_sync.condition.notify_one(); + } + if (rx_message_.isErrorMessage()) + { + node_->log( + LogLevel::ERROR, + "Invalid command just sent to the Rx! The Rx's response contains " + + std::to_string(response_size) + " bytes and reads:\n " + + block_in_string); + } + } + + void MessageHandler::handleCd(const Message& message) + { + std::string cd(reinterpret_cast(rx_message_.getPosBuffer()), 4); + rx_tcp_port = cd; + if (cd_count == 0) + { + node_->log(LogLevel::INFO, + "The connection descriptor for the TCP connection is " + cd); + } + if (cd_count < 3) + ++cd_count; + if (cd_count == 2) + { + std::mutex::scoped_lock lock(cd_sync.mutex); + cd_sync.received = true; + lock.unlock(); + cd_sync.condition.notify_one(); + }; + } +} // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/message_parser.cpp similarity index 100% rename from src/septentrio_gnss_driver/communication/rx_message.cpp rename to src/septentrio_gnss_driver/communication/message_parser.cpp From 5527e63afeb51cf2d6586bef14e52768dc8e4704 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 12 Jan 2023 12:18:03 +0100 Subject: [PATCH 017/170] Refactor message parser, WIP --- .../abstraction/typedefs.hpp | 2 + .../communication/async_manager.hpp | 182 +- .../communication/communication_core.hpp | 8 +- .../communication/message_parser.hpp | 474 +- .../communication/rx_message.hpp | 793 --- .../{message.hpp => telegram.hpp} | 19 +- ...ssage_handler.hpp => telegram_handler.hpp} | 34 +- .../communication/communication_core.cpp | 94 +- .../communication/message_parser.cpp | 5162 ++++++++--------- ...ssage_handler.cpp => telegram_handler.cpp} | 16 +- 10 files changed, 2679 insertions(+), 4105 deletions(-) delete mode 100644 include/septentrio_gnss_driver/communication/rx_message.hpp rename include/septentrio_gnss_driver/communication/{message.hpp => telegram.hpp} (87%) rename include/septentrio_gnss_driver/communication/{message_handler.hpp => telegram_handler.hpp} (87%) rename src/septentrio_gnss_driver/communication/{message_handler.cpp => telegram_handler.cpp} (88%) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index c8ad3a64..99d41974 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -178,6 +178,8 @@ class ROSaicNodeBase virtual ~ROSaicNodeBase() {} + Settings* getSettings(){return &settings_}; + void registerSubscriber() { ros::NodeHandle nh; diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 601715dd..e258e992 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -112,9 +112,9 @@ namespace io { * @param[in] buffer_size Size of the circular buffer in bytes */ AsyncManager(ROSaicNodeBase* node, std::unique_ptr stream, - MessageQueue* messageQueue) : + TelegramQueue* telegramQueue) : node_(node), - stream_(std::move(stream)), messageQueue_(messageQueue) + stream_(std::move(stream)), telegramQueue_(telegramQueue) { } @@ -271,7 +271,7 @@ namespace io { void resync() { - message_.reset(new Message); + message_.reset(new Telegram); readSync<0>(); } @@ -289,104 +289,114 @@ namespace io { { if (numBytes == 1) { - switch (index) + if (message_->data[index] == SYNC_0) { - case 0: - { - if (message_->data[index] == SYNC_0) - { - message_->stamp = stamp; - readSync<1>(); - } else - readSync<0>(); - break; - } - case 1: - { - switch (message_->data[index]) - { - case SBF_SYNC_BYTE_1: - { - message_->type = SBF; - readSbfHeader(); - break; - } - case NMEA_SYNC_BYTE_1: - { - message_->type = NMEA; - readSync<2>(); - break; - } - case RESPONSE_SYNC_BYTE_1: - { - smessage_->type = RESPONSE; - readSync<2>(); - break; - } - case CONNECTION_DESCRIPTOR_BYTE_1: - { - message_->type = CONNECTION_DESCRIPTOR; - readSync<2>(); - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync 1 read fault, should never come here."); - resync(); - break; - } - } - break; - } - case 2: + message_->stamp = stamp; + readSync<1>(); + } else { - switch (message_->data[index]) + switch (index) { - case NMEA_SYNC_BYTE_2: + case 0: { - if (message_->type == NMEA) - readString(); - else - resync(); + readSync<0>(); break; } - case RESPONSE_SYNC_BYTE_1: + case 1: { - if (message_->type == RESPONSE) - readString(); - else + switch (message_->data[index]) + { + case SBF_SYNC_BYTE_1: + { + message_->type = SBF; + readSbfHeader(); + break; + } + case NMEA_SYNC_BYTE_1: + { + message_->type = NMEA; + readSync<2>(); + break; + } + case RESPONSE_SYNC_BYTE_1: + { + message_->type = RESPONSE; + readSync<2>(); + break; + } + case CONNECTION_DESCRIPTOR_BYTE_1: + { + message_->type = CONNECTION_DESCRIPTOR; + readSync<2>(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 1 read fault, should never come here."); resync(); + break; + } + } break; } - case CONNECTION_DESCRIPTOR_BYTE_1: + case 2: { - if (message_->type == CONNECTION_DESCRIPTOR) - readString(); - else + switch (message_->data[index]) + { + case NMEA_SYNC_BYTE_2: + { + if (message_->type == NMEA) + readString(); + else + resync(); + break; + } + case ERROR_SYNC_BYTE_2: + { + if (message_->type == ERROR) + readString(); + else + resync(); + break; + } + case RESPONSE_SYNC_BYTE_2: + { + if (message_->type == RESPONSE) + readString(); + else + resync(); + break; + } + case CONNECTION_DESCRIPTOR_BYTE_2: + { + if (message_->type == CONNECTION_DESCRIPTOR) + readString(); + else + resync(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 2 read fault, should never come here."); resync(); + break; + } + } break; } default: { node_->log( LogLevel::DEBUG, - "AsyncManager sync 2 read fault, should never come here."); + "AsyncManager sync read fault, should never come here."); resync(); break; } } - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync read fault, should never come here."); - resync(); - break; - } } } else { @@ -464,7 +474,7 @@ namespace io { message_->sbfId = parsing_utilities::getId(message_->data.data()); - messageQueue_->push(message_); + telegramQueue_->push(message_); } } else { @@ -503,7 +513,7 @@ namespace io { { case SYNC_0: { - message_.reset(new Message); + message_.reset(new Telegram); message_->data[0] = byte; message_->stamp = node_->getTime(); node_->log( @@ -516,7 +526,7 @@ namespace io { { message_->data.push_back(byte); if (message_->data[message_->data.size() - 2] == CR) - messageQueue_->push(message_); + telegramQueue_->push(message_); resync(); break; } @@ -556,9 +566,9 @@ namespace io { std::thread watchdogThread_; //! Timestamp of receiving buffer Timestamp recvStamp_; - //! Message - std::shared_ptr message_; - //! MessageQueue - MessageQueue* messageQueue_; + //! Telegram + std::shared_ptr message_; + //! TelegramQueue + TelegramQueue* telegramQueue_; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 89c204c9..542a6074 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -104,7 +104,7 @@ namespace io { * @brief Constructor of the class CommIo * @param[in] node Pointer to node */ - CommIo(ROSaicNodeBase* node, Settings* settings); + CommIo(ROSaicNodeBase* node); /** * @brief Default destructor of the class CommIo */ @@ -172,9 +172,9 @@ namespace io { ROSaicNodeBase* node_; //! Settings Settings* settings_; - //! MessageQueue - MessageQueue* messageQueue_; - //! Message handlers for the inwards streaming messages + //! TelegramQueue + TelegramQueue* telegramQueue_; + //! Telegram handlers for the inwards streaming messages MessageHandler messageHandler_; //! Whether connecting to Rx was successful bool connected_ = false; diff --git a/include/septentrio_gnss_driver/communication/message_parser.hpp b/include/septentrio_gnss_driver/communication/message_parser.hpp index bf2f1448..6659c0b2 100644 --- a/include/septentrio_gnss_driver/communication/message_parser.hpp +++ b/include/septentrio_gnss_driver/communication/message_parser.hpp @@ -56,50 +56,6 @@ // // ***************************************************************************** -//! 0x24 is ASCII for $ - 1st byte in each message -#ifndef NMEA_SYNC_BYTE_1 -#define NMEA_SYNC_BYTE_1 0x24 -#endif -//! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message -#ifndef NMEA_SYNC_BYTE_2_1 -#define NMEA_SYNC_BYTE_2_1 0x47 -#endif -//! 0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message -#ifndef NMEA_SYNC_BYTE_2_2 -#define NMEA_SYNC_BYTE_2_2 0x50 -#endif -//! 0x24 is ASCII for $ - 1st byte in each response from the Rx -#ifndef RESPONSE_SYNC_BYTE_1 -#define RESPONSE_SYNC_BYTE_1 0x24 -#endif -//! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx -#ifndef RESPONSE_SYNC_BYTE_2 -#define RESPONSE_SYNC_BYTE_2 0x52 -#endif -//! 0x0D is ASCII for "Carriage Return", i.e. "Enter" -#ifndef CARRIAGE_RETURN -#define CARRIAGE_RETURN 0x0D -#endif -//! 0x0A is ASCII for "Line Feed", i.e. "New Line" -#ifndef LINE_FEED -#define LINE_FEED 0x0A -#endif -//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the -//! command was invalid -#ifndef RESPONSE_SYNC_BYTE_3 -#define RESPONSE_SYNC_BYTE_3 0x3F -#endif -//! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after -//! initiating TCP connection -#ifndef CONNECTION_DESCRIPTOR_BYTE_1 -#define CONNECTION_DESCRIPTOR_BYTE_1 0x49 -#endif -//! 0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after -//! initiating TCP connection -#ifndef CONNECTION_DESCRIPTOR_BYTE_2 -#define CONNECTION_DESCRIPTOR_BYTE_2 0x50 -#endif - // C++ libraries #include // for assert #include @@ -124,7 +80,6 @@ /** * @file rx_message.hpp - * @date 20/08/20 * @brief Defines a class that reads messages handed over from the circular buffer */ @@ -147,228 +102,69 @@ enum TypeOfPVT_Enum evPPP }; -//! Since switch only works with int (yet NMEA message IDs are strings), we need -//! enum. Note drawbacks: No variable can have a name which is already in some -//! enumeration, enums are not type safe etc.. -enum RxID_Enum +enum SbfId { - evNavSatFix, - evINSNavSatFix, - evGPSFix, - evINSGPSFix, - evPoseWithCovarianceStamped, - evINSPoseWithCovarianceStamped, - evGPGGA, - evGPRMC, - evGPGSA, - evGPGSV, - evGLGSV, - evGAGSV, - evPVTCartesian, - evPVTGeodetic, - evBaseVectorCart, - evBaseVectorGeod, - evPosCovCartesian, - evPosCovGeodetic, - evAttEuler, - evAttCovEuler, - evINSNavCart, - evINSNavGeod, - evIMUSetup, - evVelSensorSetup, - evExtEventINSNavGeod, - evExtEventINSNavCart, - evExtSensorMeas, - evGPST, - evChannelStatus, - evMeasEpoch, - evDOP, - evVelCovGeodetic, - evDiagnosticArray, - evLocalization, - evLocalizationEcef, - evReceiverStatus, - evQualityInd, - evReceiverTime, - evReceiverSetup + PVT_CARTESIAN = 4006, + PVT_GEODETIC = 4007, + BASE_VECTOR_CART = 4043, + BASE_VECTOR_GEOD = 4028, + POS_COV_CARTESIAN = 5905, + POS_COV_GEODETIC = 5906, + ATT_EULER = 5938, + ATT_COV_EULER = 5939, + CHANNEL_STATUS = 4013, + MEAS_EPOCH = 4027, + DOP = 4001, + VEL_COV_GEODETIC = 5908, + RECEIVER_STATUS = 4014, + QUALITY_IND = 4082, + RECEIVER_SETUP = 5902, + INS_NAV_CART = 4225, + INS_NAV_GEOD = 4226, + EXT_EVENT_INS_NAV_GEOD = 4230, + EXT_EVENT_INS_NAV_CART = 4229, + IMU_SETUP = 4224, + VEL_SENSOR_SETUP = 4244, + EXT_SENSOR_MEAS = 4050, + RECEIVER_TIME = 5914 }; namespace io { /** - * @class RxMessage + * @class MessageParser * @brief Can search buffer for messages, read/parse them, and so on */ - class RxMessage + class MessageParser { public: /** - * @brief Constructor of the RxMessage class + * @brief Constructor of the MessageParser class * * One can always provide a non-const value where a const one was expected. * The const-ness of the argument just means the function promises not to * change it.. Recall: static_cast by the way can remove or add const-ness, * no other C++ cast is capable of removing it (not even reinterpret_cast) - * @param[in] data Pointer to the buffer that is about to be analyzed - * @param[in] size Size of the buffer (as handed over by async_read_some) + * @param[in] node Pointer to the node) */ - RxMessage(ROSaicNodeBase* node, Settings* settings) : - node_(node), settings_(settings), unix_time_(0) + MessageParser(ROSaicNodeBase* node) : + node_(node), settings_(node->getSettings()), unix_time_(0) { - found_ = false; - crc_check_ = false; - message_size_ = 0; - - //! Pair of iterators to facilitate initialization of the map - std::pair type_of_pvt_pairs[] = { - std::make_pair(static_cast(0), evNoPVT), - std::make_pair(static_cast(1), evStandAlone), - std::make_pair(static_cast(2), evDGPS), - std::make_pair(static_cast(3), evFixed), - std::make_pair(static_cast(4), evRTKFixed), - std::make_pair(static_cast(5), evRTKFloat), - std::make_pair(static_cast(6), evSBAS), - std::make_pair(static_cast(7), evMovingBaseRTKFixed), - std::make_pair(static_cast(8), evMovingBaseRTKFloat), - std::make_pair(static_cast(10), evPPP)}; - - type_of_pvt_map = - TypeOfPVTMap(type_of_pvt_pairs, type_of_pvt_pairs + evPPP + 1); - - //! Pair of iterators to facilitate initialization of the map - std::pair rx_id_pairs[] = { - std::make_pair("NavSatFix", evNavSatFix), - std::make_pair("INSNavSatFix", evINSNavSatFix), - std::make_pair("GPSFix", evGPSFix), - std::make_pair("INSGPSFix", evINSGPSFix), - std::make_pair("PoseWithCovarianceStamped", - evPoseWithCovarianceStamped), - std::make_pair("INSPoseWithCovarianceStamped", - evINSPoseWithCovarianceStamped), - std::make_pair("$GPGGA", evGPGGA), - std::make_pair("$GPRMC", evGPRMC), - std::make_pair("$GPGSA", evGPGSA), - std::make_pair("$GPGSV", evGPGSV), - std::make_pair("$GLGSV", evGLGSV), - std::make_pair("$GAGSV", evGAGSV), - std::make_pair("4006", evPVTCartesian), - std::make_pair("4007", evPVTGeodetic), - std::make_pair("4043", evBaseVectorCart), - std::make_pair("4028", evBaseVectorGeod), - std::make_pair("5905", evPosCovCartesian), - std::make_pair("5906", evPosCovGeodetic), - std::make_pair("5938", evAttEuler), - std::make_pair("5939", evAttCovEuler), - std::make_pair("GPST", evGPST), - std::make_pair("4013", evChannelStatus), - std::make_pair("4027", evMeasEpoch), - std::make_pair("4001", evDOP), - std::make_pair("5908", evVelCovGeodetic), - std::make_pair("DiagnosticArray", evDiagnosticArray), - std::make_pair("Localization", evLocalization), - std::make_pair("LocalizationEcef", evLocalizationEcef), - std::make_pair("4014", evReceiverStatus), - std::make_pair("4082", evQualityInd), - std::make_pair("5902", evReceiverSetup), - std::make_pair("4225", evINSNavCart), - std::make_pair("4226", evINSNavGeod), - std::make_pair("4230", evExtEventINSNavGeod), - std::make_pair("4229", evExtEventINSNavCart), - std::make_pair("4224", evIMUSetup), - std::make_pair("4244", evVelSensorSetup), - std::make_pair("4050", evExtSensorMeas), - std::make_pair("5914", evReceiverTime)}; - - rx_id_map = RxIDMap(rx_id_pairs, rx_id_pairs + evReceiverSetup + 1); } /** * @brief Put new data - * @param[in] recvTimestamp Timestamp of receiving buffer - * @param[in] data Pointer to the buffer that is about to be analyzed - * @param[in] size Size of the buffer (as handed over by async_read_some) + * @param[in] telegram Telegram to be parsed */ - void newData(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size) - { - recvTimestamp_ = recvTimestamp; - data_ = data; - count_ = size; - found_ = false; - crc_check_ = false; - message_size_ = 0; - } - - //! Determines whether data_ points to the SBF block with ID "ID", e.g. 5003 - bool isMessage(const uint16_t ID); + void newData(const Telegram& telegram) {} //! Determines whether data_ points to the NMEA message with ID "ID", e.g. //! "$GPGGA" bool isMessage(std::string ID); - //! Determines whether data_ currently points to an SBF block - bool isSBF(); - //! Determines whether data_ currently points to an NMEA message - bool isNMEA(); - //! Determines whether data_ currently points to an NMEA message - bool isResponse(); - //! Determines whether data_ currently points to a connection descriptor - //! (right after initiating a TCP connection) - bool isConnectionDescriptor(); - //! Determines whether data_ currently points to an error message reply from - //! the Rx - bool isErrorMessage(); - //! Determines size of the message (also command reply) that data_ is - //! currently pointing at - std::size_t messageSize(); + //! Returns the message ID of the message where data_ is pointing at at the //! moment, SBF identifiers embellished with inverted commas, e.g. "5003" std::string messageID(); - /** - * @brief Returns the count_ variable - * @return The variable count_ - */ - std::size_t getCount() { return count_; }; - /** - * @brief Searches the buffer for the beginning of the next message (NMEA or - * SBF) - * @return A pointer to the start of the next message. - */ - const uint8_t* search(); - - /** - * @brief Gets the length of the SBF block - * - * It determines the length from the header of the SBF block. The block - * length thus includes the header length. - * @return The length of the SBF block - */ - uint16_t getBlockLength(); - - /** - * @brief Gets the current position in the read buffer - * @return The current position of the read buffer - */ - const uint8_t* getPosBuffer(); - - /** - * @brief Gets the end position in the read buffer - * @return A pointer pointing to just after the read buffer (matches - * search()'s final pointer in case no valid message is found) - */ - const uint8_t* getEndBuffer(); - - /** - * @brief Has an NMEA message, SBF block or command reply been found in the - * buffer? - * @returns True if a message with the correct header & length has been found - */ - bool found(); - - /** - * @brief Goes to the start of the next message based on the calculated - * length of current message - */ - void next(); - /** * @brief Publishing function * @param[in] topic String of topic @@ -383,62 +179,6 @@ namespace io { */ void publishTf(const LocalizationMsg& msg); - /** - * @brief Performs the CRC check (if SBF) and publishes ROS messages - * @return True if read was successful, false otherwise - */ - bool read(std::string message_key, bool search = false); - - /** - * @brief Whether or not a message has been found - */ - bool found_; - - /** - * @brief Wether all blocks from GNSS have arrived for GpsFix Message - */ - bool gnss_gpsfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for GpsFix Message - */ - bool ins_gpsfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from GNSS have arrived for NavSatFix Message - */ - bool gnss_navsatfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for NavSatFix Message - */ - bool ins_navsatfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from GNSS have arrived for Pose Message - */ - bool gnss_pose_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for Pose Message - */ - bool ins_pose_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Diagnostics Message - */ - bool diagnostics_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Localization Message - */ - bool ins_localization_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Localization Message - */ - bool ins_localization_ecef_complete(uint32_t id); - private: /** * @brief Pointer to the node @@ -446,36 +186,14 @@ namespace io { ROSaicNodeBase* node_; /** - * @brief Timestamp of receiving buffer - */ - Timestamp recvTimestamp_; - - /** - * @brief Pointer to the buffer of messages - */ - const uint8_t* data_; - - /** - * @brief Number of unread bytes in the buffer - */ - std::size_t count_; - - /** - * @brief Whether the CRC check as evaluated in the read() method was - * successful or not is stored here - */ - bool crc_check_; - - /** - * @brief Helps to determine size of response message / NMEA message / SBF - * block + * @brief Pointe to settings struct */ - std::size_t message_size_; + Settings* settings_; /** - * @brief Number of times the GPSFixMsg message has been published + * @brief Timestamp of receiving buffer */ - uint32_t count_gpsfix_ = 0; + Timestamp recvTimestamp_; /** * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks @@ -560,30 +278,6 @@ namespace io { */ ReceiverSetup last_receiversetup_; - //! Shorthand for the map responsible for matching PVTGeodetic's Mode field - //! to an enum value - typedef std::unordered_map TypeOfPVTMap; - - /** - * @brief All instances of the RxMessage class shall have access to the map - * without reinitializing it, hence static - */ - TypeOfPVTMap type_of_pvt_map; - - //! Shorthand for the map responsible for matching ROS message identifiers to - //! an enum value - typedef std::unordered_map RxIDMap; - - /** - * @brief All instances of the RxMessage class shall have access to the map - * without reinitializing it, hence static - * - * This map is for mapping ROS message, SBF and NMEA identifiers to an - * enumeration and makes the switch-case formalism in rx_message.hpp more - * explicit. - */ - RxIDMap rx_id_map; - //! When reading from an SBF file, the ROS publishing frequency is governed //! by the time stamps found in the SBF blocks therein. Timestamp unix_time_; @@ -591,86 +285,6 @@ namespace io { //! Current leap seconds as received, do not use value is -128 int8_t current_leap_seconds_ = -128; - //! For GPSFix: Whether the ChannelStatus block of the current epoch has - //! arrived or not - bool channelstatus_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the MeasEpoch block of the current epoch has arrived - //! or not - bool measepoch_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the DOP block of the current epoch has arrived or not - bool dop_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the PVTGeodetic block of the current epoch has - //! arrived or not - bool pvtgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the PosCovGeodetic block of the current epoch has - //! arrived or not - bool poscovgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the VelCovGeodetic block of the current epoch has - //! arrived or not - bool velcovgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the AttEuler block of the current epoch has arrived - //! or not - bool atteuler_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the AttCovEuler block of the current epoch has - //! arrived or not - bool attcoveuler_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the INSNavGeod block of the current epoch has arrived - //! or not - bool insnavgeod_has_arrived_gpsfix_ = false; - - //! For NavSatFix: Whether the PVTGeodetic block of the current epoch has - //! arrived or not - bool pvtgeodetic_has_arrived_navsatfix_ = false; - - //! For NavSatFix: Whether the PosCovGeodetic block of the current epoch has - //! arrived or not - bool poscovgeodetic_has_arrived_navsatfix_ = false; - - //! For NavSatFix: Whether the INSNavGeod block of the current epoch has - //! arrived or not - bool insnavgeod_has_arrived_navsatfix_ = false; - //! For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the - //! current epoch has arrived or not - bool pvtgeodetic_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the - //! current epoch has arrived or not - bool poscovgeodetic_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the AttEuler block of the current - //! epoch has arrived or not - bool atteuler_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the AttCovEuler block of the - //! current epoch has arrived or not - bool attcoveuler_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the INSNavGeod block of the - //! current epoch has arrived or not - bool insnavgeod_has_arrived_pose_ = false; - - //! For DiagnosticArray: Whether the ReceiverStatus block of the current - //! epoch has arrived or not - bool receiverstatus_has_arrived_diagnostics_ = false; - - //! For DiagnosticArray: Whether the QualityInd block of the current epoch - //! has arrived or not - bool qualityind_has_arrived_diagnostics_ = false; - - //! For Localization: Whether the INSNavGeod and INSNavCart blocks of the - //! current epoch have arrived or not - bool insnavgeod_has_arrived_localization_ = false; - bool insnavgeod_has_arrived_localization_ecef_ = false; - bool insnavcart_has_arrived_localization_ecef_ = false; - /** * @brief "Callback" function when constructing NavSatFix messages * @return A smart pointer to the ROS message NavSatFix just created @@ -747,16 +361,6 @@ namespace io { */ void wait(Timestamp time_obj); - /** - * @brief Wether all elements are true - */ - bool allTrue(std::vector& vec, uint32_t id); - - /** - * @brief Settings struct - */ - Settings* settings_; - /** * @brief Fixed UTM zone */ @@ -767,8 +371,8 @@ namespace io { * This is either done using the TOW as transmitted with the SBF block (if * "use_gnss" is true), or using the current time. * @param[in] data Pointer to the buffer - * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is - * used, otherwise the current time + * @param[in] use_gnss_time If true, the TOW as transmitted with the SBF + * block is used, otherwise the current time * @return Timestamp object containing seconds and nanoseconds since last * epoch */ diff --git a/include/septentrio_gnss_driver/communication/rx_message.hpp b/include/septentrio_gnss_driver/communication/rx_message.hpp deleted file mode 100644 index bf2f1448..00000000 --- a/include/septentrio_gnss_driver/communication/rx_message.hpp +++ /dev/null @@ -1,793 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -// ***************************************************************************** -// -// Boost Software License - Version 1.0 - August 17th, 2003 -// -// Permission is hereby granted, free of charge, to any person or organization -// obtaining a copy of the software and accompanying documentation covered by -// this license (the "Software") to use, reproduce, display, distribute, -// execute, and transmit the Software, and to prepare derivative works of the -// Software, and to permit third-parties to whom the Software is furnished to -// do so, all subject to the following: - -// The copyright notices in the Software and this entire statement, including -// the above license grant, this restriction and the following disclaimer, -// must be included in all copies of the Software, in whole or in part, and -// all derivative works of the Software, unless such copies or derivative -// works are solely in the form of machine-executable object code generated by -// a source language processor. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT -// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE -// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, -// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. -// -// ***************************************************************************** - -//! 0x24 is ASCII for $ - 1st byte in each message -#ifndef NMEA_SYNC_BYTE_1 -#define NMEA_SYNC_BYTE_1 0x24 -#endif -//! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message -#ifndef NMEA_SYNC_BYTE_2_1 -#define NMEA_SYNC_BYTE_2_1 0x47 -#endif -//! 0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message -#ifndef NMEA_SYNC_BYTE_2_2 -#define NMEA_SYNC_BYTE_2_2 0x50 -#endif -//! 0x24 is ASCII for $ - 1st byte in each response from the Rx -#ifndef RESPONSE_SYNC_BYTE_1 -#define RESPONSE_SYNC_BYTE_1 0x24 -#endif -//! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx -#ifndef RESPONSE_SYNC_BYTE_2 -#define RESPONSE_SYNC_BYTE_2 0x52 -#endif -//! 0x0D is ASCII for "Carriage Return", i.e. "Enter" -#ifndef CARRIAGE_RETURN -#define CARRIAGE_RETURN 0x0D -#endif -//! 0x0A is ASCII for "Line Feed", i.e. "New Line" -#ifndef LINE_FEED -#define LINE_FEED 0x0A -#endif -//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the -//! command was invalid -#ifndef RESPONSE_SYNC_BYTE_3 -#define RESPONSE_SYNC_BYTE_3 0x3F -#endif -//! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after -//! initiating TCP connection -#ifndef CONNECTION_DESCRIPTOR_BYTE_1 -#define CONNECTION_DESCRIPTOR_BYTE_1 0x49 -#endif -//! 0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after -//! initiating TCP connection -#ifndef CONNECTION_DESCRIPTOR_BYTE_2 -#define CONNECTION_DESCRIPTOR_BYTE_2 0x50 -#endif - -// C++ libraries -#include // for assert -#include -#include -#include -// Boost includes -#include -#include -#include -#include -// ROSaic includes -#include -#include -#include -#include -#include -#include -#include - -#ifndef RX_MESSAGE_HPP -#define RX_MESSAGE_HPP - -/** - * @file rx_message.hpp - * @date 20/08/20 - * @brief Defines a class that reads messages handed over from the circular buffer - */ - -extern bool g_read_cd; -extern uint32_t g_cd_count; - -//! Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's -//! Mode field -enum TypeOfPVT_Enum -{ - evNoPVT, - evStandAlone, - evDGPS, - evFixed, - evRTKFixed, - evRTKFloat, - evSBAS, - evMovingBaseRTKFixed, - evMovingBaseRTKFloat, - evPPP -}; - -//! Since switch only works with int (yet NMEA message IDs are strings), we need -//! enum. Note drawbacks: No variable can have a name which is already in some -//! enumeration, enums are not type safe etc.. -enum RxID_Enum -{ - evNavSatFix, - evINSNavSatFix, - evGPSFix, - evINSGPSFix, - evPoseWithCovarianceStamped, - evINSPoseWithCovarianceStamped, - evGPGGA, - evGPRMC, - evGPGSA, - evGPGSV, - evGLGSV, - evGAGSV, - evPVTCartesian, - evPVTGeodetic, - evBaseVectorCart, - evBaseVectorGeod, - evPosCovCartesian, - evPosCovGeodetic, - evAttEuler, - evAttCovEuler, - evINSNavCart, - evINSNavGeod, - evIMUSetup, - evVelSensorSetup, - evExtEventINSNavGeod, - evExtEventINSNavCart, - evExtSensorMeas, - evGPST, - evChannelStatus, - evMeasEpoch, - evDOP, - evVelCovGeodetic, - evDiagnosticArray, - evLocalization, - evLocalizationEcef, - evReceiverStatus, - evQualityInd, - evReceiverTime, - evReceiverSetup -}; - -namespace io { - - /** - * @class RxMessage - * @brief Can search buffer for messages, read/parse them, and so on - */ - class RxMessage - { - public: - /** - * @brief Constructor of the RxMessage class - * - * One can always provide a non-const value where a const one was expected. - * The const-ness of the argument just means the function promises not to - * change it.. Recall: static_cast by the way can remove or add const-ness, - * no other C++ cast is capable of removing it (not even reinterpret_cast) - * @param[in] data Pointer to the buffer that is about to be analyzed - * @param[in] size Size of the buffer (as handed over by async_read_some) - */ - RxMessage(ROSaicNodeBase* node, Settings* settings) : - node_(node), settings_(settings), unix_time_(0) - { - found_ = false; - crc_check_ = false; - message_size_ = 0; - - //! Pair of iterators to facilitate initialization of the map - std::pair type_of_pvt_pairs[] = { - std::make_pair(static_cast(0), evNoPVT), - std::make_pair(static_cast(1), evStandAlone), - std::make_pair(static_cast(2), evDGPS), - std::make_pair(static_cast(3), evFixed), - std::make_pair(static_cast(4), evRTKFixed), - std::make_pair(static_cast(5), evRTKFloat), - std::make_pair(static_cast(6), evSBAS), - std::make_pair(static_cast(7), evMovingBaseRTKFixed), - std::make_pair(static_cast(8), evMovingBaseRTKFloat), - std::make_pair(static_cast(10), evPPP)}; - - type_of_pvt_map = - TypeOfPVTMap(type_of_pvt_pairs, type_of_pvt_pairs + evPPP + 1); - - //! Pair of iterators to facilitate initialization of the map - std::pair rx_id_pairs[] = { - std::make_pair("NavSatFix", evNavSatFix), - std::make_pair("INSNavSatFix", evINSNavSatFix), - std::make_pair("GPSFix", evGPSFix), - std::make_pair("INSGPSFix", evINSGPSFix), - std::make_pair("PoseWithCovarianceStamped", - evPoseWithCovarianceStamped), - std::make_pair("INSPoseWithCovarianceStamped", - evINSPoseWithCovarianceStamped), - std::make_pair("$GPGGA", evGPGGA), - std::make_pair("$GPRMC", evGPRMC), - std::make_pair("$GPGSA", evGPGSA), - std::make_pair("$GPGSV", evGPGSV), - std::make_pair("$GLGSV", evGLGSV), - std::make_pair("$GAGSV", evGAGSV), - std::make_pair("4006", evPVTCartesian), - std::make_pair("4007", evPVTGeodetic), - std::make_pair("4043", evBaseVectorCart), - std::make_pair("4028", evBaseVectorGeod), - std::make_pair("5905", evPosCovCartesian), - std::make_pair("5906", evPosCovGeodetic), - std::make_pair("5938", evAttEuler), - std::make_pair("5939", evAttCovEuler), - std::make_pair("GPST", evGPST), - std::make_pair("4013", evChannelStatus), - std::make_pair("4027", evMeasEpoch), - std::make_pair("4001", evDOP), - std::make_pair("5908", evVelCovGeodetic), - std::make_pair("DiagnosticArray", evDiagnosticArray), - std::make_pair("Localization", evLocalization), - std::make_pair("LocalizationEcef", evLocalizationEcef), - std::make_pair("4014", evReceiverStatus), - std::make_pair("4082", evQualityInd), - std::make_pair("5902", evReceiverSetup), - std::make_pair("4225", evINSNavCart), - std::make_pair("4226", evINSNavGeod), - std::make_pair("4230", evExtEventINSNavGeod), - std::make_pair("4229", evExtEventINSNavCart), - std::make_pair("4224", evIMUSetup), - std::make_pair("4244", evVelSensorSetup), - std::make_pair("4050", evExtSensorMeas), - std::make_pair("5914", evReceiverTime)}; - - rx_id_map = RxIDMap(rx_id_pairs, rx_id_pairs + evReceiverSetup + 1); - } - - /** - * @brief Put new data - * @param[in] recvTimestamp Timestamp of receiving buffer - * @param[in] data Pointer to the buffer that is about to be analyzed - * @param[in] size Size of the buffer (as handed over by async_read_some) - */ - void newData(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size) - { - recvTimestamp_ = recvTimestamp; - data_ = data; - count_ = size; - found_ = false; - crc_check_ = false; - message_size_ = 0; - } - - //! Determines whether data_ points to the SBF block with ID "ID", e.g. 5003 - bool isMessage(const uint16_t ID); - //! Determines whether data_ points to the NMEA message with ID "ID", e.g. - //! "$GPGGA" - bool isMessage(std::string ID); - //! Determines whether data_ currently points to an SBF block - bool isSBF(); - //! Determines whether data_ currently points to an NMEA message - bool isNMEA(); - //! Determines whether data_ currently points to an NMEA message - bool isResponse(); - //! Determines whether data_ currently points to a connection descriptor - //! (right after initiating a TCP connection) - bool isConnectionDescriptor(); - //! Determines whether data_ currently points to an error message reply from - //! the Rx - bool isErrorMessage(); - //! Determines size of the message (also command reply) that data_ is - //! currently pointing at - std::size_t messageSize(); - //! Returns the message ID of the message where data_ is pointing at at the - //! moment, SBF identifiers embellished with inverted commas, e.g. "5003" - std::string messageID(); - - /** - * @brief Returns the count_ variable - * @return The variable count_ - */ - std::size_t getCount() { return count_; }; - /** - * @brief Searches the buffer for the beginning of the next message (NMEA or - * SBF) - * @return A pointer to the start of the next message. - */ - const uint8_t* search(); - - /** - * @brief Gets the length of the SBF block - * - * It determines the length from the header of the SBF block. The block - * length thus includes the header length. - * @return The length of the SBF block - */ - uint16_t getBlockLength(); - - /** - * @brief Gets the current position in the read buffer - * @return The current position of the read buffer - */ - const uint8_t* getPosBuffer(); - - /** - * @brief Gets the end position in the read buffer - * @return A pointer pointing to just after the read buffer (matches - * search()'s final pointer in case no valid message is found) - */ - const uint8_t* getEndBuffer(); - - /** - * @brief Has an NMEA message, SBF block or command reply been found in the - * buffer? - * @returns True if a message with the correct header & length has been found - */ - bool found(); - - /** - * @brief Goes to the start of the next message based on the calculated - * length of current message - */ - void next(); - - /** - * @brief Publishing function - * @param[in] topic String of topic - * @param[in] msg ROS message to be published - */ - template - void publish(const std::string& topic, const M& msg); - - /** - * @brief Publishing function - * @param[in] msg Localization message - */ - void publishTf(const LocalizationMsg& msg); - - /** - * @brief Performs the CRC check (if SBF) and publishes ROS messages - * @return True if read was successful, false otherwise - */ - bool read(std::string message_key, bool search = false); - - /** - * @brief Whether or not a message has been found - */ - bool found_; - - /** - * @brief Wether all blocks from GNSS have arrived for GpsFix Message - */ - bool gnss_gpsfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for GpsFix Message - */ - bool ins_gpsfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from GNSS have arrived for NavSatFix Message - */ - bool gnss_navsatfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for NavSatFix Message - */ - bool ins_navsatfix_complete(uint32_t id); - - /** - * @brief Wether all blocks from GNSS have arrived for Pose Message - */ - bool gnss_pose_complete(uint32_t id); - - /** - * @brief Wether all blocks from INS have arrived for Pose Message - */ - bool ins_pose_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Diagnostics Message - */ - bool diagnostics_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Localization Message - */ - bool ins_localization_complete(uint32_t id); - - /** - * @brief Wether all blocks have arrived for Localization Message - */ - bool ins_localization_ecef_complete(uint32_t id); - - private: - /** - * @brief Pointer to the node - */ - ROSaicNodeBase* node_; - - /** - * @brief Timestamp of receiving buffer - */ - Timestamp recvTimestamp_; - - /** - * @brief Pointer to the buffer of messages - */ - const uint8_t* data_; - - /** - * @brief Number of unread bytes in the buffer - */ - std::size_t count_; - - /** - * @brief Whether the CRC check as evaluated in the read() method was - * successful or not is stored here - */ - bool crc_check_; - - /** - * @brief Helps to determine size of response message / NMEA message / SBF - * block - */ - std::size_t message_size_; - - /** - * @brief Number of times the GPSFixMsg message has been published - */ - uint32_t count_gpsfix_ = 0; - - /** - * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks - * need to be stored - */ - PVTGeodeticMsg last_pvtgeodetic_; - - /** - * @brief Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic - * blocks need to be stored - */ - PosCovGeodeticMsg last_poscovgeodetic_; - - /** - * @brief Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to - * be stored - */ - AttEulerMsg last_atteuler_; - - /** - * @brief Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks - * need to be stored - */ - AttCovEulerMsg last_attcoveuler_; - - /** - * @brief Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming - * INSNavGeod blocks need to be stored - */ - INSNavGeodMsg last_insnavgeod_; - - /** - * @brief Since LoclaizationEcef needs INSNavCart, incoming - * INSNavCart blocks need to be stored - */ - INSNavCartMsg last_insnavcart_; - - /** - * @brief Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks - * need to be stored - */ - ExtSensorMeasMsg last_extsensmeas_; - - /** - * @brief Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks - * need to be stored - */ - ChannelStatus last_channelstatus_; - - /** - * @brief Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks - * need to be stored - */ - MeasEpochMsg last_measepoch_; - - /** - * @brief Since GPSFix needs DOP, incoming DOP blocks need to be stored - */ - DOP last_dop_; - - /** - * @brief Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks - * need to be stored - */ - VelCovGeodeticMsg last_velcovgeodetic_; - - /** - * @brief Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus - * blocks need to be stored - */ - ReceiverStatus last_receiverstatus_; - - /** - * @brief Since DiagnosticArray needs QualityInd, incoming QualityInd blocks - * need to be stored - */ - QualityInd last_qualityind_; - - /** - * @brief Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup - * blocks need to be stored - */ - ReceiverSetup last_receiversetup_; - - //! Shorthand for the map responsible for matching PVTGeodetic's Mode field - //! to an enum value - typedef std::unordered_map TypeOfPVTMap; - - /** - * @brief All instances of the RxMessage class shall have access to the map - * without reinitializing it, hence static - */ - TypeOfPVTMap type_of_pvt_map; - - //! Shorthand for the map responsible for matching ROS message identifiers to - //! an enum value - typedef std::unordered_map RxIDMap; - - /** - * @brief All instances of the RxMessage class shall have access to the map - * without reinitializing it, hence static - * - * This map is for mapping ROS message, SBF and NMEA identifiers to an - * enumeration and makes the switch-case formalism in rx_message.hpp more - * explicit. - */ - RxIDMap rx_id_map; - - //! When reading from an SBF file, the ROS publishing frequency is governed - //! by the time stamps found in the SBF blocks therein. - Timestamp unix_time_; - - //! Current leap seconds as received, do not use value is -128 - int8_t current_leap_seconds_ = -128; - - //! For GPSFix: Whether the ChannelStatus block of the current epoch has - //! arrived or not - bool channelstatus_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the MeasEpoch block of the current epoch has arrived - //! or not - bool measepoch_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the DOP block of the current epoch has arrived or not - bool dop_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the PVTGeodetic block of the current epoch has - //! arrived or not - bool pvtgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the PosCovGeodetic block of the current epoch has - //! arrived or not - bool poscovgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the VelCovGeodetic block of the current epoch has - //! arrived or not - bool velcovgeodetic_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the AttEuler block of the current epoch has arrived - //! or not - bool atteuler_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the AttCovEuler block of the current epoch has - //! arrived or not - bool attcoveuler_has_arrived_gpsfix_ = false; - - //! For GPSFix: Whether the INSNavGeod block of the current epoch has arrived - //! or not - bool insnavgeod_has_arrived_gpsfix_ = false; - - //! For NavSatFix: Whether the PVTGeodetic block of the current epoch has - //! arrived or not - bool pvtgeodetic_has_arrived_navsatfix_ = false; - - //! For NavSatFix: Whether the PosCovGeodetic block of the current epoch has - //! arrived or not - bool poscovgeodetic_has_arrived_navsatfix_ = false; - - //! For NavSatFix: Whether the INSNavGeod block of the current epoch has - //! arrived or not - bool insnavgeod_has_arrived_navsatfix_ = false; - //! For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the - //! current epoch has arrived or not - bool pvtgeodetic_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the - //! current epoch has arrived or not - bool poscovgeodetic_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the AttEuler block of the current - //! epoch has arrived or not - bool atteuler_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the AttCovEuler block of the - //! current epoch has arrived or not - bool attcoveuler_has_arrived_pose_ = false; - - //! For PoseWithCovarianceStamped: Whether the INSNavGeod block of the - //! current epoch has arrived or not - bool insnavgeod_has_arrived_pose_ = false; - - //! For DiagnosticArray: Whether the ReceiverStatus block of the current - //! epoch has arrived or not - bool receiverstatus_has_arrived_diagnostics_ = false; - - //! For DiagnosticArray: Whether the QualityInd block of the current epoch - //! has arrived or not - bool qualityind_has_arrived_diagnostics_ = false; - - //! For Localization: Whether the INSNavGeod and INSNavCart blocks of the - //! current epoch have arrived or not - bool insnavgeod_has_arrived_localization_ = false; - bool insnavgeod_has_arrived_localization_ecef_ = false; - bool insnavcart_has_arrived_localization_ecef_ = false; - - /** - * @brief "Callback" function when constructing NavSatFix messages - * @return A smart pointer to the ROS message NavSatFix just created - */ - NavSatFixMsg NavSatFixCallback(); - - /** - * @brief "Callback" function when constructing GPSFix messages - * @return A smart pointer to the ROS message GPSFix just created - */ - GPSFixMsg GPSFixCallback(); - - /** - * @brief "Callback" function when constructing PoseWithCovarianceStamped - * messages - * @return A smart pointer to the ROS message PoseWithCovarianceStamped just - * created - */ - PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback(); - - /** - * @brief "Callback" function when constructing - * DiagnosticArrayMsg messages - * @return A ROS message - * DiagnosticArrayMsg just created - */ - DiagnosticArrayMsg DiagnosticArrayCallback(); - - /** - * @brief "Callback" function when constructing - * ImuMsg messages - * @return A ROS message - * ImuMsg just created - */ - ImuMsg ImuCallback(); - - /** - * @brief "Callback" function when constructing - * LocalizationMsg messages in UTM - * @return A ROS message - * LocalizationMsg just created - */ - LocalizationMsg LocalizationUtmCallback(); - - /** - * @brief "Callback" function when constructing - * LocalizationMsg messages in ECEF - * @return A ROS message - * LocalizationMsg just created - */ - LocalizationMsg LocalizationEcefCallback(); - - /** - * @brief function to fill twist part of LocalizationMsg - * @param[in] roll roll [rad] - * @param[in] pitch pitch [rad] - * @param[in] yaw yaw [rad] - * @param[inout] msg LocalizationMsg to be filled - */ - void fillLocalizationMsgTwist(double roll, double pitch, double yaw, - LocalizationMsg& msg); - - /** - * @brief "Callback" function when constructing - * TwistWithCovarianceStampedMsg messages - * @param[in] fromIns Wether to contruct message from INS data - * @return A ROS message - * TwistWithCovarianceStampedMsg just created - */ - TwistWithCovarianceStampedMsg TwistCallback(bool fromIns = false); - - /** - * @brief Waits according to time when reading from file - */ - void wait(Timestamp time_obj); - - /** - * @brief Wether all elements are true - */ - bool allTrue(std::vector& vec, uint32_t id); - - /** - * @brief Settings struct - */ - Settings* settings_; - - /** - * @brief Fixed UTM zone - */ - std::shared_ptr fixedUtmZone_; - - /** - * @brief Calculates the timestamp, in the Unix Epoch time format - * This is either done using the TOW as transmitted with the SBF block (if - * "use_gnss" is true), or using the current time. - * @param[in] data Pointer to the buffer - * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is - * used, otherwise the current time - * @return Timestamp object containing seconds and nanoseconds since last - * epoch - */ - Timestamp timestampSBF(const uint8_t* data, bool use_gnss_time); - - /** - * @brief Calculates the timestamp, in the Unix Epoch time format - * This is either done using the TOW as transmitted with the SBF block (if - * "use_gnss" is true), or using the current time. - * @param[in] tow (Time of Week) Number of milliseconds that elapsed since - * the beginning of the current GPS week as transmitted by the SBF block - * @param[in] wnc (Week Number Counter) counts the number of complete weeks - * elapsed since January 6, 1980 - * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is - * used, otherwise the current time - * @return Timestamp object containing seconds and nanoseconds since last - * epoch - */ - Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time); - }; -} // namespace io -#endif // for RX_MESSAGE_HPP \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/message.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp similarity index 87% rename from include/septentrio_gnss_driver/communication/message.hpp rename to include/septentrio_gnss_driver/communication/telegram.hpp index 8d571bab..de894a86 100644 --- a/include/septentrio_gnss_driver/communication/message.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -50,9 +50,11 @@ static const std::byte NMEA_SYNC_BYTE_1 0x47; static const std::byte NMEA_SYNC_BYTE_2 0x50; //! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx static const std::byte RESPONSE_SYNC_BYTE_1 0x52; -//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the +//! 0x3A is ASCII for ? - 3rd byte in the response message from the Rx +static const std::byte RESPONSE_SYNC_BYTE_2 0x3A; +//! 0x3F is ASCII for : - 3rd byte in the response message from the Rx in case the //! command was invalid -static const std::byte RESPONSE_SYNC_BYTE_2 0x3F; +static const std::byte ERROR_SYNC_BYTE_2 0x3F; //! 0x0D is ASCII for "Carriage Return", i.e. "Enter" static const std::byte CR 0x0D; //! 0x0A is ASCII for "Line Feed", i.e. "New Line" @@ -67,26 +69,27 @@ static const std::byte CONNECTION_DESCRIPTOR_BYTE_2 0x50; static const uint16_t SBF_HEADER_SIZE = 8; static const uint16_t MAX_SBF_SIZE 65535; -enum MessageType +enum TelegramType { - INVALID, + EMPTY, SBF, NMEA, RESPONSE, + ERROR, CONNECTION_DESCRIPTOR }; -struct Message +struct Telegram { Timestamp stamp = 0; - MessageType type = INVALID; + TelegramType type = EMPTY; uint16_t sbfId = 0; std::vector data(3); - Message() : stamp(0), type(INVALID), sbfId(0), data(std::vector(3)) + Telegram() : stamp(0), type(INVALID), sbfId(0), data(std::vector(3)) { data.reserve(MAX_SBF_SIZE); } }; -typedef tbb::concurrent_bounded_queue> MessageQueue; +typedef tbb::concurrent_bounded_queue> TelegramQueue; diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp similarity index 87% rename from include/septentrio_gnss_driver/communication/message_handler.hpp rename to include/septentrio_gnss_driver/communication/telegram_handler.hpp index ac4d5b0b..af757275 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -93,12 +93,12 @@ // ROSaic and C++ includes #include -#include +#include /** - * @file message_handler.hpp - * @date 22/08/20 - * @brief Handles messages when reading NMEA/SBF/response messages + * @file telegram_handler.hpp + * @brief Handles messages when reading NMEA/SBF/response/error/connection descriptor + * messages */ struct CommSync @@ -110,23 +110,20 @@ struct CommSync namespace io { /** - * @class MessageHandler + * @class TelegramHandler * @brief Represents ensemble of (to be constructed) ROS messages, to be handled * at once by this class */ - class MessageHandler + class TelegramHandler { public: - MessageHandler(ROSaicNodeBase* node, Settings* settings) : - node_(node), rx_message_(node, settings), settings_(settings) - { - } + TelegramHandler(ROSaicNodeBase* node) : node_(node), message_parser(node) {} /** - * @brief Called every time a message is received + * @brief Called every time a telegram is received */ - void handleMessage(const Message& message); + void handleTelegram(const std::shared_ptr& telegram); CommSync* getResponseSync() { return &response_sync; } @@ -137,15 +134,16 @@ namespace io { std::string getRxTcpPort() { return rx_tcp_port; } private: - void handleSbf(const Message& message); - void handleNmea(const Message& message); - void handleResponse(const Message& message); - void handleCd(const Message& message); + void handleSbf(const std::shared_ptr& telegram); + void handleNmea(const std::shared_ptr& telegram); + void handleResponse(const std::shared_ptr& telegram); + void handleError(const std::shared_ptr& telegram); + void handleCd(const std::shared_ptr& telegram); //! Pointer to Node ROSaicNodeBase* node_; - //! RxMessage parser - RxMessage rx_message_; + //! MessageParser parser + MessageParser message_parser_; CommSync response_sync; CommSync cd_sync; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 6fb69f3f..cb891d81 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -36,61 +36,20 @@ #include #include -#ifndef ANGLE_MAX -#define ANGLE_MAX 180 -#endif - -#ifndef ANGLE_MIN -#define ANGLE_MIN -180 -#endif - -#ifndef THETA_Y_MAX -#define THETA_Y_MAX 90 -#endif - -#ifndef THETA_Y_MIN -#define THETA_Y_MIN -90 -#endif - -#ifndef LEVER_ARM_MAX -#define LEVER_ARM_MAX 100 -#endif - -#ifndef LEVER_ARM_MIN -#define LEVER_ARM_MIN -100 -#endif - -#ifndef HEADING_MAX -#define HEADING_MAX 360 -#endif - -#ifndef HEADING_MIN -#define HEADING_MIN -360 -#endif - -#ifndef PITCH_MAX -#define PITCH_MAX 90 -#endif - -#ifndef PITCH_MIN -#define PITCH_MIN -90 -#endif - -#ifndef ATTSTD_DEV_MIN -#define ATTSTD_DEV_MIN 0 -#endif - -#ifndef ATTSTD_DEV_MAX -#define ATTSTD_DEV_MAX 5 -#endif - -#ifndef POSSTD_DEV_MIN -#define POSSTD_DEV_MIN 0 -#endif - -#ifndef POSSTD_DEV_MAX -#define POSSTD_DEV_MAX 100 -#endif +static const int16_t ANGLE_MAX = 180; +static const int16_t ANGLE_MIN = -180; +static const int8_t THETA_Y_MAX = 90; +static const int8_t THETA_Y_MIN = -90; +static const int8_t LEVER_ARM_MAX = 100; +static const int8_t LEVER_ARM_MIN = -100; +static const int16_t HEADING_MAX = 360; +static const int16_t HEADING_MIN = -360; +static const int8_t PITCH_MAX = 90; +static const int8_t PITCH_MIN = -90; +static const int8_t ATTSTD_DEV_MIN = 0; +static const int8_t ATTSTD_DEV_MAX = 5; +static const int8_t POSSTD_DEV_MIN = 0; +static const int8_t POSSTD_DEV_MAX = 100; /** * @file communication_core.cpp @@ -98,29 +57,8 @@ * @brief Highest-Level view on communication services */ -//! Mutex to control changes of global variable "g_response_received" -std::mutex g_response_mutex; -//! Determines whether a command reply was received from the Rx -bool g_response_received; -//! Condition variable complementing "g_response_mutex" -boost::condition_variable g_response_condition; -//! Mutex to control changes of global variable "g_cd_received" -std::mutex g_cd_mutex; -//! Determines whether the connection descriptor was received from the Rx -bool g_cd_received; -//! Condition variable complementing "g_cd_mutex" -boost::condition_variable g_cd_condition; -//! Whether or not we still want to read the connection descriptor, which we only -//! want in the very beginning to know whether it is IP10, IP11 etc. -bool g_read_cd; -//! Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to -std::string g_rx_tcp_port; -//! Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have -//! to count the connection descriptors -uint32_t g_cd_count; - -io::CommIo::CommIo(ROSaicNodeBase* node, Settings* settings) : - node_(node), handlers_(node, settings), settings_(settings), running_(true) +io::CommIo::CommIo(ROSaicNodeBase* node) : + node_(node), settings_(node->getSettings()), running_(true) { g_response_received = false; g_cd_received = false; diff --git a/src/septentrio_gnss_driver/communication/message_parser.cpp b/src/septentrio_gnss_driver/communication/message_parser.cpp index 38f09b6d..309d9e05 100644 --- a/src/septentrio_gnss_driver/communication/message_parser.cpp +++ b/src/septentrio_gnss_driver/communication/message_parser.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include /** @@ -53,2670 +53,2550 @@ using parsing_utilities::deg2radSq; using parsing_utilities::rad2deg; using parsing_utilities::square; -PoseWithCovarianceStampedMsg -io::RxMessage::PoseWithCovarianceStampedCallback() -{ - PoseWithCovarianceStampedMsg msg; - if (settings_->septentrio_receiver_type == "gnss") +namespace io { + PoseWithCovarianceStampedMsg MessageParser::PoseWithCovarianceStampedCallback() { - // Filling in the pose data - double yaw = 0.0; - if (validValue(last_atteuler_.heading)) - yaw = last_atteuler_.heading; - double pitch = 0.0; - if (validValue(last_atteuler_.pitch)) - pitch = last_atteuler_.pitch; - double roll = 0.0; - if (validValue(last_atteuler_.roll)) - roll = last_atteuler_.roll; - msg.pose.pose.orientation = - convertEulerToQuaternionMsg(deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); - msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); - msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); - msg.pose.pose.position.z = last_pvtgeodetic_.height; - // Filling in the covariance data in row-major order - msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon; - msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat; - msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt; - msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; - msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; - msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; - msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll); - msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll); - msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch); - msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll); - msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); - } - if (settings_->septentrio_receiver_type == "ins") - { - msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude); - msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude); - msg.pose.pose.position.z = last_insnavgeod_.height; - - // Filling in the pose data - if ((last_insnavgeod_.sb_list & 1) != 0) - { - // Pos autocov - msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev); - msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev); - msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev); - } else - { - msg.pose.covariance[0] = -1.0; - msg.pose.covariance[7] = -1.0; - msg.pose.covariance[14] = -1.0; - } - if ((last_insnavgeod_.sb_list & 2) != 0) + PoseWithCovarianceStampedMsg msg; + if (settings_->septentrio_receiver_type == "gnss") { + // Filling in the pose data double yaw = 0.0; - if (validValue(last_insnavgeod_.heading)) - yaw = last_insnavgeod_.heading; + if (validValue(last_atteuler_.heading)) + yaw = last_atteuler_.heading; double pitch = 0.0; - if (validValue(last_insnavgeod_.pitch)) - pitch = last_insnavgeod_.pitch; + if (validValue(last_atteuler_.pitch)) + pitch = last_atteuler_.pitch; double roll = 0.0; - if (validValue(last_insnavgeod_.roll)) - roll = last_insnavgeod_.roll; - // Attitude + if (validValue(last_atteuler_.roll)) + roll = last_atteuler_.roll; msg.pose.pose.orientation = convertEulerToQuaternionMsg( deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); - } else - { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); + msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); + msg.pose.pose.position.z = last_pvtgeodetic_.height; + // Filling in the covariance data in row-major order + msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon; + msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat; + msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt; + msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; + msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; + msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; + msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll); + msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll); + msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch); + msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch); + msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll); + msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); + msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); } - if ((last_insnavgeod_.sb_list & 4) != 0) + if (settings_->septentrio_receiver_type == "ins") { - // Attitude autocov - if (validValue(last_insnavgeod_.roll_std_dev)) - msg.pose.covariance[21] = - square(deg2rad(last_insnavgeod_.roll_std_dev)); - else + msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude); + msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude); + msg.pose.pose.position.z = last_insnavgeod_.height; + + // Filling in the pose data + if ((last_insnavgeod_.sb_list & 1) != 0) + { + // Pos autocov + msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev); + msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev); + msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev); + } else + { + msg.pose.covariance[0] = -1.0; + msg.pose.covariance[7] = -1.0; + msg.pose.covariance[14] = -1.0; + } + if ((last_insnavgeod_.sb_list & 2) != 0) + { + double yaw = 0.0; + if (validValue(last_insnavgeod_.heading)) + yaw = last_insnavgeod_.heading; + double pitch = 0.0; + if (validValue(last_insnavgeod_.pitch)) + pitch = last_insnavgeod_.pitch; + double roll = 0.0; + if (validValue(last_insnavgeod_.roll)) + roll = last_insnavgeod_.roll; + // Attitude + msg.pose.pose.orientation = convertEulerToQuaternionMsg( + deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); + } else + { + msg.pose.pose.orientation.w = + std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = + std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = + std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = + std::numeric_limits::quiet_NaN(); + } + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocov + if (validValue(last_insnavgeod_.roll_std_dev)) + msg.pose.covariance[21] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + msg.pose.covariance[21] = -1.0; + if (validValue(last_insnavgeod_.pitch_std_dev)) + msg.pose.covariance[28] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + msg.pose.covariance[28] = -1.0; + if (validValue(last_insnavgeod_.heading_std_dev)) + msg.pose.covariance[35] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + msg.pose.covariance[35] = -1.0; + } else + { msg.pose.covariance[21] = -1.0; - if (validValue(last_insnavgeod_.pitch_std_dev)) - msg.pose.covariance[28] = - square(deg2rad(last_insnavgeod_.pitch_std_dev)); - else msg.pose.covariance[28] = -1.0; - if (validValue(last_insnavgeod_.heading_std_dev)) - msg.pose.covariance[35] = - square(deg2rad(last_insnavgeod_.heading_std_dev)); - else msg.pose.covariance[35] = -1.0; - } else - { - msg.pose.covariance[21] = -1.0; - msg.pose.covariance[28] = -1.0; - msg.pose.covariance[35] = -1.0; + } + if ((last_insnavgeod_.sb_list & 32) != 0) + { + // Pos cov + msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; + msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; + msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; + msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; + } + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude cov + msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + + msg.pose.covariance[29] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + } } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - // Pos cov - msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov; - msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov; - msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov; - msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov; + return msg; + }; + + DiagnosticArrayMsg MessageParser::DiagnosticArrayCallback() + { + DiagnosticArrayMsg msg; + std::string serialnumber(last_receiversetup_.rx_serial_number); + DiagnosticStatusMsg gnss_status; + // Constructing the "level of operation" field + uint16_t indicators_type_mask = static_cast(255); + uint16_t indicators_value_mask = static_cast(3840); + uint16_t qualityind_pos; + for (uint16_t i = static_cast(0); + i < last_qualityind_.indicators.size(); ++i) + { + if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(0)) + { + qualityind_pos = i; + if (((last_qualityind_.indicators[i] & indicators_value_mask) >> + 8) == static_cast(0)) + { + gnss_status.level = DiagnosticStatusMsg::STALE; + } else if (((last_qualityind_.indicators[i] & + indicators_value_mask) >> + 8) == static_cast(1) || + ((last_qualityind_.indicators[i] & + indicators_value_mask) >> + 8) == static_cast(2)) + { + gnss_status.level = DiagnosticStatusMsg::WARN; + } else + { + gnss_status.level = DiagnosticStatusMsg::OK; + } + break; + } } - if ((last_insnavgeod_.sb_list & 64) != 0) + // If the ReceiverStatus's RxError field is not 0, then at least one error + // has been detected. + if (last_receiverstatus_.rx_error != static_cast(0)) { - // Attitude cov - msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + gnss_status.level = DiagnosticStatusMsg::ERROR; } - } - return msg; -}; - -DiagnosticArrayMsg io::RxMessage::DiagnosticArrayCallback() -{ - DiagnosticArrayMsg msg; - std::string serialnumber(last_receiversetup_.rx_serial_number); - DiagnosticStatusMsg gnss_status; - // Constructing the "level of operation" field - uint16_t indicators_type_mask = static_cast(255); - uint16_t indicators_value_mask = static_cast(3840); - uint16_t qualityind_pos; - for (uint16_t i = static_cast(0); - i < last_qualityind_.indicators.size(); ++i) - { - if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(0)) + // Creating an array of values associated with the GNSS status + gnss_status.values.resize(static_cast(last_qualityind_.n - 1)); + for (uint16_t i = static_cast(0); + i != static_cast(last_qualityind_.n); ++i) { - qualityind_pos = i; - if (((last_qualityind_.indicators[i] & indicators_value_mask) >> 8) == - static_cast(0)) + if (i == qualityind_pos) { - gnss_status.level = DiagnosticStatusMsg::STALE; - } else if (((last_qualityind_.indicators[i] & indicators_value_mask) >> - 8) == static_cast(1) || - ((last_qualityind_.indicators[i] & indicators_value_mask) >> - 8) == static_cast(2)) + continue; + } + if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(1)) + { + gnss_status.values[i].key = "GNSS Signals, Main Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(2)) + { + gnss_status.values[i].key = "GNSS Signals, Aux1 Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(11)) + { + gnss_status.values[i].key = "RF Power, Main Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(12)) { - gnss_status.level = DiagnosticStatusMsg::WARN; + gnss_status.values[i].key = "RF Power, Aux1 Antenna"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(21)) + { + gnss_status.values[i].key = "CPU Headroom"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(25)) + { + gnss_status.values[i].key = "OCXO Stability"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); + } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(30)) + { + gnss_status.values[i].key = "Base Station Measurements"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); } else { - gnss_status.level = DiagnosticStatusMsg::OK; + assert((last_qualityind_.indicators[i] & indicators_type_mask) == + static_cast(31)); + gnss_status.values[i].key = "RTK Post-Processing"; + gnss_status.values[i].value = std::to_string( + (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); } - break; } - } - // If the ReceiverStatus's RxError field is not 0, then at least one error has - // been detected. - if (last_receiverstatus_.rx_error != static_cast(0)) - { - gnss_status.level = DiagnosticStatusMsg::ERROR; - } - // Creating an array of values associated with the GNSS status - gnss_status.values.resize(static_cast(last_qualityind_.n - 1)); - for (uint16_t i = static_cast(0); - i != static_cast(last_qualityind_.n); ++i) - { - if (i == qualityind_pos) - { - continue; - } - if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(1)) - { - gnss_status.values[i].key = "GNSS Signals, Main Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(2)) - { - gnss_status.values[i].key = "GNSS Signals, Aux1 Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(11)) - { - gnss_status.values[i].key = "RF Power, Main Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(12)) - { - gnss_status.values[i].key = "RF Power, Aux1 Antenna"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(21)) - { - gnss_status.values[i].key = "CPU Headroom"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(25)) - { - gnss_status.values[i].key = "OCXO Stability"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else if ((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(30)) - { - gnss_status.values[i].key = "Base Station Measurements"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } else - { - assert((last_qualityind_.indicators[i] & indicators_type_mask) == - static_cast(31)); - gnss_status.values[i].key = "RTK Post-Processing"; - gnss_status.values[i].value = std::to_string( - (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); - } - } - gnss_status.hardware_id = serialnumber; - gnss_status.name = "gnss"; - gnss_status.message = - "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; - msg.status.push_back(gnss_status); - return msg; -}; - -ImuMsg io::RxMessage::ImuCallback() -{ - ImuMsg msg; - - msg.linear_acceleration.x = last_extsensmeas_.acceleration_x; - msg.linear_acceleration.y = last_extsensmeas_.acceleration_y; - msg.linear_acceleration.z = last_extsensmeas_.acceleration_z; - - msg.angular_velocity.x = last_extsensmeas_.angular_rate_x; - msg.angular_velocity.y = last_extsensmeas_.angular_rate_y; - msg.angular_velocity.z = last_extsensmeas_.angular_rate_z; - - bool valid_orientation = true; - if (settings_->septentrio_receiver_type == "ins") + gnss_status.hardware_id = serialnumber; + gnss_status.name = "gnss"; + gnss_status.message = + "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; + msg.status.push_back(gnss_status); + return msg; + }; + + ImuMsg MessageParser::ImuCallback() { - if (validValue(last_insnavgeod_.block_header.tow)) - { - Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow, - last_extsensmeas_.block_header.wnc, true); - Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - true); // Filling in the orientation data + ImuMsg msg; - static int64_t maxDt = (settings_->polling_period_pvt == 0) - ? 10000000 - : settings_->polling_period_pvt * 1000000; - if ((tsImu - tsIns) > maxDt) - { - valid_orientation = false; - } else + msg.linear_acceleration.x = last_extsensmeas_.acceleration_x; + msg.linear_acceleration.y = last_extsensmeas_.acceleration_y; + msg.linear_acceleration.z = last_extsensmeas_.acceleration_z; + + msg.angular_velocity.x = last_extsensmeas_.angular_rate_x; + msg.angular_velocity.y = last_extsensmeas_.angular_rate_y; + msg.angular_velocity.z = last_extsensmeas_.angular_rate_z; + + bool valid_orientation = true; + if (settings_->septentrio_receiver_type == "ins") + { + if (validValue(last_insnavgeod_.block_header.tow)) { - if ((last_insnavgeod_.sb_list & 2) != 0) + Timestamp tsImu = + timestampSBF(last_extsensmeas_.block_header.tow, + last_extsensmeas_.block_header.wnc, true); + Timestamp tsIns = + timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + true); // Filling in the orientation data + + static int64_t maxDt = (settings_->polling_period_pvt == 0) + ? 10000000 + : settings_->polling_period_pvt * 1000000; + if ((tsImu - tsIns) > maxDt) + { + valid_orientation = false; + } else { - // Attitude - if (validValue(last_insnavgeod_.heading) && - validValue(last_insnavgeod_.pitch) && - validValue(last_insnavgeod_.roll)) + if ((last_insnavgeod_.sb_list & 2) != 0) { - msg.orientation = convertEulerToQuaternionMsg( - deg2rad(last_insnavgeod_.roll), - deg2rad(last_insnavgeod_.pitch), - deg2rad(last_insnavgeod_.heading)); + // Attitude + if (validValue(last_insnavgeod_.heading) && + validValue(last_insnavgeod_.pitch) && + validValue(last_insnavgeod_.roll)) + { + msg.orientation = convertEulerToQuaternionMsg( + deg2rad(last_insnavgeod_.roll), + deg2rad(last_insnavgeod_.pitch), + deg2rad(last_insnavgeod_.heading)); + } else + { + valid_orientation = false; + } } else { valid_orientation = false; } - } else - { - valid_orientation = false; - } - if ((last_insnavgeod_.sb_list & 4) != 0) - { - // Attitude autocov - if (validValue(last_insnavgeod_.roll_std_dev) && - validValue(last_insnavgeod_.pitch_std_dev) && - validValue(last_insnavgeod_.heading_std_dev)) + if ((last_insnavgeod_.sb_list & 4) != 0) { - msg.orientation_covariance[0] = - square(deg2rad(last_insnavgeod_.roll_std_dev)); - msg.orientation_covariance[4] = - square(deg2rad(last_insnavgeod_.pitch_std_dev)); - msg.orientation_covariance[8] = - square(deg2rad(last_insnavgeod_.heading_std_dev)); + // Attitude autocov + if (validValue(last_insnavgeod_.roll_std_dev) && + validValue(last_insnavgeod_.pitch_std_dev) && + validValue(last_insnavgeod_.heading_std_dev)) + { + msg.orientation_covariance[0] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + msg.orientation_covariance[4] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + msg.orientation_covariance[8] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + } else + { + valid_orientation = false; + } } else { valid_orientation = false; } - } else - { - valid_orientation = false; - } - if ((last_insnavgeod_.sb_list & 64) != 0) - { - // Attitude cov - msg.orientation_covariance[1] = - deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.orientation_covariance[2] = - deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.orientation_covariance[3] = - deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.orientation_covariance[5] = - deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.orientation_covariance[6] = - deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.orientation_covariance[7] = - deg2radSq(last_insnavgeod_.heading_pitch_cov); + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude cov + msg.orientation_covariance[1] = + deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.orientation_covariance[2] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.orientation_covariance[3] = + deg2radSq(last_insnavgeod_.pitch_roll_cov); + + msg.orientation_covariance[5] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.orientation_covariance[6] = + deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.orientation_covariance[7] = + deg2radSq(last_insnavgeod_.heading_pitch_cov); + } } + } else + { + valid_orientation = false; } } else { valid_orientation = false; } - } else - { - valid_orientation = false; - } - - if (!valid_orientation) - { - msg.orientation.w = std::numeric_limits::quiet_NaN(); - msg.orientation.x = std::numeric_limits::quiet_NaN(); - msg.orientation.y = std::numeric_limits::quiet_NaN(); - msg.orientation.z = std::numeric_limits::quiet_NaN(); - msg.orientation_covariance[0] = -1.0; - msg.orientation_covariance[4] = -1.0; - msg.orientation_covariance[8] = -1.0; - } - return msg; -}; + if (!valid_orientation) + { + msg.orientation.w = std::numeric_limits::quiet_NaN(); + msg.orientation.x = std::numeric_limits::quiet_NaN(); + msg.orientation.y = std::numeric_limits::quiet_NaN(); + msg.orientation.z = std::numeric_limits::quiet_NaN(); + msg.orientation_covariance[0] = -1.0; + msg.orientation_covariance[4] = -1.0; + msg.orientation_covariance[8] = -1.0; + } -TwistWithCovarianceStampedMsg -io::RxMessage::TwistCallback(bool fromIns /* = false*/) -{ - TwistWithCovarianceStampedMsg msg; + return msg; + }; - if (fromIns) + TwistWithCovarianceStampedMsg + MessageParser::TwistCallback(bool fromIns /* = false*/) { - msg.header = last_insnavgeod_.header; + TwistWithCovarianceStampedMsg msg; - if ((last_insnavgeod_.sb_list & 8) != 0) + if (fromIns) { - // Linear velocity in navigation frame - double ve = 0.0; - if (validValue(last_insnavgeod_.ve)) - ve = last_insnavgeod_.ve; - double vn = 0.0; - if (validValue(last_insnavgeod_.vn)) - vn = last_insnavgeod_.vn; - double vu = 0.0; - if (validValue(last_insnavgeod_.vu)) - vu = last_insnavgeod_.vu; - Eigen::Vector3d vel; - if (settings_->use_ros_axis_orientation) + msg.header = last_insnavgeod_.header; + + if ((last_insnavgeod_.sb_list & 8) != 0) { - // (ENU) - vel << ve, vn, vu; + // Linear velocity in navigation frame + double ve = 0.0; + if (validValue(last_insnavgeod_.ve)) + ve = last_insnavgeod_.ve; + double vn = 0.0; + if (validValue(last_insnavgeod_.vn)) + vn = last_insnavgeod_.vn; + double vu = 0.0; + if (validValue(last_insnavgeod_.vu)) + vu = last_insnavgeod_.vu; + Eigen::Vector3d vel; + if (settings_->use_ros_axis_orientation) + { + // (ENU) + vel << ve, vn, vu; + } else + { + // (NED) + vel << vn, ve, -vu; + } + // Linear velocity + msg.twist.twist.linear.x = vel(0); + msg.twist.twist.linear.y = vel(1); + msg.twist.twist.linear.z = vel(2); } else { - // (NED) - vel << vn, ve, -vu; + msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + } + + if (((last_insnavgeod_.sb_list & 16) != 0) && + ((last_insnavgeod_.sb_list & 2) != 0) && + ((last_insnavgeod_.sb_list & 8) != 0)) + { + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); + if ((last_insnavgeod_.sb_list & 128) != 0) + { + // Linear velocity covariance + if (validValue(last_insnavgeod_.ve_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(0, 0) = -1.0; + if (validValue(last_insnavgeod_.vn_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(1, 1) = -1.0; + if (validValue(last_insnavgeod_.vu_std_dev)) + covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); + else + covVel_local(2, 2) = -1.0; + + if (validValue(last_insnavgeod_.ve_vn_cov)) + covVel_local(0, 1) = covVel_local(1, 0) = + last_insnavgeod_.ve_vn_cov; + if (settings_->use_ros_axis_orientation) + { + if (validValue(last_insnavgeod_.ve_vu_cov)) + covVel_local(0, 2) = covVel_local(2, 0) = + last_insnavgeod_.ve_vu_cov; + if (validValue(last_insnavgeod_.vn_vu_cov)) + covVel_local(2, 1) = covVel_local(1, 2) = + last_insnavgeod_.vn_vu_cov; + } else + { + if (validValue(last_insnavgeod_.vn_vu_cov)) + covVel_local(0, 2) = covVel_local(2, 0) = + -last_insnavgeod_.vn_vu_cov; + if (validValue(last_insnavgeod_.ve_vu_cov)) + covVel_local(2, 1) = covVel_local(1, 2) = + -last_insnavgeod_.ve_vu_cov; + } + } else + { + covVel_local(0, 0) = -1.0; + covVel_local(1, 1) = -1.0; + covVel_local(2, 2) = -1.0; + } + + msg.twist.covariance[0] = covVel_local(0, 0); + msg.twist.covariance[1] = covVel_local(0, 1); + msg.twist.covariance[2] = covVel_local(0, 2); + msg.twist.covariance[6] = covVel_local(1, 0); + msg.twist.covariance[7] = covVel_local(1, 1); + msg.twist.covariance[8] = covVel_local(1, 2); + msg.twist.covariance[12] = covVel_local(2, 0); + msg.twist.covariance[13] = covVel_local(2, 1); + msg.twist.covariance[14] = covVel_local(2, 2); + } else + { + msg.twist.covariance[0] = -1.0; + msg.twist.covariance[7] = -1.0; + msg.twist.covariance[14] = -1.0; } - // Linear velocity - msg.twist.twist.linear.x = vel(0); - msg.twist.twist.linear.y = vel(1); - msg.twist.twist.linear.z = vel(2); + // Autocovariances of angular velocity + msg.twist.covariance[21] = -1.0; + msg.twist.covariance[28] = -1.0; + msg.twist.covariance[35] = -1.0; + } else { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); - } + msg.header = last_pvtgeodetic_.header; - if (((last_insnavgeod_.sb_list & 16) != 0) && - ((last_insnavgeod_.sb_list & 2) != 0) && - ((last_insnavgeod_.sb_list & 8) != 0)) - { - Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); - if ((last_insnavgeod_.sb_list & 128) != 0) + if (last_pvtgeodetic_.error == 0) + { + // Linear velocity in navigation frame + double ve = 0.0; + if (validValue(last_pvtgeodetic_.ve)) + ve = last_pvtgeodetic_.ve; + double vn = 0.0; + if (validValue(last_pvtgeodetic_.vn)) + vn = last_pvtgeodetic_.vn; + double vu = 0.0; + if (validValue(last_pvtgeodetic_.vu)) + vu = last_pvtgeodetic_.vu; + Eigen::Vector3d vel; + if (settings_->use_ros_axis_orientation) + { + // (ENU) + vel << ve, vn, vu; + } else + { + // (NED) + vel << vn, ve, -vu; + } + // Linear velocity + msg.twist.twist.linear.x = vel(0); + msg.twist.twist.linear.y = vel(1); + msg.twist.twist.linear.z = vel(2); + } else { - // Linear velocity covariance - if (validValue(last_insnavgeod_.ve_std_dev)) + msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + } + + if (last_velcovgeodetic_.error == 0) + { + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); + // Linear velocity covariance in navigation frame + if (validValue(last_velcovgeodetic_.cov_veve)) if (settings_->use_ros_axis_orientation) - covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); + covVel_local(0, 0) = last_velcovgeodetic_.cov_veve; else - covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); + covVel_local(1, 1) = last_velcovgeodetic_.cov_veve; else covVel_local(0, 0) = -1.0; - if (validValue(last_insnavgeod_.vn_std_dev)) + if (validValue(last_velcovgeodetic_.cov_vnvn)) if (settings_->use_ros_axis_orientation) - covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); + covVel_local(1, 1) = last_velcovgeodetic_.cov_vnvn; else - covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); + covVel_local(0, 0) = last_velcovgeodetic_.cov_vnvn; else covVel_local(1, 1) = -1.0; - if (validValue(last_insnavgeod_.vu_std_dev)) - covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); + if (validValue(last_velcovgeodetic_.cov_vuvu)) + covVel_local(2, 2) = last_velcovgeodetic_.cov_vuvu; else covVel_local(2, 2) = -1.0; - if (validValue(last_insnavgeod_.ve_vn_cov)) - covVel_local(0, 1) = covVel_local(1, 0) = - last_insnavgeod_.ve_vn_cov; + covVel_local(0, 1) = covVel_local(1, 0) = + last_velcovgeodetic_.cov_vnve; if (settings_->use_ros_axis_orientation) { - if (validValue(last_insnavgeod_.ve_vu_cov)) - covVel_local(0, 2) = covVel_local(2, 0) = - last_insnavgeod_.ve_vu_cov; - if (validValue(last_insnavgeod_.vn_vu_cov)) - covVel_local(2, 1) = covVel_local(1, 2) = - last_insnavgeod_.vn_vu_cov; + covVel_local(0, 2) = covVel_local(2, 0) = + last_velcovgeodetic_.cov_vevu; + covVel_local(2, 1) = covVel_local(1, 2) = + last_velcovgeodetic_.cov_vnvu; } else { - if (validValue(last_insnavgeod_.vn_vu_cov)) - covVel_local(0, 2) = covVel_local(2, 0) = - -last_insnavgeod_.vn_vu_cov; - if (validValue(last_insnavgeod_.ve_vu_cov)) - covVel_local(2, 1) = covVel_local(1, 2) = - -last_insnavgeod_.ve_vu_cov; + covVel_local(0, 2) = covVel_local(2, 0) = + -last_velcovgeodetic_.cov_vnvu; + covVel_local(2, 1) = covVel_local(1, 2) = + -last_velcovgeodetic_.cov_vevu; } + + msg.twist.covariance[0] = covVel_local(0, 0); + msg.twist.covariance[1] = covVel_local(0, 1); + msg.twist.covariance[2] = covVel_local(0, 2); + msg.twist.covariance[6] = covVel_local(1, 0); + msg.twist.covariance[7] = covVel_local(1, 1); + msg.twist.covariance[8] = covVel_local(1, 2); + msg.twist.covariance[12] = covVel_local(2, 0); + msg.twist.covariance[13] = covVel_local(2, 1); + msg.twist.covariance[14] = covVel_local(2, 2); } else { - covVel_local(0, 0) = -1.0; - covVel_local(1, 1) = -1.0; - covVel_local(2, 2) = -1.0; + msg.twist.covariance[0] = -1.0; + msg.twist.covariance[7] = -1.0; + msg.twist.covariance[14] = -1.0; } - - msg.twist.covariance[0] = covVel_local(0, 0); - msg.twist.covariance[1] = covVel_local(0, 1); - msg.twist.covariance[2] = covVel_local(0, 2); - msg.twist.covariance[6] = covVel_local(1, 0); - msg.twist.covariance[7] = covVel_local(1, 1); - msg.twist.covariance[8] = covVel_local(1, 2); - msg.twist.covariance[12] = covVel_local(2, 0); - msg.twist.covariance[13] = covVel_local(2, 1); - msg.twist.covariance[14] = covVel_local(2, 2); - } else - { - msg.twist.covariance[0] = -1.0; - msg.twist.covariance[7] = -1.0; - msg.twist.covariance[14] = -1.0; + // Autocovariances of angular velocity + msg.twist.covariance[21] = -1.0; + msg.twist.covariance[28] = -1.0; + msg.twist.covariance[35] = -1.0; } - // Autocovariances of angular velocity - msg.twist.covariance[21] = -1.0; - msg.twist.covariance[28] = -1.0; - msg.twist.covariance[35] = -1.0; - } else + msg.header.frame_id = "navigation"; + + return msg; + }; + + /** + * Localization in UTM coordinates. Yaw angle is converted from true north to + * grid north. Linear velocity of twist in body frame as per msg definition. + * Angular velocity not available, thus according autocovariances are set to + * -1.0. + */ + LocalizationMsg MessageParser::LocalizationUtmCallback() { - msg.header = last_pvtgeodetic_.header; + LocalizationMsg msg; - if (last_pvtgeodetic_.error == 0) - { - // Linear velocity in navigation frame - double ve = 0.0; - if (validValue(last_pvtgeodetic_.ve)) - ve = last_pvtgeodetic_.ve; - double vn = 0.0; - if (validValue(last_pvtgeodetic_.vn)) - vn = last_pvtgeodetic_.vn; - double vu = 0.0; - if (validValue(last_pvtgeodetic_.vu)) - vu = last_pvtgeodetic_.vu; - Eigen::Vector3d vel; - if (settings_->use_ros_axis_orientation) - { - // (ENU) - vel << ve, vn, vu; - } else - { - // (NED) - vel << vn, ve, -vu; - } - // Linear velocity - msg.twist.twist.linear.x = vel(0); - msg.twist.twist.linear.y = vel(1); - msg.twist.twist.linear.z = vel(2); + int zone; + std::string zonestring; + bool northernHemisphere; + double easting; + double northing; + double meridian_convergence = 0.0; + if (fixedUtmZone_) + { + double k; + GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, + northernHemisphere); + GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), zone, + northernHemisphere, easting, northing, + meridian_convergence, k, zone); + zonestring = *fixedUtmZone_; } else { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + double k; + GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), zone, + northernHemisphere, easting, northing, + meridian_convergence, k); + zonestring = GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); } + if (settings_->lock_utm_zone && !fixedUtmZone_) + fixedUtmZone_ = std::make_shared(zonestring); - if (last_velcovgeodetic_.error == 0) + // UTM position (ENU) + if (settings_->use_ros_axis_orientation) { - Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); - // Linear velocity covariance in navigation frame - if (validValue(last_velcovgeodetic_.cov_veve)) - if (settings_->use_ros_axis_orientation) - covVel_local(0, 0) = last_velcovgeodetic_.cov_veve; - else - covVel_local(1, 1) = last_velcovgeodetic_.cov_veve; - else - covVel_local(0, 0) = -1.0; - if (validValue(last_velcovgeodetic_.cov_vnvn)) - if (settings_->use_ros_axis_orientation) - covVel_local(1, 1) = last_velcovgeodetic_.cov_vnvn; - else - covVel_local(0, 0) = last_velcovgeodetic_.cov_vnvn; - else - covVel_local(1, 1) = -1.0; - if (validValue(last_velcovgeodetic_.cov_vuvu)) - covVel_local(2, 2) = last_velcovgeodetic_.cov_vuvu; - else - covVel_local(2, 2) = -1.0; - - covVel_local(0, 1) = covVel_local(1, 0) = last_velcovgeodetic_.cov_vnve; - if (settings_->use_ros_axis_orientation) - { - covVel_local(0, 2) = covVel_local(2, 0) = - last_velcovgeodetic_.cov_vevu; - covVel_local(2, 1) = covVel_local(1, 2) = - last_velcovgeodetic_.cov_vnvu; - } else - { - covVel_local(0, 2) = covVel_local(2, 0) = - -last_velcovgeodetic_.cov_vnvu; - covVel_local(2, 1) = covVel_local(1, 2) = - -last_velcovgeodetic_.cov_vevu; - } - - msg.twist.covariance[0] = covVel_local(0, 0); - msg.twist.covariance[1] = covVel_local(0, 1); - msg.twist.covariance[2] = covVel_local(0, 2); - msg.twist.covariance[6] = covVel_local(1, 0); - msg.twist.covariance[7] = covVel_local(1, 1); - msg.twist.covariance[8] = covVel_local(1, 2); - msg.twist.covariance[12] = covVel_local(2, 0); - msg.twist.covariance[13] = covVel_local(2, 1); - msg.twist.covariance[14] = covVel_local(2, 2); - } else + msg.pose.pose.position.x = easting; + msg.pose.pose.position.y = northing; + msg.pose.pose.position.z = last_insnavgeod_.height; + } else // (NED) { - msg.twist.covariance[0] = -1.0; - msg.twist.covariance[7] = -1.0; - msg.twist.covariance[14] = -1.0; + msg.pose.pose.position.x = northing; + msg.pose.pose.position.y = easting; + msg.pose.pose.position.z = -last_insnavgeod_.height; } - // Autocovariances of angular velocity - msg.twist.covariance[21] = -1.0; - msg.twist.covariance[28] = -1.0; - msg.twist.covariance[35] = -1.0; - } - - msg.header.frame_id = "navigation"; - - return msg; -}; - -/** - * Localization in UTM coordinates. Yaw angle is converted from true north to grid - * north. Linear velocity of twist in body frame as per msg definition. Angular - * velocity not available, thus according autocovariances are set to -1.0. - */ -LocalizationMsg io::RxMessage::LocalizationUtmCallback() -{ - LocalizationMsg msg; - - int zone; - std::string zonestring; - bool northernHemisphere; - double easting; - double northing; - double meridian_convergence = 0.0; - if (fixedUtmZone_) - { - double k; - GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, northernHemisphere); - GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), - rad2deg(last_insnavgeod_.longitude), zone, - northernHemisphere, easting, northing, - meridian_convergence, k, zone); - zonestring = *fixedUtmZone_; - } else - { - double k; - GeographicLib::UTMUPS::Forward( - rad2deg(last_insnavgeod_.latitude), rad2deg(last_insnavgeod_.longitude), - zone, northernHemisphere, easting, northing, meridian_convergence, k); - zonestring = GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); - } - if (settings_->lock_utm_zone && !fixedUtmZone_) - fixedUtmZone_ = std::make_shared(zonestring); - - // UTM position (ENU) - if (settings_->use_ros_axis_orientation) - { - msg.pose.pose.position.x = easting; - msg.pose.pose.position.y = northing; - msg.pose.pose.position.z = last_insnavgeod_.height; - } else // (NED) - { - msg.pose.pose.position.x = northing; - msg.pose.pose.position.y = easting; - msg.pose.pose.position.z = -last_insnavgeod_.height; - } - msg.header.frame_id = "utm_" + zonestring; - if (settings_->ins_use_poi) - msg.child_frame_id = settings_->poi_frame_id; // TODO param - else - msg.child_frame_id = settings_->frame_id; + msg.header.frame_id = "utm_" + zonestring; + if (settings_->ins_use_poi) + msg.child_frame_id = settings_->poi_frame_id; // TODO param + else + msg.child_frame_id = settings_->frame_id; - Eigen::Matrix3d P_pos = -Eigen::Matrix3d::Identity(); - if ((last_insnavgeod_.sb_list & 1) != 0) - { - // Position autocovariance - P_pos(0, 0) = square(last_insnavgeod_.longitude_std_dev); - P_pos(1, 1) = square(last_insnavgeod_.latitude_std_dev); - P_pos(2, 2) = square(last_insnavgeod_.height_std_dev); - } + Eigen::Matrix3d P_pos = -Eigen::Matrix3d::Identity(); + if ((last_insnavgeod_.sb_list & 1) != 0) + { + // Position autocovariance + P_pos(0, 0) = square(last_insnavgeod_.longitude_std_dev); + P_pos(1, 1) = square(last_insnavgeod_.latitude_std_dev); + P_pos(2, 2) = square(last_insnavgeod_.height_std_dev); + } - // Euler angles - double roll = 0.0; - if (validValue(last_insnavgeod_.roll)) - roll = deg2rad(last_insnavgeod_.roll); - double pitch = 0.0; - if (validValue(last_insnavgeod_.pitch)) - pitch = deg2rad(last_insnavgeod_.pitch); - double yaw = 0.0; - if (validValue(last_insnavgeod_.heading)) - yaw = deg2rad(last_insnavgeod_.heading); - // meridian_convergence for conversion from true north to grid north - if (settings_->use_ros_axis_orientation) - yaw += deg2rad(meridian_convergence); - else - yaw -= deg2rad(meridian_convergence); - - if ((last_insnavgeod_.sb_list & 2) != 0) - { - // Attitude - msg.pose.pose.orientation = convertEulerToQuaternionMsg(roll, pitch, yaw); - } else - { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); - } - if ((last_insnavgeod_.sb_list & 4) != 0) - { - // Attitude autocovariance - if (validValue(last_insnavgeod_.roll_std_dev)) - msg.pose.covariance[21] = square(deg2rad(last_insnavgeod_.roll_std_dev)); + // Euler angles + double roll = 0.0; + if (validValue(last_insnavgeod_.roll)) + roll = deg2rad(last_insnavgeod_.roll); + double pitch = 0.0; + if (validValue(last_insnavgeod_.pitch)) + pitch = deg2rad(last_insnavgeod_.pitch); + double yaw = 0.0; + if (validValue(last_insnavgeod_.heading)) + yaw = deg2rad(last_insnavgeod_.heading); + // meridian_convergence for conversion from true north to grid north + if (settings_->use_ros_axis_orientation) + yaw += deg2rad(meridian_convergence); else + yaw -= deg2rad(meridian_convergence); + + if ((last_insnavgeod_.sb_list & 2) != 0) + { + // Attitude + msg.pose.pose.orientation = + convertEulerToQuaternionMsg(roll, pitch, yaw); + } else + { + msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + } + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocovariance + if (validValue(last_insnavgeod_.roll_std_dev)) + msg.pose.covariance[21] = + square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + msg.pose.covariance[21] = -1.0; + if (validValue(last_insnavgeod_.pitch_std_dev)) + msg.pose.covariance[28] = + square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + msg.pose.covariance[28] = -1.0; + if (validValue(last_insnavgeod_.heading_std_dev)) + msg.pose.covariance[35] = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + msg.pose.covariance[35] = -1.0; + } else + { msg.pose.covariance[21] = -1.0; - if (validValue(last_insnavgeod_.pitch_std_dev)) - msg.pose.covariance[28] = - square(deg2rad(last_insnavgeod_.pitch_std_dev)); - else msg.pose.covariance[28] = -1.0; - if (validValue(last_insnavgeod_.heading_std_dev)) - msg.pose.covariance[35] = - square(deg2rad(last_insnavgeod_.heading_std_dev)); - else msg.pose.covariance[35] = -1.0; - } else - { - msg.pose.covariance[21] = -1.0; - msg.pose.covariance[28] = -1.0; - msg.pose.covariance[35] = -1.0; - } - - if ((last_insnavgeod_.sb_list & 32) != 0) - { - // Position covariance - P_pos(0, 1) = last_insnavgeod_.latitude_longitude_cov; - P_pos(1, 0) = last_insnavgeod_.latitude_longitude_cov; + } - if (settings_->use_ros_axis_orientation) + if ((last_insnavgeod_.sb_list & 32) != 0) { - // (ENU) - P_pos(0, 2) = last_insnavgeod_.longitude_height_cov; - P_pos(1, 2) = last_insnavgeod_.latitude_height_cov; - P_pos(2, 0) = last_insnavgeod_.longitude_height_cov; - P_pos(2, 1) = last_insnavgeod_.latitude_height_cov; - } else + // Position covariance + P_pos(0, 1) = last_insnavgeod_.latitude_longitude_cov; + P_pos(1, 0) = last_insnavgeod_.latitude_longitude_cov; + + if (settings_->use_ros_axis_orientation) + { + // (ENU) + P_pos(0, 2) = last_insnavgeod_.longitude_height_cov; + P_pos(1, 2) = last_insnavgeod_.latitude_height_cov; + P_pos(2, 0) = last_insnavgeod_.longitude_height_cov; + P_pos(2, 1) = last_insnavgeod_.latitude_height_cov; + } else + { + // (NED): down = -height + P_pos(0, 2) = -last_insnavgeod_.latitude_height_cov; + P_pos(1, 2) = -last_insnavgeod_.longitude_height_cov; + P_pos(2, 0) = -last_insnavgeod_.latitude_height_cov; + P_pos(2, 1) = -last_insnavgeod_.longitude_height_cov; + } + } + + if ((meridian_convergence != 0.0) && (last_insnavgeod_.sb_list & 1)) { - // (NED): down = -height - P_pos(0, 2) = -last_insnavgeod_.latitude_height_cov; - P_pos(1, 2) = -last_insnavgeod_.longitude_height_cov; - P_pos(2, 0) = -last_insnavgeod_.latitude_height_cov; - P_pos(2, 1) = -last_insnavgeod_.longitude_height_cov; + double cg = std::cos(meridian_convergence); + double sg = std::sin(meridian_convergence); + Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); + R(0, 0) = cg; + R(0, 1) = -sg; + R(1, 0) = sg; + R(1, 1) = cg; + P_pos = (R * P_pos * R.transpose()).eval(); } - } - if ((meridian_convergence != 0.0) && (last_insnavgeod_.sb_list & 1)) - { - double cg = std::cos(meridian_convergence); - double sg = std::sin(meridian_convergence); - Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); - R(0, 0) = cg; - R(0, 1) = -sg; - R(1, 0) = sg; - R(1, 1) = cg; - P_pos = (R * P_pos * R.transpose()).eval(); - } + msg.pose.covariance[0] = P_pos(0, 0); + msg.pose.covariance[1] = P_pos(0, 1); + msg.pose.covariance[2] = P_pos(0, 2); + msg.pose.covariance[6] = P_pos(1, 0); + msg.pose.covariance[7] = P_pos(1, 1); + msg.pose.covariance[8] = P_pos(1, 2); + msg.pose.covariance[12] = P_pos(2, 0); + msg.pose.covariance[13] = P_pos(2, 1); + msg.pose.covariance[14] = P_pos(2, 2); - msg.pose.covariance[0] = P_pos(0, 0); - msg.pose.covariance[1] = P_pos(0, 1); - msg.pose.covariance[2] = P_pos(0, 2); - msg.pose.covariance[6] = P_pos(1, 0); - msg.pose.covariance[7] = P_pos(1, 1); - msg.pose.covariance[8] = P_pos(1, 2); - msg.pose.covariance[12] = P_pos(2, 0); - msg.pose.covariance[13] = P_pos(2, 1); - msg.pose.covariance[14] = P_pos(2, 2); - - if ((last_insnavgeod_.sb_list & 64) != 0) - { - // Attitude covariancae - msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - - msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); - } + if ((last_insnavgeod_.sb_list & 64) != 0) + { + // Attitude covariancae + msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov); - fillLocalizationMsgTwist(roll, pitch, yaw, msg); + msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); + } - return msg; -}; + fillLocalizationMsgTwist(roll, pitch, yaw, msg); -/** - * Localization in ECEF coordinates. Attitude is converted from local to ECEF - * Linear velocity of twist in body frame as per msg definition. Angular - * velocity not available, thus according autocovariances are set to -1.0. - */ -LocalizationMsg io::RxMessage::LocalizationEcefCallback() -{ - LocalizationMsg msg; - - msg.header.frame_id = "ecef"; - if (settings_->ins_use_poi) - msg.child_frame_id = settings_->poi_frame_id; // TODO param - else - msg.child_frame_id = settings_->frame_id; - - // ECEF position - msg.pose.pose.position.x = last_insnavcart_.x; - msg.pose.pose.position.y = last_insnavcart_.y; - msg.pose.pose.position.z = last_insnavcart_.z; - - if ((last_insnavgeod_.sb_list & 1) != 0) - { - // Position autocovariance - msg.pose.covariance[0] = square(last_insnavcart_.x_std_dev); - msg.pose.covariance[7] = square(last_insnavcart_.y_std_dev); - msg.pose.covariance[14] = square(last_insnavcart_.z_std_dev); - } else - { - msg.pose.covariance[0] = -1.0; - msg.pose.covariance[7] = -1.0; - msg.pose.covariance[14] = -1.0; - } - if ((last_insnavcart_.sb_list & 32) != 0) - { - // Position covariance - msg.pose.covariance[1] = last_insnavcart_.xy_cov; - msg.pose.covariance[6] = last_insnavcart_.xy_cov; - msg.pose.covariance[2] = last_insnavcart_.xz_cov; - msg.pose.covariance[8] = last_insnavcart_.yz_cov; - msg.pose.covariance[12] = last_insnavcart_.xz_cov; - msg.pose.covariance[13] = last_insnavcart_.yz_cov; - } + return msg; + }; - // Euler angles - double roll = 0.0; - if (validValue(last_insnavcart_.roll)) - roll = deg2rad(last_insnavcart_.roll); - double pitch = 0.0; - if (validValue(last_insnavcart_.pitch)) - pitch = deg2rad(last_insnavcart_.pitch); - double yaw = 0.0; - if (validValue(last_insnavcart_.heading)) - yaw = deg2rad(last_insnavcart_.heading); - - if ((last_insnavcart_.sb_list & 2) != 0) + /** + * Localization in ECEF coordinates. Attitude is converted from local to ECEF + * Linear velocity of twist in body frame as per msg definition. Angular + * velocity not available, thus according autocovariances are set to -1.0. + */ + LocalizationMsg MessageParser::LocalizationEcefCallback() { - // Attitude - Eigen::Quaterniond q_local_ecef; - if (settings_->use_ros_axis_orientation) - q_local_ecef = parsing_utilities::q_enu_ecef(last_insnavgeod_.latitude, - last_insnavgeod_.longitude); + LocalizationMsg msg; + + msg.header.frame_id = "ecef"; + if (settings_->ins_use_poi) + msg.child_frame_id = settings_->poi_frame_id; // TODO param else - q_local_ecef = parsing_utilities::q_ned_ecef(last_insnavgeod_.latitude, - last_insnavgeod_.longitude); - Eigen::Quaterniond q_b_local = - parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw); + msg.child_frame_id = settings_->frame_id; - Eigen::Quaterniond q_b_ecef = q_local_ecef * q_b_local; + // ECEF position + msg.pose.pose.position.x = last_insnavcart_.x; + msg.pose.pose.position.y = last_insnavcart_.y; + msg.pose.pose.position.z = last_insnavcart_.z; - msg.pose.pose.orientation = - parsing_utilities::quaternionToQuaternionMsg(q_b_ecef); - } else - { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); - } - Eigen::Matrix3d covAtt_local = Eigen::Matrix3d::Zero(); - bool covAttValid = true; - if ((last_insnavgeod_.sb_list & 4) != 0) - { - // Attitude autocovariance - if (validValue(last_insnavgeod_.roll_std_dev)) - covAtt_local(0, 0) = square(deg2rad(last_insnavgeod_.roll_std_dev)); - else + if ((last_insnavgeod_.sb_list & 1) != 0) { - covAtt_local(0, 0) = -1.0; - covAttValid = false; + // Position autocovariance + msg.pose.covariance[0] = square(last_insnavcart_.x_std_dev); + msg.pose.covariance[7] = square(last_insnavcart_.y_std_dev); + msg.pose.covariance[14] = square(last_insnavcart_.z_std_dev); + } else + { + msg.pose.covariance[0] = -1.0; + msg.pose.covariance[7] = -1.0; + msg.pose.covariance[14] = -1.0; } - if (validValue(last_insnavgeod_.pitch_std_dev)) - covAtt_local(1, 1) = square(deg2rad(last_insnavgeod_.pitch_std_dev)); - else + if ((last_insnavcart_.sb_list & 32) != 0) { - covAtt_local(1, 1) = -1.0; - covAttValid = false; + // Position covariance + msg.pose.covariance[1] = last_insnavcart_.xy_cov; + msg.pose.covariance[6] = last_insnavcart_.xy_cov; + msg.pose.covariance[2] = last_insnavcart_.xz_cov; + msg.pose.covariance[8] = last_insnavcart_.yz_cov; + msg.pose.covariance[12] = last_insnavcart_.xz_cov; + msg.pose.covariance[13] = last_insnavcart_.yz_cov; } - if (validValue(last_insnavgeod_.heading_std_dev)) - covAtt_local(2, 2) = square(deg2rad(last_insnavgeod_.heading_std_dev)); - else + + // Euler angles + double roll = 0.0; + if (validValue(last_insnavcart_.roll)) + roll = deg2rad(last_insnavcart_.roll); + double pitch = 0.0; + if (validValue(last_insnavcart_.pitch)) + pitch = deg2rad(last_insnavcart_.pitch); + double yaw = 0.0; + if (validValue(last_insnavcart_.heading)) + yaw = deg2rad(last_insnavcart_.heading); + + if ((last_insnavcart_.sb_list & 2) != 0) { + // Attitude + Eigen::Quaterniond q_local_ecef; + if (settings_->use_ros_axis_orientation) + q_local_ecef = parsing_utilities::q_enu_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + else + q_local_ecef = parsing_utilities::q_ned_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + Eigen::Quaterniond q_b_local = + parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw); + + Eigen::Quaterniond q_b_ecef = q_local_ecef * q_b_local; + + msg.pose.pose.orientation = + parsing_utilities::quaternionToQuaternionMsg(q_b_ecef); + } else + { + msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + } + Eigen::Matrix3d covAtt_local = Eigen::Matrix3d::Zero(); + bool covAttValid = true; + if ((last_insnavgeod_.sb_list & 4) != 0) + { + // Attitude autocovariance + if (validValue(last_insnavgeod_.roll_std_dev)) + covAtt_local(0, 0) = square(deg2rad(last_insnavgeod_.roll_std_dev)); + else + { + covAtt_local(0, 0) = -1.0; + covAttValid = false; + } + if (validValue(last_insnavgeod_.pitch_std_dev)) + covAtt_local(1, 1) = square(deg2rad(last_insnavgeod_.pitch_std_dev)); + else + { + covAtt_local(1, 1) = -1.0; + covAttValid = false; + } + if (validValue(last_insnavgeod_.heading_std_dev)) + covAtt_local(2, 2) = + square(deg2rad(last_insnavgeod_.heading_std_dev)); + else + { + covAtt_local(2, 2) = -1.0; + covAttValid = false; + } + } else + { + covAtt_local(0, 0) = -1.0; + covAtt_local(1, 1) = -1.0; covAtt_local(2, 2) = -1.0; covAttValid = false; } - } else - { - covAtt_local(0, 0) = -1.0; - covAtt_local(1, 1) = -1.0; - covAtt_local(2, 2) = -1.0; - covAttValid = false; - } - if (covAttValid) - { - if ((last_insnavcart_.sb_list & 64) != 0) + if (covAttValid) { - // Attitude covariancae - covAtt_local(0, 1) = deg2radSq(last_insnavcart_.pitch_roll_cov); - covAtt_local(0, 2) = deg2radSq(last_insnavcart_.heading_roll_cov); - covAtt_local(1, 0) = deg2radSq(last_insnavcart_.pitch_roll_cov); - covAtt_local(2, 1) = deg2radSq(last_insnavcart_.heading_pitch_cov); - covAtt_local(2, 0) = deg2radSq(last_insnavcart_.heading_roll_cov); - covAtt_local(1, 2) = deg2radSq(last_insnavcart_.heading_pitch_cov); - } + if ((last_insnavcart_.sb_list & 64) != 0) + { + // Attitude covariancae + covAtt_local(0, 1) = deg2radSq(last_insnavcart_.pitch_roll_cov); + covAtt_local(0, 2) = deg2radSq(last_insnavcart_.heading_roll_cov); + covAtt_local(1, 0) = deg2radSq(last_insnavcart_.pitch_roll_cov); + covAtt_local(2, 1) = deg2radSq(last_insnavcart_.heading_pitch_cov); + covAtt_local(2, 0) = deg2radSq(last_insnavcart_.heading_roll_cov); + covAtt_local(1, 2) = deg2radSq(last_insnavcart_.heading_pitch_cov); + } - Eigen::Matrix3d R_local_ecef; - if (settings_->use_ros_axis_orientation) - R_local_ecef = parsing_utilities::R_enu_ecef(last_insnavgeod_.latitude, - last_insnavgeod_.longitude); - else - R_local_ecef = parsing_utilities::R_ned_ecef(last_insnavgeod_.latitude, - last_insnavgeod_.longitude); - // Rotate attitude covariance matrix to ecef coordinates - Eigen::Matrix3d covAtt_ecef = - R_local_ecef * covAtt_local * R_local_ecef.transpose(); - - msg.pose.covariance[21] = covAtt_ecef(0, 0); - msg.pose.covariance[22] = covAtt_ecef(0, 1); - msg.pose.covariance[23] = covAtt_ecef(0, 2); - msg.pose.covariance[27] = covAtt_ecef(1, 0); - msg.pose.covariance[28] = covAtt_ecef(1, 1); - msg.pose.covariance[29] = covAtt_ecef(1, 2); - msg.pose.covariance[33] = covAtt_ecef(2, 0); - msg.pose.covariance[34] = covAtt_ecef(2, 1); - msg.pose.covariance[35] = covAtt_ecef(2, 2); - } else - { - msg.pose.covariance[21] = -1.0; - msg.pose.covariance[28] = -1.0; - msg.pose.covariance[35] = -1.0; - } + Eigen::Matrix3d R_local_ecef; + if (settings_->use_ros_axis_orientation) + R_local_ecef = parsing_utilities::R_enu_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + else + R_local_ecef = parsing_utilities::R_ned_ecef( + last_insnavgeod_.latitude, last_insnavgeod_.longitude); + // Rotate attitude covariance matrix to ecef coordinates + Eigen::Matrix3d covAtt_ecef = + R_local_ecef * covAtt_local * R_local_ecef.transpose(); + + msg.pose.covariance[21] = covAtt_ecef(0, 0); + msg.pose.covariance[22] = covAtt_ecef(0, 1); + msg.pose.covariance[23] = covAtt_ecef(0, 2); + msg.pose.covariance[27] = covAtt_ecef(1, 0); + msg.pose.covariance[28] = covAtt_ecef(1, 1); + msg.pose.covariance[29] = covAtt_ecef(1, 2); + msg.pose.covariance[33] = covAtt_ecef(2, 0); + msg.pose.covariance[34] = covAtt_ecef(2, 1); + msg.pose.covariance[35] = covAtt_ecef(2, 2); + } else + { + msg.pose.covariance[21] = -1.0; + msg.pose.covariance[28] = -1.0; + msg.pose.covariance[35] = -1.0; + } - fillLocalizationMsgTwist(roll, pitch, yaw, msg); + fillLocalizationMsgTwist(roll, pitch, yaw, msg); - return msg; -}; + return msg; + }; -void io::RxMessage::fillLocalizationMsgTwist(double roll, double pitch, - double yaw, - LocalizationMsg& msg) -{ - Eigen::Matrix3d R_local_body = - parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); - if ((last_insnavgeod_.sb_list & 8) != 0) + void MessageParser::fillLocalizationMsgTwist(double roll, double pitch, + double yaw, LocalizationMsg& msg) { - // Linear velocity (ENU) - double ve = 0.0; - if (validValue(last_insnavgeod_.ve)) - ve = last_insnavgeod_.ve; - double vn = 0.0; - if (validValue(last_insnavgeod_.vn)) - vn = last_insnavgeod_.vn; - double vu = 0.0; - if (validValue(last_insnavgeod_.vu)) - vu = last_insnavgeod_.vu; - Eigen::Vector3d vel_local; - if (settings_->use_ros_axis_orientation) + Eigen::Matrix3d R_local_body = + parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); + if ((last_insnavgeod_.sb_list & 8) != 0) { - // (ENU) - vel_local << ve, vn, vu; + // Linear velocity (ENU) + double ve = 0.0; + if (validValue(last_insnavgeod_.ve)) + ve = last_insnavgeod_.ve; + double vn = 0.0; + if (validValue(last_insnavgeod_.vn)) + vn = last_insnavgeod_.vn; + double vu = 0.0; + if (validValue(last_insnavgeod_.vu)) + vu = last_insnavgeod_.vu; + Eigen::Vector3d vel_local; + if (settings_->use_ros_axis_orientation) + { + // (ENU) + vel_local << ve, vn, vu; + } else + { + // (NED) + vel_local << vn, ve, -vu; + } + // Linear velocity, rotate to body coordinates + Eigen::Vector3d vel_body = R_local_body * vel_local; + msg.twist.twist.linear.x = vel_body(0); + msg.twist.twist.linear.y = vel_body(1); + msg.twist.twist.linear.z = vel_body(2); } else { - // (NED) - vel_local << vn, ve, -vu; + msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); } - // Linear velocity, rotate to body coordinates - Eigen::Vector3d vel_body = R_local_body * vel_local; - msg.twist.twist.linear.x = vel_body(0); - msg.twist.twist.linear.y = vel_body(1); - msg.twist.twist.linear.z = vel_body(2); - } else - { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); - } - Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); - if ((last_insnavgeod_.sb_list & 16) != 0) - { - // Linear velocity autocovariance - if (validValue(last_insnavgeod_.ve_std_dev)) - if (settings_->use_ros_axis_orientation) - covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); + Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); + if ((last_insnavgeod_.sb_list & 16) != 0) + { + // Linear velocity autocovariance + if (validValue(last_insnavgeod_.ve_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev); + else + covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); else - covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev); - else - covVel_local(0, 0) = -1.0; - if (validValue(last_insnavgeod_.vn_std_dev)) - if (settings_->use_ros_axis_orientation) - covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); + covVel_local(0, 0) = -1.0; + if (validValue(last_insnavgeod_.vn_std_dev)) + if (settings_->use_ros_axis_orientation) + covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev); + else + covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); else - covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev); - else + covVel_local(1, 1) = -1.0; + if (validValue(last_insnavgeod_.vu_std_dev)) + covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); + else + covVel_local(2, 2) = -1.0; + } else + { + covVel_local(0, 0) = -1.0; covVel_local(1, 1) = -1.0; - if (validValue(last_insnavgeod_.vu_std_dev)) - covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev); - else covVel_local(2, 2) = -1.0; - } else - { - covVel_local(0, 0) = -1.0; - covVel_local(1, 1) = -1.0; - covVel_local(2, 2) = -1.0; - } + } - if ((last_insnavgeod_.sb_list & 128) != 0) - { - covVel_local(0, 1) = covVel_local(1, 0) = last_insnavgeod_.ve_vn_cov; - if (settings_->use_ros_axis_orientation) + if ((last_insnavgeod_.sb_list & 128) != 0) { - covVel_local(0, 2) = covVel_local(2, 0) = last_insnavgeod_.ve_vu_cov; - covVel_local(2, 1) = covVel_local(1, 2) = last_insnavgeod_.vn_vu_cov; + covVel_local(0, 1) = covVel_local(1, 0) = last_insnavgeod_.ve_vn_cov; + if (settings_->use_ros_axis_orientation) + { + covVel_local(0, 2) = covVel_local(2, 0) = last_insnavgeod_.ve_vu_cov; + covVel_local(2, 1) = covVel_local(1, 2) = last_insnavgeod_.vn_vu_cov; + } else + { + covVel_local(0, 2) = covVel_local(2, 0) = + -last_insnavgeod_.vn_vu_cov; + covVel_local(2, 1) = covVel_local(1, 2) = + -last_insnavgeod_.ve_vu_cov; + } + } + + if (((last_insnavgeod_.sb_list & 16) != 0) && + ((last_insnavgeod_.sb_list & 2) != 0) && + ((last_insnavgeod_.sb_list & 8) != 0) && + validValue(last_insnavgeod_.ve_std_dev) && + validValue(last_insnavgeod_.vn_std_dev) && + validValue(last_insnavgeod_.vu_std_dev)) + { + // Rotate velocity covariance matrix to body coordinates + Eigen::Matrix3d covVel_body = + R_local_body * covVel_local * R_local_body.transpose(); + + msg.twist.covariance[0] = covVel_body(0, 0); + msg.twist.covariance[1] = covVel_body(0, 1); + msg.twist.covariance[2] = covVel_body(0, 2); + msg.twist.covariance[6] = covVel_body(1, 0); + msg.twist.covariance[7] = covVel_body(1, 1); + msg.twist.covariance[8] = covVel_body(1, 2); + msg.twist.covariance[12] = covVel_body(2, 0); + msg.twist.covariance[13] = covVel_body(2, 1); + msg.twist.covariance[14] = covVel_body(2, 2); } else { - covVel_local(0, 2) = covVel_local(2, 0) = -last_insnavgeod_.vn_vu_cov; - covVel_local(2, 1) = covVel_local(1, 2) = -last_insnavgeod_.ve_vu_cov; + msg.twist.covariance[0] = -1.0; + msg.twist.covariance[7] = -1.0; + msg.twist.covariance[14] = -1.0; } + // Autocovariances of angular velocity + msg.twist.covariance[21] = -1.0; + msg.twist.covariance[28] = -1.0; + msg.twist.covariance[35] = -1.0; } - if (((last_insnavgeod_.sb_list & 16) != 0) && - ((last_insnavgeod_.sb_list & 2) != 0) && - ((last_insnavgeod_.sb_list & 8) != 0) && - validValue(last_insnavgeod_.ve_std_dev) && - validValue(last_insnavgeod_.vn_std_dev) && - validValue(last_insnavgeod_.vu_std_dev)) - { - // Rotate velocity covariance matrix to body coordinates - Eigen::Matrix3d covVel_body = - R_local_body * covVel_local * R_local_body.transpose(); - - msg.twist.covariance[0] = covVel_body(0, 0); - msg.twist.covariance[1] = covVel_body(0, 1); - msg.twist.covariance[2] = covVel_body(0, 2); - msg.twist.covariance[6] = covVel_body(1, 0); - msg.twist.covariance[7] = covVel_body(1, 1); - msg.twist.covariance[8] = covVel_body(1, 2); - msg.twist.covariance[12] = covVel_body(2, 0); - msg.twist.covariance[13] = covVel_body(2, 1); - msg.twist.covariance[14] = covVel_body(2, 2); - } else - { - msg.twist.covariance[0] = -1.0; - msg.twist.covariance[7] = -1.0; - msg.twist.covariance[14] = -1.0; - } - // Autocovariances of angular velocity - msg.twist.covariance[21] = -1.0; - msg.twist.covariance[28] = -1.0; - msg.twist.covariance[35] = -1.0; -} - -/** - * The position_covariance array is populated in row-major order, where the basis of - * the corresponding matrix is ENU (so Cov_lonlon is in location 11 of the matrix). - * The B2b signal type of BeiDou is not checked for usage, since the SignalInfo field - * of the PVTGeodetic block does not disclose it. For that, one would need to go to - * the ObsInfo field of the MeasEpochChannelType1 sub-block. - */ -NavSatFixMsg io::RxMessage::NavSatFixCallback() -{ - NavSatFixMsg msg; - uint16_t mask = 15; // We extract the first four bits using this mask. - if (settings_->septentrio_receiver_type == "gnss") + /** + * The position_covariance array is populated in row-major order, where the basis + * of the corresponding matrix is ENU (so Cov_lonlon is in location 11 of the + * matrix). The B2b signal type of BeiDou is not checked for usage, since the + * SignalInfo field of the PVTGeodetic block does not disclose it. For that, one + * would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block. + */ + NavSatFixMsg MessageParser::NavSatFixCallback() { - uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; - switch (type_of_pvt_map[type_of_pvt]) - { - case evNoPVT: + NavSatFixMsg msg; + uint16_t mask = 15; // We extract the first four bits using this mask. + if (settings_->septentrio_receiver_type == "gnss") { - msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; - break; - } - case evStandAlone: - case evFixed: - { - msg.status.status = NavSatStatusMsg::STATUS_FIX; - break; - } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: - { - msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; - break; - } - case evSBAS: - { - msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "PVTGeodetic's Mode field contains an invalid type of PVT solution."); - break; - } + uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; + switch (type_of_pvt_map[type_of_pvt]) + { + case evNoPVT: + { + msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = NavSatStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + { + msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "PVTGeodetic's Mode field contains an invalid type of PVT solution."); + break; + } + } + bool gps_in_pvt = false; + bool glo_in_pvt = false; + bool com_in_pvt = false; + bool gal_in_pvt = false; + uint32_t mask_2 = 1; + for (int bit = 0; bit != 31; ++bit) + { + bool in_use = last_pvtgeodetic_.signal_info & mask_2; + if (bit <= 5 && in_use) + { + gps_in_pvt = true; + } + if (8 <= bit && bit <= 12 && in_use) + glo_in_pvt = true; + if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) + com_in_pvt = true; + if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) + gal_in_pvt = true; + mask_2 *= 2; + } + // Below, booleans will be promoted to integers automatically. + uint16_t service = + gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; + msg.status.service = service; + msg.latitude = rad2deg(last_pvtgeodetic_.latitude); + msg.longitude = rad2deg(last_pvtgeodetic_.longitude); + msg.altitude = last_pvtgeodetic_.height; + msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; + msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; + msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; + return msg; } - bool gps_in_pvt = false; - bool glo_in_pvt = false; - bool com_in_pvt = false; - bool gal_in_pvt = false; - uint32_t mask_2 = 1; - for (int bit = 0; bit != 31; ++bit) - { - bool in_use = last_pvtgeodetic_.signal_info & mask_2; - if (bit <= 5 && in_use) - { - gps_in_pvt = true; - } - if (8 <= bit && bit <= 12 && in_use) - glo_in_pvt = true; - if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) - com_in_pvt = true; - if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) - gal_in_pvt = true; - mask_2 *= 2; - } - // Below, booleans will be promoted to integers automatically. - uint16_t service = - gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; - msg.status.service = service; - msg.latitude = rad2deg(last_pvtgeodetic_.latitude); - msg.longitude = rad2deg(last_pvtgeodetic_.longitude); - msg.altitude = last_pvtgeodetic_.height; - msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; - msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; - return msg; - } - if (settings_->septentrio_receiver_type == "ins") - { - NavSatFixMsg msg; - uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; - switch (type_of_pvt_map[type_of_pvt]) - { - case evNoPVT: - { - msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; - break; - } - case evStandAlone: - case evFixed: - { - msg.status.status = NavSatStatusMsg::STATUS_FIX; - break; - } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: - { - msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; - break; - } - case evSBAS: - { - msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; - break; - } - default: + if (settings_->septentrio_receiver_type == "ins") { - node_->log( - LogLevel::DEBUG, - "INSNavGeod's Mode field contains an invalid type of PVT solution."); - break; + NavSatFixMsg msg; + uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; + switch (type_of_pvt_map[type_of_pvt]) + { + case evNoPVT: + { + msg.status.status = NavSatStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = NavSatStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + { + msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX; + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "INSNavGeod's Mode field contains an invalid type of PVT solution."); + break; + } + } + bool gps_in_pvt = false; + bool glo_in_pvt = false; + bool com_in_pvt = false; + bool gal_in_pvt = false; + uint32_t mask_2 = 1; + for (int bit = 0; bit != 31; ++bit) + { + bool in_use = last_pvtgeodetic_.signal_info & mask_2; + if (bit <= 5 && in_use) + { + gps_in_pvt = true; + } + if (8 <= bit && bit <= 12 && in_use) + glo_in_pvt = true; + if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) + com_in_pvt = true; + if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) + gal_in_pvt = true; + mask_2 *= 2; + } + // Below, booleans will be promoted to integers automatically. + uint16_t service = + gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; + msg.status.service = service; + msg.latitude = rad2deg(last_insnavgeod_.latitude); + msg.longitude = rad2deg(last_insnavgeod_.longitude); + msg.altitude = last_insnavgeod_.height; + + if ((last_insnavgeod_.sb_list & 1) != 0) + { + msg.position_covariance[0] = + square(last_insnavgeod_.longitude_std_dev); + msg.position_covariance[4] = + square(last_insnavgeod_.latitude_std_dev); + msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); + } + if ((last_insnavgeod_.sb_list & 32) != 0) + { + msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; + msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; + } + msg.position_covariance_type = + NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } + return msg; + }; + + /** + * Note that the field "dip" denotes the local magnetic inclination in degrees + * (positive when the magnetic field points downwards (into the Earth)). + * This quantity cannot be calculated by most Septentrio + * receivers. We assume that for the ROS field "err_time", we are requested to + * provide the 2 sigma uncertainty on the clock bias estimate in square meters, + * not the clock drift estimate (latter would be + * "2*std::sqrt(last_velcovgeodetic_.Cov_DtDt)"). + * The "err_track" entry is calculated via the Gaussian error propagation formula + * from the eastward and the northward velocities. For the formula's usage we + * have to assume that the eastward and the northward velocities are independent + * variables. Note that elevations and azimuths of visible satellites are taken + * from the ChannelStatus block, which provides 1 degree precision, while the + * SatVisibility block could provide hundredths of degrees precision. Change if + * imperative for your application... Definition of "visible satellite" adopted + * here: We define a visible satellite as being !up to! "in sync" mode with the + * receiver, which corresponds to last_measepoch_.N (signal-to-noise ratios are + * thereby available for these), though not last_channelstatus_.N, which also + * includes those "in search". In case certain values appear unphysical, please + * consult the firmware, since those most likely refer to Do-Not-Use values. + */ + GPSFixMsg MessageParser::GPSFixCallback() + { + GPSFixMsg msg; + msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); + + // MeasEpoch Processing + std::vector cno_tracked; + std::vector svid_in_sync; + { + cno_tracked.reserve(last_measepoch_.type1.size()); + svid_in_sync.reserve(last_measepoch_.type1.size()); + for (const auto& measepoch_channel_type1 : last_measepoch_.type1) + { + // Define MeasEpochChannelType1 struct for the corresponding + // sub-block + svid_in_sync.push_back( + static_cast(measepoch_channel_type1.sv_id)); + uint8_t type_mask = + 15; // We extract the first four bits using this mask. + if (((measepoch_channel_type1.type & type_mask) == + static_cast(1)) || + ((measepoch_channel_type1.type & type_mask) == + static_cast(2))) + { + cno_tracked.push_back( + static_cast(measepoch_channel_type1.cn0) / 4); + } else + { + cno_tracked.push_back( + static_cast(measepoch_channel_type1.cn0) / 4 + + static_cast(10)); + } + } } - bool gps_in_pvt = false; - bool glo_in_pvt = false; - bool com_in_pvt = false; - bool gal_in_pvt = false; - uint32_t mask_2 = 1; - for (int bit = 0; bit != 31; ++bit) + + // ChannelStatus Processing + std::vector svid_in_sync_2; + std::vector elevation_tracked; + std::vector azimuth_tracked; + std::vector svid_pvt; + svid_pvt.clear(); + std::vector ordering; { - bool in_use = last_pvtgeodetic_.signal_info & mask_2; - if (bit <= 5 && in_use) + svid_in_sync_2.reserve(last_channelstatus_.satInfo.size()); + elevation_tracked.reserve(last_channelstatus_.satInfo.size()); + azimuth_tracked.reserve(last_channelstatus_.satInfo.size()); + for (const auto& channel_sat_info : last_channelstatus_.satInfo) { - gps_in_pvt = true; + bool to_be_added = false; + for (int32_t j = 0; j < static_cast(svid_in_sync.size()); + ++j) + { + if (svid_in_sync[j] == + static_cast(channel_sat_info.sv_id)) + { + ordering.push_back(j); + to_be_added = true; + break; + } + } + if (to_be_added) + { + svid_in_sync_2.push_back( + static_cast(channel_sat_info.sv_id)); + elevation_tracked.push_back( + static_cast(channel_sat_info.elev)); + static uint16_t azimuth_mask = 511; + azimuth_tracked.push_back(static_cast( + (channel_sat_info.az_rise_set & azimuth_mask))); + } + svid_pvt.reserve(channel_sat_info.stateInfo.size()); + for (const auto& channel_state_info : channel_sat_info.stateInfo) + { + // Define ChannelStateInfo struct for the corresponding sub-block + bool pvt_status = false; + uint16_t pvt_status_mask = std::pow(2, 15) + std::pow(2, 14); + for (int k = 15; k != -1; k -= 2) + { + uint16_t pvt_status_value = + (channel_state_info.pvt_status & pvt_status_mask) >> + k - 1; + if (pvt_status_value == 2) + { + pvt_status = true; + } + if (k > 1) + { + pvt_status_mask = pvt_status_mask - std::pow(2, k) - + std::pow(2, k - 1) + + std::pow(2, k - 2) + + std::pow(2, k - 3); + } + } + if (pvt_status) + { + svid_pvt.push_back( + static_cast(channel_sat_info.sv_id)); + } + } } - if (8 <= bit && bit <= 12 && in_use) - glo_in_pvt = true; - if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) - com_in_pvt = true; - if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) - gal_in_pvt = true; - mask_2 *= 2; } - // Below, booleans will be promoted to integers automatically. - uint16_t service = - gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8; - msg.status.service = service; - msg.latitude = rad2deg(last_insnavgeod_.latitude); - msg.longitude = rad2deg(last_insnavgeod_.longitude); - msg.altitude = last_insnavgeod_.height; + msg.status.satellite_used_prn = + svid_pvt; // Entries such as int32[] in ROS messages are to be treated as + // std::vectors. + msg.status.satellites_visible = static_cast(svid_in_sync.size()); + msg.status.satellite_visible_prn = svid_in_sync_2; + msg.status.satellite_visible_z = elevation_tracked; + msg.status.satellite_visible_azimuth = azimuth_tracked; - if ((last_insnavgeod_.sb_list & 1) != 0) + // Reordering CNO vector to that of all previous arrays + std::vector cno_tracked_reordered; + if (static_cast(last_channelstatus_.n) != 0) { - msg.position_covariance[0] = square(last_insnavgeod_.longitude_std_dev); - msg.position_covariance[4] = square(last_insnavgeod_.latitude_std_dev); - msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); + for (int32_t k = 0; k < static_cast(ordering.size()); ++k) + { + cno_tracked_reordered.push_back(cno_tracked[ordering[k]]); + } } - if ((last_insnavgeod_.sb_list & 32) != 0) + msg.status.satellite_visible_snr = cno_tracked_reordered; + msg.err_time = 2 * std::sqrt(last_poscovgeodetic_.cov_bb); + + if (settings_->septentrio_receiver_type == "gnss") { - msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; - msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; - } - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; - } - return msg; -}; -/** - * Note that the field "dip" denotes the local magnetic inclination in degrees - * (positive when the magnetic field points downwards (into the Earth)). - * This quantity cannot be calculated by most Septentrio - * receivers. We assume that for the ROS field "err_time", we are requested to - * provide the 2 sigma uncertainty on the clock bias estimate in square meters, not - * the clock drift estimate (latter would be - * "2*std::sqrt(last_velcovgeodetic_.Cov_DtDt)"). - * The "err_track" entry is calculated via the Gaussian error propagation formula - * from the eastward and the northward velocities. For the formula's usage we have to - * assume that the eastward and the northward velocities are independent variables. - * Note that elevations and azimuths of visible satellites are taken from the - * ChannelStatus block, which provides 1 degree precision, while the SatVisibility - * block could provide hundredths of degrees precision. Change if imperative for your - * application... Definition of "visible satellite" adopted here: We define a visible - * satellite as being !up to! "in sync" mode with the receiver, which corresponds to - * last_measepoch_.N (signal-to-noise ratios are thereby available for these), though - * not last_channelstatus_.N, which also includes those "in search". In case certain - * values appear unphysical, please consult the firmware, since those most likely - * refer to Do-Not-Use values. - */ -GPSFixMsg io::RxMessage::GPSFixCallback() -{ - GPSFixMsg msg; - msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); - - // MeasEpoch Processing - std::vector cno_tracked; - std::vector svid_in_sync; - { - cno_tracked.reserve(last_measepoch_.type1.size()); - svid_in_sync.reserve(last_measepoch_.type1.size()); - for (const auto& measepoch_channel_type1 : last_measepoch_.type1) - { - // Define MeasEpochChannelType1 struct for the corresponding sub-block - svid_in_sync.push_back( - static_cast(measepoch_channel_type1.sv_id)); - uint8_t type_mask = + // PVT Status Analysis + uint16_t status_mask = 15; // We extract the first four bits using this mask. - if (((measepoch_channel_type1.type & type_mask) == - static_cast(1)) || - ((measepoch_channel_type1.type & type_mask) == - static_cast(2))) + uint16_t type_of_pvt = + ((uint16_t)(last_pvtgeodetic_.mode)) & status_mask; + switch (type_of_pvt_map[type_of_pvt]) + { + case evNoPVT: + { + msg.status.status = GPSStatusMsg::STATUS_NO_FIX; + break; + } + case evStandAlone: + case evFixed: + { + msg.status.status = GPSStatusMsg::STATUS_FIX; + break; + } + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: + { + msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + { + uint16_t reference_id = last_pvtgeodetic_.reference_id; + // Here come the PRNs of the 4 WAAS satellites.. + if (reference_id == 131 || reference_id == 133 || + reference_id == 135 || reference_id == 135) + { + msg.status.status = GPSStatusMsg::STATUS_WAAS_FIX; + } else + { + msg.status.status = GPSStatusMsg::STATUS_SBAS_FIX; + } + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "PVTGeodetic's Mode field contains an invalid type of PVT solution."); + break; + } + } + // Doppler is not used when calculating the velocities of, say, + // mosaic-x5, hence: + msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; + // Doppler is not used when calculating the orientation of, say, + // mosaic-x5, hence: + msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; + msg.status.position_source = GPSStatusMsg::SOURCE_GPS; + msg.latitude = rad2deg(last_pvtgeodetic_.latitude); + msg.longitude = rad2deg(last_pvtgeodetic_.longitude); + msg.altitude = last_pvtgeodetic_.height; + // Note that cog is of type float32 while track is of type float64. + msg.track = last_pvtgeodetic_.cog; + msg.speed = std::sqrt(square(last_pvtgeodetic_.vn) + + square(last_pvtgeodetic_.ve)); + msg.climb = last_pvtgeodetic_.vu; + msg.pitch = last_atteuler_.pitch; + msg.roll = last_atteuler_.roll; + if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) + { + msg.gdop = -1.0; + } else + { + msg.gdop = + std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); + } + if (last_dop_.pdop == 0.0) + { + msg.pdop = -1.0; + } else + { + msg.pdop = last_dop_.pdop; + } + if (last_dop_.hdop == 0.0) { - cno_tracked.push_back( - static_cast(measepoch_channel_type1.cn0) / 4); + msg.hdop = -1.0; } else { - cno_tracked.push_back( - static_cast(measepoch_channel_type1.cn0) / 4 + - static_cast(10)); + msg.hdop = last_dop_.hdop; } + if (last_dop_.vdop == 0.0) + { + msg.vdop = -1.0; + } else + { + msg.vdop = last_dop_.vdop; + } + if (last_dop_.tdop == 0.0) + { + msg.tdop = -1.0; + } else + { + msg.tdop = last_dop_.tdop; + } + msg.time = + static_cast(last_pvtgeodetic_.block_header.tow) / 1000 + + static_cast(last_pvtgeodetic_.block_header.wnc * 7 * 24 * + 60 * 60); + msg.err = + 2 * + (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + + static_cast(last_poscovgeodetic_.cov_lonlon) + + static_cast(last_poscovgeodetic_.cov_hgthgt))); + msg.err_horz = + 2 * + (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + + static_cast(last_poscovgeodetic_.cov_lonlon))); + msg.err_vert = + 2 * std::sqrt(static_cast(last_poscovgeodetic_.cov_hgthgt)); + msg.err_track = + 2 * (std::sqrt(square(1.0 / (last_pvtgeodetic_.vn + + square(last_pvtgeodetic_.ve) / + last_pvtgeodetic_.vn)) * + last_poscovgeodetic_.cov_lonlon + + square((last_pvtgeodetic_.ve) / + (square(last_pvtgeodetic_.vn) + + square(last_pvtgeodetic_.ve))) * + last_poscovgeodetic_.cov_latlat)); + msg.err_speed = + 2 * (std::sqrt(static_cast(last_velcovgeodetic_.cov_vnvn) + + static_cast(last_velcovgeodetic_.cov_veve))); + msg.err_climb = + 2 * std::sqrt(static_cast(last_velcovgeodetic_.cov_vuvu)); + msg.err_pitch = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_pitchpitch)); + msg.err_roll = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_rollroll)); + msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; + msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; + msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; + msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; + msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; + msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; } - } - // ChannelStatus Processing - std::vector svid_in_sync_2; - std::vector elevation_tracked; - std::vector azimuth_tracked; - std::vector svid_pvt; - svid_pvt.clear(); - std::vector ordering; - { - svid_in_sync_2.reserve(last_channelstatus_.satInfo.size()); - elevation_tracked.reserve(last_channelstatus_.satInfo.size()); - azimuth_tracked.reserve(last_channelstatus_.satInfo.size()); - for (const auto& channel_sat_info : last_channelstatus_.satInfo) + if (settings_->septentrio_receiver_type == "ins") { - bool to_be_added = false; - for (int32_t j = 0; j < static_cast(svid_in_sync.size()); ++j) + // PVT Status Analysis + uint16_t status_mask = + 15; // We extract the first four bits using this mask. + uint16_t type_of_pvt = + ((uint16_t)(last_insnavgeod_.gnss_mode)) & status_mask; + switch (type_of_pvt_map[type_of_pvt]) { - if (svid_in_sync[j] == static_cast(channel_sat_info.sv_id)) - { - ordering.push_back(j); - to_be_added = true; - break; - } + case evNoPVT: + { + msg.status.status = GPSStatusMsg::STATUS_NO_FIX; + break; } - if (to_be_added) + case evStandAlone: + case evFixed: { - svid_in_sync_2.push_back( - static_cast(channel_sat_info.sv_id)); - elevation_tracked.push_back( - static_cast(channel_sat_info.elev)); - static uint16_t azimuth_mask = 511; - azimuth_tracked.push_back(static_cast( - (channel_sat_info.az_rise_set & azimuth_mask))); + msg.status.status = GPSStatusMsg::STATUS_FIX; + break; } - svid_pvt.reserve(channel_sat_info.stateInfo.size()); - for (const auto& channel_state_info : channel_sat_info.stateInfo) + case evDGPS: + case evRTKFixed: + case evRTKFloat: + case evMovingBaseRTKFixed: + case evMovingBaseRTKFloat: + case evPPP: { - // Define ChannelStateInfo struct for the corresponding sub-block - bool pvt_status = false; - uint16_t pvt_status_mask = std::pow(2, 15) + std::pow(2, 14); - for (int k = 15; k != -1; k -= 2) - { - uint16_t pvt_status_value = - (channel_state_info.pvt_status & pvt_status_mask) >> k - 1; - if (pvt_status_value == 2) - { - pvt_status = true; - } - if (k > 1) - { - pvt_status_mask = pvt_status_mask - std::pow(2, k) - - std::pow(2, k - 1) + std::pow(2, k - 2) + - std::pow(2, k - 3); - } - } - if (pvt_status) - { - svid_pvt.push_back(static_cast(channel_sat_info.sv_id)); - } + msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; + break; + } + case evSBAS: + default: + { + node_->log( + LogLevel::DEBUG, + "INSNavGeod's Mode field contains an invalid type of PVT solution."); + break; + } + } + // Doppler is not used when calculating the velocities of, say, + // mosaic-x5, hence: + msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; + // Doppler is not used when calculating the orientation of, say, + // mosaic-x5, hence: + msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; + msg.status.position_source = GPSStatusMsg::SOURCE_GPS; + msg.latitude = rad2deg(last_insnavgeod_.latitude); + msg.longitude = rad2deg(last_insnavgeod_.longitude); + msg.altitude = last_insnavgeod_.height; + // Note that cog is of type float32 while track is of type float64. + if ((last_insnavgeod_.sb_list & 2) != 0) + { + msg.track = last_insnavgeod_.heading; + msg.pitch = last_insnavgeod_.pitch; + msg.roll = last_insnavgeod_.roll; + } + if ((last_insnavgeod_.sb_list & 8) != 0) + { + msg.speed = std::sqrt(square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve)); + + msg.climb = last_insnavgeod_.vu; + } + if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) + { + msg.gdop = -1.0; + } else + { + msg.gdop = + std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); + } + if (last_dop_.pdop == 0.0) + { + msg.pdop = -1.0; + } else + { + msg.pdop = last_dop_.pdop; + } + if (last_dop_.hdop == 0.0) + { + msg.hdop = -1.0; + } else + { + msg.hdop = last_dop_.hdop; + } + if (last_dop_.vdop == 0.0) + { + msg.vdop = -1.0; + } else + { + msg.vdop = last_dop_.vdop; + } + if (last_dop_.tdop == 0.0) + { + msg.tdop = -1.0; + } else + { + msg.tdop = last_dop_.tdop; + } + msg.time = + static_cast(last_insnavgeod_.block_header.tow) / 1000 + + static_cast(last_insnavgeod_.block_header.wnc * 7 * 24 * 60 * + 60); + if ((last_insnavgeod_.sb_list & 1) != 0) + { + msg.err = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + + square(last_insnavgeod_.longitude_std_dev) + + square(last_insnavgeod_.height_std_dev))); + msg.err_horz = + 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + + square(last_insnavgeod_.longitude_std_dev))); + msg.err_vert = + 2 * (std::sqrt(square(last_insnavgeod_.height_std_dev))); + } + if (((last_insnavgeod_.sb_list & 8) != 0) || + ((last_insnavgeod_.sb_list & 1) != 0)) + { + msg.err_track = + 2 * (std::sqrt(square(1.0 / (last_insnavgeod_.vn + + square(last_insnavgeod_.ve) / + last_insnavgeod_.vn)) * + square(last_insnavgeod_.longitude_std_dev) + + square((last_insnavgeod_.ve) / + (square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve))) * + square(last_insnavgeod_.latitude_std_dev))); + } + if ((last_insnavgeod_.sb_list & 8) != 0) + { + msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.vn) + + square(last_insnavgeod_.ve))); + msg.err_climb = 2 * std::sqrt(square(last_insnavgeod_.vn)); + } + if ((last_insnavgeod_.sb_list & 2) != 0) + { + msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.pitch)); + } + if ((last_insnavgeod_.sb_list & 2) != 0) + { + msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.roll)); + } + if ((last_insnavgeod_.sb_list & 1) != 0) + { + msg.position_covariance[0] = + square(last_insnavgeod_.longitude_std_dev); + msg.position_covariance[4] = + square(last_insnavgeod_.latitude_std_dev); + msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); + } + if ((last_insnavgeod_.sb_list & 32) != 0) + { + msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; + msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; + msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; + msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; } + msg.position_covariance_type = + NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } + return msg; + }; + + Timestamp MessageParser::timestampSBF(const uint8_t* data, bool use_gnss_time) + { + uint32_t tow = parsing_utilities::getTow(data); + uint16_t wnc = parsing_utilities::getWnc(data); + + return timestampSBF(tow, wnc, use_gnss_time); } - msg.status.satellite_used_prn = - svid_pvt; // Entries such as int32[] in ROS messages are to be treated as - // std::vectors. - msg.status.satellites_visible = static_cast(svid_in_sync.size()); - msg.status.satellite_visible_prn = svid_in_sync_2; - msg.status.satellite_visible_z = elevation_tracked; - msg.status.satellite_visible_azimuth = azimuth_tracked; - - // Reordering CNO vector to that of all previous arrays - std::vector cno_tracked_reordered; - if (static_cast(last_channelstatus_.n) != 0) + + /// If the current time shall be employed, it is calculated via the time(NULL) + /// function found in the \ library At the time of writing the code + /// (2020), the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the + /// settings_->leap_seconds ROSaic parameter accordingly as soon as the + /// next leap second is inserted into the UTC time. + Timestamp MessageParser::timestampSBF(uint32_t tow, uint16_t wnc, + bool use_gnss_time) { - for (int32_t k = 0; k < static_cast(ordering.size()); ++k) + Timestamp time_obj; + if (use_gnss_time) + { + // conversion from GPS time of week and week number to UTC taking leap + // seconds into account + static uint64_t secToNSec = 1000000000; + static uint64_t mSec2NSec = 1000000; + static uint64_t nsOfGpsStart = + 315964800 * + secToNSec; // GPS week counter starts at 1980-01-06 which is + // 315964800 seconds since Unix epoch (1970-01-01 UTC) + static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec; + + time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek; + + if (current_leap_seconds_ != -128) + time_obj -= current_leap_seconds_ * secToNSec; + } else { - cno_tracked_reordered.push_back(cno_tracked[ordering[k]]); + time_obj = recvTimestamp_; } + return time_obj; } - msg.status.satellite_visible_snr = cno_tracked_reordered; - msg.err_time = 2 * std::sqrt(last_poscovgeodetic_.cov_bb); - if (settings_->septentrio_receiver_type == "gnss") + bool MessageParser::isMessage(std::string id) { - - // PVT Status Analysis - uint16_t status_mask = 15; // We extract the first four bits using this mask. - uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & status_mask; - switch (type_of_pvt_map[type_of_pvt]) + if (this->isNMEA()) { - case evNoPVT: + boost::char_separator sep(","); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(data_), + nmea_size); + tokenizer tokens(block_in_string, sep); + if (*tokens.begin() == id) + { + return true; + } else + { + return false; + } + } else { - msg.status.status = GPSStatusMsg::STATUS_NO_FIX; - break; + return false; } - case evStandAlone: - case evFixed: + } + + std::string MessageParser::messageID() + { + if (this->isSBF()) { - msg.status.status = GPSStatusMsg::STATUS_FIX; - break; + std::stringstream ss; + ss << parsing_utilities::getId(data_); + return ss.str(); } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: + if (this->isNMEA()) { - msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; - break; + boost::char_separator sep(","); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(data_), + nmea_size); + tokenizer tokens(block_in_string, sep); + return *tokens.begin(); } - case evSBAS: + return std::string(); // less CPU work than return ""; + } + + /** + * If GNSS time is used, Publishing is only done with valid leap seconds + */ + template + void MessageParser::publish(const std::string& topic, const M& msg) + { + // TODO: maybe publish only if wnc and tow is valid? + if (!settings_->use_gnss_time || + (settings_->use_gnss_time && (current_leap_seconds_ != -128))) { - uint16_t reference_id = last_pvtgeodetic_.reference_id; - // Here come the PRNs of the 4 WAAS satellites.. - if (reference_id == 131 || reference_id == 133 || reference_id == 135 || - reference_id == 135) - { - msg.status.status = GPSStatusMsg::STATUS_WAAS_FIX; - } else - { - msg.status.status = GPSStatusMsg::STATUS_SBAS_FIX; - } - break; - } - default: + node_->publishMessage(topic, msg); + } else { node_->log( LogLevel::DEBUG, - "PVTGeodetic's Mode field contains an invalid type of PVT solution."); - break; - } - } - // Doppler is not used when calculating the velocities of, say, mosaic-x5, - // hence: - msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; - // Doppler is not used when calculating the orientation of, say, mosaic-x5, - // hence: - msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; - msg.status.position_source = GPSStatusMsg::SOURCE_GPS; - msg.latitude = rad2deg(last_pvtgeodetic_.latitude); - msg.longitude = rad2deg(last_pvtgeodetic_.longitude); - msg.altitude = last_pvtgeodetic_.height; - // Note that cog is of type float32 while track is of type float64. - msg.track = last_pvtgeodetic_.cog; - msg.speed = - std::sqrt(square(last_pvtgeodetic_.vn) + square(last_pvtgeodetic_.ve)); - msg.climb = last_pvtgeodetic_.vu; - msg.pitch = last_atteuler_.pitch; - msg.roll = last_atteuler_.roll; - if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) - { - msg.gdop = -1.0; - } else - { - msg.gdop = std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); - } - if (last_dop_.pdop == 0.0) - { - msg.pdop = -1.0; - } else - { - msg.pdop = last_dop_.pdop; - } - if (last_dop_.hdop == 0.0) - { - msg.hdop = -1.0; - } else - { - msg.hdop = last_dop_.hdop; - } - if (last_dop_.vdop == 0.0) - { - msg.vdop = -1.0; - } else - { - msg.vdop = last_dop_.vdop; + "Not publishing message with GNSS time because no leap seconds are available yet."); + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + node_->log( + LogLevel::WARN, + "No leap seconds were set and none were received from log yet."); } - if (last_dop_.tdop == 0.0) + } + + /** + * If GNSS time is used, Publishing is only done with valid leap seconds + */ + void MessageParser::publishTf(const LocalizationMsg& msg) + { + // TODO: maybe publish only if wnc and tow is valid? + if (!settings_->use_gnss_time || + (settings_->use_gnss_time && (current_leap_seconds_ != -128) && + (current_leap_seconds_ != 0))) { - msg.tdop = -1.0; + node_->publishTf(msg); } else { - msg.tdop = last_dop_.tdop; + node_->log( + LogLevel::DEBUG, + "Not publishing tf with GNSS time because no leap seconds are available yet."); + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + node_->log( + LogLevel::WARN, + "No leap seconds were set and none were received from log yet."); } - msg.time = static_cast(last_pvtgeodetic_.block_header.tow) / 1000 + - static_cast(last_pvtgeodetic_.block_header.wnc * 7 * 24 * - 60 * 60); - msg.err = - 2 * (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + - static_cast(last_poscovgeodetic_.cov_lonlon) + - static_cast(last_poscovgeodetic_.cov_hgthgt))); - msg.err_horz = - 2 * (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + - static_cast(last_poscovgeodetic_.cov_lonlon))); - msg.err_vert = - 2 * std::sqrt(static_cast(last_poscovgeodetic_.cov_hgthgt)); - msg.err_track = - 2 * - (std::sqrt( - square(1.0 / (last_pvtgeodetic_.vn + - square(last_pvtgeodetic_.ve) / last_pvtgeodetic_.vn)) * - last_poscovgeodetic_.cov_lonlon + - square((last_pvtgeodetic_.ve) / (square(last_pvtgeodetic_.vn) + - square(last_pvtgeodetic_.ve))) * - last_poscovgeodetic_.cov_latlat)); - msg.err_speed = - 2 * (std::sqrt(static_cast(last_velcovgeodetic_.cov_vnvn) + - static_cast(last_velcovgeodetic_.cov_veve))); - msg.err_climb = - 2 * std::sqrt(static_cast(last_velcovgeodetic_.cov_vuvu)); - msg.err_pitch = - 2 * std::sqrt(static_cast(last_attcoveuler_.cov_pitchpitch)); - msg.err_roll = - 2 * std::sqrt(static_cast(last_attcoveuler_.cov_rollroll)); - msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon; - msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat; - msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt; - msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; - msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; } - if (settings_->septentrio_receiver_type == "ins") + /** + * Note that putting the default in the definition's argument list instead of the + * declaration's is an added extra that is not available for function templates, + * hence no search = false here. Also note that the SBF block header part of the + * SBF-echoing ROS messages have ID fields that only show the block number as + * found in the firmware (e.g. 4007 for PVTGeodetic), without the revision + * number. NMEA 0183 messages are at most 82 characters long in principle, but + * most Septentrio Rxs by default increase precision on lat/lon s.t. the maximum + * allowed e.g. for GGA seems to be 89 on a mosaic-x5. Luckily, when parsing we + * do not care since we just search for \\. + */ + bool MessageParser::parseSbf(const std::shared_ptr& telegram) { - // PVT Status Analysis - uint16_t status_mask = 15; // We extract the first four bits using this mask. - uint16_t type_of_pvt = - ((uint16_t)(last_insnavgeod_.gnss_mode)) & status_mask; - switch (type_of_pvt_map[type_of_pvt]) + switch (telegram->sbfId) { - case evNoPVT: + case PVT_CARTESIAN: // Position and velocity in XYZ { - msg.status.status = GPSStatusMsg::STATUS_NO_FIX; + PVTCartesianMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!PVTCartesianParser(node_, dvec.begin(), dvec.end(), msg)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in PVTCartesian"); + break; + } + msg.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/pvtcartesian", msg); break; } - case evStandAlone: - case evFixed: + case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame + // (ENU frame) { - msg.status.status = GPSStatusMsg::STATUS_FIX; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!PVTGeodeticParser(node_, dvec.begin(), dvec.end(), + last_pvtgeodetic_)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in PVTGeodetic"); + break; + } + last_pvtgeodetic_.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); + pvtgeodetic_has_arrived_gpsfix_ = true; + pvtgeodetic_has_arrived_navsatfix_ = true; + pvtgeodetic_has_arrived_pose_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_pvtgeodetic) + publish("/pvtgeodetic", last_pvtgeodetic_); break; } - case evDGPS: - case evRTKFixed: - case evRTKFloat: - case evMovingBaseRTKFixed: - case evMovingBaseRTKFloat: - case evPPP: + case BASE_VECTOR_CART: { - msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; + BaseVectorCartMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!BaseVectorCartParser(node_, dvec.begin(), dvec.end(), msg)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in BaseVectorCart"); + break; + } + msg.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/basevectorcart", msg); break; } - case evSBAS: - default: + case BASE_VECTOR_GEOD: { - node_->log( - LogLevel::DEBUG, - "INSNavGeod's Mode field contains an invalid type of PVT solution."); + BaseVectorGeodMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!BaseVectorGeodParser(node_, dvec.begin(), dvec.end(), msg)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in BaseVectorGeod"); + break; + } + msg.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/basevectorgeod", msg); break; } - } - // Doppler is not used when calculating the velocities of, say, mosaic-x5, - // hence: - msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; - // Doppler is not used when calculating the orientation of, say, mosaic-x5, - // hence: - msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; - msg.status.position_source = GPSStatusMsg::SOURCE_GPS; - msg.latitude = rad2deg(last_insnavgeod_.latitude); - msg.longitude = rad2deg(last_insnavgeod_.longitude); - msg.altitude = last_insnavgeod_.height; - // Note that cog is of type float32 while track is of type float64. - if ((last_insnavgeod_.sb_list & 2) != 0) + case POS_COV_CARTESIAN: { - msg.track = last_insnavgeod_.heading; - msg.pitch = last_insnavgeod_.pitch; - msg.roll = last_insnavgeod_.roll; + PosCovCartesianMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!PosCovCartesianParser(node_, dvec.begin(), dvec.end(), msg)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in PosCovCartesian"); + break; + } + msg.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/poscovcartesian", msg); + break; } - if ((last_insnavgeod_.sb_list & 8) != 0) + case POS_COV_GEODETIC: { - msg.speed = - std::sqrt(square(last_insnavgeod_.vn) + square(last_insnavgeod_.ve)); - - msg.climb = last_insnavgeod_.vu; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!PosCovGeodeticParser(node_, dvec.begin(), dvec.end(), + last_poscovgeodetic_)) + { + poscovgeodetic_has_arrived_gpsfix_ = false; + poscovgeodetic_has_arrived_navsatfix_ = false; + poscovgeodetic_has_arrived_pose_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in PosCovGeodetic"); + break; + } + last_poscovgeodetic_.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); + poscovgeodetic_has_arrived_gpsfix_ = true; + poscovgeodetic_has_arrived_navsatfix_ = true; + poscovgeodetic_has_arrived_pose_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_poscovgeodetic) + publish("/poscovgeodetic", last_poscovgeodetic_); + break; } - if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) - { - msg.gdop = -1.0; - } else + case ATT_EULER: { - msg.gdop = std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop)); + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!AttEulerParser(node_, dvec.begin(), dvec.end(), last_atteuler_, + settings_->use_ros_axis_orientation)) + { + atteuler_has_arrived_gpsfix_ = false; + atteuler_has_arrived_pose_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in AttEuler"); + break; + } + last_atteuler_.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_atteuler_.header.stamp = timestampToRos(time_obj); + atteuler_has_arrived_gpsfix_ = true; + atteuler_has_arrived_pose_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_atteuler) + publish("/atteuler", last_atteuler_); + break; } - if (last_dop_.pdop == 0.0) - { - msg.pdop = -1.0; - } else + case ATT_COV_EULER: { - msg.pdop = last_dop_.pdop; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!AttCovEulerParser(node_, dvec.begin(), dvec.end(), + last_attcoveuler_, + settings_->use_ros_axis_orientation)) + { + attcoveuler_has_arrived_gpsfix_ = false; + attcoveuler_has_arrived_pose_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in AttCovEuler"); + break; + } + last_attcoveuler_.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_attcoveuler_.header.stamp = timestampToRos(time_obj); + attcoveuler_has_arrived_gpsfix_ = true; + attcoveuler_has_arrived_pose_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_attcoveuler) + publish("/attcoveuler", last_attcoveuler_); + break; } - if (last_dop_.hdop == 0.0) - { - msg.hdop = -1.0; - } else + case INS_NAV_CART: // Position, velocity and orientation in cartesian + // coordinate frame (ENU frame) { - msg.hdop = last_dop_.hdop; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), last_insnavcart_, + settings_->use_ros_axis_orientation)) + { + insnavgeod_has_arrived_localization_ecef_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in INSNavCart"); + break; + } + if (settings_->ins_use_poi) + { + last_insnavcart_.header.frame_id = settings_->poi_frame_id; + } else + { + last_insnavcart_.header.frame_id = settings_->frame_id; + } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_insnavcart_.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ecef_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/insnavcart", last_insnavcart_); + break; } - if (last_dop_.vdop == 0.0) + case INS_NAV_GEOD: // Position, velocity and orientation in geodetic + // coordinate frame (ENU frame) { - msg.vdop = -1.0; - } else - { - msg.vdop = last_dop_.vdop; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), last_insnavgeod_, + settings_->use_ros_axis_orientation)) + { + insnavgeod_has_arrived_gpsfix_ = false; + insnavgeod_has_arrived_navsatfix_ = false; + insnavgeod_has_arrived_pose_ = false; + insnavgeod_has_arrived_localization_ = false; + insnavgeod_has_arrived_localization_ecef_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in INSNavGeod"); + break; + } + if (settings_->ins_use_poi) + { + last_insnavgeod_.header.frame_id = settings_->poi_frame_id; + } else + { + last_insnavgeod_.header.frame_id = settings_->frame_id; + } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_insnavgeod_.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_gpsfix_ = true; + insnavgeod_has_arrived_navsatfix_ = true; + insnavgeod_has_arrived_pose_ = true; + insnavgeod_has_arrived_localization_ = true; + insnavgeod_has_arrived_localization_ecef_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_insnavgeod) + publish("/insnavgeod", last_insnavgeod_); + if (settings_->publish_twist) + { + TwistWithCovarianceStampedMsg twist = TwistCallback(true); + publish("/twist_ins", twist); + } + break; } - if (last_dop_.tdop == 0.0) - { - msg.tdop = -1.0; - } else + + case IMU_SETUP: // IMU orientation and lever arm { - msg.tdop = last_dop_.tdop; + IMUSetupMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!IMUSetupParser(node_, dvec.begin(), dvec.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in IMUSetup"); + break; + } + msg.header.frame_id = settings_->vehicle_frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/imusetup", msg); + break; } - msg.time = static_cast(last_insnavgeod_.block_header.tow) / 1000 + - static_cast(last_insnavgeod_.block_header.wnc * 7 * 24 * - 60 * 60); - if ((last_insnavgeod_.sb_list & 1) != 0) + + case VEL_SENSOR_SETUP: // Velocity sensor lever arm { - msg.err = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + - square(last_insnavgeod_.longitude_std_dev) + - square(last_insnavgeod_.height_std_dev))); - msg.err_horz = - 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + - square(last_insnavgeod_.longitude_std_dev))); - msg.err_vert = 2 * (std::sqrt(square(last_insnavgeod_.height_std_dev))); + VelSensorSetupMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!VelSensorSetupParser(node_, dvec.begin(), dvec.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in VelSensorSetup"); + break; + } + msg.header.frame_id = settings_->vehicle_frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/velsensorsetup", msg); + break; } - if (((last_insnavgeod_.sb_list & 8) != 0) || - ((last_insnavgeod_.sb_list & 1) != 0)) + + case EXT_EVENT_INS_NAV_CART: // Position, velocity and orientation in + // cartesian coordinate frame (ENU frame) { - msg.err_track = - 2 * - (std::sqrt( - square(1.0 / (last_insnavgeod_.vn + square(last_insnavgeod_.ve) / - last_insnavgeod_.vn)) * - square(last_insnavgeod_.longitude_std_dev) + - square((last_insnavgeod_.ve) / (square(last_insnavgeod_.vn) + - square(last_insnavgeod_.ve))) * - square(last_insnavgeod_.latitude_std_dev))); + INSNavCartMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log( + LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ExtEventINSNavCart"); + break; + } + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/exteventinsnavcart", msg); + break; } - if ((last_insnavgeod_.sb_list & 8) != 0) + + case EXT_EVENT_INS_NAV_GEOD: { - msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.vn) + - square(last_insnavgeod_.ve))); - msg.err_climb = 2 * std::sqrt(square(last_insnavgeod_.vn)); + INSNavGeodMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log( + LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ExtEventINSNavGeod"); + break; + } + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/exteventinsnavgeod", msg); + break; } - if ((last_insnavgeod_.sb_list & 2) != 0) + + case EXT_SENSOR_MEAS: { - msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.pitch)); + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + bool hasImuMeas = false; + if (!ExtSensorMeasParser( + node_, dvec.begin(), dvec.end(), last_extsensmeas_, + settings_->use_ros_axis_orientation, hasImuMeas)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ExtSensorMeas"); + break; + } + last_extsensmeas_.header.frame_id = settings_->imu_frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_extsensmeas_.header.stamp = timestampToRos(time_obj); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_extsensormeas) + publish("/extsensormeas", last_extsensmeas_); + if (settings_->publish_imu && hasImuMeas) + { + ImuMsg msg; + try + { + msg = ImuCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, "ImuMsg: " + std::string(e.what())); + break; + } + msg.header.frame_id = settings_->imu_frame_id; + msg.header.stamp = last_extsensmeas_.header.stamp; + publish("/imu", msg); + } + break; } - if ((last_insnavgeod_.sb_list & 2) != 0) + case CHANNEL_STATUS: { - msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.roll)); + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!ChannelStatusParser(node_, dvec.begin(), dvec.end(), + last_channelstatus_)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ChannelStatus"); + break; + } + channelstatus_has_arrived_gpsfix_ = true; + break; } - if ((last_insnavgeod_.sb_list & 1) != 0) - { - msg.position_covariance[0] = square(last_insnavgeod_.longitude_std_dev); - msg.position_covariance[4] = square(last_insnavgeod_.latitude_std_dev); - msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev); - } - if ((last_insnavgeod_.sb_list & 32) != 0) - { - msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov; - msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov; - msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov; - msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov; - } - msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; - } - return msg; -}; - -Timestamp io::RxMessage::timestampSBF(const uint8_t* data, - bool use_gnss_time) -{ - uint32_t tow = parsing_utilities::getTow(data); - uint16_t wnc = parsing_utilities::getWnc(data); - - return timestampSBF(tow, wnc, use_gnss_time); -} - -/// If the current time shall be employed, it is calculated via the time(NULL) -/// function found in the \ library At the time of writing the code (2020), -/// the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the -/// settings_->leap_seconds ROSaic parameter accordingly as soon as the next leap -/// second is inserted into the UTC time. -Timestamp io::RxMessage::timestampSBF(uint32_t tow, uint16_t wnc, - bool use_gnss_time) -{ - Timestamp time_obj; - if (use_gnss_time) - { - // conversion from GPS time of week and week number to UTC taking leap - // seconds into account - static uint64_t secToNSec = 1000000000; - static uint64_t mSec2NSec = 1000000; - static uint64_t nsOfGpsStart = - 315964800 * - secToNSec; // GPS week counter starts at 1980-01-06 which is 315964800 - // seconds since Unix epoch (1970-01-01 UTC) - static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec; - - time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek; - - if (current_leap_seconds_ != -128) - time_obj -= current_leap_seconds_ * secToNSec; - } else - { - time_obj = recvTimestamp_; - } - return time_obj; -} - -bool io::RxMessage::found() -{ - if (found_) - return true; - - // Verify header bytes - if (!this->isSBF() && !this->isNMEA() && !this->isResponse() && - !(g_read_cd && this->isConnectionDescriptor())) - { - return false; - } - - found_ = true; - return true; -} - -const uint8_t* io::RxMessage::search() -{ - if (found_) - { - next(); - } - // Search for message or a response header - for (; count_ > 0; --count_, ++data_) - { - if (this->isSBF() || this->isNMEA() || this->isResponse() || - (g_read_cd && this->isConnectionDescriptor())) + case MEAS_EPOCH: { + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!MeasEpochParser(node_, dvec.begin(), dvec.end(), last_measepoch_)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in MeasEpoch"); + break; + } + last_measepoch_.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_measepoch_.header.stamp = timestampToRos(time_obj); + measepoch_has_arrived_gpsfix_ = true; + if (settings_->publish_measepoch) + publish("/measepoch", last_measepoch_); break; } - } - found_ = true; - return data_; -} - -std::size_t io::RxMessage::messageSize() -{ - uint16_t pos = 0; - message_size_ = 0; - std::size_t count_copy = count_; - if (this->isResponse()) - { - do - { - ++message_size_; - ++pos; - --count_copy; - if (count_copy == 0) - break; - } while (!((data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED)) || - (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED && - data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 && - data_[pos + 4] == 0x4E) || - (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED && - data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 && - data_[pos + 4] == 0x53) || - (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED && - data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 && - data_[pos + 4] == 0x52)); - } else - { - do + case DOP: { - ++message_size_; - ++pos; - --count_copy; - if (count_copy == 0) + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!DOPParser(node_, dvec.begin(), dvec.end(), last_dop_)) + { + dop_has_arrived_gpsfix_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in DOP"); break; - } while (!((data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED) || - data_[pos] == CARRIAGE_RETURN || data_[pos] == LINE_FEED)); - } - return message_size_; -} - -bool io::RxMessage::isMessage(const uint16_t id) -{ - if (this->isSBF()) - { - return (parsing_utilities::getId(data_) == static_cast(id)); - } else - { - return false; - } -} - -bool io::RxMessage::isMessage(std::string id) -{ - if (this->isNMEA()) - { - boost::char_separator sep(","); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - if (*tokens.begin() == id) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io::RxMessage::isSBF() -{ - if (count_ >= 2) - { - if (data_[0] == SBF_SYNC_BYTE_1 && data_[1] == SBF_SYNC_BYTE_2) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io::RxMessage::isNMEA() -{ - if (count_ >= 2) - { - if ((data_[0] == NMEA_SYNC_BYTE_1 && data_[1] == NMEA_SYNC_BYTE_2_1) || - (data_[0] == NMEA_SYNC_BYTE_1 && data_[1] == NMEA_SYNC_BYTE_2_2)) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -bool io::RxMessage::isResponse() -{ - if (count_ >= 2) - { - if (data_[0] == RESPONSE_SYNC_BYTE_1 && data_[1] == RESPONSE_SYNC_BYTE_2) - { - return true; - } else - { - return false; + } + dop_has_arrived_gpsfix_ = true; + break; } - } else - { - return false; - } -} - -bool io::RxMessage::isConnectionDescriptor() -{ - if (count_ >= 2) - { - if (data_[0] == CONNECTION_DESCRIPTOR_BYTE_1 && - data_[1] == CONNECTION_DESCRIPTOR_BYTE_2) - { - return true; - } else + case VEL_COV_GEODETIC: { - return false; - } - } else - { - return false; - } -} - -bool io::RxMessage::isErrorMessage() -{ - if (count_ >= 3) - { - if (data_[0] == RESPONSE_SYNC_BYTE_1 && data_[1] == RESPONSE_SYNC_BYTE_2 && - data_[2] == RESPONSE_SYNC_BYTE_3) - { - return true; - } else - { - return false; - } - } else - { - return false; - } -} - -std::string io::RxMessage::messageID() -{ - if (this->isSBF()) - { - std::stringstream ss; - ss << parsing_utilities::getId(data_); - return ss.str(); - } - if (this->isNMEA()) - { - boost::char_separator sep(","); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - return *tokens.begin(); - } - return std::string(); // less CPU work than return ""; -} - -const uint8_t* io::RxMessage::getPosBuffer() { return data_; } - -const uint8_t* io::RxMessage::getEndBuffer() { return data_ + count_; } - -uint16_t io::RxMessage::getBlockLength() -{ - if (this->isSBF()) - { - uint16_t block_length; - // Note that static_cast(data_[6]) would just take the one byte - // "data_[6]" and cast it as requested, !neglecting! the byte "data_[7]". - block_length = parsing_utilities::parseUInt16(data_ + 6); - return block_length; - } else - { - return 0; - } -} - -/** - * This method won't make data_ jump to the next message if the current one is an - * NMEA message or a command reply. In that case, search() will check the bytes one - * by one for the new message's sync bytes ($P, $G or $R). - */ -void io::RxMessage::next() -{ - std::size_t jump_size; - if (found()) - { - if (this->isNMEA() || this->isResponse() || - (g_read_cd && this->isConnectionDescriptor())) - { - if (g_read_cd && this->isConnectionDescriptor() && g_cd_count == 2) + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!VelCovGeodeticParser(node_, dvec.begin(), dvec.end(), + last_velcovgeodetic_)) { - g_read_cd = false; + velcovgeodetic_has_arrived_gpsfix_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in VelCovGeodetic"); + break; } - jump_size = static_cast(1); - } - if (this->isSBF()) - { - if (crc_check_) + last_velcovgeodetic_.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); + velcovgeodetic_has_arrived_gpsfix_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - jump_size = static_cast(this->getBlockLength()); - // Some corrupted messages that survive the CRC check (this happened) - // could tell ROSaic their size is 0, which would lead to an endless - // while loop in the ReadCallback() method of the CallbackHandlers - // class. - if (jump_size == 0) - jump_size = static_cast(1); - } else + wait(time_obj); + } + if (settings_->publish_velcovgeodetic) + publish("/velcovgeodetic", last_velcovgeodetic_); + if (settings_->publish_twist) { - jump_size = static_cast(1); + TwistWithCovarianceStampedMsg twist = TwistCallback(); + publish("/twist", twist); } - } - } - found_ = false; - data_ += jump_size; - count_ -= jump_size; - // node_->log(LogLevel::DEBUG, "Jump about to happen with jump size %li and count - // after jump being %li.", jump_size, count_); - return; // For readability -} - -/** - * If GNSS time is used, Publishing is only done with valid leap seconds - */ -template -void io::RxMessage::publish(const std::string& topic, const M& msg) -{ - // TODO: maybe publish only if wnc and tow is valid? - if (!settings_->use_gnss_time || - (settings_->use_gnss_time && (current_leap_seconds_ != -128))) - { - node_->publishMessage(topic, msg); - } else - { - node_->log( - LogLevel::DEBUG, - "Not publishing message with GNSS time because no leap seconds are available yet."); - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - node_->log( - LogLevel::WARN, - "No leap seconds were set and none were received from log yet."); - } -} - -/** - * If GNSS time is used, Publishing is only done with valid leap seconds - */ -void io::RxMessage::publishTf(const LocalizationMsg& msg) -{ - // TODO: maybe publish only if wnc and tow is valid? - if (!settings_->use_gnss_time || - (settings_->use_gnss_time && (current_leap_seconds_ != -128) && - (current_leap_seconds_ != 0))) - { - node_->publishTf(msg); - } else - { - node_->log( - LogLevel::DEBUG, - "Not publishing tf with GNSS time because no leap seconds are available yet."); - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - node_->log( - LogLevel::WARN, - "No leap seconds were set and none were received from log yet."); - } -} - -/** - * Note that putting the default in the definition's argument list instead of the - * declaration's is an added extra that is not available for function templates, - * hence no search = false here. Also note that the SBF block header part of the - * SBF-echoing ROS messages have ID fields that only show the block number as found - * in the firmware (e.g. 4007 for PVTGeodetic), without the revision number. NMEA - * 0183 messages are at most 82 characters long in principle, but most Septentrio Rxs - * by default increase precision on lat/lon s.t. the maximum allowed e.g. for GGA - * seems to be 89 on a mosaic-x5. Luckily, when parsing we do not care since we just - * search for \\. - */ -bool io::RxMessage::read(std::string message_key, bool search) -{ - if (search) - this->search(); - if (!found()) - return false; - if (this->isSBF()) - { - // If the CRC check is unsuccessful, return false - crc_check_ = isValid(data_); - if (!crc_check_) - { - node_->log( - LogLevel::DEBUG, - "CRC Check returned False. Not a valid data block. Retrieving full SBF block."); - return false; - } - } - switch (rx_id_map[message_key]) - { - case evPVTCartesian: // Position and velocity in XYZ - { // The curly bracket here is crucial: Declarations inside a block remain - // inside, and will die at - // the end of the block. Otherwise variable overloading etc. - PVTCartesianMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PVTCartesianParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PVTCartesian"); break; } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) + case RECEIVER_STATUS: { - wait(time_obj); - } - publish("/pvtcartesian", msg); - break; - } - case evPVTGeodetic: // Position and velocity in geodetic coordinate frame (ENU - // frame) - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PVTGeodeticParser(node_, dvec.begin(), dvec.end(), last_pvtgeodetic_)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PVTGeodetic"); - break; - } - last_pvtgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_gpsfix_ = true; - pvtgeodetic_has_arrived_navsatfix_ = true; - pvtgeodetic_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_pvtgeodetic) - publish("/pvtgeodetic", last_pvtgeodetic_); - break; - } - case evBaseVectorCart: - { - BaseVectorCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!BaseVectorCartParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in BaseVectorCart"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/basevectorcart", msg); - break; - } - case evBaseVectorGeod: - { - BaseVectorGeodMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!BaseVectorGeodParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in BaseVectorGeod"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/basevectorgeod", msg); - break; - } - case evPosCovCartesian: - { - PosCovCartesianMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PosCovCartesianParser(node_, dvec.begin(), dvec.end(), msg)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PosCovCartesian"); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/poscovcartesian", msg); - break; - } - case evPosCovGeodetic: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PosCovGeodeticParser(node_, dvec.begin(), dvec.end(), - last_poscovgeodetic_)) - { - poscovgeodetic_has_arrived_gpsfix_ = false; - poscovgeodetic_has_arrived_navsatfix_ = false; - poscovgeodetic_has_arrived_pose_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PosCovGeodetic"); - break; - } - last_poscovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); - poscovgeodetic_has_arrived_gpsfix_ = true; - poscovgeodetic_has_arrived_navsatfix_ = true; - poscovgeodetic_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_poscovgeodetic) - publish("/poscovgeodetic", last_poscovgeodetic_); - break; - } - case evAttEuler: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!AttEulerParser(node_, dvec.begin(), dvec.end(), last_atteuler_, - settings_->use_ros_axis_orientation)) - { - atteuler_has_arrived_gpsfix_ = false; - atteuler_has_arrived_pose_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in AttEuler"); - break; - } - last_atteuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_atteuler_.header.stamp = timestampToRos(time_obj); - atteuler_has_arrived_gpsfix_ = true; - atteuler_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_atteuler) - publish("/atteuler", last_atteuler_); - break; - } - case evAttCovEuler: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!AttCovEulerParser(node_, dvec.begin(), dvec.end(), last_attcoveuler_, - settings_->use_ros_axis_orientation)) - { - attcoveuler_has_arrived_gpsfix_ = false; - attcoveuler_has_arrived_pose_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in AttCovEuler"); - break; - } - last_attcoveuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_attcoveuler_.header.stamp = timestampToRos(time_obj); - attcoveuler_has_arrived_gpsfix_ = true; - attcoveuler_has_arrived_pose_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_attcoveuler) - publish("/attcoveuler", last_attcoveuler_); - break; - } - case evINSNavCart: // Position, velocity and orientation in cartesian coordinate - // frame (ENU frame) - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), last_insnavcart_, - settings_->use_ros_axis_orientation)) - { - insnavgeod_has_arrived_localization_ecef_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in INSNavCart"); - break; - } - if (settings_->ins_use_poi) - { - last_insnavcart_.header.frame_id = settings_->poi_frame_id; - } else - { - last_insnavcart_.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_insnavcart_.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ecef_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/insnavcart", last_insnavcart_); - break; - } - case evINSNavGeod: // Position, velocity and orientation in geodetic coordinate - // frame (ENU frame) - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), last_insnavgeod_, - settings_->use_ros_axis_orientation)) - { - insnavgeod_has_arrived_gpsfix_ = false; - insnavgeod_has_arrived_navsatfix_ = false; - insnavgeod_has_arrived_pose_ = false; - insnavgeod_has_arrived_localization_ = false; - insnavgeod_has_arrived_localization_ecef_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in INSNavGeod"); - break; - } - if (settings_->ins_use_poi) - { - last_insnavgeod_.header.frame_id = settings_->poi_frame_id; - } else - { - last_insnavgeod_.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_insnavgeod_.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_gpsfix_ = true; - insnavgeod_has_arrived_navsatfix_ = true; - insnavgeod_has_arrived_pose_ = true; - insnavgeod_has_arrived_localization_ = true; - insnavgeod_has_arrived_localization_ecef_ = true; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_insnavgeod) - publish("/insnavgeod", last_insnavgeod_); - if (settings_->publish_twist) - { - TwistWithCovarianceStampedMsg twist = TwistCallback(true); - publish("/twist_ins", twist); - } - break; - } - - case evIMUSetup: // IMU orientation and lever arm - { - IMUSetupMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!IMUSetupParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in IMUSetup"); - break; - } - msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/imusetup", msg); - break; - } - - case evVelSensorSetup: // Velocity sensor lever arm - { - VelSensorSetupMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!VelSensorSetupParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in VelSensorSetup"); - break; - } - msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/velsensorsetup", msg); - break; - } - - case evExtEventINSNavCart: // Position, velocity and orientation in cartesian - // coordinate frame (ENU frame) - { - INSNavCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtEventINSNavCart"); + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!ReceiverStatusParser(node_, dvec.begin(), dvec.end(), + last_receiverstatus_)) + { + receiverstatus_has_arrived_diagnostics_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ReceiverStatus"); + break; + } + receiverstatus_has_arrived_diagnostics_ = true; break; } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else + case QUALITY_IND: { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/exteventinsnavcart", msg); - break; - } - - case evExtEventINSNavGeod: - { - INSNavGeodMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtEventINSNavGeod"); + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!QualityIndParser(node_, dvec.begin(), dvec.end(), last_qualityind_)) + { + qualityind_has_arrived_diagnostics_ = false; + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in QualityInd"); + break; + } + qualityind_has_arrived_diagnostics_ = true; break; } - if (settings_->ins_use_poi) + case RECEIVER_SETUP: { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/exteventinsnavgeod", msg); - break; - } + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!ReceiverSetupParser(node_, dvec.begin(), dvec.end(), + last_receiversetup_)) + { + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ReceiverSetup"); + break; + } + static int32_t ins_major = 1; + static int32_t ins_minor = 3; + static int32_t ins_patch = 2; + static int32_t gnss_major = 4; + static int32_t gnss_minor = 10; + static int32_t gnss_patch = 0; + boost::tokenizer<> tok(last_receiversetup_.rx_version); + boost::tokenizer<>::iterator it = tok.begin(); + std::vector major_minor_patch; + major_minor_patch.reserve(3); + for (boost::tokenizer<>::iterator it = tok.begin(); it != tok.end(); + ++it) + { + int32_t v = std::atoi(it->c_str()); + major_minor_patch.push_back(v); + } + if (major_minor_patch.size() < 3) + { + node_->log( + LogLevel::ERROR, + "septentrio_gnss_driver: parse error of firmware version."); + } else + { + if ((settings_->septentrio_receiver_type == "ins") || + settings_->ins_in_gnss_mode) + { + if ((major_minor_patch[0] < ins_major) || + ((major_minor_patch[0] == ins_major) && + (major_minor_patch[1] < ins_minor)) || + ((major_minor_patch[0] == ins_major) && + (major_minor_patch[1] == ins_minor) && + (major_minor_patch[2] < ins_patch))) + { + node_->log( + LogLevel::WARN, + "INS receiver has firmware version: " + + last_receiversetup_.rx_version + + ", which does not support all features. Please update to at least " + + std::to_string(ins_major) + "." + + std::to_string(ins_minor) + "." + + std::to_string(ins_patch) + " or consult README."); + } + } else if (settings_->septentrio_receiver_type == "gnss") + { + if (major_minor_patch[0] < 3) + { + node_->log( + LogLevel::WARN, + "INS receiver seems to be used as GNSS. Some settings may trigger warnings or errors. Consider using 'ins_in_gnss_mode' as receiver type."); + } else if ((major_minor_patch[0] < gnss_major) || + ((major_minor_patch[0] == gnss_major) && + (major_minor_patch[1] < gnss_minor)) || + ((major_minor_patch[0] == gnss_major) && + (major_minor_patch[1] == gnss_minor) && + (major_minor_patch[2] < gnss_patch))) + { + node_->log( + LogLevel::WARN, + "GNSS receiver has firmware version: " + + last_receiversetup_.rx_version + + ", which may not support all features. Please update to at least " + + std::to_string(gnss_major) + "." + + std::to_string(gnss_minor) + "." + + std::to_string(gnss_patch) + " or consult README."); + } else + node_->log(LogLevel::ERROR, "gnss"); + } + } - case evExtSensorMeas: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - bool hasImuMeas = false; - if (!ExtSensorMeasParser(node_, dvec.begin(), dvec.end(), last_extsensmeas_, - settings_->use_ros_axis_orientation, hasImuMeas)) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtSensorMeas"); break; } - last_extsensmeas_.header.frame_id = settings_->imu_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_extsensmeas_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_extsensormeas) - publish("/extsensormeas", last_extsensmeas_); - if (settings_->publish_imu && hasImuMeas) + case RECEIVER_TIME: { - ImuMsg msg; - try + ReceiverTimeMsg msg; + std::vector dvec(data_, + data_ + parsing_utilities::getLength(data_)); + if (!ReceiverTimeParser(node_, dvec.begin(), dvec.end(), msg)) { - msg = ImuCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "ImuMsg: " + std::string(e.what())); + node_->log(LogLevel::ERROR, + "septentrio_gnss_driver: parse error in ReceiverTime"); break; } - msg.header.frame_id = settings_->imu_frame_id; - msg.header.stamp = last_extsensmeas_.header.stamp; - publish("/imu", msg); + current_leap_seconds_ = msg.delta_ls; + break; } - break; - } - - case evGPST: - { - TimeReferenceMsg msg; - Timestamp time_obj = - timestampSBF(data_, true); // We need the GPS time, hence true - msg.time_ref = timestampToRos(time_obj); - msg.source = "GPST"; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) + default: { - wait(time_obj); + node_->log(LogLevel::DEBUG, "unknown SBF block received."); + break; } - publish("/gpst", msg); - break; + // Many more to be implemented... + } + return true; } - case evGPGGA: + + void processjointMessages() { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - // No kept delimiters, hence "". Also, we specify that empty tokens should - // show up in the output when two delimiters are next to each other. Hence we - // also append the checksum part of the GGA message to "body" below, though - // it is not parsed. - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpggaParser::parseASCII - NMEASentence gga_message(id, body); - GpggaMsg msg; - Timestamp time_obj = node_->getTime(); - GpggaParser parser_obj; + case evDiagnosticArray: + { + DiagnosticArrayMsg msg; try { - msg = parser_obj.parseASCII(gga_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) + msg = DiagnosticArrayCallback(); + } catch (std::runtime_error& e) { - node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what())); + node_->log(LogLevel::DEBUG, + "DiagnosticArrayMsg: " + std::string(e.what())); break; } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgga", msg); - break; - } - case evGPRMC: - { - Timestamp time_obj = node_->getTime(); - - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) + if (settings_->septentrio_receiver_type == "gnss") { - body.push_back(*tok_iter); + msg.header.frame_id = settings_->frame_id; } - // Create NmeaSentence struct to pass to GprmcParser::parseASCII - NMEASentence rmc_message(id, body); - GprmcMsg msg; - GprmcParser parser_obj; - try - { - msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) + if (settings_->septentrio_receiver_type == "ins") { - node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what())); - break; + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + receiverstatus_has_arrived_diagnostics_ = false; + qualityind_has_arrived_diagnostics_ = false; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - Timestamp time_obj = timestampFromRos(msg.header.stamp); wait(time_obj); } - publish("/gprmc", msg); + publish("/diagnostics", msg); break; } - case evGPGSA: + case evLocalization: { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsaParser::parseASCII - NMEASentence gsa_message(id, body); - GpgsaMsg msg; - GpgsaParser parser_obj; + LocalizationMsg msg; try { - msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) + msg = LocalizationUtmCallback(); + } catch (std::runtime_error& e) { - node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what())); + node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); break; } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ = false; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - Timestamp time_obj = timestampFromRos(msg.header.stamp); wait(time_obj); } - publish("/gpgsa", msg); + if (settings_->publish_localization) + publish("/localization", msg); + if (settings_->publish_tf) + publishTf(msg); break; } - case evGPGSV: - case evGLGSV: - case evGAGSV: + case evLocalizationEcef: { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsvParser::parseASCII - NMEASentence gsv_message(id, body); - GpgsvMsg msg; - GpgsvParser parser_obj; + LocalizationMsg msg; try { - msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) + msg = LocalizationEcefCallback(); + } catch (std::runtime_error& e) { - node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what())); + node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); break; } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } + Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ecef_ = false; + insnavgeod_has_arrived_localization_ecef_ = false; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - Timestamp time_obj = timestampFromRos(msg.header.stamp); wait(time_obj); } - publish("/gpgsv", msg); + if (settings_->publish_localization_ecef) + publish("/localization_ecef", msg); + if (settings_->publish_tf_ecef) + publishTf(msg); break; } @@ -2919,395 +2799,327 @@ bool io::RxMessage::read(std::string message_key, bool search) break; } } - case evChannelStatus: + } + + void MessageParser::wait(Timestamp time_obj) + { + Timestamp unix_old = unix_time_; + unix_time_ = time_obj; + if ((unix_old != 0) && (unix_time_ != unix_old)) + { + if (unix_time_ > unix_old) + { + auto sleep_nsec = unix_time_ - unix_old; + + std::stringstream ss; + ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; + node_->log(LogLevel::DEBUG, ss.str()); + + std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); + } + } + + // set leap seconds to paramter only if it was not set otherwise (by + // ReceiverTime) + if (current_leap_seconds_ == -128) + current_leap_seconds_ = settings_->leap_seconds; + } + + void parseNMEA() + { + case evGPST: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ChannelStatusParser(node_, dvec.begin(), dvec.end(), - last_channelstatus_)) + TimeReferenceMsg msg; + Timestamp time_obj = + timestampSBF(data_, true); // We need the GPS time, hence true + msg.time_ref = timestampToRos(time_obj); + msg.source = "GPST"; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ChannelStatus"); - break; + wait(time_obj); } - channelstatus_has_arrived_gpsfix_ = true; + publish("/gpst", msg); break; } - case evMeasEpoch: + case evGPGGA: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!MeasEpochParser(node_, dvec.begin(), dvec.end(), last_measepoch_)) + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(data_), nmea_size); + tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + // No kept delimiters, hence "". Also, we specify that empty tokens + // should show up in the output when two delimiters are next to each + // other. Hence we also append the checksum part of the GGA message to + // "body" below, though it is not parsed. + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in MeasEpoch"); - break; + body.push_back(*tok_iter); } - last_measepoch_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_measepoch_.header.stamp = timestampToRos(time_obj); - measepoch_has_arrived_gpsfix_ = true; - if (settings_->publish_measepoch) - publish("/measepoch", last_measepoch_); - break; - } - case evDOP: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!DOPParser(node_, dvec.begin(), dvec.end(), last_dop_)) + // Create NmeaSentence struct to pass to GpggaParser::parseASCII + NMEASentence gga_message(id, body); + GpggaMsg msg; + Timestamp time_obj = node_->getTime(); + GpggaParser parser_obj; + try { - dop_has_arrived_gpsfix_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in DOP"); + msg = parser_obj.parseASCII(gga_message, settings_->frame_id, + settings_->use_gnss_time, time_obj); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what())); break; } - dop_has_arrived_gpsfix_ = true; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgga", msg); break; } - case evVelCovGeodetic: + case evGPRMC: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!VelCovGeodeticParser(node_, dvec.begin(), dvec.end(), - last_velcovgeodetic_)) + Timestamp time_obj = node_->getTime(); + + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(data_), nmea_size); + tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) { - velcovgeodetic_has_arrived_gpsfix_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in VelCovGeodetic"); + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GprmcParser::parseASCII + NMEASentence rmc_message(id, body); + GprmcMsg msg; + GprmcParser parser_obj; + try + { + msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, + settings_->use_gnss_time, time_obj); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what())); break; } - last_velcovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); - velcovgeodetic_has_arrived_gpsfix_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { + Timestamp time_obj = timestampFromRos(msg.header.stamp); wait(time_obj); } - if (settings_->publish_velcovgeodetic) - publish("/velcovgeodetic", last_velcovgeodetic_); - if (settings_->publish_twist) - { - TwistWithCovarianceStampedMsg twist = TwistCallback(); - publish("/twist", twist); - } + publish("/gprmc", msg); break; } - case evDiagnosticArray: + case evGPGSA: { - DiagnosticArrayMsg msg; + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(data_), nmea_size); + tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GpgsaParser::parseASCII + NMEASentence gsa_message(id, body); + GpgsaMsg msg; + GpgsaParser parser_obj; try { - msg = DiagnosticArrayCallback(); - } catch (std::runtime_error& e) + msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, + settings_->use_gnss_time, node_->getTime()); + } catch (ParseException& e) { - node_->log(LogLevel::DEBUG, - "DiagnosticArrayMsg: " + std::string(e.what())); + node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what())); break; } if (settings_->septentrio_receiver_type == "gnss") { - msg.header.frame_id = settings_->frame_id; + Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); } if (settings_->septentrio_receiver_type == "ins") { - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } + Timestamp time_obj; + time_obj = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - receiverstatus_has_arrived_diagnostics_ = false; - qualityind_has_arrived_diagnostics_ = false; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { + Timestamp time_obj = timestampFromRos(msg.header.stamp); wait(time_obj); } - publish("/diagnostics", msg); + publish("/gpgsa", msg); break; } - case evLocalization: + case evGPGSV: + case evGLGSV: + case evGAGSV: { - LocalizationMsg msg; + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(data_), nmea_size); + tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GpgsvParser::parseASCII + NMEASentence gsv_message(id, body); + GpgsvMsg msg; + GpgsvParser parser_obj; try { - msg = LocalizationUtmCallback(); - } catch (std::runtime_error& e) + msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, + settings_->use_gnss_time, node_->getTime()); + } catch (ParseException& e) { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); + node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what())); break; } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) + if (settings_->septentrio_receiver_type == "gnss") { - wait(time_obj); + Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); } - if (settings_->publish_localization) - publish("/localization", msg); - if (settings_->publish_tf) - publishTf(msg); - break; - } - case evLocalizationEcef: - { - LocalizationMsg msg; - try - { - msg = LocalizationEcefCallback(); - } catch (std::runtime_error& e) + if (settings_->septentrio_receiver_type == "ins") { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); - break; + Timestamp time_obj; + time_obj = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ecef_ = false; - insnavgeod_has_arrived_localization_ecef_ = false; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { + Timestamp time_obj = timestampFromRos(msg.header.stamp); wait(time_obj); } - if (settings_->publish_localization_ecef) - publish("/localization_ecef", msg); - if (settings_->publish_tf_ecef) - publishTf(msg); + publish("/gpgsv", msg); break; } - case evReceiverStatus: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverStatusParser(node_, dvec.begin(), dvec.end(), - last_receiverstatus_)) - { - receiverstatus_has_arrived_diagnostics_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverStatus"); - break; + + bool MessageParser::gnss_gpsfix_complete(uint32_t id) + { + std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, + measepoch_has_arrived_gpsfix_, + dop_has_arrived_gpsfix_, + pvtgeodetic_has_arrived_gpsfix_, + poscovgeodetic_has_arrived_gpsfix_, + velcovgeodetic_has_arrived_gpsfix_, + atteuler_has_arrived_gpsfix_, + attcoveuler_has_arrived_gpsfix_}; + return allTrue(gpsfix_vec, id); } - receiverstatus_has_arrived_diagnostics_ = true; - break; - } - case evQualityInd: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!QualityIndParser(node_, dvec.begin(), dvec.end(), last_qualityind_)) + + bool MessageParser::ins_gpsfix_complete(uint32_t id) { - qualityind_has_arrived_diagnostics_ = false; - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in QualityInd"); - break; + std::vector gpsfix_vec = { + channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, + dop_has_arrived_gpsfix_, insnavgeod_has_arrived_gpsfix_}; + return allTrue(gpsfix_vec, id); } - qualityind_has_arrived_diagnostics_ = true; - break; - } - case evReceiverSetup: - { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverSetupParser(node_, dvec.begin(), dvec.end(), - last_receiversetup_)) + + bool MessageParser::gnss_navsatfix_complete(uint32_t id) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverSetup"); - break; + std::vector navsatfix_vec = { + pvtgeodetic_has_arrived_navsatfix_, + poscovgeodetic_has_arrived_navsatfix_}; + return allTrue(navsatfix_vec, id); } - static int32_t ins_major = 1; - static int32_t ins_minor = 3; - static int32_t ins_patch = 2; - static int32_t gnss_major = 4; - static int32_t gnss_minor = 10; - static int32_t gnss_patch = 0; - boost::tokenizer<> tok(last_receiversetup_.rx_version); - boost::tokenizer<>::iterator it = tok.begin(); - std::vector major_minor_patch; - major_minor_patch.reserve(3); - for (boost::tokenizer<>::iterator it = tok.begin(); it != tok.end(); ++it) - { - int32_t v = std::atoi(it->c_str()); - major_minor_patch.push_back(v); - } - if (major_minor_patch.size() < 3) - { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error of firmware version."); - } else + + bool MessageParser::ins_navsatfix_complete(uint32_t id) { - if ((settings_->septentrio_receiver_type == "ins") || - settings_->ins_in_gnss_mode) - { - if ((major_minor_patch[0] < ins_major) || - ((major_minor_patch[0] == ins_major) && - (major_minor_patch[1] < ins_minor)) || - ((major_minor_patch[0] == ins_major) && - (major_minor_patch[1] == ins_minor) && - (major_minor_patch[2] < ins_patch))) - { - node_->log( - LogLevel::WARN, - "INS receiver has firmware version: " + - last_receiversetup_.rx_version + - ", which does not support all features. Please update to at least " + - std::to_string(ins_major) + "." + - std::to_string(ins_minor) + "." + - std::to_string(ins_patch) + " or consult README."); - } - } else if (settings_->septentrio_receiver_type == "gnss") - { - if (major_minor_patch[0] < 3) - { - node_->log( - LogLevel::WARN, - "INS receiver seems to be used as GNSS. Some settings may trigger warnings or errors. Consider using 'ins_in_gnss_mode' as receiver type."); - } else if ((major_minor_patch[0] < gnss_major) || - ((major_minor_patch[0] == gnss_major) && - (major_minor_patch[1] < gnss_minor)) || - ((major_minor_patch[0] == gnss_major) && - (major_minor_patch[1] == gnss_minor) && - (major_minor_patch[2] < gnss_patch))) - { - node_->log( - LogLevel::WARN, - "GNSS receiver has firmware version: " + - last_receiversetup_.rx_version + - ", which may not support all features. Please update to at least " + - std::to_string(gnss_major) + "." + - std::to_string(gnss_minor) + "." + - std::to_string(gnss_patch) + " or consult README."); - } else - node_->log(LogLevel::ERROR, "gnss"); - } + std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; + return allTrue(navsatfix_vec, id); } - break; - } - case evReceiverTime: - { - ReceiverTimeMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverTimeParser(node_, dvec.begin(), dvec.end(), msg)) + bool MessageParser::gnss_pose_complete(uint32_t id) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverTime"); - break; + std::vector pose_vec = { + pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, + atteuler_has_arrived_pose_, attcoveuler_has_arrived_pose_}; + return allTrue(pose_vec, id); } - current_leap_seconds_ = msg.delta_ls; - break; - } - // Many more to be implemented... - } - return true; -} - -void io::RxMessage::wait(Timestamp time_obj) -{ - Timestamp unix_old = unix_time_; - unix_time_ = time_obj; - if ((unix_old != 0) && (unix_time_ != unix_old)) - { - if (unix_time_ > unix_old) + bool MessageParser::ins_pose_complete(uint32_t id) { - auto sleep_nsec = unix_time_ - unix_old; + std::vector pose_vec = {insnavgeod_has_arrived_pose_}; + return allTrue(pose_vec, id); + } - std::stringstream ss; - ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; - node_->log(LogLevel::DEBUG, ss.str()); + bool MessageParser::diagnostics_complete(uint32_t id) + { + std::vector diagnostics_vec = { + receiverstatus_has_arrived_diagnostics_, + qualityind_has_arrived_diagnostics_}; + return allTrue(diagnostics_vec, id); + } - std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); + bool MessageParser::ins_localization_complete(uint32_t id) + { + std::vector loc_vec = {insnavgeod_has_arrived_localization_}; + return allTrue(loc_vec, id); + } + + bool MessageParser::ins_localization_ecef_complete(uint32_t id) + { + std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, + insnavcart_has_arrived_localization_ecef_}; + return allTrue(loc_vec, id); + } + + bool MessageParser::allTrue(std::vector & vec, uint32_t id) + { + vec.erase(vec.begin() + id); + // Checks whether all entries in vec are true + return (std::all_of(vec.begin(), vec.end(), [](bool v) { return v; }) == + true); } - } - // set leap seconds to paramter only if it was not set otherwise (by - // ReceiverTime) - if (current_leap_seconds_ == -128) - current_leap_seconds_ = settings_->leap_seconds; -} - -bool io::RxMessage::gnss_gpsfix_complete(uint32_t id) -{ - std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, - measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, - pvtgeodetic_has_arrived_gpsfix_, - poscovgeodetic_has_arrived_gpsfix_, - velcovgeodetic_has_arrived_gpsfix_, - atteuler_has_arrived_gpsfix_, - attcoveuler_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); -} - -bool io::RxMessage::ins_gpsfix_complete(uint32_t id) -{ - std::vector gpsfix_vec = { - channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, insnavgeod_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); -} - -bool io::RxMessage::gnss_navsatfix_complete(uint32_t id) -{ - std::vector navsatfix_vec = {pvtgeodetic_has_arrived_navsatfix_, - poscovgeodetic_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); -} - -bool io::RxMessage::ins_navsatfix_complete(uint32_t id) -{ - std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); -} - -bool io::RxMessage::gnss_pose_complete(uint32_t id) -{ - std::vector pose_vec = { - pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, - atteuler_has_arrived_pose_, attcoveuler_has_arrived_pose_}; - return allTrue(pose_vec, id); -} - -bool io::RxMessage::ins_pose_complete(uint32_t id) -{ - std::vector pose_vec = {insnavgeod_has_arrived_pose_}; - return allTrue(pose_vec, id); -} - -bool io::RxMessage::diagnostics_complete(uint32_t id) -{ - std::vector diagnostics_vec = {receiverstatus_has_arrived_diagnostics_, - qualityind_has_arrived_diagnostics_}; - return allTrue(diagnostics_vec, id); -} - -bool io::RxMessage::ins_localization_complete(uint32_t id) -{ - std::vector loc_vec = {insnavgeod_has_arrived_localization_}; - return allTrue(loc_vec, id); -} - -bool io::RxMessage::ins_localization_ecef_complete(uint32_t id) -{ - std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, - insnavcart_has_arrived_localization_ecef_}; - return allTrue(loc_vec, id); -} - -bool io::RxMessage::allTrue(std::vector& vec, uint32_t id) -{ - vec.erase(vec.begin() + id); - // Checks whether all entries in vec are true - return (std::all_of(vec.begin(), vec.end(), [](bool v) { return v; }) == true); -} + } // namespace io diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp similarity index 88% rename from src/septentrio_gnss_driver/communication/message_handler.cpp rename to src/septentrio_gnss_driver/communication/telegram_handler.cpp index 27ac171a..6dd0aba7 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -28,21 +28,21 @@ // // ***************************************************************************** -#include +#include /** - * @file message_handler.cpp + * @file telegram_handler.cpp * @date 22/08/20 * @brief Handles callbacks when reading NMEA/SBF messages */ namespace io { - void MessageHandler::handle() {} + void TelegramHandler::handle() {} - void MessageHandler::handleSbf(const Message& message) {} + void TelegramHandler::handleSbf(const std::shared_ptr& telegram) {} - void MessageHandler::handleNmea(const Message& message) + void TelegramHandler::handleNmea(const std::shared_ptr& telegram) { boost::char_separator sep("\r"); // Carriage Return (CR) typedef boost::tokenizer> tokenizer; @@ -58,7 +58,7 @@ namespace io { " bytes and is ready to be parsed. It reads: " + *tokens.begin()); } - void MessageHandler::handleResponse(const Message& message) + void TelegramHandler::handleResponse(const std::shared_ptr& telegram) { std::size_t response_size = rx_message_.messageSize(); std::string block_in_string( @@ -73,7 +73,7 @@ namespace io { lock.unlock(); response_sync.condition.notify_one(); } - if (rx_message_.isErrorMessage()) + if (rx_message_.isErrorTelegram()) { node_->log( LogLevel::ERROR, @@ -83,7 +83,7 @@ namespace io { } } - void MessageHandler::handleCd(const Message& message) + void TelegramHandler::handleCd(const std::shared_ptr& telegram) { std::string cd(reinterpret_cast(rx_message_.getPosBuffer()), 4); rx_tcp_port = cd; From c7145dea89e93f73d54c0fd7cac357d0680364fc Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 12 Jan 2023 23:18:17 +0100 Subject: [PATCH 018/170] Improve io handling, WIP --- CMakeLists.txt | 4 +- .../communication/async_manager.hpp | 166 +- .../communication/communication_core.hpp | 9 +- .../communication/io.hpp | 72 +- .../communication/message_parser.hpp | 8 +- .../communication/pcap_reader.hpp | 5 +- .../communication/settings.h | 4 + .../communication/telegram.hpp | 45 +- include/septentrio_gnss_driver/crc/crc.h | 46 +- .../node/rosaic_node.hpp | 7 +- .../parsers/nmea_parsers/gpgga.hpp | 7 +- .../parsers/nmea_parsers/gpgsa.hpp | 7 +- .../parsers/nmea_parsers/gpgsv.hpp | 7 +- .../parsers/nmea_parsers/gprmc.hpp | 7 +- .../parsers/nmea_sentence.hpp | 7 +- .../parsers/parse_exception.hpp | 7 +- .../parsers/parser_base_class.hpp | 7 +- .../parsers/parsing_utilities.hpp | 7 +- .../parsers/string_utilities.h | 7 +- .../communication/communication_core.cpp | 1492 +++++++++-------- src/septentrio_gnss_driver/crc/crc.cpp | 103 +- 21 files changed, 1027 insertions(+), 997 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f4356e83..1118a0f5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -188,8 +188,8 @@ add_executable(${PROJECT_NAME}_node src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp src/septentrio_gnss_driver/crc/crc.cpp src/septentrio_gnss_driver/communication/communication_core.cpp - src/septentrio_gnss_driver/communication/rx_message.cpp - src/septentrio_gnss_driver/communication/callback_handlers.cpp + src/septentrio_gnss_driver/communication/message_parser.cpp + src/septentrio_gnss_driver/communication/telegram_handler.cpp src/septentrio_gnss_driver/communication/pcap_reader.cpp ) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index e258e992..4c5cb33a 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -56,6 +56,8 @@ // // ***************************************************************************** +#pragma once + // Boost includes #include #include @@ -66,10 +68,8 @@ #include // local includes -#include "io.hpp" -#include "message.hpp" - -#pragma once +#include +#include /** * @file async_manager.hpp @@ -93,6 +93,8 @@ namespace io { virtual ~AsyncManagerBase() {} //! Sends commands to the receiver virtual bool send(const std::string& cmd) = 0; + //! Connects the stream + [[nodiscard]] virtual bool connect() = 0; }; /** @@ -100,21 +102,19 @@ namespace io { * @brief This is the central interface between ROSaic and the Rx(s), managing * I/O operations such as reading messages and sending commands.. * - * StreamT is either boost::asio::serial_port or boost::asio::tcp::ip + * SocketT is either boost::asio::serial_port or boost::asio::tcp::ip */ - template + template class AsyncManager : public AsyncManagerBase { public: /** * @brief Class constructor - * @param stream data stream - * @param[in] buffer_size Size of the circular buffer in bytes + * @param[in] node Pointer to node + * @param[in] telegramQueue Telegram queue */ - AsyncManager(ROSaicNodeBase* node, std::unique_ptr stream, - TelegramQueue* telegramQueue) : - node_(node), - stream_(std::move(stream)), telegramQueue_(telegramQueue) + AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue) : + node_(node), ioSocket_(node, &ioService_), telegramQueue_(telegramQueue) { } @@ -130,15 +130,9 @@ namespace io { node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); } - [[nodicard]] bool connect() + [[nodiscard]] bool connect() { - if (!ioSocket_) - { - if (!initIo()) - return false; - } - - if (ioSocket_->connect()) + if (ioSocket_.connect()) { return false; } @@ -157,68 +151,10 @@ namespace io { "AsyncManager message size to be sent to the Rx would be 0"); } - ioService_.post(boost::bind(&AsyncManager::write, this, cmd)); + ioService_.post(boost::bind(&AsyncManager::write, this, cmd)); } private: - [[nodiscard]] initIo() - { - node_->log(LogLevel::DEBUG, "Called initializeIo() method"); - boost::smatch match; - // In fact: smatch is a typedef of match_results - if (boost::regex_match(node_->settings_.device, match, - boost::regex("(tcp)://(.+):(\\d+)"))) - // C++ needs \\d instead of \d: Both mean decimal. - // Note that regex_match can be used with a smatch object to store - // results, or without. In any case, true is returned if and only if it - // matches the !complete! string. - { - // The first sub_match (index 0) contained in a match_result always - // represents the full match within a target sequence made by a - // regex, and subsequent sub_matches represent sub-expression matches - // corresponding in sequence to the left parenthesis delimiting the - // sub-expression in the regex, i.e. $n Perl is equivalent to m[n] in - // boost regex. - - ioSocket.reset(new TcpIo(node_, &ioService_, match[2], match[3])) - } else if (boost::regex_match( - node_->settings_.device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) - { - node_->settings_.read_from_sbf_log = true; - node_->settings_.use_gnss_time = true; - // connectionThread_ = boost::thread( - // boost::bind(&CommIo::prepareSBFFileReading, this, match[2])); - - } else if (boost::regex_match( - node_->settings_.device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) - { - node_->settings_.read_from_pcap = true; - node_->settings_.use_gnss_time = true; - // connectionThread_ = boost::thread( - // boost::bind(&CommIo::preparePCAPFileReading, this, match[2])); - - } else if (boost::regex_match(node_->settings_.device, match, - boost::regex("(serial):(.+)"))) - { - std::string proto(match[2]); - std::stringstream ss; - ss << "Searching for serial port" << proto; - node_->log(LogLevel::DEBUG, ss.str()); - node_->settings_.device = proto; - ioSocket.reset(new SerialIo(node_, &ioService_, settings_->device, - settings_->baudrate, - settings_->hw_flow_control)) - } else - { - std::stringstream ss; - ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; - node_->log(LogLevel::ERROR, ss.str()); - } - node_->log(LogLevel::DEBUG, "Leaving initializeIo() method"); - } - void receive() { resync(); @@ -261,7 +197,7 @@ namespace io { void write(const std::string& cmd) { - boost::asio::write(*stream_, + boost::asio::write(ioSocket_.stream(), boost::asio::buffer(cmd.data(), cmd.size())); // Prints the data that was sent node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + @@ -281,7 +217,8 @@ namespace io { static_assert(index < 3); boost::asio::async_read( - *stream_, boost::asio::buffer(message_->data.data() + index, 1), + ioSocket_.stream(), + boost::asio::buffer(message_->data.data() + index, 1), [this](boost::system::error_code ec, std::size_t numBytes) { Timestamp stamp = node_->getTime(); @@ -289,7 +226,9 @@ namespace io { { if (numBytes == 1) { - if (message_->data[index] == SYNC_0) + std::byte& currByte = message_->data[index]; + + if (currByte == SYNC_0) { message_->stamp = stamp; readSync<1>(); @@ -299,12 +238,21 @@ namespace io { { case 0: { - readSync<0>(); + if ((currByte == CONNECTION_DESCRIPTOR_BYTE_I) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_C) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_U) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_N) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_D)) + { + message_->type = CONNECTION_DESCRIPTOR; + readCd(); + } else + readSync<0>(); break; } case 1: { - switch (message_->data[index]) + switch (currByte) { case SBF_SYNC_BYTE_1: { @@ -324,12 +272,6 @@ namespace io { readSync<2>(); break; } - case CONNECTION_DESCRIPTOR_BYTE_1: - { - message_->type = CONNECTION_DESCRIPTOR; - readSync<2>(); - break; - } default: { node_->log( @@ -343,7 +285,7 @@ namespace io { } case 2: { - switch (message_->data[index]) + switch (currByte) { case NMEA_SYNC_BYTE_2: { @@ -369,14 +311,6 @@ namespace io { resync(); break; } - case CONNECTION_DESCRIPTOR_BYTE_2: - { - if (message_->type == CONNECTION_DESCRIPTOR) - readString(); - else - resync(); - break; - } default: { node_->log( @@ -420,7 +354,7 @@ namespace io { message_->data.resize(SBF_HEADER_SIZE); boost::asio::async_read( - *stream_, + ioSocket_.stream(), boost::asio::buffer(message_->data.data() + 2, SBF_HEADER_SIZE - 2), [this](boost::system::error_code ec, std::size_t numBytes) { if (!ec) @@ -461,7 +395,7 @@ namespace io { message_->data.resize(length); boost::asio::async_read( - *stream_, + ioSocket_.stream(), boost::asio::buffer(message_->data.data() + SBF_HEADER_SIZE, length - SBF_HEADER_SIZE), [this](boost::system::error_code ec, std::size_t numBytes) { @@ -469,7 +403,7 @@ namespace io { { if (numBytes == (length - SBF_HEADER_SIZE)) { - if (isValid(message_->data.data())) // TODO namespace crc + if (crc::isValid(message_->data.data())) { message_->sbfId = parsing_utilities::getId(message_->data.data()); @@ -492,6 +426,12 @@ namespace io { }); } + void readCd() + { + message_->data.resize(1); + readStringElements(); + } + void readString() { message_->data.resize(3); @@ -500,21 +440,22 @@ namespace io { void readStringElements() { - std::byte byte; + std::array buf; boost::asio::async_read( - *stream_, boost::asio::buffer(byte, 1), + ioSocket_.stream(), boost::asio::buffer(buf.data(), 1), [this](boost::system::error_code ec, std::size_t numBytes) { if (!ec) { if (numBytes == 1) { - switch (byte) + message_->data.push_back(buf[0]); + switch (buf[0]) { case SYNC_0: { message_.reset(new Telegram); - message_->data[0] = byte; + message_->data[0] = buf[0]; message_->stamp = node_->getTime(); node_->log( LogLevel::DEBUG, @@ -524,15 +465,20 @@ namespace io { } case LF: { - message_->data.push_back(byte); if (message_->data[message_->data.size() - 2] == CR) telegramQueue_->push(message_); resync(); break; } + case CONNECTION_DESCRIPTOR_FOOTER: + { + if (message_->type == CONNECTION_DESCRIPTOR) + telegramQueue_->push(message_); + resync(); + break; + } default: { - message_->data.push_back(byte); readString(); break; } @@ -555,11 +501,9 @@ namespace io { }); } - //! Stream, represents either interface connection or file - std::unique_ptr stream_; //! Pointer to the node ROSaicNodeBase* node_; - std::unique_ptr ioSocket_; + SocketT ioSocket_; std::atomic running_; boost::asio::io_service ioService_; std::thread ioThread_; diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 542a6074..3699e8c7 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -173,7 +173,7 @@ namespace io { //! Settings Settings* settings_; //! TelegramQueue - TelegramQueue* telegramQueue_; + TelegramQueue telegramQueue_; //! Telegram handlers for the inwards streaming messages MessageHandler messageHandler_; //! Whether connecting to Rx was successful @@ -202,11 +202,8 @@ namespace io { bool nmeaActivated_ = false; - //! Indicator for threads to exit - std::atomic stopping_; - - friend class CallbackHandlers; - friend class RxMessage; + //! Indicator for threads to run + std::atomic running_; //! Communication ports std::string mainPort_; diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 7af37b2d..5087a97f 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -49,45 +49,56 @@ const static std::array baudrates = { 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; namespace io { - class IoBase - { - public: - IoBase(ROSaicNodeBase* node) : node_(node) {} - - virtual ~IoBase(); - - virtual [[nodiscard]] bool connect() = 0; - - private: - ROSaicNodeBase* node_; - }; - class TcpIo : public IoBase + class TcpIo { public: - TcpIo(ROSaicNodeBase* node, boost::asio::io_service* ioService, - const std::string& ip, const std::string& port) : - IoBase(node), - ioService_(ioService), socket_(*ioService_) + TcpIo(ROSaicNodeBase* node, boost::asio::io_service* ioService) : + node_(node), ioService_(ioService), socket_(*ioService_) { - boost::asio::ip::tcp::resolver resolver(*ioService_); - boost::asio::ip::tcp::resolver::query query(ip, port); - endpointIterator_ = resolver.resolve(query); } ~TcpIo() { socket_.close(); } + const boost::asio::ip::tcp::socket& stream() { return socket_; } + [[nodiscard]] bool connect() { + boost::asio::ip::tcp::resolver::iterator endpointIterator; + try + { + boost::asio::ip::tcp::resolver resolver(*ioService_); + boost::asio::ip::tcp::resolver::query query( + node_->getSettings()->tcp_ip, node->getSettings()->tcp_port); + endpointIterator = resolver.resolve(query); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::ERROR, "Could not resolve " + host + + " on port " + port + ": " + + e.what()); + return false; + } + socket_.reset(new boost::asio::ip::tcp::socket(*ioService_)); - socket_.connect(*endpointIterator_); + try + { + socket_.connect(*endpointIterator); - socket_.set_option(boost::asio::ip::tcp::no_delay(true)); + socket_.set_option(boost::asio::ip::tcp::no_delay(true)); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::ERROR, + "Could not connect to " + endpointIterator->host_name() + + ": " + endpointIterator->service_name() + ": " + + e.what()); + return false; + } + return true; } private: - boost::asio::ip::tcp::resolver::iterator endpointIterator_; + ROSaicNodeBase* node_; boost::asio::io_service* ioService_; boost::asio::ip::tcp::socket socket_; }; @@ -97,13 +108,16 @@ namespace io { public: SerialIo(ROSaicNodeBase* node, boost::asio::io_service* ioService, std::string serialPort, uint32_t baudrate, bool flowcontrol) : - IoBase(node), - flowcontrol_(flowcontrol), baudrate_(baudrate), serialPort_(ioService_) + node_(node), + flowcontrol_(node->getSettings()->flowcontrol), + baudrate_(node->getSettings()->baudrate), serialPort_(ioService_) { } ~SerialIo() { serialPort_.close(); } + const boost::asio::serial_port& stream() { return socket_; } + [[nodiscard]] bool connect() { if (serialPort_.is_open()) @@ -149,7 +163,7 @@ namespace io { tcgetattr(fd, &tio); // Hardware flow control settings_->. - if (flowcontrol_) + if (flowcontrol_ == "RTS|CTS") { tio.c_iflag &= ~(IXOFF | IXON); tio.c_cflag |= CRTSCTS; @@ -186,8 +200,9 @@ namespace io { { serialPort_.get_option( current_baudrate); // Note that this sets - // current_baudrate.value() often to 115200, - // since by default, all Rx COM ports, + // current_baudrate.value() often to + // 115200, since by default, all Rx COM + // ports, // at least for mosaic Rxs, are set to a baudrate of 115200 baud, // using 8 data-bits, no parity and 1 stop-bit. } catch (boost::system::system_error& e) @@ -269,6 +284,7 @@ namespace io { } private: + ROSaicNodeBase* node_; bool flowcontrol_; std::string port_; uint32_t baudrate_; diff --git a/include/septentrio_gnss_driver/communication/message_parser.hpp b/include/septentrio_gnss_driver/communication/message_parser.hpp index 6659c0b2..b1c92796 100644 --- a/include/septentrio_gnss_driver/communication/message_parser.hpp +++ b/include/septentrio_gnss_driver/communication/message_parser.hpp @@ -56,6 +56,8 @@ // // ***************************************************************************** +#pragma once + // C++ libraries #include // for assert #include @@ -75,9 +77,6 @@ #include #include -#ifndef RX_MESSAGE_HPP -#define RX_MESSAGE_HPP - /** * @file rx_message.hpp * @brief Defines a class that reads messages handed over from the circular buffer @@ -393,5 +392,4 @@ namespace io { */ Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time); }; -} // namespace io -#endif // for RX_MESSAGE_HPP \ No newline at end of file +} // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/pcap_reader.hpp b/include/septentrio_gnss_driver/communication/pcap_reader.hpp index 7ee5bdea..ebad8b09 100644 --- a/include/septentrio_gnss_driver/communication/pcap_reader.hpp +++ b/include/septentrio_gnss_driver/communication/pcap_reader.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PCAP_READER_H -#define PCAP_READER_H +#pragma once #include #include @@ -117,5 +116,3 @@ namespace pcapReader { buffer_t m_lastPkt; }; } // namespace pcapReader - -#endif // PCAP_READER_H diff --git a/include/septentrio_gnss_driver/communication/settings.h b/include/septentrio_gnss_driver/communication/settings.h index 26469dbf..03011c96 100644 --- a/include/septentrio_gnss_driver/communication/settings.h +++ b/include/septentrio_gnss_driver/communication/settings.h @@ -106,6 +106,10 @@ struct Settings bool activate_debug_log; //! Device port std::string device; + //! TCP IP + std::string tcp_ip; + //! TCP port + std::string tcp_port; //! Username for login std::string login_user; //! Password for login diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index de894a86..5dbfee03 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -45,26 +45,41 @@ static const std::byte SYNC_0 = 0x24; //! 0x40 is ASCII for @ - 2nd byte to indicate SBF block static const std::byte SBF_SYNC_BYTE_1 = 0x40; //! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message -static const std::byte NMEA_SYNC_BYTE_1 0x47; -//! 0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message -static const std::byte NMEA_SYNC_BYTE_2 0x50; +static const std::byte NMEA_SYNC_BYTE_1 = 0x47; +//! 0x50 is ASCII for P - 3rd byte to indicate NMEA-type ASCII message +static const std::byte NMEA_SYNC_BYTE_2 = 0x50; +//! 0x49 is ASCII for I - 2nd byte to indicate INS NMEA-type ASCII message +static const std::byte NMEA_INS_SYNC_BYTE_1 = 0x49; +//! 0x4E is ASCII for N - 3rd byte to indicate NMEA-type ASCII message +static const std::byte NMEA_INS_SYNC_BYTE_2 = 0x4E; //! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx -static const std::byte RESPONSE_SYNC_BYTE_1 0x52; -//! 0x3A is ASCII for ? - 3rd byte in the response message from the Rx -static const std::byte RESPONSE_SYNC_BYTE_2 0x3A; -//! 0x3F is ASCII for : - 3rd byte in the response message from the Rx in case the +static const std::byte RESPONSE_SYNC_BYTE_1 = 0x52; +//! 0x3A is ASCII for : - 3rd byte in the response message from the Rx +static const std::byte RESPONSE_SYNC_BYTE_2 = 0x3A; +//! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the //! command was invalid -static const std::byte ERROR_SYNC_BYTE_2 0x3F; +static const std::byte ERROR_SYNC_BYTE_2 = 0x3F; //! 0x0D is ASCII for "Carriage Return", i.e. "Enter" -static const std::byte CR 0x0D; +static const std::byte CR = 0x0D; //! 0x0A is ASCII for "Line Feed", i.e. "New Line" -static const std::byte LF 0x0A; +static const std::byte LF = 0x0A; //! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after -//! initiating TCP connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_1 0x49; -//! 0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after -//! initiating TCP connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_2 0x50; +//! initiating IP connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_I = 0x49; +//! 0x43 is ASCII for C - 1st character of connection descriptor sent by the Rx after +//! initiating COM connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_C = 0x43; +//! 0x55 is ASCII for U - 1st character of connection descriptor sent by the Rx after +//! initiating USB connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_U = 0x55; +//! 0x4E is ASCII for N - 1st character of connection descriptor sent by the Rx after +//! initiating NTRIP connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_N = 0x4E; +//! 0x44 is ASCII for D - 1st character of connection descriptor sent by the Rx after +//! initiating DSK connection +static const std::byte CONNECTION_DESCRIPTOR_BYTE_D = 0x44; +//! 0x3E is ASCII for > - end character of connection descriptor +static const std::byte CONNECTION_DESCRIPTOR_FOOTER = 0x3E; static const uint16_t SBF_HEADER_SIZE = 8; static const uint16_t MAX_SBF_SIZE 65535; diff --git a/include/septentrio_gnss_driver/crc/crc.h b/include/septentrio_gnss_driver/crc/crc.h index 62332fcb..2d463ffb 100644 --- a/include/septentrio_gnss_driver/crc/crc.h +++ b/include/septentrio_gnss_driver/crc/crc.h @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef CRC_H -#define CRC_H +#pragma once // ROSaic includes // The following imports structs into which SBF blocks can be unpacked then shipped @@ -40,27 +39,28 @@ #include #include -/** - * @file crc.h - * @brief Declares the functions to compute and validate the CRC of a buffer - * @date 17/08/20 - */ +namespace crc { + /** + * @file crc.h + * @brief Declares the functions to compute and validate the CRC of a buffer + * @date 17/08/20 + */ -/** - * @brief This function computes the CRC-8-CCITT (Cyclic Redundancy Check) of a - * buffer "buf" of "buf_length" bytes - * @param[in] buf The buffer at hand - * @param[in] buf_length Number of bytes in "buf" - * @return The calculated CRC - */ -uint16_t compute16CCITT(const uint8_t* buf, size_t buf_length); + /** + * @brief This function computes the CRC-8-CCITT (Cyclic Redundancy Check) of a + * buffer "buf" of "buf_length" bytes + * @param[in] buf The buffer at hand + * @param[in] buf_length Number of bytes in "buf" + * @return The calculated CRC + */ + uint16_t compute16CCITT(const uint8_t* buf, size_t buf_length); -/** - * @brief Validates whether the calculated CRC of the SBF block at hand matches the - * CRC field of the streamed SBF block - * @param block The SBF block that we are interested in - * @return True if the CRC check of the SBFBlock has passed, false otherwise - */ -bool isValid(const uint8_t* block); + /** + * @brief Validates whether the calculated CRC of the SBF block at hand matches + * the CRC field of the streamed SBF block + * @param block The SBF block that we are interested in + * @return True if the CRC check of the SBFBlock has passed, false otherwise + */ + bool isValid(const uint8_t* block); -#endif // CRC_H +} // namespace crc diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp index caccb06e..29e9bdf5 100644 --- a/include/septentrio_gnss_driver/node/rosaic_node.hpp +++ b/include/septentrio_gnss_driver/node/rosaic_node.hpp @@ -56,8 +56,7 @@ // // ***************************************************************************** -#ifndef ROSAIC_NODE_HPP -#define ROSAIC_NODE_HPP +#pragma once /** * @file rosaic_node.hpp @@ -133,6 +132,4 @@ namespace rosaic_node { tf2_ros::Buffer tfBuffer_; std::unique_ptr tfListener_; }; -} // namespace rosaic_node - -#endif // for ROSAIC_NODE_HPP \ No newline at end of file +} // namespace rosaic_node \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp index 086fd7d6..6d6fb3c6 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp @@ -56,8 +56,7 @@ // // ***************************************************************************** -#ifndef GPGGA_HPP -#define GPGGA_HPP +#pragma once // ROSaic includes #include @@ -116,6 +115,4 @@ class GpggaParser : public BaseParser * was valid */ bool was_last_gpgga_valid_; -}; - -#endif // GPGGA_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp index 1fd22495..6e46de8a 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp @@ -56,8 +56,7 @@ // // ***************************************************************************** -#ifndef GPGSA_HPP -#define GPGSA_HPP +#pragma once // ROSaic includes #include @@ -103,6 +102,4 @@ class GpgsaParser : public BaseParser * @brief Declares the string MESSAGE_ID */ static const std::string MESSAGE_ID; -}; - -#endif // GPGSA_HPP \ No newline at end of file +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp index 0698cd14..ca1cf77c 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp @@ -56,8 +56,7 @@ // // ***************************************************************************** -#ifndef GPGSV_HPP -#define GPGSV_HPP +#pragma once // ROSaic includes #include @@ -103,6 +102,4 @@ class GpgsvParser : public BaseParser * @brief Declares the string MESSAGE_ID */ static const std::string MESSAGE_ID; -}; - -#endif // GPGSV_HPP \ No newline at end of file +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp index 87445ccd..a54648c8 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp @@ -56,9 +56,6 @@ // // ***************************************************************************** -#ifndef GPRMC_HPP -#define GPRMC_HPP - // ROSaic includes #include #include @@ -123,6 +120,4 @@ class GprmcParser : public BaseParser * was valid */ bool was_last_gprmc_valid_; -}; - -#endif // GPRMC_HPP \ No newline at end of file +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp b/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp index f6032228..a61d489b 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_sentence.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef NMEA_SENTENCE_HPP -#define NMEA_SENTENCE_HPP +#pragma once // C++ library includes #include @@ -65,6 +64,4 @@ class NMEASentence protected: std::string id_; std::vector body_; -}; - -#endif // NMEA_SENTENCE_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/parse_exception.hpp b/include/septentrio_gnss_driver/parsers/parse_exception.hpp index 0e6e23fa..b9c8ae95 100644 --- a/include/septentrio_gnss_driver/parsers/parse_exception.hpp +++ b/include/septentrio_gnss_driver/parsers/parse_exception.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PARSE_EXCEPTION_HPP -#define PARSE_EXCEPTION_HPP +#pragma once // C++ library includes #include @@ -88,6 +87,4 @@ class ParseException : public std::runtime_error { public: explicit ParseException(const std::string& error) : std::runtime_error(error) {} -}; - -#endif // PARSE_EXCEPTION_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/parser_base_class.hpp b/include/septentrio_gnss_driver/parsers/parser_base_class.hpp index 871a7b1e..82b0c696 100644 --- a/include/septentrio_gnss_driver/parsers/parser_base_class.hpp +++ b/include/septentrio_gnss_driver/parsers/parser_base_class.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PARSER_BASE_CLASS_HPP -#define PARSER_BASE_CLASS_HPP +#pragma once // ROSaic includes #include "nmea_sentence.hpp" @@ -128,6 +127,4 @@ class BaseParser { throw ParseException("ParseASCII not implemented."); }; -}; - -#endif // PARSER_BASE_CLASS_HPP +}; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index f0d42ff3..cd405e22 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef PARSING_UTILITIES_HPP -#define PARSING_UTILITIES_HPP +#pragma once // C++ library includes #include // C++ header, corresponds to in C @@ -405,6 +404,4 @@ namespace parsing_utilities { * @return SBF GPS week counter */ uint16_t getWnc(const uint8_t* buffer); -} // namespace parsing_utilities - -#endif // PARSING_UTILITIES_HPP +} // namespace parsing_utilities \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.h b/include/septentrio_gnss_driver/parsers/string_utilities.h index 587f2238..b4e87d22 100644 --- a/include/septentrio_gnss_driver/parsers/string_utilities.h +++ b/include/septentrio_gnss_driver/parsers/string_utilities.h @@ -28,8 +28,7 @@ // // ***************************************************************************** -#ifndef STRING_UTILITIES_H -#define STRING_UTILITIES_H +#pragma once // C and C++ library includes #include @@ -145,6 +144,4 @@ namespace string_utilities { * @return true if string contains space */ bool containsSpace(const std::string str); -} // namespace string_utilities - -#endif // STRING_UTILITIES_H +} // namespace string_utilities \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index cb891d81..e4c54ff6 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -57,867 +57,949 @@ static const int8_t POSSTD_DEV_MAX = 100; * @brief Highest-Level view on communication services */ -io::CommIo::CommIo(ROSaicNodeBase* node) : - node_(node), settings_(node->getSettings()), running_(true) -{ - g_response_received = false; - g_cd_received = false; - g_read_cd = true; - g_cd_count = 0; -} - -io::CommIo::~CommIo() -{ - if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) +namespace io { + + CommIo::CommIo(ROSaicNodeBase* node) : + node_(node), settings_(node->getSettings()), running_(true) { - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); - manager_.get()->send(cmd); - send("sdio, " + mainPort_ + ", auto, none\x0D"); - for (auto ntrip : settings_->rtk_settings.ntrip) + g_response_received = false; + g_cd_received = false; + g_read_cd = true; + g_cd_count = 0; + } + + CommIo::~CommIo() + { + if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { - if (!ntrip.id.empty() && !ntrip.keep_open) + std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); + manager_.get()->send(cmd); + send("sdio, " + mainPort_ + ", auto, none\x0D"); + for (auto ntrip : settings_->rtk_settings.ntrip) { - send("snts, " + ntrip.id + ", off \x0D"); + if (!ntrip.id.empty() && !ntrip.keep_open) + { + send("snts, " + ntrip.id + ", off \x0D"); + } } - } - for (auto ip_server : settings_->rtk_settings.ip_server) - { - if (!ip_server.id.empty() && !ip_server.keep_open) + for (auto ip_server : settings_->rtk_settings.ip_server) { - send("sdio, " + ip_server.id + ", auto, none\x0D"); - send("siss, " + ip_server.id + ", 0\x0D"); + if (!ip_server.id.empty() && !ip_server.keep_open) + { + send("sdio, " + ip_server.id + ", auto, none\x0D"); + send("siss, " + ip_server.id + ", 0\x0D"); + } } - } - for (auto serial : settings_->rtk_settings.serial) - { - if (!serial.port.empty() && !serial.keep_open) - { - send("sdio, " + serial.port + ", auto, none\x0D"); - if (serial.port.rfind("COM", 0) == 0) - send("scs, " + serial.port + - ", baud115200, bits8, No, bit1, none\x0D"); + for (auto serial : settings_->rtk_settings.serial) + { + if (!serial.port.empty() && !serial.keep_open) + { + send("sdio, " + serial.port + ", auto, none\x0D"); + if (serial.port.rfind("COM", 0) == 0) + send("scs, " + serial.port + + ", baud115200, bits8, No, bit1, none\x0D"); + } } - } - if (!settings_->ins_vsm_ip_server_id.empty()) - { - if (!settings_->ins_vsm_ip_server_keep_open) - { - send("sdio, " + settings_->ins_vsm_ip_server_id + - ", auto, none\x0D"); - send("siss, " + settings_->ins_vsm_ip_server_id + ", 0\x0D"); + if (!settings_->ins_vsm_ip_server_id.empty()) + { + if (!settings_->ins_vsm_ip_server_keep_open) + { + send("sdio, " + settings_->ins_vsm_ip_server_id + + ", auto, none\x0D"); + send("siss, " + settings_->ins_vsm_ip_server_id + ", 0\x0D"); + } } - } - if (!settings_->ins_vsm_serial_port.empty()) - { - if (!settings_->ins_vsm_serial_keep_open) - { - if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) - send("scs, " + settings_->ins_vsm_serial_port + - ", baud115200, bits8, No, bit1, none\x0D"); - send("sdio, " + settings_->ins_vsm_serial_port + - ", auto, none\x0D"); + if (!settings_->ins_vsm_serial_port.empty()) + { + if (!settings_->ins_vsm_serial_keep_open) + { + if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) + send("scs, " + settings_->ins_vsm_serial_port + + ", baud115200, bits8, No, bit1, none\x0D"); + send("sdio, " + settings_->ins_vsm_serial_port + + ", auto, none\x0D"); + } } - } - - send("logout \x0D"); - } - running_ = false; - connectionThread_.join(); -} - -void io::CommIo::initializeIo() -{ - node_->log(LogLevel::DEBUG, "Called connect() method"); - node_->log( - LogLevel::DEBUG, - "Started timer for calling reconnect() method until connection succeeds"); - - boost::asio::io_service io; - boost::posix_time::millisec wait_ms( - static_cast(settings_->reconnect_delay_s * 1000)); - while (running_) - { - boost::asio::deadline_timer t(io, wait_ms); - if (manager_->connect()) - break; - t.wait(); - } - node_->log(LogLevel::DEBUG, "Successully connected. Leaving connect() method"); -} - -//! The send() method of AsyncManager class is paramount for this purpose. -//! Note that std::to_string() is from C++11 onwards only. -//! Since ROSaic can be launched before booting the Rx, we have to watch out for -//! escape characters that are sent by the Rx to indicate that it is in upgrade mode. -//! Those characters would then be mingled with the first command we send to it in -//! this method and could result in an invalid command. Hence we first enter command -//! mode via "SSSSSSSSSS". -void io::CommIo::configureRx() -{ - node_->log(LogLevel::DEBUG, "Called configureRx() method"); - { - // wait for connection - std::mutex::scoped_lock lock(connection_mutex_); - connection_condition_.wait(lock, [this]() { return connected_; }); - } + send("logout \x0D"); + } - // Determining communication mode: TCP vs USB/Serial - unsigned stream = 1; - boost::smatch match; - boost::regex_match(settings_->device, match, - boost::regex("(tcp)://(.+):(\\d+)")); - std::string proto(match[1]); - resetMainPort(); - if (proto == "tcp") - { - mainPort_ = g_rx_tcp_port; - } else - { - mainPort_ = settings_->rx_serial_port; - // After booting, the Rx sends the characters "x?" to all ports, which could - // potentially mingle with our first command. Hence send a safeguard command - // "lif", whose potentially false processing is harmless. - send("lif, Identification \x0D"); + running_ = false; + connectionThread_.join(); } - std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand( - settings_->polling_period_pvt); - - std::string rest_interval = parsing_utilities::convertUserPeriodToRxCommand( - settings_->polling_period_rest); - - // Credentials for login - if (!settings_->login_user.empty() && !settings_->login_password.empty()) + void CommIo::initializeIo() { - if (string_utilities::containsSpace(settings_->login_password)) - send("login, " + settings_->login_user + ", \"" + - settings_->login_password + "\" \x0D"); - else - send("login, " + settings_->login_user + ", " + - settings_->login_password + " \x0D"); + node_->log(LogLevel::DEBUG, "Called connect() method"); + node_->log( + LogLevel::DEBUG, + "Started timer for calling reconnect() method until connection succeeds"); + + boost::asio::io_service io; + boost::posix_time::millisec wait_ms( + static_cast(settings_->reconnect_delay_s * 1000)); + while (running_) + { + boost::asio::deadline_timer t(io, wait_ms); + initIo(); + if (manager_->connect()) + break; + t.wait(); + } + node_->log(LogLevel::DEBUG, + "Successully connected. Leaving connect() method"); } - // Turning off all current SBF/NMEA output - send("sso, all, none, none, off \x0D"); - send("sno, all, none, none, off \x0D"); - - // Activate NTP server - if (settings_->use_gnss_time) - send("sntp, on \x0D"); - - // Setting the datum to be used by the Rx (not the NMEA output though, which only - // provides MSL and undulation (by default with respect to WGS84), but not - // ellipsoidal height) + void CommIo::initIo() { - std::stringstream ss; - // WGS84 is equivalent to Default and kept for backwards compatibility - if (settings_->datum == "Default") - settings_->datum = "WGS84"; - ss << "sgd, " << settings_->datum << "\x0D"; - send(ss.str()); + node_->log(LogLevel::DEBUG, "Called initializeIo() method"); + boost::smatch match; + // In fact: smatch is a typedef of match_results + if (boost::regex_match(node_->settings_.device, match, + boost::regex("(tcp)://(.+):(\\d+)"))) + // C++ needs \\d instead of \d: Both mean decimal. + // Note that regex_match can be used with a smatch object to store + // results, or without. In any case, true is returned if and only if it + // matches the !complete! string. + { + // The first sub_match (index 0) contained in a match_result always + // represents the full match within a target sequence made by a + // regex, and subsequent sub_matches represent sub-expression matches + // corresponding in sequence to the left parenthesis delimiting the + // sub-expression in the regex, i.e. $n Perl is equivalent to m[n] in + // boost regex. + settings_->tcp_ip = match[2]; + settings_->tcp_port = match[3]; + manager_->reset(new AsyncManager(node_, &telegramQueue_)); + } else if (boost::regex_match( + node_->settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) + { + node_->settings_.read_from_sbf_log = true; + node_->settings_.use_gnss_time = true; + // connectionThread_ = boost::thread( + // boost::bind(&CommIo::prepareSBFFileReading, this, match[2])); + + } else if (boost::regex_match( + node_->settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) + { + node_->settings_.read_from_pcap = true; + node_->settings_.use_gnss_time = true; + // connectionThread_ = boost::thread( + // boost::bind(&CommIo::preparePCAPFileReading, this, match[2])); + + } else if (boost::regex_match(node_->settings_.device, match, + boost::regex("(serial):(.+)"))) + { + std::string proto(match[2]); + std::stringstream ss; + ss << "Searching for serial port" << proto; + node_->log(LogLevel::DEBUG, ss.str()); + node_->settings_.device = proto; + manager_->reset(new AsyncManager(node_, &telegramQueue_)); + } else + { + std::stringstream ss; + ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; + node_->log(LogLevel::ERROR, ss.str()); + return false; + } + node_->log(LogLevel::DEBUG, "Leaving initializeIo() method"); + return true; } - // Setting up SBF blocks with rx_period_pvt + //! The send() method of AsyncManager class is paramount for this purpose. + //! Note that std::to_string() is from C++11 onwards only. + //! Since ROSaic can be launched before booting the Rx, we have to watch out for + //! escape characters that are sent by the Rx to indicate that it is in upgrade + //! mode. Those characters would then be mingled with the first command we send + //! to it in this method and could result in an invalid command. Hence we first + //! enter command mode via "SSSSSSSSSS". + void CommIo::configureRx() { - std::stringstream blocks; - if (settings_->use_gnss_time) - { - blocks << " +ReceiverTime"; - } - if (settings_->publish_pvtcartesian) - { - blocks << " +PVTCartesian"; - } - if (settings_->publish_pvtgeodetic || settings_->publish_twist || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +PVTGeodetic"; - } - if (settings_->publish_basevectorcart) - { - blocks << " +BaseVectorCart"; - } - if (settings_->publish_basevectorgeod) - { - blocks << " +BaseVectorGeod"; - } - if (settings_->publish_poscovcartesian) - { - blocks << " +PosCovCartesian"; - } - if (settings_->publish_poscovgeodetic || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +PosCovGeodetic"; - } - if (settings_->publish_velcovgeodetic || settings_->publish_twist || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss"))) + node_->log(LogLevel::DEBUG, "Called configureRx() method"); { - blocks << " +VelCovGeodetic"; + // wait for connection + std::mutex::scoped_lock lock(connection_mutex_); + connection_condition_.wait(lock, [this]() { return connected_; }); } - if (settings_->publish_atteuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) + + // Determining communication mode: TCP vs USB/Serial + unsigned stream = 1; + boost::smatch match; + boost::regex_match(settings_->device, match, + boost::regex("(tcp)://(.+):(\\d+)")); + std::string proto(match[1]); + resetMainPort(); + if (proto == "tcp") { - blocks << " +AttEuler"; - } - if (settings_->publish_attcoveuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) + mainPort_ = g_rx_tcp_port; + } else { - blocks << " +AttCovEuler"; + mainPort_ = settings_->rx_serial_port; + // After booting, the Rx sends the characters "x?" to all ports, which + // could potentially mingle with our first command. Hence send a + // safeguard command "lif", whose potentially false processing is + // harmless. + send("lif, Identification \x0D"); } - if (settings_->publish_measepoch || settings_->publish_gpsfix) + + std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand( + settings_->polling_period_pvt); + + std::string rest_interval = parsing_utilities::convertUserPeriodToRxCommand( + settings_->polling_period_rest); + + // Credentials for login + if (!settings_->login_user.empty() && !settings_->login_password.empty()) { - blocks << " +MeasEpoch"; + if (string_utilities::containsSpace(settings_->login_password)) + send("login, " + settings_->login_user + ", \"" + + settings_->login_password + "\" \x0D"); + else + send("login, " + settings_->login_user + ", " + + settings_->login_password + " \x0D"); } - if (settings_->publish_gpsfix) + + // Turning off all current SBF/NMEA output + send("sso, all, none, none, off \x0D"); + send("sno, all, none, none, off \x0D"); + + // Activate NTP server + if (settings_->use_gnss_time) + send("sntp, on \x0D"); + + // Setting the datum to be used by the Rx (not the NMEA output though, which + // only provides MSL and undulation (by default with respect to WGS84), but + // not ellipsoidal height) { - blocks << " +ChannelStatus +DOP"; + std::stringstream ss; + // WGS84 is equivalent to Default and kept for backwards compatibility + if (settings_->datum == "Default") + settings_->datum = "WGS84"; + ss << "sgd, " << settings_->datum << "\x0D"; + send(ss.str()); } - // Setting SBF output of Rx depending on the receiver type - // If INS then... - if (settings_->septentrio_receiver_type == "ins") + + // Setting up SBF blocks with rx_period_pvt { - if (settings_->publish_insnavcart || - settings_->publish_localization_ecef || settings_->publish_tf_ecef) + std::stringstream blocks; + if (settings_->use_gnss_time) { - blocks << " +INSNavCart"; + blocks << " +ReceiverTime"; } - if (settings_->publish_insnavgeod || settings_->publish_navsatfix || - settings_->publish_gpsfix || settings_->publish_pose || - settings_->publish_imu || settings_->publish_localization || - settings_->publish_tf || settings_->publish_twist || - settings_->publish_localization_ecef || settings_->publish_tf_ecef) + if (settings_->publish_pvtcartesian) { - blocks << " +INSNavGeod"; + blocks << " +PVTCartesian"; + } + if (settings_->publish_pvtgeodetic || settings_->publish_twist || + (settings_->publish_navsatfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +PVTGeodetic"; } - if (settings_->publish_exteventinsnavgeod) + if (settings_->publish_basevectorcart) { - blocks << " +ExtEventINSNavGeod"; + blocks << " +BaseVectorCart"; } - if (settings_->publish_exteventinsnavcart) + if (settings_->publish_basevectorgeod) { - blocks << " +ExtEventINSNavCart"; + blocks << " +BaseVectorGeod"; } - if (settings_->publish_extsensormeas || settings_->publish_imu) + if (settings_->publish_poscovcartesian) { - blocks << " +ExtSensorMeas"; + blocks << " +PosCovCartesian"; } - } - std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; - send(ss.str()); - ++stream; - } - // Setting up SBF blocks with rx_period_rest - { - std::stringstream blocks; - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_imusetup) + if (settings_->publish_poscovgeodetic || + (settings_->publish_navsatfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +PosCovGeodetic"; + } + if (settings_->publish_velcovgeodetic || settings_->publish_twist || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss"))) { - blocks << " +IMUSetup"; + blocks << " +VelCovGeodetic"; } - if (settings_->publish_velsensorsetup) + if (settings_->publish_atteuler || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) { - blocks << " +VelSensorSetup"; + blocks << " +AttEuler"; } + if (settings_->publish_attcoveuler || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +AttCovEuler"; + } + if (settings_->publish_measepoch || settings_->publish_gpsfix) + { + blocks << " +MeasEpoch"; + } + if (settings_->publish_gpsfix) + { + blocks << " +ChannelStatus +DOP"; + } + // Setting SBF output of Rx depending on the receiver type + // If INS then... + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->publish_insnavcart || + settings_->publish_localization_ecef || + settings_->publish_tf_ecef) + { + blocks << " +INSNavCart"; + } + if (settings_->publish_insnavgeod || settings_->publish_navsatfix || + settings_->publish_gpsfix || settings_->publish_pose || + settings_->publish_imu || settings_->publish_localization || + settings_->publish_tf || settings_->publish_twist || + settings_->publish_localization_ecef || + settings_->publish_tf_ecef) + { + blocks << " +INSNavGeod"; + } + if (settings_->publish_exteventinsnavgeod) + { + blocks << " +ExtEventINSNavGeod"; + } + if (settings_->publish_exteventinsnavcart) + { + blocks << " +ExtEventINSNavCart"; + } + if (settings_->publish_extsensormeas || settings_->publish_imu) + { + blocks << " +ExtSensorMeas"; + } + } + std::stringstream ss; + ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," + << blocks.str() << ", " << pvt_interval << "\x0D"; + send(ss.str()); + ++stream; } - if (settings_->publish_diagnostics) - { - blocks << " +ReceiverStatus +QualityInd"; - } - - blocks << " +ReceiverSetup"; - - std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << rest_interval << "\x0D"; - send(ss.str()); - ++stream; - } - - // Setting up NMEA streams - { - send("snti, GP\x0D"); - - std::stringstream blocks; - if (settings_->publish_gpgga) - { - blocks << " +GGA"; - } - if (settings_->publish_gprmc) - { - blocks << " +RMC"; - } - if (settings_->publish_gpgsa) - { - blocks << " +GSA"; - } - if (settings_->publish_gpgsv) + // Setting up SBF blocks with rx_period_rest { - blocks << " +GSV"; - } + std::stringstream blocks; + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->publish_imusetup) + { + blocks << " +IMUSetup"; + } + if (settings_->publish_velsensorsetup) + { + blocks << " +VelSensorSetup"; + } + } + if (settings_->publish_diagnostics) + { + blocks << " +ReceiverStatus +QualityInd"; + } - std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; - send(ss.str()); - ++stream; - } + blocks << " +ReceiverSetup"; - if ((settings_->septentrio_receiver_type == "ins") || - settings_->ins_in_gnss_mode) - { - { std::stringstream ss; - ss << "sat, Main, \"" << settings_->ant_type << "\"" - << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," + << blocks.str() << ", " << rest_interval << "\x0D"; send(ss.str()); + ++stream; } - // Configure Aux1 antenna - { - std::stringstream ss; - ss << "sat, Aux1, \"" << settings_->ant_type << "\"" - << "\x0D"; - send(ss.str()); - } - } else if (settings_->septentrio_receiver_type == "gnss") - { - // Setting the marker-to-ARP offsets. This comes after the "sso, ..., - // ReceiverSetup, ..." command, since the latter is only generated when a - // user-command is entered to change one or more values in the block. + // Setting up NMEA streams { - std::stringstream ss; - ss << "sao, Main, " - << string_utilities::trimDecimalPlaces(settings_->delta_e) << ", " - << string_utilities::trimDecimalPlaces(settings_->delta_n) << ", " - << string_utilities::trimDecimalPlaces(settings_->delta_u) << ", \"" - << settings_->ant_type << "\", " << settings_->ant_serial_nr - << "\x0D"; - send(ss.str()); - } + send("snti, GP\x0D"); + + std::stringstream blocks; + if (settings_->publish_gpgga) + { + blocks << " +GGA"; + } + if (settings_->publish_gprmc) + { + blocks << " +RMC"; + } + if (settings_->publish_gpgsa) + { + blocks << " +GSA"; + } + if (settings_->publish_gpgsv) + { + blocks << " +GSV"; + } - // Configure Aux1 antenna - { std::stringstream ss; - ss << "sao, Aux1, " << string_utilities::trimDecimalPlaces(0.0) << ", " - << string_utilities::trimDecimalPlaces(0.0) << ", " - << string_utilities::trimDecimalPlaces(0.0) << ", \"" - << settings_->ant_aux1_type << "\", " << settings_->ant_aux1_serial_nr - << "\x0D"; + ss << "sno, Stream" << std::to_string(stream) << ", " << mainPort_ << "," + << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); + ++stream; } - } - // Configuring the corrections connection - for (auto ntrip : settings_->rtk_settings.ntrip) - { - if (!ntrip.id.empty()) + if ((settings_->septentrio_receiver_type == "ins") || + settings_->ins_in_gnss_mode) { - // First disable any existing NTRIP connection on NTR1 - send("snts, " + ntrip.id + ", off \x0D"); { std::stringstream ss; - ss << "snts, " << ntrip.id << ", Client, " << ntrip.caster << ", " - << std::to_string(ntrip.caster_port) << ", " << ntrip.username - << ", " << ntrip.password << ", " << ntrip.mountpoint << ", " - << ntrip.version << ", " << ntrip.send_gga << " \x0D"; + ss << "sat, Main, \"" << settings_->ant_type << "\"" + << "\x0D"; send(ss.str()); } - if (ntrip.tls) - { - std::stringstream ss; - ss << "sntt, " << ntrip.id << ", on, \"" << ntrip.fingerprint - << "\" \x0D"; - send(ss.str()); - } else + + // Configure Aux1 antenna { std::stringstream ss; - ss << "sntt, " << ntrip.id << ", off \x0D"; + ss << "sat, Aux1, \"" << settings_->ant_type << "\"" + << "\x0D"; send(ss.str()); } - } - } - - for (auto ip_server : settings_->rtk_settings.ip_server) - { - if (!ip_server.id.empty()) - // Since the Rx does not have internet (and you will not - // be able to share it via USB), we need to forward the - // corrections ourselves, though not on the same port. + } else if (settings_->septentrio_receiver_type == "gnss") { + // Setting the marker-to-ARP offsets. This comes after the "sso, ..., + // ReceiverSetup, ..." command, since the latter is only generated when a + // user-command is entered to change one or more values in the block. { std::stringstream ss; - // In case this IP server was used before, old configuration is lost - // of course. - ss << "siss, " << ip_server.id << ", " - << std::to_string(ip_server.port) << ", TCP2Way \x0D"; + ss << "sao, Main, " + << string_utilities::trimDecimalPlaces(settings_->delta_e) << ", " + << string_utilities::trimDecimalPlaces(settings_->delta_n) << ", " + << string_utilities::trimDecimalPlaces(settings_->delta_u) + << ", \"" << settings_->ant_type << "\", " + << settings_->ant_serial_nr << "\x0D"; send(ss.str()); } + + // Configure Aux1 antenna { std::stringstream ss; - ss << "sdio, " << ip_server.id << ", " << ip_server.rtk_standard - << ", +SBF+NMEA \x0D"; + ss << "sao, Aux1, " << string_utilities::trimDecimalPlaces(0.0) + << ", " << string_utilities::trimDecimalPlaces(0.0) << ", " + << string_utilities::trimDecimalPlaces(0.0) << ", \"" + << settings_->ant_aux1_type << "\", " + << settings_->ant_aux1_serial_nr << "\x0D"; send(ss.str()); } - if (ip_server.send_gga != "off") + } + + // Configuring the corrections connection + for (auto ntrip : settings_->rtk_settings.ntrip) + { + if (!ntrip.id.empty()) { - std::string rate = ip_server.send_gga; - if (ip_server.send_gga == "auto") - rate = "sec1"; - std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << ip_server.id - << ", GGA, " << rate << " \x0D"; - ++stream; - send(ss.str()); + // First disable any existing NTRIP connection on NTR1 + send("snts, " + ntrip.id + ", off \x0D"); + { + std::stringstream ss; + ss << "snts, " << ntrip.id << ", Client, " << ntrip.caster + << ", " << std::to_string(ntrip.caster_port) << ", " + << ntrip.username << ", " << ntrip.password << ", " + << ntrip.mountpoint << ", " << ntrip.version << ", " + << ntrip.send_gga << " \x0D"; + send(ss.str()); + } + if (ntrip.tls) + { + std::stringstream ss; + ss << "sntt, " << ntrip.id << ", on, \"" << ntrip.fingerprint + << "\" \x0D"; + send(ss.str()); + } else + { + std::stringstream ss; + ss << "sntt, " << ntrip.id << ", off \x0D"; + send(ss.str()); + } } } - } - for (auto serial : settings_->rtk_settings.serial) - { - if (!serial.port.empty()) + for (auto ip_server : settings_->rtk_settings.ip_server) { - if (serial.port.rfind("COM", 0) == 0) - send("scs, " + serial.port + ", baud" + - std::to_string(serial.baud_rate) + - ", bits8, No, bit1, none\x0D"); + if (!ip_server.id.empty()) + // Since the Rx does not have internet (and you will not + // be able to share it via USB), we need to forward the + // corrections ourselves, though not on the same port. + { + { + std::stringstream ss; + // In case this IP server was used before, old configuration is + // lost of course. + ss << "siss, " << ip_server.id << ", " + << std::to_string(ip_server.port) << ", TCP2Way \x0D"; + send(ss.str()); + } + { + std::stringstream ss; + ss << "sdio, " << ip_server.id << ", " << ip_server.rtk_standard + << ", +SBF+NMEA \x0D"; + send(ss.str()); + } + if (ip_server.send_gga != "off") + { + std::string rate = ip_server.send_gga; + if (ip_server.send_gga == "auto") + rate = "sec1"; + std::stringstream ss; + ss << "sno, Stream" << std::to_string(stream) << ", " + << ip_server.id << ", GGA, " << rate << " \x0D"; + ++stream; + send(ss.str()); + } + } + } - std::stringstream ss; - ss << "sdio, " << serial.port << ", " << serial.rtk_standard - << ", +SBF+NMEA \x0D"; - send(ss.str()); - if (serial.send_gga != "off") + for (auto serial : settings_->rtk_settings.serial) + { + if (!serial.port.empty()) { - std::string rate = serial.send_gga; - if (serial.send_gga == "auto") - rate = "sec1"; + if (serial.port.rfind("COM", 0) == 0) + send("scs, " + serial.port + ", baud" + + std::to_string(serial.baud_rate) + + ", bits8, No, bit1, none\x0D"); + std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << serial.port - << ", GGA, " << rate << " \x0D"; - ++stream; + ss << "sdio, " << serial.port << ", " << serial.rtk_standard + << ", +SBF+NMEA \x0D"; send(ss.str()); + if (serial.send_gga != "off") + { + std::string rate = serial.send_gga; + if (serial.send_gga == "auto") + rate = "sec1"; + std::stringstream ss; + ss << "sno, Stream" << std::to_string(stream) << ", " + << serial.port << ", GGA, " << rate << " \x0D"; + ++stream; + send(ss.str()); + } } } - } - - // Setting multi antenna - if (settings_->multi_antenna) - { - send("sga, MultiAntenna \x0D"); - } else - { - send("sga, none \x0D"); - } - // Setting the Attitude Determination - { - if (settings_->heading_offset >= HEADING_MIN && - settings_->heading_offset <= HEADING_MAX && - settings_->pitch_offset >= PITCH_MIN && - settings_->pitch_offset <= PITCH_MAX) + // Setting multi antenna + if (settings_->multi_antenna) { - std::stringstream ss; - ss << "sto, " - << string_utilities::trimDecimalPlaces(settings_->heading_offset) - << ", " - << string_utilities::trimDecimalPlaces(settings_->pitch_offset) - << " \x0D"; - send(ss.str()); + send("sga, MultiAntenna \x0D"); } else { - node_->log(LogLevel::ERROR, - "Please specify a valid parameter for heading and pitch"); - } - } - - // Setting the INS-related commands - if (settings_->septentrio_receiver_type == "ins") - { - // IMU orientation - { - std::stringstream ss; - if (settings_->theta_x >= ANGLE_MIN && settings_->theta_x <= ANGLE_MAX && - settings_->theta_y >= THETA_Y_MIN && - settings_->theta_y <= THETA_Y_MAX && - settings_->theta_z >= ANGLE_MIN && settings_->theta_z <= ANGLE_MAX) - { - ss << " sio, " - << "manual" - << ", " << string_utilities::trimDecimalPlaces(settings_->theta_x) - << ", " << string_utilities::trimDecimalPlaces(settings_->theta_y) - << ", " << string_utilities::trimDecimalPlaces(settings_->theta_z) - << " \x0D"; - send(ss.str()); - } else - { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for IMU orientation angles"); - } + send("sga, none \x0D"); } - // Setting the INS antenna lever arm offset + // Setting the Attitude Determination { - if (settings_->ant_lever_x >= LEVER_ARM_MIN && - settings_->ant_lever_x <= LEVER_ARM_MAX && - settings_->ant_lever_y >= LEVER_ARM_MIN && - settings_->ant_lever_y <= LEVER_ARM_MAX && - settings_->ant_lever_z >= LEVER_ARM_MIN && - settings_->ant_lever_z <= LEVER_ARM_MAX) + if (settings_->heading_offset >= HEADING_MIN && + settings_->heading_offset <= HEADING_MAX && + settings_->pitch_offset >= PITCH_MIN && + settings_->pitch_offset <= PITCH_MAX) { std::stringstream ss; - ss << "sial, " - << string_utilities::trimDecimalPlaces(settings_->ant_lever_x) - << ", " - << string_utilities::trimDecimalPlaces(settings_->ant_lever_y) + ss << "sto, " + << string_utilities::trimDecimalPlaces(settings_->heading_offset) << ", " - << string_utilities::trimDecimalPlaces(settings_->ant_lever_z) + << string_utilities::trimDecimalPlaces(settings_->pitch_offset) << " \x0D"; send(ss.str()); } else { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for x, y and z in the config file under ant_lever_arm"); + node_->log(LogLevel::ERROR, + "Please specify a valid parameter for heading and pitch"); } } - // Setting the user defined point offset + // Setting the INS-related commands + if (settings_->septentrio_receiver_type == "ins") { - if (settings_->poi_x >= LEVER_ARM_MIN && - settings_->poi_x <= LEVER_ARM_MAX && - settings_->poi_y >= LEVER_ARM_MIN && - settings_->poi_y <= LEVER_ARM_MAX && - settings_->poi_z >= LEVER_ARM_MIN && - settings_->poi_z <= LEVER_ARM_MAX) + // IMU orientation { std::stringstream ss; - ss << "sipl, POI1, " - << string_utilities::trimDecimalPlaces(settings_->poi_x) << ", " - << string_utilities::trimDecimalPlaces(settings_->poi_y) << ", " - << string_utilities::trimDecimalPlaces(settings_->poi_z) - << " \x0D"; - send(ss.str()); - } else - { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm"); + if (settings_->theta_x >= ANGLE_MIN && + settings_->theta_x <= ANGLE_MAX && + settings_->theta_y >= THETA_Y_MIN && + settings_->theta_y <= THETA_Y_MAX && + settings_->theta_z >= ANGLE_MIN && + settings_->theta_z <= ANGLE_MAX) + { + ss << " sio, " + << "manual" + << ", " + << string_utilities::trimDecimalPlaces(settings_->theta_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->theta_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->theta_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + LogLevel::ERROR, + "Please specify a correct value for IMU orientation angles"); + } } - } - // Setting the Velocity sensor lever arm offset - { - if (settings_->vsm_x >= LEVER_ARM_MIN && - settings_->vsm_x <= LEVER_ARM_MAX && - settings_->vsm_y >= LEVER_ARM_MIN && - settings_->vsm_y <= LEVER_ARM_MAX && - settings_->vsm_z >= LEVER_ARM_MIN && - settings_->vsm_z <= LEVER_ARM_MAX) - { - std::stringstream ss; - ss << "sivl, VSM1, " - << string_utilities::trimDecimalPlaces(settings_->vsm_x) << ", " - << string_utilities::trimDecimalPlaces(settings_->vsm_y) << ", " - << string_utilities::trimDecimalPlaces(settings_->vsm_z) - << " \x0D"; - send(ss.str()); - } else - { - node_->log( - LogLevel::ERROR, - "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vsm_lever_arm"); + // Setting the INS antenna lever arm offset + { + if (settings_->ant_lever_x >= LEVER_ARM_MIN && + settings_->ant_lever_x <= LEVER_ARM_MAX && + settings_->ant_lever_y >= LEVER_ARM_MIN && + settings_->ant_lever_y <= LEVER_ARM_MAX && + settings_->ant_lever_z >= LEVER_ARM_MIN && + settings_->ant_lever_z <= LEVER_ARM_MAX) + { + std::stringstream ss; + ss << "sial, " + << string_utilities::trimDecimalPlaces(settings_->ant_lever_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->ant_lever_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->ant_lever_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + LogLevel::ERROR, + "Please specify a correct value for x, y and z in the config file under ant_lever_arm"); + } } - } - // Setting the INS Solution Reference Point: MainAnt or POI1 - // First disable any existing INS sub-block connection - { - std::stringstream ss; - ss << "sinc, off, all, MainAnt \x0D"; - send(ss.str()); - } + // Setting the user defined point offset + { + if (settings_->poi_x >= LEVER_ARM_MIN && + settings_->poi_x <= LEVER_ARM_MAX && + settings_->poi_y >= LEVER_ARM_MIN && + settings_->poi_y <= LEVER_ARM_MAX && + settings_->poi_z >= LEVER_ARM_MIN && + settings_->poi_z <= LEVER_ARM_MAX) + { + std::stringstream ss; + ss << "sipl, POI1, " + << string_utilities::trimDecimalPlaces(settings_->poi_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->poi_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->poi_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + LogLevel::ERROR, + "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm"); + } + } - // INS solution reference point - { - std::stringstream ss; - if (settings_->ins_use_poi) - { - ss << "sinc, on, all, " - << "POI1" - << " \x0D"; - send(ss.str()); - } else - { - ss << "sinc, on, all, " - << "MainAnt" - << " \x0D"; - send(ss.str()); + // Setting the Velocity sensor lever arm offset + { + if (settings_->vsm_x >= LEVER_ARM_MIN && + settings_->vsm_x <= LEVER_ARM_MAX && + settings_->vsm_y >= LEVER_ARM_MIN && + settings_->vsm_y <= LEVER_ARM_MAX && + settings_->vsm_z >= LEVER_ARM_MIN && + settings_->vsm_z <= LEVER_ARM_MAX) + { + std::stringstream ss; + ss << "sivl, VSM1, " + << string_utilities::trimDecimalPlaces(settings_->vsm_x) + << ", " + << string_utilities::trimDecimalPlaces(settings_->vsm_y) + << ", " + << string_utilities::trimDecimalPlaces(settings_->vsm_z) + << " \x0D"; + send(ss.str()); + } else + { + node_->log( + LogLevel::ERROR, + "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vsm_lever_arm"); + } } - } - // Setting the INS heading - { - std::stringstream ss; - if (settings_->ins_initial_heading == "auto") + // Setting the INS Solution Reference Point: MainAnt or POI1 + // First disable any existing INS sub-block connection { - ss << "siih, " << settings_->ins_initial_heading << " \x0D"; + std::stringstream ss; + ss << "sinc, off, all, MainAnt \x0D"; send(ss.str()); - } else if (settings_->ins_initial_heading == "stored") + } + + // INS solution reference point { - ss << "siih, " << settings_->ins_initial_heading << " \x0D"; - send(ss.str()); - } else + std::stringstream ss; + if (settings_->ins_use_poi) + { + ss << "sinc, on, all, " + << "POI1" + << " \x0D"; + send(ss.str()); + } else + { + ss << "sinc, on, all, " + << "MainAnt" + << " \x0D"; + send(ss.str()); + } + } + + // Setting the INS heading { - node_->log(LogLevel::ERROR, - "Invalid mode specified for ins_initial_heading."); + std::stringstream ss; + if (settings_->ins_initial_heading == "auto") + { + ss << "siih, " << settings_->ins_initial_heading << " \x0D"; + send(ss.str()); + } else if (settings_->ins_initial_heading == "stored") + { + ss << "siih, " << settings_->ins_initial_heading << " \x0D"; + send(ss.str()); + } else + { + node_->log(LogLevel::ERROR, + "Invalid mode specified for ins_initial_heading."); + } + } + + // Setting the INS navigation filter + { + if (settings_->att_std_dev >= ATTSTD_DEV_MIN && + settings_->att_std_dev <= ATTSTD_DEV_MAX && + settings_->pos_std_dev >= POSSTD_DEV_MIN && + settings_->pos_std_dev <= POSSTD_DEV_MAX) + { + std::stringstream ss; + ss << "sism, " + << string_utilities::trimDecimalPlaces(settings_->att_std_dev) + << ", " + << string_utilities::trimDecimalPlaces(settings_->pos_std_dev) + << " \x0D"; + send(ss.str()); + } else + { + node_->log(LogLevel::ERROR, + "Please specify a valid AttStsDev and PosStdDev"); + } } } - // Setting the INS navigation filter + if (settings_->septentrio_receiver_type == "ins") { - if (settings_->att_std_dev >= ATTSTD_DEV_MIN && - settings_->att_std_dev <= ATTSTD_DEV_MAX && - settings_->pos_std_dev >= POSSTD_DEV_MIN && - settings_->pos_std_dev <= POSSTD_DEV_MAX) + if (!settings_->ins_vsm_ip_server_id.empty()) { - std::stringstream ss; - ss << "sism, " - << string_utilities::trimDecimalPlaces(settings_->att_std_dev) - << ", " - << string_utilities::trimDecimalPlaces(settings_->pos_std_dev) - << " \x0D"; - send(ss.str()); - } else + send("siss, " + settings_->ins_vsm_ip_server_id + ", " + + std::to_string(settings_->ins_vsm_ip_server_port) + + ", TCP2Way \x0D"); + send("sdio, IPS2, NMEA, none\x0D"); + } + if (!settings_->ins_vsm_serial_port.empty()) { - node_->log(LogLevel::ERROR, - "Please specify a valid AttStsDev and PosStdDev"); + if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) + send("scs, " + settings_->ins_vsm_serial_port + ", baud" + + std::to_string(settings_->ins_vsm_serial_baud_rate) + + ", bits8, No, bit1, none\x0D"); + send("sdio, " + settings_->ins_vsm_serial_port + ", NMEA\x0D"); + } + if ((settings_->ins_vsm_ros_source == "odometry") || + (settings_->ins_vsm_ros_source == "twist")) + { + std::string s; + s = "sdio, " + mainPort_ + ", NMEA, +NMEA +SBF\x0D"; + send(s); + nmeaActivated_ = true; } } + node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); } - if (settings_->septentrio_receiver_type == "ins") + void CommIo::sendVelocity(const std::string& velNmea) { - if (!settings_->ins_vsm_ip_server_id.empty()) - { - send("siss, " + settings_->ins_vsm_ip_server_id + ", " + - std::to_string(settings_->ins_vsm_ip_server_port) + - ", TCP2Way \x0D"); - send("sdio, IPS2, NMEA, none\x0D"); - } - if (!settings_->ins_vsm_serial_port.empty()) - { - if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0) - send("scs, " + settings_->ins_vsm_serial_port + ", baud" + - std::to_string(settings_->ins_vsm_serial_baud_rate) + - ", bits8, No, bit1, none\x0D"); - send("sdio, " + settings_->ins_vsm_serial_port + ", NMEA\x0D"); - } - if ((settings_->ins_vsm_ros_source == "odometry") || - (settings_->ins_vsm_ros_source == "twist")) - { - std::string s; - s = "sdio, " + mainPort_ + ", NMEA, +NMEA +SBF\x0D"; - send(s); - nmeaActivated_ = true; - } + if (nmeaActivated_) + manager_.get()->send(velNmea); } - node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); -} - -void io::CommIo::sendVelocity(const std::string& velNmea) -{ - if (nmeaActivated_) - manager_.get()->send(velNmea); -} - -void io::CommIo::resetMainPort() -{ - CommSync* cdSync = messageHandler.getCdSync(); - // It is imperative to hold a lock on the mutex "g_cd_mutex" while - // modifying the variable and "g_cd_received". - std::mutex::scoped_lock lock_cd(cdSync.mutex); - // Escape sequence (escape from correction mode), ensuring that we can send - // our real commands afterwards... - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); - manager_.get()->send(cmd); - // We wait for the connection descriptor before we send another command, - // otherwise the latter would not be processed. - cdSync.condition.wait(lock_cd, [cdSync]() { return cdSync.received; }); - cdSync.received = false; -} - -void io::CommIo::prepareSBFFileReading(std::string file_name) -{ - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from" << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializeSBFFileReading(file_name); - } catch (std::runtime_error& e) + + void CommIo::resetMainPort() { - std::stringstream ss; - ss << "CommIo::initializeSBFFileReading() failed for SBF File" << file_name - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); + CommSync* cdSync = messageHandler.getCdSync(); + // It is imperative to hold a lock on the mutex "g_cd_mutex" while + // modifying the variable and "g_cd_received". + std::mutex::scoped_lock lock_cd(cdSync.mutex); + // Escape sequence (escape from correction mode), ensuring that we can send + // our real commands afterwards... + std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); + manager_.get()->send(cmd); + // We wait for the connection descriptor before we send another command, + // otherwise the latter would not be processed. + cdSync.condition.wait(lock_cd, [cdSync]() { return cdSync.received; }); + cdSync.received = false; } -} -void io::CommIo::preparePCAPFileReading(std::string file_name) -{ - try + void CommIo::prepareSBFFileReading(std::string file_name) { - std::stringstream ss; - ss << "Setting up everything needed to read from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializePCAPFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "CommIO::initializePCAPFileReading() failed for SBF File " << file_name - << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); + try + { + std::stringstream ss; + ss << "Setting up everything needed to read from" << file_name; + node_->log(LogLevel::DEBUG, ss.str()); + initializeSBFFileReading(file_name); + } catch (std::runtime_error& e) + { + std::stringstream ss; + ss << "CommIo::initializeSBFFileReading() failed for SBF File" + << file_name << " due to: " << e.what(); + node_->log(LogLevel::ERROR, ss.str()); + } } -} - -void io::CommIo::initializeSBFFileReading(std::string file_name) -{ - node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); - std::size_t buffer_size = 8192; - uint8_t* to_be_parsed; - to_be_parsed = new uint8_t[buffer_size]; - std::ifstream bin_file(file_name, std::ios::binary); - std::vector vec_buf; - if (bin_file.good()) - { - /* Reads binary data using streambuffer iterators. - Copies all SBF file content into bin_data. */ - std::vector v_buf((std::istreambuf_iterator(bin_file)), - (std::istreambuf_iterator())); - vec_buf = v_buf; - bin_file.close(); - } else + + void CommIo::preparePCAPFileReading(std::string file_name) { - throw std::runtime_error("I could not find your file. Or it is corrupted."); + try + { + std::stringstream ss; + ss << "Setting up everything needed to read from " << file_name; + node_->log(LogLevel::DEBUG, ss.str()); + initializePCAPFileReading(file_name); + } catch (std::runtime_error& e) + { + std::stringstream ss; + ss << "CommIO::initializePCAPFileReading() failed for SBF File " + << file_name << " due to: " << e.what(); + node_->log(LogLevel::ERROR, ss.str()); + } } - // The spec now guarantees that vectors store their elements contiguously. - to_be_parsed = vec_buf.data(); - std::stringstream ss; - ss << "Opened and copied over from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - while (running_) // Loop will stop if we are done reading the SBF file + void CommIo::initializeSBFFileReading(std::string file_name) { - try + node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); + std::size_t buffer_size = 8192; + uint8_t* to_be_parsed; + to_be_parsed = new uint8_t[buffer_size]; + std::ifstream bin_file(file_name, std::ios::binary); + std::vector vec_buf; + if (bin_file.good()) + { + /* Reads binary data using streambuffer iterators. + Copies all SBF file content into bin_data. */ + std::vector v_buf((std::istreambuf_iterator(bin_file)), + (std::istreambuf_iterator())); + vec_buf = v_buf; + bin_file.close(); + } else { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - buffer_size); - handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); - } catch (std::size_t& parsing_failed_here) + throw std::runtime_error( + "I could not find your file. Or it is corrupted."); + } + // The spec now guarantees that vectors store their elements contiguously. + to_be_parsed = vec_buf.data(); + std::stringstream ss; + ss << "Opened and copied over from " << file_name; + node_->log(LogLevel::DEBUG, ss.str()); + + while (running_) // Loop will stop if we are done reading the SBF file { + try + { + node_->log( + LogLevel::DEBUG, + "Calling read_callback_() method, with number of bytes to be parsed being " + + buffer_size); + handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); + } catch (std::size_t& parsing_failed_here) + { + if (to_be_parsed - vec_buf.data() >= + vec_buf.size() * sizeof(uint8_t)) + { + break; + } + to_be_parsed = to_be_parsed + parsing_failed_here; + node_->log(LogLevel::DEBUG, + "Parsing_failed_here is " + parsing_failed_here); + continue; + } if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) { break; } - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; - } - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; + to_be_parsed = to_be_parsed + buffer_size; } - to_be_parsed = to_be_parsed + buffer_size; + node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method.."); } - node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method.."); -} - -void io::CommIo::initializePCAPFileReading(std::string file_name) -{ - node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() method.."); - pcapReader::buffer_t vec_buf; - pcapReader::PcapDevice device(node_, vec_buf); - if (!device.connect(file_name.c_str())) + void CommIo::initializePCAPFileReading(std::string file_name) { - node_->log(LogLevel::ERROR, "Unable to find file or either it is corrupted"); - return; - } + node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() method.."); + pcapReader::buffer_t vec_buf; + pcapReader::PcapDevice device(node_, vec_buf); - node_->log(LogLevel::INFO, "Reading ..."); - while (device.isConnected() && device.read() == pcapReader::READ_SUCCESS) - ; - device.disconnect(); + if (!device.connect(file_name.c_str())) + { + node_->log(LogLevel::ERROR, + "Unable to find file or either it is corrupted"); + return; + } - std::size_t buffer_size = pcapReader::PcapDevice::BUFFSIZE; - uint8_t* to_be_parsed = new uint8_t[buffer_size]; - to_be_parsed = vec_buf.data(); + node_->log(LogLevel::INFO, "Reading ..."); + while (device.isConnected() && device.read() == pcapReader::READ_SUCCESS) + ; + device.disconnect(); - while (running_) // Loop will stop if we are done reading the SBF file - { - try - { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - buffer_size); - handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); - } catch (std::size_t& parsing_failed_here) + std::size_t buffer_size = pcapReader::PcapDevice::BUFFSIZE; + uint8_t* to_be_parsed = new uint8_t[buffer_size]; + to_be_parsed = vec_buf.data(); + + while (running_) // Loop will stop if we are done reading the SBF file { + try + { + node_->log( + LogLevel::DEBUG, + "Calling read_callback_() method, with number of bytes to be parsed being " + + buffer_size); + handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); + } catch (std::size_t& parsing_failed_here) + { + if (to_be_parsed - vec_buf.data() >= + vec_buf.size() * sizeof(uint8_t)) + { + break; + } + if (!parsing_failed_here) + parsing_failed_here = 1; + + to_be_parsed = to_be_parsed + parsing_failed_here; + node_->log(LogLevel::DEBUG, + "Parsing_failed_here is " + parsing_failed_here); + continue; + } if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) { break; } - if (!parsing_failed_here) - parsing_failed_here = 1; - - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; + to_be_parsed = to_be_parsed + buffer_size; } - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; - } - to_be_parsed = to_be_parsed + buffer_size; + node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method.."); } - node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method.."); -} - -void io::CommIo::send(const std::string& cmd) -{ - CommSync* responseSync = messageHandler.getResponseSync(); - // It is imperative to hold a lock on the mutex "g_response_mutex" while - // modifying the variable "g_response_received". - std::mutex::scoped_lock lock(responseSync.mutex); - // Determine byte size of cmd and hand over to send() method of manager_ - manager_.get()->send(cmd); - responseSync.condition.wait(lock, []() { return responseSync.received; }); - responseSync.received = false; -} \ No newline at end of file + + void CommIo::send(const std::string& cmd) + { + CommSync* responseSync = messageHandler.getResponseSync(); + // It is imperative to hold a lock on the mutex "g_response_mutex" while + // modifying the variable "g_response_received". + std::mutex::scoped_lock lock(responseSync.mutex); + // Determine byte size of cmd and hand over to send() method of manager_ + manager_.get()->send(cmd); + responseSync.condition.wait(lock, []() { return responseSync.received; }); + responseSync.received = false; + } + +} // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/crc/crc.cpp b/src/septentrio_gnss_driver/crc/crc.cpp index 00e655ef..4b1714ba 100644 --- a/src/septentrio_gnss_driver/crc/crc.cpp +++ b/src/septentrio_gnss_driver/crc/crc.cpp @@ -17,60 +17,69 @@ // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// POSSIBILITY OF SUCH DAMAGE. // // ***************************************************************************** #include #include -/** - * @file crc.cpp - * @brief Defines the CRC table and the functions to compute and validate the CRC of an SBF block - * @date 17/08/20 - */ +namespace crc { + /** + * @file crc.cpp + * @brief Defines the CRC table and the functions to compute and validate the CRC + * of an SBF block + * @date 17/08/20 + */ -uint16_t compute16CCITT (const uint8_t *buf, size_t buf_length) // The CRC we choose is 2 bytes, remember, hence uint16_t.. -{ - uint16_t crc = 0; // Seed is 0, as suggested by the firmware, will compute CRC in the forward direction.. - - for (size_t i = 0; i < buf_length; i++) - { - crc = (crc << 8) ^ CRC_LOOK_UP[uint8_t( (crc >> 8) ^ buf[i])]; - // The ^ (bitwise XOR) in C or C++ takes two numbers as operands and does XOR on every bit of two numbers. - // The result of XOR is 1 if the two bits are different. - // The << (left shift) in C or C++ takes two numbers, left shifts the bits of the first operand, - // the second operand decides the number of places to shift. - // The >> (right shift) in C or C++ takes two numbers, right shifts the bits of the first operand, - // the second operand decides the number of places to shift; you can just loose the smallest values if big-endian. - // The left shift and right shift operators should not be used for negative numbers. - // The left-shift and right-shift operators are equivalent to multiplication and division by 2 respectively, - // hence only rightshift is non-exact (remainder is not retained). - // CRC_LOOK_UP is constructed from truncated polynomial (divisor). - // The above implements a kind of CRC 32 algorithm: efficient, fast. - } + uint16_t compute16CCITT(const uint8_t* buf, + size_t buf_length) // The CRC we choose is 2 bytes, + // remember, hence uint16_t.. + { + uint16_t crc = 0; // Seed is 0, as suggested by the firmware, will compute + // CRC in the forward direction.. - return crc; -} + for (size_t i = 0; i < buf_length; i++) + { + crc = (crc << 8) ^ CRC_LOOK_UP[uint8_t((crc >> 8) ^ buf[i])]; + // The ^ (bitwise XOR) in C or C++ takes two numbers as operands and does + // XOR on every bit of two numbers. The result of XOR is 1 if the two + // bits are different. The << (left shift) in C or C++ takes two numbers, + // left shifts the bits of the first operand, the second operand decides + // the number of places to shift. The >> (right shift) in C or C++ takes + // two numbers, right shifts the bits of the first operand, the second + // operand decides the number of places to shift; you can just loose the + // smallest values if big-endian. The left shift and right shift + // operators should not be used for negative numbers. The left-shift and + // right-shift operators are equivalent to multiplication and division by + // 2 respectively, hence only rightshift is non-exact (remainder is not + // retained). CRC_LOOK_UP is constructed from truncated polynomial + // (divisor). The above implements a kind of CRC 32 algorithm: efficient, + // fast. + } -bool isValid(const uint8_t *block) -{ - // We need all of the message except for the first 4 bytes (Sync and CRC), i.e. we start at the address of ID. - uint16_t length = parsing_utilities::getLength(block); - if (length > 4) - { - uint16_t crc = compute16CCITT(block + 4, length - 4); - return (crc == parsing_utilities::getCrc(block)); - } - else - { - return false; - } -} + return crc; + } + + bool isValid(const uint8_t* block) + { + // We need all of the message except for the first 4 bytes (Sync and CRC), + // i.e. we start at the address of ID. + uint16_t length = parsing_utilities::getLength(block); + if (length > 4) + { + uint16_t crc = compute16CCITT(block + 4, length - 4); + return (crc == parsing_utilities::getCrc(block)); + } else + { + return false; + } + } +} // namespace crc From 021137affc64a674769d6e0a21bdbf3036072fd0 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 13 Jan 2023 16:22:37 +0100 Subject: [PATCH 019/170] Refactor and cleanup --- CMakeLists.txt | 6 +- README.md | 2 + .../abstraction/typedefs.hpp | 2 +- .../communication/async_manager.hpp | 708 ++++++++++-------- .../communication/communication_core.hpp | 72 +- .../communication/io.hpp | 97 +-- .../communication/telegram.hpp | 75 +- .../communication/telegram_handler.hpp | 69 +- .../node/rosaic_node.hpp | 2 +- .../packed_structs/sbf_structs.hpp | 9 +- package.xml | 1 + .../communication/communication_core.cpp | 249 +++--- .../communication/telegram_handler.cpp | 49 +- .../node/rosaic_node.cpp | 15 +- 14 files changed, 723 insertions(+), 633 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1118a0f5..228784c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED) # bug with 1.71: spirit is not found ... COMPONENTS system thread regex spirit) LIST(APPEND CMAKE_MODULE_PATH "/usr/share/cmake/geographiclib") find_package(GeographicLib REQUIRED) +find_package(TBB REQUIRED) ## For PCAP file handling find_library(libpcap_LIBRARIES pcap) @@ -166,6 +167,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GeographicLib_INCLUDE_DIRS} + ${TBB_INCLUDE_DIRS} ) ## Add cmake target dependencies of the library @@ -179,7 +181,6 @@ include_directories( add_executable(${PROJECT_NAME}_node src/septentrio_gnss_driver/node/main.cpp src/septentrio_gnss_driver/node/rosaic_node.cpp - src/septentrio_gnss_driver/communication/circular_buffer.cpp src/septentrio_gnss_driver/parsers/parsing_utilities.cpp src/septentrio_gnss_driver/parsers/string_utilities.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp @@ -188,7 +189,7 @@ add_executable(${PROJECT_NAME}_node src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp src/septentrio_gnss_driver/crc/crc.cpp src/septentrio_gnss_driver/communication/communication_core.cpp - src/septentrio_gnss_driver/communication/message_parser.cpp + #src/septentrio_gnss_driver/communication/message_parser.cpp src/septentrio_gnss_driver/communication/telegram_handler.cpp src/septentrio_gnss_driver/communication/pcap_reader.cpp ) @@ -209,6 +210,7 @@ target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBRARIES} ${libpcap_LIBRARIES} ${GeographicLib_LIBRARIES} + tbb ) ############# diff --git a/README.md b/README.md index 114389d3..4ad25250 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,8 @@ Compatiblity with PCAP captures are incorporated through [pcap libraries](https: `sudo apt install libpcap-dev`.

Conversions from LLA to UTM are incorporated through [GeographicLib](https://geographiclib.sourceforge.io/). Install the necessary headers via

`sudo apt install libgeographic-dev` +Internal data pipelining is realized with TBB. Install the necessary library via

+`sudo apt install libtbb-dev`

## Usage
diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 99d41974..7331e7d4 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -178,7 +178,7 @@ class ROSaicNodeBase virtual ~ROSaicNodeBase() {} - Settings* getSettings(){return &settings_}; + Settings* getSettings() { return &settings_; } void registerSubscriber() { diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 4c5cb33a..52ee7c9f 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -65,7 +65,7 @@ // ROSaic includes #include -#include +#include // local includes #include @@ -91,10 +91,10 @@ namespace io { { public: virtual ~AsyncManagerBase() {} - //! Sends commands to the receiver - virtual bool send(const std::string& cmd) = 0; //! Connects the stream [[nodiscard]] virtual bool connect() = 0; + //! Sends commands to the receiver + virtual void send(const std::string& cmd) = 0; }; /** @@ -102,9 +102,9 @@ namespace io { * @brief This is the central interface between ROSaic and the Rx(s), managing * I/O operations such as reading messages and sending commands.. * - * SocketT is either boost::asio::serial_port or boost::asio::tcp::ip + * IoType is either boost::asio::serial_port or boost::asio::tcp::ip */ - template + template class AsyncManager : public AsyncManagerBase { public: @@ -113,406 +113,472 @@ namespace io { * @param[in] node Pointer to node * @param[in] telegramQueue Telegram queue */ - AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue) : - node_(node), ioSocket_(node, &ioService_), telegramQueue_(telegramQueue) - { - } + AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue); - ~AsyncManager() - { - running_ = false; - flushOutputQueue(); - close(); - node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); - ioService_.stop(); - ioThread_.join(); - watchdogThread_.join(); - node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); - } + ~AsyncManager(); - [[nodiscard]] bool connect() - { - if (ioSocket_.connect()) - { - return false; - } - receive(); + [[nodiscard]] bool connect(); - watchdogThread_ = std::thread(std::bind(&TcpClient::runWatchdog, this)); - return true; - } + void send(const std::string& cmd); - void send(const std::string& cmd) - { - if (cmd.size() == 0) - { - node_->log( - LogLevel::ERROR, - "AsyncManager message size to be sent to the Rx would be 0"); - } + private: + void receive(); + void close(); + void runIoService(); + void runWatchdog(); + void write(const std::string& cmd); + void resync(); + template + void readSync(); + void readSbfHeader(); + void readSbf(std::size_t length); + void readCd(); + void readString(); + void readStringElements(); - ioService_.post(boost::bind(&AsyncManager::write, this, cmd)); - } + //! Pointer to the node + ROSaicNodeBase* node_; + IoType ioInterface_; + boost::asio::io_service ioService_; + std::atomic running_; + std::thread ioThread_; + std::thread watchdogThread_; + //! Timestamp of receiving buffer + Timestamp recvStamp_; + //! Telegram + std::shared_ptr telegram_; + //! TelegramQueue + TelegramQueue* telegramQueue_; + }; - private: - void receive() - { - resync(); - ioThread_ = std::thread( - std::thread(std::bind(&AsyncManager::runIoService, this))); - } + template + AsyncManager::AsyncManager(ROSaicNodeBase* node, + TelegramQueue* telegramQueue) : + node_(node), + ioInterface_(node, &ioService_), telegramQueue_(telegramQueue) + { + } - void flushOutputQueue() - { - ioService_.post([this]() { write(); }); - } + template + AsyncManager::~AsyncManager() + { + running_ = false; + close(); + node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); + ioService_.stop(); + ioThread_.join(); + watchdogThread_.join(); + node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); + } + + template + [[nodiscard]] bool AsyncManager::connect() + { + running_ = true; - void close() + if (ioInterface_.connect()) { - ioService_.post([this]() { socket_->close(); }); + return false; } + receive(); - void runIoService() + watchdogThread_ = std::thread(std::bind(&AsyncManager::runWatchdog, this)); + return true; + } + + template + void AsyncManager::send(const std::string& cmd) + { + if (cmd.size() == 0) { - ioService_.run(); - node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); + node_->log(LogLevel::ERROR, + "AsyncManager message size to be sent to the Rx would be 0"); + return; } - void runWatchdog() + ioService_.post(boost::bind(&AsyncManager::write, this, cmd)); + } + + template + void AsyncManager::receive() + { + resync(); + ioThread_ = std::thread( + std::thread(std::bind(&AsyncManager::runIoService, this))); + } + + template + void AsyncManager::close() + { + ioService_.post([this]() { ioInterface_.close(); }); + } + + template + void AsyncManager::runIoService() + { + ioService_.run(); + node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); + } + + template + void AsyncManager::runWatchdog() + { + while (running_) { - while (running_) - { - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - if (running_ && ioService_.stopped()) - { - node_->log(LogLevel::DEBUG, - "AsyncManager connection lost. Trying to reconnect."); - ioService_.reset(); - ioThread_->join(); - receive(); - } + if (running_ && ioService_.stopped()) + { + node_->log(LogLevel::DEBUG, + "AsyncManager connection lost. Trying to reconnect."); + ioService_.reset(); + ioThread_.join(); + while (!ioInterface_.connect()) + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + receive(); } } + } - void write(const std::string& cmd) - { - boost::asio::write(ioSocket_.stream(), - boost::asio::buffer(cmd.data(), cmd.size())); - // Prints the data that was sent - node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + - std::to_string(cmd.size()) + - " bytes to the Rx: " + cmd); - } + template + void AsyncManager::write(const std::string& cmd) + { + boost::asio::async_write( + ioInterface_.stream_, boost::asio::buffer(cmd.data(), cmd.size()), + [this, cmd](boost::system::error_code ec, std::size_t /*length*/) { + if (!ec) + { + // Prints the data that was sent + node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); + } else + { + node_->log(LogLevel::ERROR, + "AsyncManager was unable to send the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); + } + }); + } - void resync() - { - message_.reset(new Telegram); - readSync<0>(); - } + template + void AsyncManager::resync() + { + telegram_.reset(new Telegram); + readSync<0>(); + } - template - void readSync() - { - static_assert(index < 3); + template + template + void AsyncManager::readSync() + { + static_assert(index < 3); - boost::asio::async_read( - ioSocket_.stream(), - boost::asio::buffer(message_->data.data() + index, 1), - [this](boost::system::error_code ec, std::size_t numBytes) { - Timestamp stamp = node_->getTime(); + boost::asio::async_read( + ioInterface_.stream_, + boost::asio::buffer(telegram_->message.data() + index, 1), + [this](boost::system::error_code ec, std::size_t numBytes) { + Timestamp stamp = node_->getTime(); - if (!ec) + if (!ec) + { + if (numBytes == 1) { - if (numBytes == 1) - { - std::byte& currByte = message_->data[index]; + uint8_t& currByte = telegram_->message[index]; - if (currByte == SYNC_0) + if (currByte == SYNC_BYTE_1) + { + telegram_->stamp = stamp; + readSync<1>(); + } else + { + switch (index) + { + case 0: { - message_->stamp = stamp; - readSync<1>(); - } else + if ((currByte == CONNECTION_DESCRIPTOR_BYTE_I) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_C) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_U) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_N) || + (currByte == CONNECTION_DESCRIPTOR_BYTE_D)) + { + telegram_->type = + message_type::CONNECTION_DESCRIPTOR; + readCd(); + } else + readSync<0>(); + break; + } + case 1: { - switch (index) + switch (currByte) { - case 0: + case SBF_SYNC_BYTE_2: + { + telegram_->type = message_type::SBF; + readSbfHeader(); + break; + } + case NMEA_SYNC_BYTE_2: + { + telegram_->type = message_type::NMEA; + readSync<2>(); + break; + } + case NMEA_INS_SYNC_BYTE_2: { - if ((currByte == CONNECTION_DESCRIPTOR_BYTE_I) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_C) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_U) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_N) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_D)) - { - message_->type = CONNECTION_DESCRIPTOR; - readCd(); - } else - readSync<0>(); + telegram_->type = message_type::NMEA_INS; + readSync<2>(); break; } - case 1: + case RESPONSE_SYNC_BYTE_2: { - switch (currByte) - { - case SBF_SYNC_BYTE_1: - { - message_->type = SBF; - readSbfHeader(); - break; - } - case NMEA_SYNC_BYTE_1: - { - message_->type = NMEA; - readSync<2>(); - break; - } - case RESPONSE_SYNC_BYTE_1: - { - message_->type = RESPONSE; - readSync<2>(); - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync 1 read fault, should never come here."); + telegram_->type = message_type::RESPONSE; + readSync<2>(); + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync 1 read fault, should never come here."); + resync(); + break; + } + } + break; + } + case 2: + { + switch (currByte) + { + case NMEA_SYNC_BYTE_3: + { + if (telegram_->type == message_type::NMEA) + readString(); + else + resync(); + break; + } + case NMEA_INS_SYNC_BYTE_3: + { + if (telegram_->type == message_type::NMEA) + readString(); + else resync(); - break; - } - } break; } - case 2: + case ERROR_SYNC_BYTE_3: { - switch (currByte) - { - case NMEA_SYNC_BYTE_2: - { - if (message_->type == NMEA) - readString(); - else - resync(); - break; - } - case ERROR_SYNC_BYTE_2: - { - if (message_->type == ERROR) - readString(); - else - resync(); - break; - } - case RESPONSE_SYNC_BYTE_2: - { - if (message_->type == RESPONSE) - readString(); - else - resync(); - break; - } - default: - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync 2 read fault, should never come here."); + if (telegram_->type == + message_type::ERROR_RESPONSE) + readString(); + else + resync(); + break; + } + case RESPONSE_SYNC_BYTE_3: + { + if (telegram_->type == message_type::RESPONSE) + readString(); + else resync(); - break; - } - } break; } default: { node_->log( LogLevel::DEBUG, - "AsyncManager sync read fault, should never come here."); + "AsyncManager sync 2 read fault, should never come here."); resync(); break; } } + break; + } + default: + { + node_->log( + LogLevel::DEBUG, + "AsyncManager sync read fault, should never come here."); + resync(); + break; + } } - } else - { - node_->log( - LogLevel::DEBUG, - "AsyncManager sync read fault, wrong number of bytes read: " + - std::to_string(numBytes)); - resync(); } } else { - node_->log(LogLevel::DEBUG, - "AsyncManager sync read error: " + ec.message()); + node_->log( + LogLevel::DEBUG, + "AsyncManager sync read fault, wrong number of bytes read: " + + std::to_string(numBytes)); resync(); } - }); - } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager sync read error: " + ec.message()); + resync(); + } + }); + } - void readSbfHeader() - { - message_->data.resize(SBF_HEADER_SIZE); + template + void AsyncManager::readSbfHeader() + { + telegram_->message.resize(SBF_HEADER_SIZE); - boost::asio::async_read( - ioSocket_.stream(), - boost::asio::buffer(message_->data.data() + 2, SBF_HEADER_SIZE - 2), - [this](boost::system::error_code ec, std::size_t numBytes) { - if (!ec) + boost::asio::async_read( + ioInterface_.stream_, + boost::asio::buffer(telegram_->message.data() + 2, SBF_HEADER_SIZE - 2), + [this](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (SBF_HEADER_SIZE - 2)) { - if (numBytes == (SBF_HEADER_SIZE - 2)) - { - unit16_t length = - parsing_utilities::getLength(message_->data.data()); - if (length > MAX_SBF_SIZE) - { - node_->log( - LogLevel::DEBUG, - "AsyncManager SBF header read fault, length of block exceeds " + - std::to_string(MAX_SBF_SIZE) + ": " + - std::to_string(length)); - } else - readSbf(length); - } else + uint16_t length = + parsing_utilities::getLength(telegram_->message.data()); + if (length > MAX_SBF_SIZE) { node_->log( LogLevel::DEBUG, - "AsyncManager SBF header read fault, wrong number of bytes read: " + - std::to_string(numBytes)); - resync(); - } + "AsyncManager SBF header read fault, length of block exceeds " + + std::to_string(MAX_SBF_SIZE) + ": " + + std::to_string(length)); + } else + readSbf(length); } else { - node_->log(LogLevel::DEBUG, - "AsyncManager SBF header read error: " + - ec.message()); + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF header read fault, wrong number of bytes read: " + + std::to_string(numBytes)); resync(); } - }); - } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager SBF header read error: " + + ec.message()); + resync(); + } + }); + } - void readSbf(std::size_t length) - { - message_->data.resize(length); - - boost::asio::async_read( - ioSocket_.stream(), - boost::asio::buffer(message_->data.data() + SBF_HEADER_SIZE, - length - SBF_HEADER_SIZE), - [this](boost::system::error_code ec, std::size_t numBytes) { - if (!ec) + template + void AsyncManager::readSbf(std::size_t length) + { + telegram_->message.resize(length); + + boost::asio::async_read( + ioInterface_.stream_, + boost::asio::buffer(telegram_->message.data() + SBF_HEADER_SIZE, + length - SBF_HEADER_SIZE), + [this, length](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == (length - SBF_HEADER_SIZE)) { - if (numBytes == (length - SBF_HEADER_SIZE)) + if (crc::isValid(telegram_->message.data())) { - if (crc::isValid(message_->data.data())) - { - message_->sbfId = - parsing_utilities::getId(message_->data.data()); + telegram_->sbfId = + parsing_utilities::getId(telegram_->message.data()); - telegramQueue_->push(message_); - } - } else - { - node_->log( - LogLevel::DEBUG, - "AsyncManager SBF read fault, wrong number of bytes read: " + - std::to_string(numBytes)); + telegramQueue_->push(telegram_); } } else { - node_->log(LogLevel::DEBUG, - "AsyncManager SBF read error: " + ec.message()); + node_->log( + LogLevel::DEBUG, + "AsyncManager SBF read fault, wrong number of bytes read: " + + std::to_string(numBytes)); } - resync(); - }); - } + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager SBF read error: " + ec.message()); + } + resync(); + }); + } - void readCd() - { - message_->data.resize(1); - readStringElements(); - } + template + void AsyncManager::readCd() + { + telegram_->message.resize(1); + readStringElements(); + } - void readString() - { - message_->data.resize(3); - readStringElements(); - } + template + void AsyncManager::readString() + { + telegram_->message.resize(3); + readStringElements(); + } - void readStringElements() - { - std::array buf; + template + void AsyncManager::readStringElements() + { + std::array buf; - boost::asio::async_read( - ioSocket_.stream(), boost::asio::buffer(buf.data(), 1), - [this](boost::system::error_code ec, std::size_t numBytes) { - if (!ec) + boost::asio::async_read( + ioInterface_.stream_, boost::asio::buffer(buf.data(), 1), + [this, buf](boost::system::error_code ec, std::size_t numBytes) { + if (!ec) + { + if (numBytes == 1) { - if (numBytes == 1) + telegram_->message.push_back(buf[0]); + switch (buf[0]) { - message_->data.push_back(buf[0]); - switch (buf[0]) - { - case SYNC_0: - { - message_.reset(new Telegram); - message_->data[0] = buf[0]; - message_->stamp = node_->getTime(); - node_->log( - LogLevel::DEBUG, - "AsyncManager string read fault, sync 0 found.")); - readSync<1>(); - break; - } - case LF: - { - if (message_->data[message_->data.size() - 2] == CR) - telegramQueue_->push(message_); - resync(); - break; - } - case CONNECTION_DESCRIPTOR_FOOTER: - { - if (message_->type == CONNECTION_DESCRIPTOR) - telegramQueue_->push(message_); - resync(); - break; - } - default: - { - readString(); - break; - } - } - } else + case SYNC_BYTE_1: { + telegram_.reset(new Telegram); + telegram_->message[0] = buf[0]; + telegram_->stamp = node_->getTime(); node_->log( LogLevel::DEBUG, - "AsyncManager string read fault, wrong number of bytes read: " + - std::to_string(numBytes)); + "AsyncManager string read fault, sync 0 found."); + readSync<1>(); + break; + } + case LF: + { + if (telegram_->message[telegram_->message.size() - 2] == + CR) + telegramQueue_->push(telegram_); + resync(); + break; + } + case CONNECTION_DESCRIPTOR_FOOTER: + { + if (telegram_->type == + message_type::CONNECTION_DESCRIPTOR) + telegramQueue_->push(telegram_); resync(); + break; + } + default: + { + readString(); + break; + } } } else { - node_->log(LogLevel::DEBUG, - "AsyncManager string read error: " + - ec.message()); + node_->log( + LogLevel::DEBUG, + "AsyncManager string read fault, wrong number of bytes read: " + + std::to_string(numBytes)); resync(); } - }); - } - - //! Pointer to the node - ROSaicNodeBase* node_; - SocketT ioSocket_; - std::atomic running_; - boost::asio::io_service ioService_; - std::thread ioThread_; - std::thread watchdogThread_; - //! Timestamp of receiving buffer - Timestamp recvStamp_; - //! Telegram - std::shared_ptr message_; - //! TelegramQueue - TelegramQueue* telegramQueue_; - }; + } else + { + node_->log(LogLevel::DEBUG, + "AsyncManager string read error: " + ec.message()); + resync(); + } + }); + } } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 3699e8c7..30c2aa31 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -72,6 +72,7 @@ #include // for usleep() // ROSaic includes #include +#include /** * @file communication_core.hpp @@ -93,32 +94,31 @@ namespace io { 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000}; /** - * @class CommIo + * @class CommunicationCore * @brief Handles communication with and configuration of the mosaic (and beyond) * receiver(s) */ - class CommIo + class CommunicationCore { public: /** - * @brief Constructor of the class CommIo + * @brief Constructor of the class CommunicationCore * @param[in] node Pointer to node */ - CommIo(ROSaicNodeBase* node); + CommunicationCore(ROSaicNodeBase* node); /** - * @brief Default destructor of the class CommIo + * @brief Default destructor of the class CommunicationCore */ - ~CommIo(); + ~CommunicationCore(); /** - * @brief Initializes the I/O handling + * @brief Connects the data stream */ - void initializeIo(); + void connect(); /** * @brief Configures Rx: Which SBF/NMEA messages it should output and later * correction settings - * @param[in] settings The device's settings * */ void configureRx(); @@ -131,9 +131,16 @@ namespace io { private: /** - * @brief Reset main port so it can receive commands + * @brief Initializes the I/O handling + * * @return Wether connection was successful + */ + bool initializeIo(); + + /** + * @brief Reset main connection so it can receive commands + * @return Main connection descriptor */ - void resetMainPort(); + std::string resetMainConnection(); /** * @brief Sets up the stage for SBF file reading @@ -162,6 +169,8 @@ namespace io { */ void initializePCAPFileReading(std::string file_name); + void processTelegrams(); + /** * @brief Hands over to the send() method of manager_ * @param cmd The command to hand over @@ -174,47 +183,22 @@ namespace io { Settings* settings_; //! TelegramQueue TelegramQueue telegramQueue_; - //! Telegram handlers for the inwards streaming messages - MessageHandler messageHandler_; - //! Whether connecting to Rx was successful - bool connected_ = false; - //! Since the configureRx() method should only be called once the connection - //! was established, we need the threads to communicate this to each other. - //! Associated mutex.. - std::mutex connection_mutex_; - //! Since the configureRx() method should only be called once the connection - //! was established, we need the threads to communicate this to each other. - //! Associated condition variable.. - std::condition_variable connection_condition_; - //! Host name of TCP server - std::string tcp_host_; - //! TCP port number - std::string tcp_port_; - //! Whether yet-to-be-established connection to Rx will be serial or TCP - bool serial_; - //! Saves the port description - std::string serial_port_; + //! TelegramHandler + TelegramHandler telegramHandler_; + //! Processing thread + std::thread processingThread_; + //! Whether connecting was successful + bool initializedIo_ = false; //! Processes I/O stream data //! This declaration is deliberately stream-independent (Serial or TCP). std::unique_ptr manager_; - //! Baudrate at the moment, unless InitializeSerial or ResetSerial fail - uint32_t baudrate_; bool nmeaActivated_ = false; //! Indicator for threads to run std::atomic running_; - //! Communication ports - std::string mainPort_; - - //! Host currently connected to - std::string host_; - //! Port over which TCP/IP connection is currently established - std::string port_; - //! Sleep time in microseconds (there is no Unix command for milliseconds) - //! after setting the baudrate to certain value (important between - //! increments) - const static unsigned int SET_BAUDRATE_SLEEP_ = 500000; + //! Main communication port + std::string mainConnectionDescriptor_; }; } // namespace io diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 5087a97f..22fca050 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -54,38 +54,42 @@ namespace io { { public: TcpIo(ROSaicNodeBase* node, boost::asio::io_service* ioService) : - node_(node), ioService_(ioService), socket_(*ioService_) + node_(node), ioService_(ioService), stream_(*ioService_) { } - ~TcpIo() { socket_.close(); } + ~TcpIo() { stream_.close(); } - const boost::asio::ip::tcp::socket& stream() { return socket_; } + void close() { stream_.close(); } [[nodiscard]] bool connect() { + if (stream_.is_open()) + { + stream_.close(); + } + boost::asio::ip::tcp::resolver::iterator endpointIterator; try { boost::asio::ip::tcp::resolver resolver(*ioService_); boost::asio::ip::tcp::resolver::query query( - node_->getSettings()->tcp_ip, node->getSettings()->tcp_port); + node_->getSettings()->tcp_ip, node_->getSettings()->tcp_port); endpointIterator = resolver.resolve(query); } catch (std::runtime_error& e) { - node_->log(LogLevel::ERROR, "Could not resolve " + host + - " on port " + port + ": " + - e.what()); + node_->log(LogLevel::ERROR, + "Could not resolve " + node_->getSettings()->tcp_ip + + " on port " + node_->getSettings()->tcp_port + ": " + + e.what()); return false; } - socket_.reset(new boost::asio::ip::tcp::socket(*ioService_)); - try { - socket_.connect(*endpointIterator); + stream_.connect(*endpointIterator); - socket_.set_option(boost::asio::ip::tcp::no_delay(true)); + stream_.set_option(boost::asio::ip::tcp::no_delay(true)); } catch (std::runtime_error& e) { node_->log(LogLevel::ERROR, @@ -100,29 +104,31 @@ namespace io { private: ROSaicNodeBase* node_; boost::asio::io_service* ioService_; - boost::asio::ip::tcp::socket socket_; + + public: + boost::asio::ip::tcp::socket stream_; }; - class SerialIo : public IoBase + class SerialIo { public: - SerialIo(ROSaicNodeBase* node, boost::asio::io_service* ioService, - std::string serialPort, uint32_t baudrate, bool flowcontrol) : - node_(node), - flowcontrol_(node->getSettings()->flowcontrol), - baudrate_(node->getSettings()->baudrate), serialPort_(ioService_) + SerialIo(ROSaicNodeBase* node, boost::asio::io_service* ioService) : + node_(node), flowcontrol_(node->getSettings()->hw_flow_control), + baudrate_(node->getSettings()->baudrate), stream_(*ioService_) { } - ~SerialIo() { serialPort_.close(); } + ~SerialIo() { stream_.close(); } + + void close() { stream_.close(); } - const boost::asio::serial_port& stream() { return socket_; } + const boost::asio::serial_port& stream() { return stream_; } [[nodiscard]] bool connect() { - if (serialPort_.is_open()) + if (stream_.is_open()) { - serialPort_.close(); + stream_.close(); } bool opened = false; @@ -131,13 +137,13 @@ namespace io { { try { - serialPort_.open(port_); + stream_.open(port_); opened = true; } catch (const boost::system::system_error& err) { node_->log(LogLevel::ERROR, - "SerialCoket: Could not open serial port " + - std::to_string(port_) + ". Error: " + err.what() + + "SerialCoket: Could not open serial port " + port_ + + ". Error: " + err.what() + ". Will retry every second."); using namespace std::chrono_literals; @@ -146,23 +152,22 @@ namespace io { } // No Parity, 8bits data, 1 stop Bit - serialPort_.set_option( - boost::asio::serial_port_base::baud_rate(baudrate_)); - serialPort_.set_option(boost::asio::serial_port_base::parity( + stream_.set_option(boost::asio::serial_port_base::baud_rate(baudrate_)); + stream_.set_option(boost::asio::serial_port_base::parity( boost::asio::serial_port_base::parity::none)); - serialPort_.set_option(boost::asio::serial_port_base::character_size(8)); - serialPort_.set_option(boost::asio::serial_port_base::stop_bits( + stream_.set_option(boost::asio::serial_port_base::character_size(8)); + stream_.set_option(boost::asio::serial_port_base::stop_bits( boost::asio::serial_port_base::stop_bits::one)); - serialPort_.set_option(boost::asio::serial_port_base::flow_control( + stream_.set_option(boost::asio::serial_port_base::flow_control( boost::asio::serial_port_base::flow_control::none)); - int fd = serialPort_.native_handle(); + int fd = stream_.native_handle(); termios tio; // Get terminal attribute, follows the syntax // int tcgetattr(int fd, struct termios *termios_p); tcgetattr(fd, &tio); - // Hardware flow control settings_->. + // Hardware flow control settings if (flowcontrol_ == "RTS|CTS") { tio.c_iflag &= ~(IXOFF | IXON); @@ -189,7 +194,7 @@ namespace io { return setBaudrate(); } - [[nodiscard]] bool setBaurate() + [[nodiscard]] bool setBaudrate() { // Setting the baudrate, incrementally.. node_->log(LogLevel::DEBUG, @@ -198,11 +203,10 @@ namespace io { node_->log(LogLevel::DEBUG, "Initiated current_baudrate object..."); try { - serialPort_.get_option( - current_baudrate); // Note that this sets - // current_baudrate.value() often to - // 115200, since by default, all Rx COM - // ports, + stream_.get_option(current_baudrate); // Note that this sets + // current_baudrate.value() + // often to 115200, since by + // default, all Rx COM ports, // at least for mosaic Rxs, are set to a baudrate of 115200 baud, // using 8 data-bits, no parity and 1 stop-bit. } catch (boost::system::system_error& e) @@ -216,7 +220,7 @@ namespace io { boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. { - serialPort_.get_option(current_baudrate, e_loop); + stream_.get_option(current_baudrate, e_loop); } while(e_loop); */ return false; @@ -241,7 +245,7 @@ namespace io { // Increment until Baudrate[i] matches current_baudrate. try { - serialPort_.set_option( + stream_.set_option( boost::asio::serial_port_base::baud_rate(baudrates[i])); } catch (boost::system::system_error& e) { @@ -257,7 +261,7 @@ namespace io { try { - serialPort_.get_option(current_baudrate); + stream_.get_option(current_baudrate); } catch (boost::system::system_error& e) { @@ -269,7 +273,7 @@ namespace io { boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. { - serialPort_.get_option(current_baudrate, e_loop); + stream_.get_option(current_baudrate, e_loop); } while(e_loop); */ return false; @@ -281,14 +285,17 @@ namespace io { node_->log(LogLevel::INFO, "Set ASIO baudrate to " + std::to_string(current_baudrate.value()) + ", leaving InitializeSerial() method"); + return true; } private: ROSaicNodeBase* node_; - bool flowcontrol_; + std::string flowcontrol_; std::string port_; uint32_t baudrate_; boost::asio::io_service* ioService_; - boost::asio::serial_port serialPort_; + + public: + boost::asio::serial_port stream_; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 5dbfee03..5d1bdc1c 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -31,7 +31,7 @@ #pragma once // C++ -#include +#include #include // TBB @@ -41,70 +41,75 @@ #include //! 0x24 is ASCII for $ - 1st byte in each message -static const std::byte SYNC_0 = 0x24; +static const uint8_t SYNC_BYTE_1 = 0x24; //! 0x40 is ASCII for @ - 2nd byte to indicate SBF block -static const std::byte SBF_SYNC_BYTE_1 = 0x40; +static const uint8_t SBF_SYNC_BYTE_2 = 0x40; //! 0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message -static const std::byte NMEA_SYNC_BYTE_1 = 0x47; +static const uint8_t NMEA_SYNC_BYTE_2 = 0x47; //! 0x50 is ASCII for P - 3rd byte to indicate NMEA-type ASCII message -static const std::byte NMEA_SYNC_BYTE_2 = 0x50; +static const uint8_t NMEA_SYNC_BYTE_3 = 0x50; //! 0x49 is ASCII for I - 2nd byte to indicate INS NMEA-type ASCII message -static const std::byte NMEA_INS_SYNC_BYTE_1 = 0x49; +static const uint8_t NMEA_INS_SYNC_BYTE_2 = 0x49; //! 0x4E is ASCII for N - 3rd byte to indicate NMEA-type ASCII message -static const std::byte NMEA_INS_SYNC_BYTE_2 = 0x4E; +static const uint8_t NMEA_INS_SYNC_BYTE_3 = 0x4E; //! 0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx -static const std::byte RESPONSE_SYNC_BYTE_1 = 0x52; +static const uint8_t RESPONSE_SYNC_BYTE_2 = 0x52; //! 0x3A is ASCII for : - 3rd byte in the response message from the Rx -static const std::byte RESPONSE_SYNC_BYTE_2 = 0x3A; +static const uint8_t RESPONSE_SYNC_BYTE_3 = 0x3A; //! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the //! command was invalid -static const std::byte ERROR_SYNC_BYTE_2 = 0x3F; +static const uint8_t ERROR_SYNC_BYTE_3 = 0x3F; //! 0x0D is ASCII for "Carriage Return", i.e. "Enter" -static const std::byte CR = 0x0D; +static const uint8_t CR = 0x0D; //! 0x0A is ASCII for "Line Feed", i.e. "New Line" -static const std::byte LF = 0x0A; +static const uint8_t LF = 0x0A; //! 0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after //! initiating IP connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_I = 0x49; +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_I = 0x49; //! 0x43 is ASCII for C - 1st character of connection descriptor sent by the Rx after //! initiating COM connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_C = 0x43; +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_C = 0x43; //! 0x55 is ASCII for U - 1st character of connection descriptor sent by the Rx after //! initiating USB connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_U = 0x55; +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_U = 0x55; //! 0x4E is ASCII for N - 1st character of connection descriptor sent by the Rx after //! initiating NTRIP connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_N = 0x4E; +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_N = 0x4E; //! 0x44 is ASCII for D - 1st character of connection descriptor sent by the Rx after //! initiating DSK connection -static const std::byte CONNECTION_DESCRIPTOR_BYTE_D = 0x44; +static const uint8_t CONNECTION_DESCRIPTOR_BYTE_D = 0x44; //! 0x3E is ASCII for > - end character of connection descriptor -static const std::byte CONNECTION_DESCRIPTOR_FOOTER = 0x3E; +static const uint8_t CONNECTION_DESCRIPTOR_FOOTER = 0x3E; static const uint16_t SBF_HEADER_SIZE = 8; -static const uint16_t MAX_SBF_SIZE 65535; +static const uint16_t MAX_SBF_SIZE = 65535; -enum TelegramType -{ - EMPTY, - SBF, - NMEA, - RESPONSE, - ERROR, - CONNECTION_DESCRIPTOR -}; +namespace message_type { + enum TelegramType + { + EMPTY, + SBF, + NMEA, + NMEA_INS, + RESPONSE, + ERROR_RESPONSE, + CONNECTION_DESCRIPTOR + }; +} struct Telegram { - Timestamp stamp = 0; - TelegramType type = EMPTY; - uint16_t sbfId = 0; - std::vector data(3); + Timestamp stamp; + message_type::TelegramType type; + uint16_t sbfId; + std::vector message; - Telegram() : stamp(0), type(INVALID), sbfId(0), data(std::vector(3)) + Telegram() : + stamp(0), type(message_type::EMPTY), sbfId(0), + message(std::vector(3)) { - data.reserve(MAX_SBF_SIZE); + message.reserve(MAX_SBF_SIZE); } }; -typedef tbb::concurrent_bounded_queue> TelegramQueue; +typedef tbb::concurrent_bounded_queue> TelegramQueue; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index af757275..6deacf67 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -58,6 +58,10 @@ #pragma once +// C++ includes +#include +#include + // Boost includes #include // In C++, writing a loop that iterates over a sequence is tedious --> @@ -91,9 +95,10 @@ #include #include -// ROSaic and C++ includes -#include -#include +// ROSaic includes +#include +#include +// #include /** * @file telegram_handler.hpp @@ -101,13 +106,6 @@ * messages */ -struct CommSync -{ - bool received = false; - std::mutex mutex; - std::condition_variable condition; -}; - namespace io { /** * @class TelegramHandler @@ -118,20 +116,44 @@ namespace io { { public: - TelegramHandler(ROSaicNodeBase* node) : node_(node), message_parser(node) {} + TelegramHandler(ROSaicNodeBase* node) : + node_(node), + /*message_parser(node),*/ cdReceived_(cdPromise_.get_future()), + responseReceived_(responsePromise_.get_future()) + { + } /** * @brief Called every time a telegram is received */ void handleTelegram(const std::shared_ptr& telegram); - CommSync* getResponseSync() { return &response_sync; } - - CommSync* getCdSync() { return &cd_sync; } - - uint8_t getCdCount() { return cd_count; } - - std::string getRxTcpPort() { return rx_tcp_port; } + //! Returns the connection descriptor + void resetWaitforMainCd() + { + mainConnectionDescriptor_ = std::string(); + // reset promise + cdPromise_ = std::promise(); + cdReceived_ = cdPromise_.get_future(); + } + + //! Returns the connection descriptor + std::string getMainCd() + { + cdReceived_.wait(); + return mainConnectionDescriptor_; + } + + //! Resets wait for response + void resetWaitForResponse() + { + // reset promise + responsePromise_ = std::promise(); + responseReceived_ = responsePromise_.get_future(); + } + + //! Waits for response + void waitForResponse() { responseReceived_.wait(); } private: void handleSbf(const std::shared_ptr& telegram); @@ -143,12 +165,13 @@ namespace io { ROSaicNodeBase* node_; //! MessageParser parser - MessageParser message_parser_; + // MessageParser messageParser_; //TODO - CommSync response_sync; - CommSync cd_sync; - uint8_t cd_count; - std::string rx_tcp_port; + std::promise cdPromise_; + std::future cdReceived_; + std::promise responsePromise_; + std::future responseReceived_; + std::string mainConnectionDescriptor_ = std::string(); }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp index 29e9bdf5..e4aa3135 100644 --- a/include/septentrio_gnss_driver/node/rosaic_node.hpp +++ b/include/septentrio_gnss_driver/node/rosaic_node.hpp @@ -127,7 +127,7 @@ namespace rosaic_node { void sendVelocity(const std::string& velNmea); //! Handles communication with the Rx - io::CommIo IO_; + io::CommunicationCore IO_; //! tf2 buffer and listener tf2_ros::Buffer tfBuffer_; std::unique_ptr tfListener_; diff --git a/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp b/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp index e3f44e23..9ca4f181 100644 --- a/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp +++ b/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp @@ -59,14 +59,15 @@ static const uint16_t MAXSB_MEASEPOCH_T2 = static const uint8_t MAXSB_NBVECTORINFO = 30; //! 0x24 is ASCII for $ - 1st byte in each message -static const uint8_t SBF_SYNC_BYTE_1 = 0x24; +static const uint8_t SBF_SYNC_1 = 0x24; //! 0x40 is ASCII for @ - 2nd byte to indicate SBF block -static const uint8_t SBF_SYNC_BYTE_2 = 0x40; +static const uint8_t SBF_SYNC_2 = 0x40; // C++ #include // Boost #include +// ROSaic #include #include @@ -372,13 +373,13 @@ template bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header) { qiLittleEndianParser(it, block_header.sync_1); - if (block_header.sync_1 != SBF_SYNC_BYTE_1) + if (block_header.sync_1 != SBF_SYNC_1) { node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 1."); return false; } qiLittleEndianParser(it, block_header.sync_2); - if (block_header.sync_2 != SBF_SYNC_BYTE_2) + if (block_header.sync_2 != SBF_SYNC_2) { node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 2."); return false; diff --git a/package.xml b/package.xml index 3f8601b4..bd49b034 100644 --- a/package.xml +++ b/package.xml @@ -63,6 +63,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tbb cpp_common rosconsole diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index e4c54ff6..7149d182 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -59,22 +59,22 @@ static const int8_t POSSTD_DEV_MAX = 100; namespace io { - CommIo::CommIo(ROSaicNodeBase* node) : - node_(node), settings_(node->getSettings()), running_(true) + CommunicationCore::CommunicationCore(ROSaicNodeBase* node) : + node_(node), settings_(node->getSettings()), telegramHandler_(node), + running_(true) { - g_response_received = false; - g_cd_received = false; - g_read_cd = true; - g_cd_count = 0; + running_ = true; + + processingThread_ = + std::thread(std::bind(&CommunicationCore::processTelegrams, this)); } - CommIo::~CommIo() + CommunicationCore::~CommunicationCore() { if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); - manager_.get()->send(cmd); - send("sdio, " + mainPort_ + ", auto, none\x0D"); + resetMainConnection(); + send("sdio, " + mainConnectionDescriptor_ + ", auto, none\x0D"); for (auto ntrip : settings_->rtk_settings.ntrip) { if (!ntrip.id.empty() && !ntrip.keep_open) @@ -125,15 +125,17 @@ namespace io { } running_ = false; - connectionThread_.join(); + std::shared_ptr telegram(new Telegram); + telegramQueue_.push(telegram); + processingThread_.join(); } - void CommIo::initializeIo() + void CommunicationCore::connect() { node_->log(LogLevel::DEBUG, "Called connect() method"); node_->log( LogLevel::DEBUG, - "Started timer for calling reconnect() method until connection succeeds"); + "Started timer for calling connect() method until connection succeeds"); boost::asio::io_service io; boost::posix_time::millisec wait_ms( @@ -141,21 +143,36 @@ namespace io { while (running_) { boost::asio::deadline_timer t(io, wait_ms); - initIo(); - if (manager_->connect()) - break; + if (initializeIo()) + { + if (manager_->connect()) + { + initializedIo_ = true; + break; + } + } t.wait(); } + + // Sends commands to the Rx regarding which SBF/NMEA messages it should + // output + // and sets all its necessary corrections-related parameters + if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) + { + node_->log(LogLevel::DEBUG, "Configure Rx."); + configureRx(); + } + node_->log(LogLevel::DEBUG, "Successully connected. Leaving connect() method"); } - void CommIo::initIo() + [[nodiscard]] bool CommunicationCore::initializeIo() { node_->log(LogLevel::DEBUG, "Called initializeIo() method"); boost::smatch match; // In fact: smatch is a typedef of match_results - if (boost::regex_match(node_->settings_.device, match, + if (boost::regex_match(settings_->device, match, boost::regex("(tcp)://(.+):(\\d+)"))) // C++ needs \\d instead of \d: Both mean decimal. // Note that regex_match can be used with a smatch object to store @@ -170,34 +187,36 @@ namespace io { // boost regex. settings_->tcp_ip = match[2]; settings_->tcp_port = match[3]; - manager_->reset(new AsyncManager(node_, &telegramQueue_)); + manager_.reset(new AsyncManager(node_, &telegramQueue_)); } else if (boost::regex_match( - node_->settings_.device, match, + settings_->device, match, boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) { - node_->settings_.read_from_sbf_log = true; - node_->settings_.use_gnss_time = true; + settings_->read_from_sbf_log = true; + settings_->use_gnss_time = true; // connectionThread_ = boost::thread( - // boost::bind(&CommIo::prepareSBFFileReading, this, match[2])); + // boost::bind(&CommunicationCore::prepareSBFFileReading, this, + // match[2])); } else if (boost::regex_match( - node_->settings_.device, match, + settings_->device, match, boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) { - node_->settings_.read_from_pcap = true; - node_->settings_.use_gnss_time = true; + settings_->read_from_pcap = true; + settings_->use_gnss_time = true; // connectionThread_ = boost::thread( - // boost::bind(&CommIo::preparePCAPFileReading, this, match[2])); + // boost::bind(&CommunicationCore::preparePCAPFileReading, this, + // match[2])); - } else if (boost::regex_match(node_->settings_.device, match, + } else if (boost::regex_match(settings_->device, match, boost::regex("(serial):(.+)"))) { std::string proto(match[2]); std::stringstream ss; ss << "Searching for serial port" << proto; node_->log(LogLevel::DEBUG, ss.str()); - node_->settings_.device = proto; - manager_->reset(new AsyncManager(node_, &telegramQueue_)); + settings_->device = proto; + manager_.reset(new AsyncManager(node_, &telegramQueue_)); } else { std::stringstream ss; @@ -216,13 +235,15 @@ namespace io { //! mode. Those characters would then be mingled with the first command we send //! to it in this method and could result in an invalid command. Hence we first //! enter command mode via "SSSSSSSSSS". - void CommIo::configureRx() + void CommunicationCore::configureRx() { node_->log(LogLevel::DEBUG, "Called configureRx() method"); + + if (!initializedIo_) { - // wait for connection - std::mutex::scoped_lock lock(connection_mutex_); - connection_condition_.wait(lock, [this]() { return connected_; }); + node_->log(LogLevel::DEBUG, + "Called configureRx() method but IO is not initialized."); + return; } // Determining communication mode: TCP vs USB/Serial @@ -231,13 +252,14 @@ namespace io { boost::regex_match(settings_->device, match, boost::regex("(tcp)://(.+):(\\d+)")); std::string proto(match[1]); - resetMainPort(); + mainConnectionDescriptor_ = resetMainConnection(); if (proto == "tcp") { - mainPort_ = g_rx_tcp_port; + // mainConnectionDescriptor_ = manager_->getConnectionDescriptor(); } else { - mainPort_ = settings_->rx_serial_port; + // TODO check if rx_serial_portcan be removed + mainConnectionDescriptor_ = settings_->rx_serial_port; // After booting, the Rx sends the characters "x?" to all ports, which // could potentially mingle with our first command. Hence send a // safeguard command "lif", whose potentially false processing is @@ -388,8 +410,9 @@ namespace io { } } std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " + << mainConnectionDescriptor_ << "," << blocks.str() << ", " + << pvt_interval << "\x0D"; send(ss.str()); ++stream; } @@ -415,8 +438,9 @@ namespace io { blocks << " +ReceiverSetup"; std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << rest_interval << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " + << mainConnectionDescriptor_ << "," << blocks.str() << ", " + << rest_interval << "\x0D"; send(ss.str()); ++stream; } @@ -444,8 +468,9 @@ namespace io { } std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << mainPort_ << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; + ss << "sno, Stream" << std::to_string(stream) << ", " + << mainConnectionDescriptor_ << "," << blocks.str() << ", " + << pvt_interval << "\x0D"; send(ss.str()); ++stream; } @@ -814,7 +839,7 @@ namespace io { (settings_->ins_vsm_ros_source == "twist")) { std::string s; - s = "sdio, " + mainPort_ + ", NMEA, +NMEA +SBF\x0D"; + s = "sdio, " + mainConnectionDescriptor_ + ", NMEA, +NMEA +SBF\x0D"; send(s); nmeaActivated_ = true; } @@ -822,29 +847,23 @@ namespace io { node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); } - void CommIo::sendVelocity(const std::string& velNmea) + void CommunicationCore::sendVelocity(const std::string& velNmea) { if (nmeaActivated_) manager_.get()->send(velNmea); } - void CommIo::resetMainPort() + std::string CommunicationCore::resetMainConnection() { - CommSync* cdSync = messageHandler.getCdSync(); - // It is imperative to hold a lock on the mutex "g_cd_mutex" while - // modifying the variable and "g_cd_received". - std::mutex::scoped_lock lock_cd(cdSync.mutex); // Escape sequence (escape from correction mode), ensuring that we can send // our real commands afterwards... std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); + telegramHandler_.resetWaitforMainCd(); manager_.get()->send(cmd); - // We wait for the connection descriptor before we send another command, - // otherwise the latter would not be processed. - cdSync.condition.wait(lock_cd, [cdSync]() { return cdSync.received; }); - cdSync.received = false; + return telegramHandler_.getMainCd(); } - void CommIo::prepareSBFFileReading(std::string file_name) + void CommunicationCore::prepareSBFFileReading(std::string file_name) { try { @@ -855,13 +874,13 @@ namespace io { } catch (std::runtime_error& e) { std::stringstream ss; - ss << "CommIo::initializeSBFFileReading() failed for SBF File" + ss << "CommunicationCore::initializeSBFFileReading() failed for SBF File" << file_name << " due to: " << e.what(); node_->log(LogLevel::ERROR, ss.str()); } } - void CommIo::preparePCAPFileReading(std::string file_name) + void CommunicationCore::preparePCAPFileReading(std::string file_name) { try { @@ -872,15 +891,15 @@ namespace io { } catch (std::runtime_error& e) { std::stringstream ss; - ss << "CommIO::initializePCAPFileReading() failed for SBF File " + ss << "CommunicationCore::initializePCAPFileReading() failed for SBF File " << file_name << " due to: " << e.what(); node_->log(LogLevel::ERROR, ss.str()); } } - void CommIo::initializeSBFFileReading(std::string file_name) + void CommunicationCore::initializeSBFFileReading(std::string file_name) { - node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); + /*node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); std::size_t buffer_size = 8192; uint8_t* to_be_parsed; to_be_parsed = new uint8_t[buffer_size]; @@ -890,56 +909,54 @@ namespace io { { /* Reads binary data using streambuffer iterators. Copies all SBF file content into bin_data. */ - std::vector v_buf((std::istreambuf_iterator(bin_file)), - (std::istreambuf_iterator())); - vec_buf = v_buf; - bin_file.close(); - } else - { - throw std::runtime_error( - "I could not find your file. Or it is corrupted."); - } - // The spec now guarantees that vectors store their elements contiguously. - to_be_parsed = vec_buf.data(); - std::stringstream ss; - ss << "Opened and copied over from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); + /*std::vector v_buf((std::istreambuf_iterator(bin_file)), + (std::istreambuf_iterator())); + vec_buf = v_buf; + bin_file.close(); + } + else + { + throw std::runtime_error("I could not find your file. Or it is corrupted."); + } + // The spec now guarantees that vectors store their elements contiguously. + to_be_parsed = vec_buf.data(); + std::stringstream ss; + ss << "Opened and copied over from " << file_name; + node_->log(LogLevel::DEBUG, ss.str()); - while (running_) // Loop will stop if we are done reading the SBF file + while (running_) // Loop will stop if we are done reading the SBF file + { + try + { + node_->log( + LogLevel::DEBUG, + "Calling read_callback_() method, with number of bytes to be parsed + being " + buffer_size); handlers_.readCallback(node_->getTime(), to_be_parsed, + buffer_size); } catch (std::size_t& parsing_failed_here) { - try - { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - buffer_size); - handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); - } catch (std::size_t& parsing_failed_here) - { - if (to_be_parsed - vec_buf.data() >= - vec_buf.size() * sizeof(uint8_t)) - { - break; - } - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; - } if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) { break; } - to_be_parsed = to_be_parsed + buffer_size; + to_be_parsed = to_be_parsed + parsing_failed_here; + node_->log(LogLevel::DEBUG, + "Parsing_failed_here is " + parsing_failed_here); + continue; } - node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method.."); + if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) + { + break; + } + to_be_parsed = to_be_parsed + buffer_size; + } + node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method..");*/ } - void CommIo::initializePCAPFileReading(std::string file_name) + void CommunicationCore::initializePCAPFileReading(std::string file_name) { - node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() method.."); - pcapReader::buffer_t vec_buf; - pcapReader::PcapDevice device(node_, vec_buf); + /*node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() + method.."); pcapReader::buffer_t vec_buf; pcapReader::PcapDevice + device(node_, vec_buf); if (!device.connect(file_name.c_str())) { @@ -963,10 +980,9 @@ namespace io { { node_->log( LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed being " + - buffer_size); - handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size); - } catch (std::size_t& parsing_failed_here) + "Calling read_callback_() method, with number of bytes to be + parsed being " + buffer_size); handlers_.readCallback(node_->getTime(), + to_be_parsed, buffer_size); } catch (std::size_t& parsing_failed_here) { if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) @@ -987,19 +1003,26 @@ namespace io { } to_be_parsed = to_be_parsed + buffer_size; } - node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method.."); + node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() + method..");*/ + } + + void CommunicationCore::processTelegrams() + { + while (running_) + { + std::shared_ptr telegram; + telegramQueue_.pop(telegram); + if (telegram->type != message_type::EMPTY) + telegramHandler_.handleTelegram(telegram); + } } - void CommIo::send(const std::string& cmd) + void CommunicationCore::send(const std::string& cmd) { - CommSync* responseSync = messageHandler.getResponseSync(); - // It is imperative to hold a lock on the mutex "g_response_mutex" while - // modifying the variable "g_response_received". - std::mutex::scoped_lock lock(responseSync.mutex); - // Determine byte size of cmd and hand over to send() method of manager_ + telegramHandler_.resetWaitForResponse(); manager_.get()->send(cmd); - responseSync.condition.wait(lock, []() { return responseSync.received; }); - responseSync.received = false; + telegramHandler_.waitForResponse(); } } // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 6dd0aba7..fb8ec8ec 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -38,7 +38,9 @@ namespace io { - void TelegramHandler::handle() {} + void TelegramHandler::handleTelegram(const std::shared_ptr& telegram) + { + } void TelegramHandler::handleSbf(const std::shared_ptr& telegram) {} @@ -46,60 +48,45 @@ namespace io { { boost::char_separator sep("\r"); // Carriage Return (CR) typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = rx_message_.messageSize(); // Syntax: new_string_name (const char* s, size_t n); size_t is // either 2 or 8 bytes, depending on your system - std::string block_in_string( - reinterpret_cast(rx_message_.getPosBuffer()), nmea_size); + std::string block_in_string(telegram->message.begin(), + telegram->message.end()); tokenizer tokens(block_in_string, sep); node_->log( LogLevel::DEBUG, - "The NMEA message contains " + std::to_string(nmea_size) + + "The NMEA message contains " + std::to_string(block_in_string.size()) + " bytes and is ready to be parsed. It reads: " + *tokens.begin()); } void TelegramHandler::handleResponse(const std::shared_ptr& telegram) { - std::size_t response_size = rx_message_.messageSize(); - std::string block_in_string( - reinterpret_cast(rx_message_.getPosBuffer()), - response_size); + std::string block_in_string(telegram->message.begin(), + telegram->message.end()); node_->log(LogLevel::DEBUG, "The Rx's response contains " + - std::to_string(response_size) + + std::to_string(block_in_string.size()) + " bytes and reads:\n " + block_in_string); - { - std::mutex::scoped_lock lock(response_sync.mutex); - response_sync.received = true; - lock.unlock(); - response_sync.condition.notify_one(); - } - if (rx_message_.isErrorTelegram()) + + if (telegram->type == message_type::ERROR_RESPONSE) { node_->log( LogLevel::ERROR, "Invalid command just sent to the Rx! The Rx's response contains " + - std::to_string(response_size) + " bytes and reads:\n " + + std::to_string(block_in_string.size()) + " bytes and reads:\n " + block_in_string); } + responsePromise_.set_value(); } void TelegramHandler::handleCd(const std::shared_ptr& telegram) { - std::string cd(reinterpret_cast(rx_message_.getPosBuffer()), 4); - rx_tcp_port = cd; - if (cd_count == 0) + if (mainConnectionDescriptor_.empty()) { + mainConnectionDescriptor_ = + std::string(telegram->message.begin(), telegram->message.end()); node_->log(LogLevel::INFO, - "The connection descriptor for the TCP connection is " + cd); + "The connection descriptor is " + mainConnectionDescriptor_); + cdPromise_.set_value(); } - if (cd_count < 3) - ++cd_count; - if (cd_count == 2) - { - std::mutex::scoped_lock lock(cd_sync.mutex); - cd_sync.received = true; - lock.unlock(); - cd_sync.condition.notify_one(); - }; } } // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 2190aea8..7b67b8a2 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -38,7 +38,7 @@ * @brief The heart of the ROSaic driver: The ROS node that represents it */ -rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_) +rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) { param("activate_debug_log", settings_.activate_debug_log, false); if (settings_.activate_debug_log) @@ -59,18 +59,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_) return; // Initializes Connection - IO_.initializeIo(); - - // Subscribes to all requested Rx messages by adding entries to the C++ multimap - // storing the callback handlers and publishes ROS messages - IO_.defineMessages(); - - // Sends commands to the Rx regarding which SBF/NMEA messages it should output - // and sets all its necessary corrections-related parameters - if (!settings_.read_from_sbf_log && !settings_.read_from_pcap) - { - IO_.configureRx(); - } + IO_.connect(); this->log(LogLevel::DEBUG, "Leaving ROSaicNode() constructor.."); } From dbd52d0c56f2a1834d6589b76943950b8609baf3 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 14 Jan 2023 00:42:36 +0100 Subject: [PATCH 020/170] Rearrange io handling, WIP --- .../communication/async_manager.hpp | 44 ++++++----- .../communication/io.hpp | 79 +++++++++---------- .../communication/telegram_handler.hpp | 2 + .../communication/communication_core.cpp | 11 ++- .../communication/telegram_handler.cpp | 6 +- 5 files changed, 72 insertions(+), 70 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 52ee7c9f..174bc807 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -139,10 +139,11 @@ namespace io { //! Pointer to the node ROSaicNodeBase* node_; IoType ioInterface_; - boost::asio::io_service ioService_; std::atomic running_; std::thread ioThread_; std::thread watchdogThread_; + + std::array buf_; //! Timestamp of receiving buffer Timestamp recvStamp_; //! Telegram @@ -155,8 +156,9 @@ namespace io { AsyncManager::AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue) : node_(node), - ioInterface_(node, &ioService_), telegramQueue_(telegramQueue) + ioInterface_(node), telegramQueue_(telegramQueue) { + node_->log(LogLevel::DEBUG, "AsyncManager created."); } template @@ -165,7 +167,7 @@ namespace io { running_ = false; close(); node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); - ioService_.stop(); + ioInterface_.ioService_.stop(); ioThread_.join(); watchdogThread_.join(); node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); @@ -176,7 +178,7 @@ namespace io { { running_ = true; - if (ioInterface_.connect()) + if (!ioInterface_.connect()) { return false; } @@ -196,7 +198,8 @@ namespace io { return; } - ioService_.post(boost::bind(&AsyncManager::write, this, cmd)); + ioInterface_.ioService_.post( + boost::bind(&AsyncManager::write, this, cmd)); } template @@ -210,13 +213,13 @@ namespace io { template void AsyncManager::close() { - ioService_.post([this]() { ioInterface_.close(); }); + ioInterface_.ioService_.post([this]() { ioInterface_.close(); }); } template void AsyncManager::runIoService() { - ioService_.run(); + ioInterface_.ioService_.run(); node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); } @@ -227,11 +230,11 @@ namespace io { { std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - if (running_ && ioService_.stopped()) + if (running_ && ioInterface_.ioService_.stopped()) { node_->log(LogLevel::DEBUG, "AsyncManager connection lost. Trying to reconnect."); - ioService_.reset(); + ioInterface_.ioService_.reset(); ioThread_.join(); while (!ioInterface_.connect()) std::this_thread::sleep_for(std::chrono::milliseconds(1000)); @@ -244,7 +247,7 @@ namespace io { void AsyncManager::write(const std::string& cmd) { boost::asio::async_write( - ioInterface_.stream_, boost::asio::buffer(cmd.data(), cmd.size()), + *(ioInterface_.stream_), boost::asio::buffer(cmd.data(), cmd.size()), [this, cmd](boost::system::error_code ec, std::size_t /*length*/) { if (!ec) { @@ -276,7 +279,7 @@ namespace io { static_assert(index < 3); boost::asio::async_read( - ioInterface_.stream_, + *(ioInterface_.stream_), boost::asio::buffer(telegram_->message.data() + index, 1), [this](boost::system::error_code ec, std::size_t numBytes) { Timestamp stamp = node_->getTime(); @@ -430,7 +433,7 @@ namespace io { telegram_->message.resize(SBF_HEADER_SIZE); boost::asio::async_read( - ioInterface_.stream_, + *(ioInterface_.stream_), boost::asio::buffer(telegram_->message.data() + 2, SBF_HEADER_SIZE - 2), [this](boost::system::error_code ec, std::size_t numBytes) { if (!ec) @@ -472,7 +475,7 @@ namespace io { telegram_->message.resize(length); boost::asio::async_read( - ioInterface_.stream_, + *(ioInterface_.stream_), boost::asio::buffer(telegram_->message.data() + SBF_HEADER_SIZE, length - SBF_HEADER_SIZE), [this, length](boost::system::error_code ec, std::size_t numBytes) { @@ -520,22 +523,21 @@ namespace io { template void AsyncManager::readStringElements() { - std::array buf; - boost::asio::async_read( - ioInterface_.stream_, boost::asio::buffer(buf.data(), 1), - [this, buf](boost::system::error_code ec, std::size_t numBytes) { + *(ioInterface_.stream_), boost::asio::buffer(buf_.data(), 1), + [this](boost::system::error_code ec, std::size_t numBytes) { if (!ec) { if (numBytes == 1) { - telegram_->message.push_back(buf[0]); - switch (buf[0]) + telegram_->message.push_back(buf_[0]); + + switch (buf_[0]) { case SYNC_BYTE_1: { telegram_.reset(new Telegram); - telegram_->message[0] = buf[0]; + telegram_->message[0] = buf_[0]; telegram_->stamp = node_->getTime(); node_->log( LogLevel::DEBUG, @@ -561,7 +563,7 @@ namespace io { } default: { - readString(); + readStringElements(); break; } } diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 22fca050..5c6f6c88 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -53,26 +53,19 @@ namespace io { class TcpIo { public: - TcpIo(ROSaicNodeBase* node, boost::asio::io_service* ioService) : - node_(node), ioService_(ioService), stream_(*ioService_) - { - } + TcpIo(ROSaicNodeBase* node) : node_(node) {} - ~TcpIo() { stream_.close(); } + ~TcpIo() { stream_->close(); } - void close() { stream_.close(); } + void close() { stream_->close(); } [[nodiscard]] bool connect() { - if (stream_.is_open()) - { - stream_.close(); - } - boost::asio::ip::tcp::resolver::iterator endpointIterator; + try { - boost::asio::ip::tcp::resolver resolver(*ioService_); + boost::asio::ip::tcp::resolver resolver(ioService_); boost::asio::ip::tcp::resolver::query query( node_->getSettings()->tcp_ip, node_->getSettings()->tcp_port); endpointIterator = resolver.resolve(query); @@ -85,11 +78,13 @@ namespace io { return false; } + stream_.reset(new boost::asio::ip::tcp::socket(ioService_)); + try { - stream_.connect(*endpointIterator); + stream_->connect(*endpointIterator); - stream_.set_option(boost::asio::ip::tcp::no_delay(true)); + stream_->set_option(boost::asio::ip::tcp::no_delay(true)); } catch (std::runtime_error& e) { node_->log(LogLevel::ERROR, @@ -103,32 +98,30 @@ namespace io { private: ROSaicNodeBase* node_; - boost::asio::io_service* ioService_; public: - boost::asio::ip::tcp::socket stream_; + boost::asio::io_service ioService_; + std::unique_ptr stream_; }; - class SerialIo + struct SerialIo { - public: - SerialIo(ROSaicNodeBase* node, boost::asio::io_service* ioService) : + SerialIo(ROSaicNodeBase* node) : node_(node), flowcontrol_(node->getSettings()->hw_flow_control), - baudrate_(node->getSettings()->baudrate), stream_(*ioService_) + baudrate_(node->getSettings()->baudrate) { + stream_.reset(new boost::asio::serial_port(ioService_)); } - ~SerialIo() { stream_.close(); } - - void close() { stream_.close(); } + ~SerialIo() { stream_->close(); } - const boost::asio::serial_port& stream() { return stream_; } + void close() { stream_->close(); } [[nodiscard]] bool connect() { - if (stream_.is_open()) + if (stream_->is_open()) { - stream_.close(); + stream_->close(); } bool opened = false; @@ -137,7 +130,7 @@ namespace io { { try { - stream_.open(port_); + stream_->open(port_); opened = true; } catch (const boost::system::system_error& err) { @@ -152,16 +145,16 @@ namespace io { } // No Parity, 8bits data, 1 stop Bit - stream_.set_option(boost::asio::serial_port_base::baud_rate(baudrate_)); - stream_.set_option(boost::asio::serial_port_base::parity( + stream_->set_option(boost::asio::serial_port_base::baud_rate(baudrate_)); + stream_->set_option(boost::asio::serial_port_base::parity( boost::asio::serial_port_base::parity::none)); - stream_.set_option(boost::asio::serial_port_base::character_size(8)); - stream_.set_option(boost::asio::serial_port_base::stop_bits( + stream_->set_option(boost::asio::serial_port_base::character_size(8)); + stream_->set_option(boost::asio::serial_port_base::stop_bits( boost::asio::serial_port_base::stop_bits::one)); - stream_.set_option(boost::asio::serial_port_base::flow_control( + stream_->set_option(boost::asio::serial_port_base::flow_control( boost::asio::serial_port_base::flow_control::none)); - int fd = stream_.native_handle(); + int fd = stream_->native_handle(); termios tio; // Get terminal attribute, follows the syntax // int tcgetattr(int fd, struct termios *termios_p); @@ -203,10 +196,10 @@ namespace io { node_->log(LogLevel::DEBUG, "Initiated current_baudrate object..."); try { - stream_.get_option(current_baudrate); // Note that this sets - // current_baudrate.value() - // often to 115200, since by - // default, all Rx COM ports, + stream_->get_option(current_baudrate); // Note that this sets + // current_baudrate.value() + // often to 115200, since by + // default, all Rx COM ports, // at least for mosaic Rxs, are set to a baudrate of 115200 baud, // using 8 data-bits, no parity and 1 stop-bit. } catch (boost::system::system_error& e) @@ -220,7 +213,7 @@ namespace io { boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. { - stream_.get_option(current_baudrate, e_loop); + stream_->get_option(current_baudrate, e_loop); } while(e_loop); */ return false; @@ -245,7 +238,7 @@ namespace io { // Increment until Baudrate[i] matches current_baudrate. try { - stream_.set_option( + stream_->set_option( boost::asio::serial_port_base::baud_rate(baudrates[i])); } catch (boost::system::system_error& e) { @@ -261,7 +254,7 @@ namespace io { try { - stream_.get_option(current_baudrate); + stream_->get_option(current_baudrate); } catch (boost::system::system_error& e) { @@ -273,7 +266,7 @@ namespace io { boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. { - stream_.get_option(current_baudrate, e_loop); + stream_->get_option(current_baudrate, e_loop); } while(e_loop); */ return false; @@ -293,9 +286,9 @@ namespace io { std::string flowcontrol_; std::string port_; uint32_t baudrate_; - boost::asio::io_service* ioService_; public: - boost::asio::serial_port stream_; + boost::asio::io_service ioService_; + std::unique_ptr stream_; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 6deacf67..373c5802 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -131,6 +131,7 @@ namespace io { //! Returns the connection descriptor void resetWaitforMainCd() { + cdCtr_ = 0; mainConnectionDescriptor_ = std::string(); // reset promise cdPromise_ = std::promise(); @@ -167,6 +168,7 @@ namespace io { //! MessageParser parser // MessageParser messageParser_; //TODO + uint8_t cdCtr_ = 0; std::promise cdPromise_; std::future cdReceived_; std::promise responsePromise_; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 7149d182..55bcb6d2 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -140,18 +140,20 @@ namespace io { boost::asio::io_service io; boost::posix_time::millisec wait_ms( static_cast(settings_->reconnect_delay_s * 1000)); - while (running_) + if (initializeIo()) { - boost::asio::deadline_timer t(io, wait_ms); - if (initializeIo()) + while (running_) { + boost::asio::deadline_timer t(io, wait_ms); + if (manager_->connect()) { initializedIo_ = true; break; } + + t.wait(); } - t.wait(); } // Sends commands to the Rx regarding which SBF/NMEA messages it should @@ -253,6 +255,7 @@ namespace io { boost::regex("(tcp)://(.+):(\\d+)")); std::string proto(match[1]); mainConnectionDescriptor_ = resetMainConnection(); + node_->log(LogLevel::DEBUG, "Cd: " + mainConnectionDescriptor_); if (proto == "tcp") { // mainConnectionDescriptor_ = manager_->getConnectionDescriptor(); diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index fb8ec8ec..bb86ee57 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -80,13 +80,15 @@ namespace io { void TelegramHandler::handleCd(const std::shared_ptr& telegram) { - if (mainConnectionDescriptor_.empty()) + if (cdCtr_ < 2) { mainConnectionDescriptor_ = std::string(telegram->message.begin(), telegram->message.end()); node_->log(LogLevel::INFO, "The connection descriptor is " + mainConnectionDescriptor_); - cdPromise_.set_value(); + ++cdCtr_; + if (cdCtr_ == 2) + cdPromise_.set_value(); } } } // namespace io \ No newline at end of file From 9aaae8892ff0ebd8331e194f992a90cf8a5495cb Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 14 Jan 2023 21:48:51 +0100 Subject: [PATCH 021/170] Add message parser, WIP --- CMakeLists.txt | 28 +- .../abstraction/typedefs.hpp | 4 +- .../communication/async_manager.hpp | 2 +- .../{settings.h => settings.hpp} | 0 .../communication/telegram_handler.hpp | 7 +- .../crc/{crc.h => crc.hpp} | 4 +- .../message_parser.hpp | 27 +- .../parsers/nmea_parsers/gpgga.hpp | 2 +- .../parsers/nmea_parsers/gpgsa.hpp | 2 +- .../parsers/nmea_parsers/gpgsv.hpp | 2 +- .../parsers/nmea_parsers/gprmc.hpp | 2 +- .../sbf_blocks.hpp} | 4 +- ...tring_utilities.h => string_utilities.hpp} | 2 +- .../communication/telegram_handler.cpp | 39 +- src/septentrio_gnss_driver/crc/crc.cpp | 2 +- .../message_parser.cpp | 1453 ++++++++--------- .../parsers/parsing_utilities.cpp | 2 +- .../parsers/string_utilities.cpp | 2 +- 18 files changed, 768 insertions(+), 816 deletions(-) rename include/septentrio_gnss_driver/communication/{settings.h => settings.hpp} (100%) rename include/septentrio_gnss_driver/crc/{crc.h => crc.hpp} (97%) rename include/septentrio_gnss_driver/{communication => parsers}/message_parser.hpp (95%) rename include/septentrio_gnss_driver/{packed_structs/sbf_structs.hpp => parsers/sbf_blocks.hpp} (99%) rename include/septentrio_gnss_driver/parsers/{string_utilities.h => string_utilities.hpp} (99%) rename src/septentrio_gnss_driver/{communication => parsers}/message_parser.cpp (75%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 228784c1..039a1a42 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -178,20 +178,20 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node - src/septentrio_gnss_driver/node/main.cpp - src/septentrio_gnss_driver/node/rosaic_node.cpp - src/septentrio_gnss_driver/parsers/parsing_utilities.cpp - src/septentrio_gnss_driver/parsers/string_utilities.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp - src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp - src/septentrio_gnss_driver/crc/crc.cpp - src/septentrio_gnss_driver/communication/communication_core.cpp - #src/septentrio_gnss_driver/communication/message_parser.cpp - src/septentrio_gnss_driver/communication/telegram_handler.cpp - src/septentrio_gnss_driver/communication/pcap_reader.cpp +add_executable(${PROJECT_NAME}_node + src/septentrio_gnss_driver/communication/communication_core.cpp + src/septentrio_gnss_driver/communication/pcap_reader.cpp + src/septentrio_gnss_driver/communication/telegram_handler.cpp + src/septentrio_gnss_driver/crc/crc.cpp + src/septentrio_gnss_driver/node/main.cpp + src/septentrio_gnss_driver/node/rosaic_node.cpp + src/septentrio_gnss_driver/parsers/message_parser.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp + src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp + src/septentrio_gnss_driver/parsers/parsing_utilities.cpp + src/septentrio_gnss_driver/parsers/string_utilities.cpp ) ## Rename C++ executable without prefix diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 7331e7d4..a80bd511 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -81,8 +81,8 @@ #include #include // Rosaic includes -#include -#include +#include +#include // Timestamp in nanoseconds (Unix epoch) typedef uint64_t Timestamp; diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 174bc807..d2b8cc94 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -64,7 +64,7 @@ #include // ROSaic includes -#include +#include #include // local includes diff --git a/include/septentrio_gnss_driver/communication/settings.h b/include/septentrio_gnss_driver/communication/settings.hpp similarity index 100% rename from include/septentrio_gnss_driver/communication/settings.h rename to include/septentrio_gnss_driver/communication/settings.hpp diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 373c5802..7752b417 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -98,7 +98,7 @@ // ROSaic includes #include #include -// #include +#include /** * @file telegram_handler.hpp @@ -117,8 +117,7 @@ namespace io { public: TelegramHandler(ROSaicNodeBase* node) : - node_(node), - /*message_parser(node),*/ cdReceived_(cdPromise_.get_future()), + node_(node), messageParser_(node), cdReceived_(cdPromise_.get_future()), responseReceived_(responsePromise_.get_future()) { } @@ -166,7 +165,7 @@ namespace io { ROSaicNodeBase* node_; //! MessageParser parser - // MessageParser messageParser_; //TODO + MessageParser messageParser_; uint8_t cdCtr_ = 0; std::promise cdPromise_; diff --git a/include/septentrio_gnss_driver/crc/crc.h b/include/septentrio_gnss_driver/crc/crc.hpp similarity index 97% rename from include/septentrio_gnss_driver/crc/crc.h rename to include/septentrio_gnss_driver/crc/crc.hpp index 2d463ffb..ffacc61c 100644 --- a/include/septentrio_gnss_driver/crc/crc.h +++ b/include/septentrio_gnss_driver/crc/crc.hpp @@ -33,7 +33,7 @@ // ROSaic includes // The following imports structs into which SBF blocks can be unpacked then shipped // to handler functions -#include +#include // C++ libary includes #include #include @@ -41,7 +41,7 @@ namespace crc { /** - * @file crc.h + * @file crc.hpp * @brief Declares the functions to compute and validate the CRC of a buffer * @date 17/08/20 */ diff --git a/include/septentrio_gnss_driver/communication/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp similarity index 95% rename from include/septentrio_gnss_driver/communication/message_parser.hpp rename to include/septentrio_gnss_driver/parsers/message_parser.hpp index b1c92796..3b896165 100644 --- a/include/septentrio_gnss_driver/communication/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -70,21 +70,19 @@ #include // ROSaic includes #include -#include +#include +#include #include #include #include #include -#include +#include /** - * @file rx_message.hpp + * @file message_parser.hpp * @brief Defines a class that reads messages handed over from the circular buffer */ -extern bool g_read_cd; -extern uint32_t g_cd_count; - //! Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's //! Mode field enum TypeOfPVT_Enum @@ -152,18 +150,12 @@ namespace io { } /** - * @brief Put new data + * @brief Parse SBF block * @param[in] telegram Telegram to be parsed */ - void newData(const Telegram& telegram) {} - //! Determines whether data_ points to the NMEA message with ID "ID", e.g. - //! "$GPGGA" - bool isMessage(std::string ID); - - //! Returns the message ID of the message where data_ is pointing at at the - //! moment, SBF identifiers embellished with inverted commas, e.g. "5003" - std::string messageID(); + void parseSbf(const std::shared_ptr& telegram); + private: /** * @brief Publishing function * @param[in] topic String of topic @@ -178,7 +170,6 @@ namespace io { */ void publishTf(const LocalizationMsg& msg); - private: /** * @brief Pointer to the node */ @@ -251,7 +242,7 @@ namespace io { /** * @brief Since GPSFix needs DOP, incoming DOP blocks need to be stored */ - DOP last_dop_; + Dop last_dop_; /** * @brief Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks @@ -375,7 +366,7 @@ namespace io { * @return Timestamp object containing seconds and nanoseconds since last * epoch */ - Timestamp timestampSBF(const uint8_t* data, bool use_gnss_time); + Timestamp timestampSBF(const std::vector& data, bool use_gnss_time); /** * @brief Calculates the timestamp, in the Unix Epoch time format diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp index 6d6fb3c6..f7105bcc 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp @@ -60,7 +60,7 @@ // ROSaic includes #include -#include +#include // Boost and ROS includes #include diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp index 6e46de8a..95f510a3 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp @@ -60,7 +60,7 @@ // ROSaic includes #include -#include +#include // Boost and ROS includes #include diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp index ca1cf77c..9c5ef9c8 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp @@ -60,7 +60,7 @@ // ROSaic includes #include -#include +#include // Boost and ROS includes #include diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp index a54648c8..b6069ef1 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp @@ -58,7 +58,7 @@ // ROSaic includes #include -#include +#include // Boost and ROS includes #include diff --git a/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp similarity index 99% rename from include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp rename to include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 9ca4f181..0933f78d 100644 --- a/include/septentrio_gnss_driver/packed_structs/sbf_structs.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -142,7 +142,7 @@ struct ChannelStatus * @class DOP * @brief Struct for the SBF block "DOP" */ -struct DOP +struct Dop { BlockHeader block_header; @@ -486,7 +486,7 @@ bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& m * @brief Qi based parser for the SBF block "DOP" */ template -bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, DOP& msg) +bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.h b/include/septentrio_gnss_driver/parsers/string_utilities.hpp similarity index 99% rename from include/septentrio_gnss_driver/parsers/string_utilities.h rename to include/septentrio_gnss_driver/parsers/string_utilities.hpp index b4e87d22..df694cde 100644 --- a/include/septentrio_gnss_driver/parsers/string_utilities.h +++ b/include/septentrio_gnss_driver/parsers/string_utilities.hpp @@ -36,7 +36,7 @@ #include /** - * @file string_utilities.h + * @file string_utilities.hpp * @brief Declares lower-level string utility functions used when parsing messages * @date 13/08/20 */ diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index bb86ee57..6812ab1d 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -40,9 +40,46 @@ namespace io { void TelegramHandler::handleTelegram(const std::shared_ptr& telegram) { + switch (telegram->type) + { + case message_type::SBF: + { + handleSbf(telegram); + break; + } + case message_type::NMEA: + { + handleNmea(telegram); + break; + } + case message_type::NMEA_INS: + { + handleNmea(telegram); + break; + } + case message_type::RESPONSE: + { + handleResponse(telegram); + break; + } + case message_type::ERROR_RESPONSE: + { + handleResponse(telegram); + break; + } + default: + { + node_->log(LogLevel::DEBUG, + "TelegramHandler received an unknown message to handle"); + break; + } + } } - void TelegramHandler::handleSbf(const std::shared_ptr& telegram) {} + void TelegramHandler::handleSbf(const std::shared_ptr& telegram) + { + messageParser_.parseSbf(telegram); + } void TelegramHandler::handleNmea(const std::shared_ptr& telegram) { diff --git a/src/septentrio_gnss_driver/crc/crc.cpp b/src/septentrio_gnss_driver/crc/crc.cpp index 4b1714ba..8b794e2d 100644 --- a/src/septentrio_gnss_driver/crc/crc.cpp +++ b/src/septentrio_gnss_driver/crc/crc.cpp @@ -28,7 +28,7 @@ // // ***************************************************************************** -#include +#include #include namespace crc { diff --git a/src/septentrio_gnss_driver/communication/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp similarity index 75% rename from src/septentrio_gnss_driver/communication/message_parser.cpp rename to src/septentrio_gnss_driver/parsers/message_parser.cpp index 309d9e05..9e873a25 100644 --- a/src/septentrio_gnss_driver/communication/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include /** @@ -1100,7 +1100,7 @@ namespace io { if (settings_->septentrio_receiver_type == "gnss") { uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; - switch (type_of_pvt_map[type_of_pvt]) + switch (type_of_pvt) { case evNoPVT: { @@ -1180,7 +1180,7 @@ namespace io { { NavSatFixMsg msg; uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; - switch (type_of_pvt_map[type_of_pvt]) + switch (type_of_pvt) { case evNoPVT: { @@ -1417,7 +1417,7 @@ namespace io { 15; // We extract the first four bits using this mask. uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & status_mask; - switch (type_of_pvt_map[type_of_pvt]) + switch (type_of_pvt) { case evNoPVT: { @@ -1567,7 +1567,7 @@ namespace io { 15; // We extract the first four bits using this mask. uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & status_mask; - switch (type_of_pvt_map[type_of_pvt]) + switch (type_of_pvt) { case evNoPVT: { @@ -1724,10 +1724,11 @@ namespace io { return msg; }; - Timestamp MessageParser::timestampSBF(const uint8_t* data, bool use_gnss_time) + Timestamp MessageParser::timestampSBF(const std::vector& data, + bool use_gnss_time) { - uint32_t tow = parsing_utilities::getTow(data); - uint16_t wnc = parsing_utilities::getWnc(data); + uint32_t tow = parsing_utilities::getTow(data.data()); + uint16_t wnc = parsing_utilities::getWnc(data.data()); return timestampSBF(tow, wnc, use_gnss_time); } @@ -1764,50 +1765,6 @@ namespace io { return time_obj; } - bool MessageParser::isMessage(std::string id) - { - if (this->isNMEA()) - { - boost::char_separator sep(","); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), - nmea_size); - tokenizer tokens(block_in_string, sep); - if (*tokens.begin() == id) - { - return true; - } else - { - return false; - } - } else - { - return false; - } - } - - std::string MessageParser::messageID() - { - if (this->isSBF()) - { - std::stringstream ss; - ss << parsing_utilities::getId(data_); - return ss.str(); - } - if (this->isNMEA()) - { - boost::char_separator sep(","); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), - nmea_size); - tokenizer tokens(block_in_string, sep); - return *tokens.begin(); - } - return std::string(); // less CPU work than return ""; - } - /** * If GNSS time is used, Publishing is only done with valid leap seconds */ @@ -1865,23 +1822,24 @@ namespace io { * allowed e.g. for GGA seems to be 89 on a mosaic-x5. Luckily, when parsing we * do not care since we just search for \\. */ - bool MessageParser::parseSbf(const std::shared_ptr& telegram) + void MessageParser::parseSbf(const std::shared_ptr& telegram) { switch (telegram->sbfId) { case PVT_CARTESIAN: // Position and velocity in XYZ { PVTCartesianMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PVTCartesianParser(node_, dvec.begin(), dvec.end(), msg)) + + if (!PVTCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PVTCartesian"); break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -1894,21 +1852,17 @@ namespace io { case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame // (ENU frame) { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PVTGeodeticParser(node_, dvec.begin(), dvec.end(), - last_pvtgeodetic_)) + if (!PVTGeodeticParser(node_, telegram->message.begin(), + telegram->message.end(), last_pvtgeodetic_)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PVTGeodetic"); break; } last_pvtgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_gpsfix_ = true; - pvtgeodetic_has_arrived_navsatfix_ = true; - pvtgeodetic_has_arrived_pose_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -1921,16 +1875,17 @@ namespace io { case BASE_VECTOR_CART: { BaseVectorCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!BaseVectorCartParser(node_, dvec.begin(), dvec.end(), msg)) + + if (!BaseVectorCartParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in BaseVectorCart"); break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -1943,16 +1898,17 @@ namespace io { case BASE_VECTOR_GEOD: { BaseVectorGeodMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!BaseVectorGeodParser(node_, dvec.begin(), dvec.end(), msg)) + + if (!BaseVectorGeodParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in BaseVectorGeod"); break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -1965,16 +1921,17 @@ namespace io { case POS_COV_CARTESIAN: { PosCovCartesianMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PosCovCartesianParser(node_, dvec.begin(), dvec.end(), msg)) + + if (!PosCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PosCovCartesian"); break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -1986,24 +1943,18 @@ namespace io { } case POS_COV_GEODETIC: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!PosCovGeodeticParser(node_, dvec.begin(), dvec.end(), - last_poscovgeodetic_)) + + if (!PosCovGeodeticParser(node_, telegram->message.begin(), + telegram->message.end(), last_poscovgeodetic_)) { - poscovgeodetic_has_arrived_gpsfix_ = false; - poscovgeodetic_has_arrived_navsatfix_ = false; - poscovgeodetic_has_arrived_pose_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PosCovGeodetic"); break; } last_poscovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); - poscovgeodetic_has_arrived_gpsfix_ = true; - poscovgeodetic_has_arrived_navsatfix_ = true; - poscovgeodetic_has_arrived_pose_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -2015,22 +1966,19 @@ namespace io { } case ATT_EULER: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!AttEulerParser(node_, dvec.begin(), dvec.end(), last_atteuler_, + + if (!AttEulerParser(node_, telegram->message.begin(), + telegram->message.end(), last_atteuler_, settings_->use_ros_axis_orientation)) { - atteuler_has_arrived_gpsfix_ = false; - atteuler_has_arrived_pose_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in AttEuler"); break; } last_atteuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_atteuler_.header.stamp = timestampToRos(time_obj); - atteuler_has_arrived_gpsfix_ = true; - atteuler_has_arrived_pose_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -2042,23 +1990,19 @@ namespace io { } case ATT_COV_EULER: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!AttCovEulerParser(node_, dvec.begin(), dvec.end(), - last_attcoveuler_, + + if (!AttCovEulerParser(node_, telegram->message.begin(), + telegram->message.end(), last_attcoveuler_, settings_->use_ros_axis_orientation)) { - attcoveuler_has_arrived_gpsfix_ = false; - attcoveuler_has_arrived_pose_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in AttCovEuler"); break; } last_attcoveuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_attcoveuler_.header.stamp = timestampToRos(time_obj); - attcoveuler_has_arrived_gpsfix_ = true; - attcoveuler_has_arrived_pose_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -2071,12 +2015,11 @@ namespace io { case INS_NAV_CART: // Position, velocity and orientation in cartesian // coordinate frame (ENU frame) { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), last_insnavcart_, + + if (!INSNavCartParser(node_, telegram->message.begin(), + telegram->message.end(), last_insnavcart_, settings_->use_ros_axis_orientation)) { - insnavgeod_has_arrived_localization_ecef_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in INSNavCart"); break; @@ -2088,9 +2031,9 @@ namespace io { { last_insnavcart_.header.frame_id = settings_->frame_id; } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_insnavcart_.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ecef_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -2102,16 +2045,11 @@ namespace io { case INS_NAV_GEOD: // Position, velocity and orientation in geodetic // coordinate frame (ENU frame) { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), last_insnavgeod_, + + if (!INSNavGeodParser(node_, telegram->message.begin(), + telegram->message.end(), last_insnavgeod_, settings_->use_ros_axis_orientation)) { - insnavgeod_has_arrived_gpsfix_ = false; - insnavgeod_has_arrived_navsatfix_ = false; - insnavgeod_has_arrived_pose_ = false; - insnavgeod_has_arrived_localization_ = false; - insnavgeod_has_arrived_localization_ecef_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in INSNavGeod"); break; @@ -2123,13 +2061,9 @@ namespace io { { last_insnavgeod_.header.frame_id = settings_->frame_id; } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_insnavgeod_.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_gpsfix_ = true; - insnavgeod_has_arrived_navsatfix_ = true; - insnavgeod_has_arrived_pose_ = true; - insnavgeod_has_arrived_localization_ = true; - insnavgeod_has_arrived_localization_ecef_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -2148,9 +2082,9 @@ namespace io { case IMU_SETUP: // IMU orientation and lever arm { IMUSetupMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!IMUSetupParser(node_, dvec.begin(), dvec.end(), msg, + + if (!IMUSetupParser(node_, telegram->message.begin(), + telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { node_->log(LogLevel::ERROR, @@ -2158,7 +2092,8 @@ namespace io { break; } msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -2172,9 +2107,9 @@ namespace io { case VEL_SENSOR_SETUP: // Velocity sensor lever arm { VelSensorSetupMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!VelSensorSetupParser(node_, dvec.begin(), dvec.end(), msg, + + if (!VelSensorSetupParser(node_, telegram->message.begin(), + telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { node_->log(LogLevel::ERROR, @@ -2182,7 +2117,8 @@ namespace io { break; } msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -2197,9 +2133,9 @@ namespace io { // cartesian coordinate frame (ENU frame) { INSNavCartMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, + + if (!INSNavCartParser(node_, telegram->message.begin(), + telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { node_->log( @@ -2214,7 +2150,8 @@ namespace io { { msg.header.frame_id = settings_->frame_id; } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -2228,9 +2165,9 @@ namespace io { case EXT_EVENT_INS_NAV_GEOD: { INSNavGeodMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), msg, + + if (!INSNavGeodParser(node_, telegram->message.begin(), + telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { node_->log( @@ -2245,7 +2182,8 @@ namespace io { { msg.header.frame_id = settings_->frame_id; } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -2258,19 +2196,20 @@ namespace io { case EXT_SENSOR_MEAS: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); + bool hasImuMeas = false; - if (!ExtSensorMeasParser( - node_, dvec.begin(), dvec.end(), last_extsensmeas_, - settings_->use_ros_axis_orientation, hasImuMeas)) + if (!ExtSensorMeasParser(node_, telegram->message.begin(), + telegram->message.end(), last_extsensmeas_, + settings_->use_ros_axis_orientation, + hasImuMeas)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ExtSensorMeas"); break; } last_extsensmeas_.header.frame_id = settings_->imu_frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_extsensmeas_.header.stamp = timestampToRos(time_obj); // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) @@ -2298,66 +2237,60 @@ namespace io { } case CHANNEL_STATUS: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ChannelStatusParser(node_, dvec.begin(), dvec.end(), - last_channelstatus_)) + + if (!ChannelStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_channelstatus_)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ChannelStatus"); break; } - channelstatus_has_arrived_gpsfix_ = true; break; } case MEAS_EPOCH: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!MeasEpochParser(node_, dvec.begin(), dvec.end(), last_measepoch_)) + + if (!MeasEpochParser(node_, telegram->message.begin(), + telegram->message.end(), last_measepoch_)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in MeasEpoch"); break; } last_measepoch_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_measepoch_.header.stamp = timestampToRos(time_obj); - measepoch_has_arrived_gpsfix_ = true; if (settings_->publish_measepoch) publish("/measepoch", last_measepoch_); break; } case DOP: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!DOPParser(node_, dvec.begin(), dvec.end(), last_dop_)) + + if (!DOPParser(node_, telegram->message.begin(), telegram->message.end(), + last_dop_)) { - dop_has_arrived_gpsfix_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in DOP"); break; } - dop_has_arrived_gpsfix_ = true; break; } case VEL_COV_GEODETIC: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!VelCovGeodeticParser(node_, dvec.begin(), dvec.end(), - last_velcovgeodetic_)) + + if (!VelCovGeodeticParser(node_, telegram->message.begin(), + telegram->message.end(), last_velcovgeodetic_)) { - velcovgeodetic_has_arrived_gpsfix_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in VelCovGeodetic"); break; } last_velcovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); + Timestamp time_obj = + timestampSBF(telegram->message, settings_->use_gnss_time); last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); - velcovgeodetic_has_arrived_gpsfix_ = true; // Wait as long as necessary (only when reading from SBF/PCAP file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -2374,39 +2307,32 @@ namespace io { } case RECEIVER_STATUS: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverStatusParser(node_, dvec.begin(), dvec.end(), - last_receiverstatus_)) + + if (!ReceiverStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_receiverstatus_)) { - receiverstatus_has_arrived_diagnostics_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ReceiverStatus"); break; } - receiverstatus_has_arrived_diagnostics_ = true; break; } case QUALITY_IND: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!QualityIndParser(node_, dvec.begin(), dvec.end(), last_qualityind_)) + + if (!QualityIndParser(node_, telegram->message.begin(), + telegram->message.end(), last_qualityind_)) { - qualityind_has_arrived_diagnostics_ = false; node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in QualityInd"); break; } - qualityind_has_arrived_diagnostics_ = true; break; } case RECEIVER_SETUP: { - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverSetupParser(node_, dvec.begin(), dvec.end(), - last_receiversetup_)) + if (!ReceiverSetupParser(node_, telegram->message.begin(), + telegram->message.end(), last_receiversetup_)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ReceiverSetup"); @@ -2486,9 +2412,9 @@ namespace io { case RECEIVER_TIME: { ReceiverTimeMsg msg; - std::vector dvec(data_, - data_ + parsing_utilities::getLength(data_)); - if (!ReceiverTimeParser(node_, dvec.begin(), dvec.end(), msg)) + + if (!ReceiverTimeParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) { node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ReceiverTime"); @@ -2504,301 +2430,300 @@ namespace io { } // Many more to be implemented... } - return true; } void processjointMessages() { - - case evDiagnosticArray: - { - DiagnosticArrayMsg msg; - try - { - msg = DiagnosticArrayCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "DiagnosticArrayMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - msg.header.frame_id = settings_->frame_id; - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - receiverstatus_has_arrived_diagnostics_ = false; - qualityind_has_arrived_diagnostics_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/diagnostics", msg); - break; - } - case evLocalization: - { - LocalizationMsg msg; - try - { - msg = LocalizationUtmCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); - break; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_localization) - publish("/localization", msg); - if (settings_->publish_tf) - publishTf(msg); - break; - } - case evLocalizationEcef: - { - LocalizationMsg msg; - try - { - msg = LocalizationEcefCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what())); - break; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ecef_ = false; - insnavgeod_has_arrived_localization_ecef_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_localization_ecef) - publish("/localization_ecef", msg); - if (settings_->publish_tf_ecef) - publishTf(msg); - break; - } - - if (settings_->septentrio_receiver_type == "gnss") - { - case evNavSatFix: - { - NavSatFixMsg msg; - try - { - msg = NavSatFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "NavSatFixMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_navsatfix_ = false; - poscovgeodetic_has_arrived_navsatfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/navsatfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSNavSatFix: - { - NavSatFixMsg msg; - try - { - msg = NavSatFixCallback(); - } catch (std::runtime_error& e) + /* + case evDiagnosticArray: { - node_->log(LogLevel::DEBUG, - "NavSatFixMsg: " + std::string(e.what())); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_navsatfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/navsatfix", msg); - break; - } - } - - if (settings_->septentrio_receiver_type == "gnss") - { - case evGPSFix: - { - GPSFixMsg msg; - try - { - msg = GPSFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "GPSFixMsg: " + std::string(e.what())); + DiagnosticArrayMsg msg; + try + { + msg = DiagnosticArrayCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, + "DiagnosticArrayMsg: " + std::string(e.what())); + break; + } + if (settings_->septentrio_receiver_type == "gnss") + { + msg.header.frame_id = settings_->frame_id; + } + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } + } + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + receiverstatus_has_arrived_diagnostics_ = false; + qualityind_has_arrived_diagnostics_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/diagnostics", msg); break; } - msg.header.frame_id = settings_->frame_id; - msg.status.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - msg.status.header.stamp = timestampToRos(time_obj); - ++count_gpsfix_; - channelstatus_has_arrived_gpsfix_ = false; - measepoch_has_arrived_gpsfix_ = false; - dop_has_arrived_gpsfix_ = false; - pvtgeodetic_has_arrived_gpsfix_ = false; - poscovgeodetic_has_arrived_gpsfix_ = false; - velcovgeodetic_has_arrived_gpsfix_ = false; - atteuler_has_arrived_gpsfix_ = false; - attcoveuler_has_arrived_gpsfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) + case evLocalization: { - wait(time_obj); - } - publish("/gpsfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSGPSFix: - { - GPSFixMsg msg; - try - { - msg = GPSFixCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "GPSFixMsg: " + std::string(e.what())); + LocalizationMsg msg; + try + { + msg = LocalizationUtmCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, "LocalizationMsg: " + + std::string(e.what())); break; + } + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_localization) + publish("/localization", msg); + if (settings_->publish_tf) + publishTf(msg); break; } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - msg.status.header.frame_id = msg.header.frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - msg.status.header.stamp = timestampToRos(time_obj); - ++count_gpsfix_; - channelstatus_has_arrived_gpsfix_ = false; - measepoch_has_arrived_gpsfix_ = false; - dop_has_arrived_gpsfix_ = false; - insnavgeod_has_arrived_gpsfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpsfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - case evPoseWithCovarianceStamped: - { - PoseWithCovarianceStampedMsg msg; - try - { - msg = PoseWithCovarianceStampedCallback(); - } catch (std::runtime_error& e) + case evLocalizationEcef: { - node_->log(LogLevel::DEBUG, - "PoseWithCovarianceStampedMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_pose_ = false; - poscovgeodetic_has_arrived_pose_ = false; - atteuler_has_arrived_pose_ = false; - attcoveuler_has_arrived_pose_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pose", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSPoseWithCovarianceStamped: - { - PoseWithCovarianceStampedMsg msg; - try - { - msg = PoseWithCovarianceStampedCallback(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "PoseWithCovarianceStampedMsg: " + std::string(e.what())); + LocalizationMsg msg; + try + { + msg = LocalizationEcefCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, "LocalizationMsg: " + + std::string(e.what())); break; + } + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_localization_ecef_ = false; + insnavgeod_has_arrived_localization_ecef_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + if (settings_->publish_localization_ecef) + publish("/localization_ecef", msg); + if (settings_->publish_tf_ecef) + publishTf(msg); break; } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_pose_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pose", msg); - break; - } - } + + if (settings_->septentrio_receiver_type == "gnss") + { + case evNavSatFix: + { + NavSatFixMsg msg; + try + { + msg = NavSatFixCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, + "NavSatFixMsg: " + std::string(e.what())); + break; + } + msg.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + pvtgeodetic_has_arrived_navsatfix_ = false; + poscovgeodetic_has_arrived_navsatfix_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP + file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/navsatfix", msg); + break; + } + } + if (settings_->septentrio_receiver_type == "ins") + { + case evINSNavSatFix: + { + NavSatFixMsg msg; + try + { + msg = NavSatFixCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, + "NavSatFixMsg: " + std::string(e.what())); + break; + } + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_navsatfix_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP + file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/navsatfix", msg); + break; + } + } + + if (settings_->septentrio_receiver_type == "gnss") + { + case evGPSFix: + { + GPSFixMsg msg; + try + { + msg = GPSFixCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, "GPSFixMsg: " + + std::string(e.what())); break; + } + msg.header.frame_id = settings_->frame_id; + msg.status.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + msg.status.header.stamp = timestampToRos(time_obj); + ++count_gpsfix_; + channelstatus_has_arrived_gpsfix_ = false; + measepoch_has_arrived_gpsfix_ = false; + dop_has_arrived_gpsfix_ = false; + pvtgeodetic_has_arrived_gpsfix_ = false; + poscovgeodetic_has_arrived_gpsfix_ = false; + velcovgeodetic_has_arrived_gpsfix_ = false; + atteuler_has_arrived_gpsfix_ = false; + attcoveuler_has_arrived_gpsfix_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP + file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/gpsfix", msg); + break; + } + } + if (settings_->septentrio_receiver_type == "ins") + { + case evINSGPSFix: + { + GPSFixMsg msg; + try + { + msg = GPSFixCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, "GPSFixMsg: " + + std::string(e.what())); break; + } + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } + msg.status.header.frame_id = msg.header.frame_id; + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + msg.status.header.stamp = timestampToRos(time_obj); + ++count_gpsfix_; + channelstatus_has_arrived_gpsfix_ = false; + measepoch_has_arrived_gpsfix_ = false; + dop_has_arrived_gpsfix_ = false; + insnavgeod_has_arrived_gpsfix_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP + file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/gpsfix", msg); + break; + } + } + if (settings_->septentrio_receiver_type == "gnss") + { + case evPoseWithCovarianceStamped: + { + PoseWithCovarianceStampedMsg msg; + try + { + msg = PoseWithCovarianceStampedCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, + "PoseWithCovarianceStampedMsg: " + + std::string(e.what())); break; + } + msg.header.frame_id = settings_->frame_id; + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + pvtgeodetic_has_arrived_pose_ = false; + poscovgeodetic_has_arrived_pose_ = false; + atteuler_has_arrived_pose_ = false; + attcoveuler_has_arrived_pose_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP + file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/pose", msg); + break; + } + } + if (settings_->septentrio_receiver_type == "ins") + { + case evINSPoseWithCovarianceStamped: + { + PoseWithCovarianceStampedMsg msg; + try + { + msg = PoseWithCovarianceStampedCallback(); + } catch (std::runtime_error& e) + { + node_->log(LogLevel::DEBUG, + "PoseWithCovarianceStampedMsg: " + + std::string(e.what())); break; + } + if (settings_->ins_use_poi) + { + msg.header.frame_id = settings_->poi_frame_id; + } else + { + msg.header.frame_id = settings_->frame_id; + } + Timestamp time_obj = timestampSBF(telegram->message, + settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); + insnavgeod_has_arrived_pose_ = false; + // Wait as long as necessary (only when reading from SBF/PCAP + file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/pose", msg); + break; + } + }*/ } void MessageParser::wait(Timestamp time_obj) @@ -2826,300 +2751,300 @@ namespace io { } void parseNMEA() - { - case evGPST: - { - TimeReferenceMsg msg; - Timestamp time_obj = - timestampSBF(data_, true); // We need the GPS time, hence true - msg.time_ref = timestampToRos(time_obj); - msg.source = "GPST"; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpst", msg); - break; - } - case evGPGGA: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - // No kept delimiters, hence "". Also, we specify that empty tokens - // should show up in the output when two delimiters are next to each - // other. Hence we also append the checksum part of the GGA message to - // "body" below, though it is not parsed. - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpggaParser::parseASCII - NMEASentence gga_message(id, body); - GpggaMsg msg; - Timestamp time_obj = node_->getTime(); - GpggaParser parser_obj; - try - { - msg = parser_obj.parseASCII(gga_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what())); - break; - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgga", msg); - break; + { /* + case evGPST: + { + TimeReferenceMsg msg; + Timestamp time_obj = + timestampSBF(telegram->message, true); // We need the GPS time, hence + true msg.time_ref = timestampToRos(time_obj); msg.source = "GPST"; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/gpst", msg); + break; + } + case evGPGGA: + { + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + // No kept delimiters, hence "". Also, we specify that empty tokens + // should show up in the output when two delimiters are next to each + // other. Hence we also append the checksum part of the GGA message to + // "body" below, though it is not parsed. + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GpggaParser::parseASCII + NMEASentence gga_message(id, body); + GpggaMsg msg; + Timestamp time_obj = node_->getTime(); + GpggaParser parser_obj; + try + { + msg = parser_obj.parseASCII(gga_message, settings_->frame_id, + settings_->use_gnss_time, time_obj); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what())); + break; + } + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgga", msg); + break; + } + case evGPRMC: + { + Timestamp time_obj = node_->getTime(); + + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GprmcParser::parseASCII + NMEASentence rmc_message(id, body); + GprmcMsg msg; + GprmcParser parser_obj; + try + { + msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, + settings_->use_gnss_time, time_obj); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what())); + break; + } + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gprmc", msg); + break; + } + case evGPGSA: + { + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GpgsaParser::parseASCII + NMEASentence gsa_message(id, body); + GpgsaMsg msg; + GpgsaParser parser_obj; + try + { + msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, + settings_->use_gnss_time, node_->getTime()); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what())); + break; + } + if (settings_->septentrio_receiver_type == "gnss") + { + Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + if (settings_->septentrio_receiver_type == "ins") + { + Timestamp time_obj; + time_obj = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgsa", msg); + break; + } + case evGPGSV: + case evGLGSV: + case evGAGSV: + { + boost::char_separator sep("\r"); + typedef boost::tokenizer> tokenizer; + std::size_t nmea_size = this->messageSize(); + std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); + + std::string id = this->messageID(); + std::string one_message = *tokens.begin(); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + tokenizer tokens_2(one_message, sep_2); + std::vector body; + for (tokenizer::iterator tok_iter = tokens_2.begin(); + tok_iter != tokens_2.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + // Create NmeaSentence struct to pass to GpgsvParser::parseASCII + NMEASentence gsv_message(id, body); + GpgsvMsg msg; + GpgsvParser parser_obj; + try + { + msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, + settings_->use_gnss_time, node_->getTime()); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what())); + break; + } + if (settings_->septentrio_receiver_type == "gnss") + { + Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + if (settings_->septentrio_receiver_type == "ins") + { + Timestamp time_obj; + time_obj = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgsv", msg); + break; + } + + bool MessageParser::gnss_gpsfix_complete(uint32_t id) + { + std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, + measepoch_has_arrived_gpsfix_, + dop_has_arrived_gpsfix_, + pvtgeodetic_has_arrived_gpsfix_, + poscovgeodetic_has_arrived_gpsfix_, + velcovgeodetic_has_arrived_gpsfix_, + atteuler_has_arrived_gpsfix_, + attcoveuler_has_arrived_gpsfix_}; + return allTrue(gpsfix_vec, id); + } + + bool MessageParser::ins_gpsfix_complete(uint32_t id) + { + std::vector gpsfix_vec = { + channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, + dop_has_arrived_gpsfix_, insnavgeod_has_arrived_gpsfix_}; + return allTrue(gpsfix_vec, id); + } + + bool MessageParser::gnss_navsatfix_complete(uint32_t id) + { + std::vector navsatfix_vec = { + pvtgeodetic_has_arrived_navsatfix_, + poscovgeodetic_has_arrived_navsatfix_}; + return allTrue(navsatfix_vec, id); + } + + bool MessageParser::ins_navsatfix_complete(uint32_t id) + { + std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; + return allTrue(navsatfix_vec, id); + } + + bool MessageParser::gnss_pose_complete(uint32_t id) + { + std::vector pose_vec = { + pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, + atteuler_has_arrived_pose_, attcoveuler_has_arrived_pose_}; + return allTrue(pose_vec, id); + } + + bool MessageParser::ins_pose_complete(uint32_t id) + { + std::vector pose_vec = {insnavgeod_has_arrived_pose_}; + return allTrue(pose_vec, id); + } + + bool MessageParser::diagnostics_complete(uint32_t id) + { + std::vector diagnostics_vec = { + receiverstatus_has_arrived_diagnostics_, + qualityind_has_arrived_diagnostics_}; + return allTrue(diagnostics_vec, id); + } + + bool MessageParser::ins_localization_complete(uint32_t id) + { + std::vector loc_vec = {insnavgeod_has_arrived_localization_}; + return allTrue(loc_vec, id); + } + + bool MessageParser::ins_localization_ecef_complete(uint32_t id) + { + std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, + insnavcart_has_arrived_localization_ecef_}; + return allTrue(loc_vec, id); + } + + bool MessageParser::allTrue(std::vector & vec, uint32_t id) + { + vec.erase(vec.begin() + id); + // Checks whether all entries in vec are true + return (std::all_of(vec.begin(), vec.end(), [](bool v) { return v; }) == + true); + }*/ } - case evGPRMC: - { - Timestamp time_obj = node_->getTime(); - - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GprmcParser::parseASCII - NMEASentence rmc_message(id, body); - GprmcMsg msg; - GprmcParser parser_obj; - try - { - msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what())); - break; - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gprmc", msg); - break; - } - case evGPGSA: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsaParser::parseASCII - NMEASentence gsa_message(id, body); - GpgsaMsg msg; - GpgsaParser parser_obj; - try - { - msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsa", msg); - break; - } - case evGPGSV: - case evGLGSV: - case evGAGSV: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(data_), nmea_size); - tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsvParser::parseASCII - NMEASentence gsv_message(id, body); - GpgsvMsg msg; - GpgsvParser parser_obj; - try - { - msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsv", msg); - break; - } - - bool MessageParser::gnss_gpsfix_complete(uint32_t id) - { - std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, - measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, - pvtgeodetic_has_arrived_gpsfix_, - poscovgeodetic_has_arrived_gpsfix_, - velcovgeodetic_has_arrived_gpsfix_, - atteuler_has_arrived_gpsfix_, - attcoveuler_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); - } - - bool MessageParser::ins_gpsfix_complete(uint32_t id) - { - std::vector gpsfix_vec = { - channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, insnavgeod_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); - } - - bool MessageParser::gnss_navsatfix_complete(uint32_t id) - { - std::vector navsatfix_vec = { - pvtgeodetic_has_arrived_navsatfix_, - poscovgeodetic_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); - } - - bool MessageParser::ins_navsatfix_complete(uint32_t id) - { - std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); - } - - bool MessageParser::gnss_pose_complete(uint32_t id) - { - std::vector pose_vec = { - pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, - atteuler_has_arrived_pose_, attcoveuler_has_arrived_pose_}; - return allTrue(pose_vec, id); - } - - bool MessageParser::ins_pose_complete(uint32_t id) - { - std::vector pose_vec = {insnavgeod_has_arrived_pose_}; - return allTrue(pose_vec, id); - } - - bool MessageParser::diagnostics_complete(uint32_t id) - { - std::vector diagnostics_vec = { - receiverstatus_has_arrived_diagnostics_, - qualityind_has_arrived_diagnostics_}; - return allTrue(diagnostics_vec, id); - } - - bool MessageParser::ins_localization_complete(uint32_t id) - { - std::vector loc_vec = {insnavgeod_has_arrived_localization_}; - return allTrue(loc_vec, id); - } - - bool MessageParser::ins_localization_ecef_complete(uint32_t id) - { - std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, - insnavcart_has_arrived_localization_ecef_}; - return allTrue(loc_vec, id); - } - - bool MessageParser::allTrue(std::vector & vec, uint32_t id) - { - vec.erase(vec.begin() + id); - // Checks whether all entries in vec are true - return (std::all_of(vec.begin(), vec.end(), [](bool v) { return v; }) == - true); - } - } // namespace io +} // namespace io diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index 1cc18ac4..37aa2e02 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -30,7 +30,7 @@ // ROSaic includes #include -#include +#include // C++ library includes #include // Boost diff --git a/src/septentrio_gnss_driver/parsers/string_utilities.cpp b/src/septentrio_gnss_driver/parsers/string_utilities.cpp index 040b539c..a3d8d8ae 100644 --- a/src/septentrio_gnss_driver/parsers/string_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/string_utilities.cpp @@ -29,7 +29,7 @@ // ***************************************************************************** // ROSaic includes -#include +#include // C++ library includes #include #include From aed817c0f4e6ff5e25358d7af7fb3a2c31291d1c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 00:01:31 +0100 Subject: [PATCH 022/170] Change syncronization to semaphore --- .../communication/async_manager.hpp | 22 +++++++- .../communication/io.hpp | 13 +++++ .../communication/telegram.hpp | 2 + .../communication/telegram_handler.hpp | 55 +++++++++++-------- .../communication/communication_core.cpp | 2 - .../communication/telegram_handler.cpp | 44 +++++++++++---- .../parsers/message_parser.cpp | 5 +- 7 files changed, 105 insertions(+), 38 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index d2b8cc94..c78e8f67 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -343,9 +343,12 @@ namespace io { } default: { + std::stringstream ss; + ss << std::hex << currByte; node_->log( LogLevel::DEBUG, - "AsyncManager sync 1 read fault, should never come here."); + "AsyncManager sync byte 2 read fault, should never come here.. Received byte was " + + ss.str()); resync(); break; } @@ -389,11 +392,22 @@ namespace io { resync(); break; } + case RESPONSE_SYNC_BYTE_3a: + { + if (telegram_->type == message_type::RESPONSE) + readString(); + else + resync(); + break; + } default: { + std::stringstream ss; + ss << std::hex << currByte; node_->log( LogLevel::DEBUG, - "AsyncManager sync 2 read fault, should never come here."); + "AsyncManager sync byte 3 read fault, should never come here. Received byte was " + + ss.str()); resync(); break; } @@ -531,6 +545,10 @@ namespace io { if (numBytes == 1) { telegram_->message.push_back(buf_[0]); + /*node_->log(LogLevel::DEBUG, + "Buffer: " + + std::string(telegram_->message.begin(), + telegram_->message.end()));*/ switch (buf_[0]) { diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 5c6f6c88..b49a07fc 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -80,11 +80,19 @@ namespace io { stream_.reset(new boost::asio::ip::tcp::socket(ioService_)); + node_->log(LogLevel::INFO, "Connecting to tcp://" + + node_->getSettings()->tcp_ip + ":" + + node_->getSettings()->tcp_port + "..."); + try { stream_->connect(*endpointIterator); stream_->set_option(boost::asio::ip::tcp::no_delay(true)); + + node_->log(LogLevel::INFO, + "Connected to " + endpointIterator->host_name() + ":" + + endpointIterator->service_name() + "."); } catch (std::runtime_error& e) { node_->log(LogLevel::ERROR, @@ -130,6 +138,11 @@ namespace io { { try { + node_->log(LogLevel::INFO, + "Connecting serially to device" + + node_->getSettings()->device + + ", targeted baudrate: " + + std::to_string(node_->getSettings()->baudrate)); stream_->open(port_); opened = true; } catch (const boost::system::system_error& err) diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 5d1bdc1c..6df4f10e 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -56,6 +56,8 @@ static const uint8_t NMEA_INS_SYNC_BYTE_3 = 0x4E; static const uint8_t RESPONSE_SYNC_BYTE_2 = 0x52; //! 0x3A is ASCII for : - 3rd byte in the response message from the Rx static const uint8_t RESPONSE_SYNC_BYTE_3 = 0x3A; +//! 0x21 is ASCII for ! 3rd byte in the response message from the Rx +static const uint8_t RESPONSE_SYNC_BYTE_3a = 0x21; //! 0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the //! command was invalid static const uint8_t ERROR_SYNC_BYTE_3 = 0x3F; diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 7752b417..31137651 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -107,6 +107,34 @@ */ namespace io { + class Semaphore + { + public: + Semaphore() : block_(true) {} + + void notify() + { + std::unique_lock lock(mtx_); + block_ = false; + cv_.notify_one(); + } + + void wait() + { + std::unique_lock lock(mtx_); + while (block_) + { + cv_.wait(lock); + } + block_ = true; + } + + private: + std::mutex mtx_; + std::condition_variable cv_; + bool block_; + }; + /** * @class TelegramHandler * @brief Represents ensemble of (to be constructed) ROS messages, to be handled @@ -116,11 +144,7 @@ namespace io { { public: - TelegramHandler(ROSaicNodeBase* node) : - node_(node), messageParser_(node), cdReceived_(cdPromise_.get_future()), - responseReceived_(responsePromise_.get_future()) - { - } + TelegramHandler(ROSaicNodeBase* node) : node_(node), messageParser_(node) {} /** * @brief Called every time a telegram is received @@ -132,28 +156,17 @@ namespace io { { cdCtr_ = 0; mainConnectionDescriptor_ = std::string(); - // reset promise - cdPromise_ = std::promise(); - cdReceived_ = cdPromise_.get_future(); } //! Returns the connection descriptor std::string getMainCd() { - cdReceived_.wait(); + cdSemaphore_.wait(); return mainConnectionDescriptor_; } - //! Resets wait for response - void resetWaitForResponse() - { - // reset promise - responsePromise_ = std::promise(); - responseReceived_ = responsePromise_.get_future(); - } - //! Waits for response - void waitForResponse() { responseReceived_.wait(); } + void waitForResponse() { responseSemaphore_.wait(); } private: void handleSbf(const std::shared_ptr& telegram); @@ -168,10 +181,8 @@ namespace io { MessageParser messageParser_; uint8_t cdCtr_ = 0; - std::promise cdPromise_; - std::future cdReceived_; - std::promise responsePromise_; - std::future responseReceived_; + Semaphore cdSemaphore_; + Semaphore responseSemaphore_; std::string mainConnectionDescriptor_ = std::string(); }; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 55bcb6d2..5640eecd 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -255,7 +255,6 @@ namespace io { boost::regex("(tcp)://(.+):(\\d+)")); std::string proto(match[1]); mainConnectionDescriptor_ = resetMainConnection(); - node_->log(LogLevel::DEBUG, "Cd: " + mainConnectionDescriptor_); if (proto == "tcp") { // mainConnectionDescriptor_ = manager_->getConnectionDescriptor(); @@ -1023,7 +1022,6 @@ namespace io { void CommunicationCore::send(const std::string& cmd) { - telegramHandler_.resetWaitForResponse(); manager_.get()->send(cmd); telegramHandler_.waitForResponse(); } diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 6812ab1d..e6056edc 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -67,6 +67,11 @@ namespace io { handleResponse(telegram); break; } + case message_type::CONNECTION_DESCRIPTOR: + { + handleCd(telegram); + break; + } default: { node_->log(LogLevel::DEBUG, @@ -90,19 +95,16 @@ namespace io { std::string block_in_string(telegram->message.begin(), telegram->message.end()); tokenizer tokens(block_in_string, sep); - node_->log( + /*node_->log( LogLevel::DEBUG, "The NMEA message contains " + std::to_string(block_in_string.size()) + - " bytes and is ready to be parsed. It reads: " + *tokens.begin()); + " bytes and is ready to be parsed. It reads: " + *tokens.begin());*/ } void TelegramHandler::handleResponse(const std::shared_ptr& telegram) { std::string block_in_string(telegram->message.begin(), telegram->message.end()); - node_->log(LogLevel::DEBUG, "The Rx's response contains " + - std::to_string(block_in_string.size()) + - " bytes and reads:\n " + block_in_string); if (telegram->type == message_type::ERROR_RESPONSE) { @@ -111,8 +113,20 @@ namespace io { "Invalid command just sent to the Rx! The Rx's response contains " + std::to_string(block_in_string.size()) + " bytes and reads:\n " + block_in_string); + } else + { + node_->log(LogLevel::DEBUG, "The Rx's response contains " + + std::to_string(block_in_string.size()) + + " bytes and reads:\n " + + block_in_string); + } + try + { + responseSemaphore_.notify(); + } catch (std::exception& e) + { + node_->log(LogLevel::DEBUG, "handleResponse " + std::string(e.what())); } - responsePromise_.set_value(); } void TelegramHandler::handleCd(const std::shared_ptr& telegram) @@ -120,12 +134,22 @@ namespace io { if (cdCtr_ < 2) { mainConnectionDescriptor_ = - std::string(telegram->message.begin(), telegram->message.end()); - node_->log(LogLevel::INFO, - "The connection descriptor is " + mainConnectionDescriptor_); + std::string(telegram->message.begin(), telegram->message.end() - 1); + ++cdCtr_; if (cdCtr_ == 2) - cdPromise_.set_value(); + { + node_->log(LogLevel::INFO, "The connection descriptor is " + + mainConnectionDescriptor_); + try + { + cdSemaphore_.notify(); + } catch (std::exception& e) + { + node_->log(LogLevel::DEBUG, + "handleCd cd " + std::string(e.what())); + } + } } } } // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 9e873a25..5ab7cea2 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -1778,9 +1778,10 @@ namespace io { node_->publishMessage(topic, msg); } else { - node_->log( + /*node_->log( LogLevel::DEBUG, - "Not publishing message with GNSS time because no leap seconds are available yet."); + "Not publishing message with GNSS time because no leap seconds are + available yet.");*/ if (settings_->read_from_sbf_log || settings_->read_from_pcap) node_->log( LogLevel::WARN, From e63a60475be6fe5b851646f51d4ee2b1fa4e723f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 01:24:17 +0100 Subject: [PATCH 023/170] Add NMEA handling --- .../communication/telegram_handler.hpp | 5 + .../parsers/message_parser.hpp | 11 + .../communication/telegram_handler.cpp | 8 +- .../parsers/message_parser.cpp | 481 +++++++----------- 4 files changed, 203 insertions(+), 302 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 31137651..426ea07b 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -184,6 +184,11 @@ namespace io { Semaphore cdSemaphore_; Semaphore responseSemaphore_; std::string mainConnectionDescriptor_ = std::string(); + + std::unordered_map nmeaMap_{ + {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPST", 1}, {"$INST", 1}, + {"$GPRMC", 2}, {"$INRMC", 2}, {"$GPGSA", 3}, {"$INGSA", 3}, + {"$GAGSV", 4}, {"$INGSV", 4}}; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index 3b896165..84b20cab 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -155,6 +155,12 @@ namespace io { */ void parseSbf(const std::shared_ptr& telegram); + /** + * @brief Parse NMEA block + * @param[in] telegram Telegram to be parsed + */ + void parseNmea(const std::shared_ptr& telegram); + private: /** * @brief Publishing function @@ -185,6 +191,11 @@ namespace io { */ Timestamp recvTimestamp_; + std::unordered_map nmeaMap_{ + {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPST", 1}, {"$INST", 1}, + {"$GPRMC", 2}, {"$INRMC", 2}, {"$GPGSA", 3}, {"$INGSA", 3}, + {"$GAGSV", 4}, {"$INGSV", 4}}; + /** * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks * need to be stored diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index e6056edc..7c6ce8be 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -88,17 +88,11 @@ namespace io { void TelegramHandler::handleNmea(const std::shared_ptr& telegram) { - boost::char_separator sep("\r"); // Carriage Return (CR) - typedef boost::tokenizer> tokenizer; - // Syntax: new_string_name (const char* s, size_t n); size_t is - // either 2 or 8 bytes, depending on your system - std::string block_in_string(telegram->message.begin(), - telegram->message.end()); - tokenizer tokens(block_in_string, sep); /*node_->log( LogLevel::DEBUG, "The NMEA message contains " + std::to_string(block_in_string.size()) + " bytes and is ready to be parsed. It reads: " + *tokens.begin());*/ + messageParser_.parseNmea(telegram); } void TelegramHandler::handleResponse(const std::shared_ptr& telegram) diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 5ab7cea2..b51e1927 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -2751,301 +2751,192 @@ namespace io { current_leap_seconds_ = settings_->leap_seconds; } - void parseNMEA() - { /* - case evGPST: - { - TimeReferenceMsg msg; - Timestamp time_obj = - timestampSBF(telegram->message, true); // We need the GPS time, hence - true msg.time_ref = timestampToRos(time_obj); msg.source = "GPST"; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpst", msg); - break; - } - case evGPGGA: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - // No kept delimiters, hence "". Also, we specify that empty tokens - // should show up in the output when two delimiters are next to each - // other. Hence we also append the checksum part of the GGA message to - // "body" below, though it is not parsed. - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpggaParser::parseASCII - NMEASentence gga_message(id, body); - GpggaMsg msg; - Timestamp time_obj = node_->getTime(); - GpggaParser parser_obj; - try - { - msg = parser_obj.parseASCII(gga_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what())); - break; - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgga", msg); - break; - } - case evGPRMC: - { - Timestamp time_obj = node_->getTime(); - - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GprmcParser::parseASCII - NMEASentence rmc_message(id, body); - GprmcMsg msg; - GprmcParser parser_obj; - try - { - msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, - settings_->use_gnss_time, time_obj); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what())); - break; - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gprmc", msg); - break; - } - case evGPGSA: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsaParser::parseASCII - NMEASentence gsa_message(id, body); - GpgsaMsg msg; - GpgsaParser parser_obj; - try - { - msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsa", msg); - break; - } - case evGPGSV: - case evGLGSV: - case evGAGSV: - { - boost::char_separator sep("\r"); - typedef boost::tokenizer> tokenizer; - std::size_t nmea_size = this->messageSize(); - std::string block_in_string(reinterpret_cast(telegram->message), nmea_size); tokenizer tokens(block_in_string, sep); - - std::string id = this->messageID(); - std::string one_message = *tokens.begin(); - boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); - tokenizer tokens_2(one_message, sep_2); - std::vector body; - for (tokenizer::iterator tok_iter = tokens_2.begin(); - tok_iter != tokens_2.end(); ++tok_iter) - { - body.push_back(*tok_iter); - } - // Create NmeaSentence struct to pass to GpgsvParser::parseASCII - NMEASentence gsv_message(id, body); - GpgsvMsg msg; - GpgsvParser parser_obj; - try - { - msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, - settings_->use_gnss_time, node_->getTime()); - } catch (ParseException& e) - { - node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - Timestamp time_obj; - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - if (settings_->septentrio_receiver_type == "ins") - { - Timestamp time_obj; - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); - msg.header.stamp = timestampToRos(time_obj); - } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsv", msg); - break; - } - - bool MessageParser::gnss_gpsfix_complete(uint32_t id) - { - std::vector gpsfix_vec = {channelstatus_has_arrived_gpsfix_, - measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, - pvtgeodetic_has_arrived_gpsfix_, - poscovgeodetic_has_arrived_gpsfix_, - velcovgeodetic_has_arrived_gpsfix_, - atteuler_has_arrived_gpsfix_, - attcoveuler_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); - } - - bool MessageParser::ins_gpsfix_complete(uint32_t id) - { - std::vector gpsfix_vec = { - channelstatus_has_arrived_gpsfix_, measepoch_has_arrived_gpsfix_, - dop_has_arrived_gpsfix_, insnavgeod_has_arrived_gpsfix_}; - return allTrue(gpsfix_vec, id); - } - - bool MessageParser::gnss_navsatfix_complete(uint32_t id) - { - std::vector navsatfix_vec = { - pvtgeodetic_has_arrived_navsatfix_, - poscovgeodetic_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); - } - - bool MessageParser::ins_navsatfix_complete(uint32_t id) - { - std::vector navsatfix_vec = {insnavgeod_has_arrived_navsatfix_}; - return allTrue(navsatfix_vec, id); - } - - bool MessageParser::gnss_pose_complete(uint32_t id) - { - std::vector pose_vec = { - pvtgeodetic_has_arrived_pose_, poscovgeodetic_has_arrived_pose_, - atteuler_has_arrived_pose_, attcoveuler_has_arrived_pose_}; - return allTrue(pose_vec, id); - } - - bool MessageParser::ins_pose_complete(uint32_t id) - { - std::vector pose_vec = {insnavgeod_has_arrived_pose_}; - return allTrue(pose_vec, id); - } - - bool MessageParser::diagnostics_complete(uint32_t id) - { - std::vector diagnostics_vec = { - receiverstatus_has_arrived_diagnostics_, - qualityind_has_arrived_diagnostics_}; - return allTrue(diagnostics_vec, id); - } - - bool MessageParser::ins_localization_complete(uint32_t id) - { - std::vector loc_vec = {insnavgeod_has_arrived_localization_}; - return allTrue(loc_vec, id); - } - - bool MessageParser::ins_localization_ecef_complete(uint32_t id) - { - std::vector loc_vec = {insnavgeod_has_arrived_localization_ecef_, - insnavcart_has_arrived_localization_ecef_}; - return allTrue(loc_vec, id); - } - - bool MessageParser::allTrue(std::vector & vec, uint32_t id) - { - vec.erase(vec.begin() + id); - // Checks whether all entries in vec are true - return (std::all_of(vec.begin(), vec.end(), [](bool v) { return v; }) == - true); - }*/ + void MessageParser::parseNmea(const std::shared_ptr& telegram) + { + std::string message(telegram->message.begin(), telegram->message.end()); + boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); + boost::tokenizer> tokens(message, sep_2); + std::vector body; + body.reserve(20); + for (boost::tokenizer>::iterator tok_iter = + tokens.begin(); + tok_iter != tokens.end(); ++tok_iter) + { + body.push_back(*tok_iter); + } + + std::string id(body[0].begin() + 1, body[0].end()); + + auto it = nmeaMap_.find(body[0]); + if (it != nmeaMap_.end()) + { + switch (it->second) + { + case 0: + { + // Create NmeaSentence struct to pass to GpggaParser::parseASCII + NMEASentence gga_message(id, body); + GpggaMsg msg; + GpggaParser parser_obj; + try + { + msg = parser_obj.parseASCII(gga_message, settings_->frame_id, + settings_->use_gnss_time, + telegram->stamp); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, + "GpggaMsg: " + std::string(e.what())); + break; + } + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgga", msg); + break; + } + case 1: + { + TimeReferenceMsg msg; + Timestamp time_obj = timestampSBF( + telegram->message, true); // We need the GPS time, hence + msg.time_ref = timestampToRos(time_obj); + msg.source = "GPST"; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/gpst", msg); + break; + } + case 2: + { + // Create NmeaSentence struct to pass to GprmcParser::parseASCII + NMEASentence rmc_message(id, body); + GprmcMsg msg; + GprmcParser parser_obj; + try + { + msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, + settings_->use_gnss_time, + telegram->stamp); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, + "GprmcMsg: " + std::string(e.what())); + break; + } + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gprmc", msg); + break; + } + case 3: + { + // Create NmeaSentence struct to pass to GpgsaParser::parseASCII + NMEASentence gsa_message(id, body); + GpgsaMsg msg; + GpgsaParser parser_obj; + try + { + msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, + settings_->use_gnss_time, + node_->getTime()); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, + "GpgsaMsg: " + std::string(e.what())); + break; + } + if (settings_->use_gnss_time) + { + if (settings_->septentrio_receiver_type == "gnss") + { + Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + if (settings_->septentrio_receiver_type == "ins") + { + Timestamp time_obj; + time_obj = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + } else + msg.header.stamp = timestampToRos(telegram->stamp); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgsa", msg); + break; + } + case 4: + { + // Create NmeaSentence struct to pass to GpgsvParser::parseASCII + NMEASentence gsv_message(id, body); + GpgsvMsg msg; + GpgsvParser parser_obj; + try + { + msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, + settings_->use_gnss_time, + node_->getTime()); + } catch (ParseException& e) + { + node_->log(LogLevel::DEBUG, + "GpgsvMsg: " + std::string(e.what())); + break; + } + if (settings_->use_gnss_time) + { + + if (settings_->septentrio_receiver_type == "gnss") + { + Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + if (settings_->septentrio_receiver_type == "ins") + { + Timestamp time_obj; + time_obj = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc, + settings_->use_gnss_time); + msg.header.stamp = timestampToRos(time_obj); + } + } else + msg.header.stamp = timestampToRos(telegram->stamp); + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + Timestamp time_obj = timestampFromRos(msg.header.stamp); + wait(time_obj); + } + publish("/gpgsv", msg); + break; + } + } + } else + { + node_->log(LogLevel::DEBUG, "Unknown NMEA message: " + body[0]); + } } } // namespace io From 0d5800598b418c7e590231fa276d39f1b4dd7854 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 01:37:14 +0100 Subject: [PATCH 024/170] Remove obsolete includes --- .../communication/communication_core.hpp | 5 --- .../communication/telegram_handler.hpp | 41 +------------------ 2 files changed, 1 insertion(+), 45 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 30c2aa31..605a8550 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -61,15 +61,10 @@ // Boost includes #include #include -#include -#include -#include // dealing with bad file descriptor error -#include // C++ library includes #include #include #include -#include // for usleep() // ROSaic includes #include #include diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 426ea07b..8684ddc1 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -59,41 +59,7 @@ #pragma once // C++ includes -#include -#include - -// Boost includes -#include -// In C++, writing a loop that iterates over a sequence is tedious --> -// BOOST_FOREACH(char ch, "Hello World") -#include -// E.g. boost::function f = std::atoi;defines a pointer f that can -// point to functions that expect a parameter of type const char* and return a value -// of type int Generally, any place in which a function pointer would be used to -// defer a call or make a callback, Boost.Function can be used instead to allow the -// user greater flexibility in the implementation of the target. -#include -// Boost's thread enables the use of multiple threads of execution with shared data -// in portable C++ code. It provides classes and functions for managing the threads -// themselves, along with others for synchronizing data between the threads or -// providing separate copies of data specific to individual threads. -#include -#include -// The tokenizer class provides a container view of a series of tokens contained in a -// sequence, e.g. if you are not interested in non-words... -#include -// Join algorithm is a counterpart to split algorithms. It joins strings from a -// 'list' by adding user defined separator. -#include -// The class boost::posix_time::ptime that we will use defines a location-independent -// time. It uses the type boost::gregorian::date, yet also stores a time. -#include -// Boost.Asio may be used to perform both synchronous and asynchronous operations on -// I/O objects such as sockets. -#include -#include -#include -#include +#include // ROSaic includes #include @@ -184,11 +150,6 @@ namespace io { Semaphore cdSemaphore_; Semaphore responseSemaphore_; std::string mainConnectionDescriptor_ = std::string(); - - std::unordered_map nmeaMap_{ - {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPST", 1}, {"$INST", 1}, - {"$GPRMC", 2}, {"$INRMC", 2}, {"$GPGSA", 3}, {"$INGSA", 3}, - {"$GAGSV", 4}, {"$INGSV", 4}}; }; } // namespace io \ No newline at end of file From c8f55797a5abceceb0a3afb1bc44ce2e0211451a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 03:24:31 +0100 Subject: [PATCH 025/170] Exchange concurrent queue --- CMakeLists.txt | 3 - README.md | 2 - .../communication/telegram.hpp | 56 +++++++++++++++++-- .../parsers/nmea_parsers/gpgga.hpp | 2 - .../parsers/nmea_parsers/gpgsv.hpp | 2 - package.xml | 1 - 6 files changed, 52 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 039a1a42..9ea7c818 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED) # bug with 1.71: spirit is not found ... COMPONENTS system thread regex spirit) LIST(APPEND CMAKE_MODULE_PATH "/usr/share/cmake/geographiclib") find_package(GeographicLib REQUIRED) -find_package(TBB REQUIRED) ## For PCAP file handling find_library(libpcap_LIBRARIES pcap) @@ -167,7 +166,6 @@ include_directories( ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GeographicLib_INCLUDE_DIRS} - ${TBB_INCLUDE_DIRS} ) ## Add cmake target dependencies of the library @@ -210,7 +208,6 @@ target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBRARIES} ${libpcap_LIBRARIES} ${GeographicLib_LIBRARIES} - tbb ) ############# diff --git a/README.md b/README.md index 4ad25250..114389d3 100644 --- a/README.md +++ b/README.md @@ -29,8 +29,6 @@ Compatiblity with PCAP captures are incorporated through [pcap libraries](https: `sudo apt install libpcap-dev`.

Conversions from LLA to UTM are incorporated through [GeographicLib](https://geographiclib.sourceforge.io/). Install the necessary headers via

`sudo apt install libgeographic-dev` -Internal data pipelining is realized with TBB. Install the necessary library via

-`sudo apt install libtbb-dev`

## Usage
diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 6df4f10e..b5671435 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -31,12 +31,12 @@ #pragma once // C++ +#include #include +#include +#include #include -// TBB -#include - // ROSaic #include @@ -114,4 +114,52 @@ struct Telegram } }; -typedef tbb::concurrent_bounded_queue> TelegramQueue; \ No newline at end of file +template +class ConcurrentQueue +{ +public: + [[nodiscard]] bool empty() const noexcept; + [[nodiscard]] size_t size() const noexcept; + void push(const T& input) noexcept; + void pop(T& output) noexcept; + +private: + std::queue queue_; + std::condition_variable cond_; + std::mutex mtx_; +}; + +template +[[nodiscard]] bool ConcurrentQueue::empty() const noexcept +{ + std::lock_guard lck(mtx_); + return queue_.empty(); +} + +template +[[nodiscard]] size_t ConcurrentQueue::size() const noexcept +{ + std::lock_guard lck(mtx_); + return queue_.size(); +} + +template +void ConcurrentQueue::push(const T& input) noexcept +{ + { + std::lock_guard lck(mtx_); + queue_.push(input); + } + cond_.notify_one(); +} + +template +void ConcurrentQueue::pop(T& output) noexcept +{ + std::unique_lock lck(mtx_); + cond_.wait(lck, [this] { return !queue_.empty(); }); + output = queue_.front(); + queue_.pop(); +} + +typedef ConcurrentQueue> TelegramQueue; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp index f7105bcc..91c2e642 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp @@ -61,8 +61,6 @@ // ROSaic includes #include #include -// Boost and ROS includes -#include /** * @file gpgga.hpp diff --git a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp index 9c5ef9c8..761b4341 100644 --- a/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp +++ b/include/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp @@ -61,8 +61,6 @@ // ROSaic includes #include #include -// Boost and ROS includes -#include /** * @file gpgsv.hpp diff --git a/package.xml b/package.xml index bd49b034..3f8601b4 100644 --- a/package.xml +++ b/package.xml @@ -63,7 +63,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tbb cpp_common rosconsole From 2984ae864a53632d3aafba01caf08c35c21e0b10 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 18:39:11 +0100 Subject: [PATCH 026/170] Add packing of generic string messages --- .../communication/async_manager.hpp | 35 +++++++++---------- .../communication/telegram.hpp | 3 +- .../communication/telegram_handler.cpp | 9 ++++- .../parsers/message_parser.cpp | 4 +++ 4 files changed, 31 insertions(+), 20 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index c78e8f67..c2b17b0b 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -132,7 +132,7 @@ namespace io { void readSync(); void readSbfHeader(); void readSbf(std::size_t length); - void readCd(); + void readUnknown(); void readString(); void readStringElements(); @@ -300,17 +300,8 @@ namespace io { { case 0: { - if ((currByte == CONNECTION_DESCRIPTOR_BYTE_I) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_C) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_U) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_N) || - (currByte == CONNECTION_DESCRIPTOR_BYTE_D)) - { - telegram_->type = - message_type::CONNECTION_DESCRIPTOR; - readCd(); - } else - readSync<0>(); + telegram_->type = message_type::UNKNOWN; + readUnknown(); break; } case 1: @@ -503,7 +494,10 @@ namespace io { parsing_utilities::getId(telegram_->message.data()); telegramQueue_->push(telegram_); - } + } else + node_->log(LogLevel::DEBUG, + "AsyncManager crc failed for SBF " + + std::to_string(telegram_->sbfId) + "."); } else { node_->log( @@ -521,7 +515,7 @@ namespace io { } template - void AsyncManager::readCd() + void AsyncManager::readUnknown() { telegram_->message.resize(1); readStringElements(); @@ -559,7 +553,7 @@ namespace io { telegram_->stamp = node_->getTime(); node_->log( LogLevel::DEBUG, - "AsyncManager string read fault, sync 0 found."); + "AsyncManager string read fault, sync 1 found."); readSync<1>(); break; } @@ -568,14 +562,19 @@ namespace io { if (telegram_->message[telegram_->message.size() - 2] == CR) telegramQueue_->push(telegram_); + else + node_->log( + LogLevel::DEBUG, + "LF wo CR: " + + std::string(telegram_->message.begin(), + telegram_->message.end())); resync(); break; } case CONNECTION_DESCRIPTOR_FOOTER: { - if (telegram_->type == - message_type::CONNECTION_DESCRIPTOR) - telegramQueue_->push(telegram_); + telegram_->type = message_type::CONNECTION_DESCRIPTOR; + telegramQueue_->push(telegram_); resync(); break; } diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index b5671435..37e43e62 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -95,7 +95,8 @@ namespace message_type { NMEA_INS, RESPONSE, ERROR_RESPONSE, - CONNECTION_DESCRIPTOR + CONNECTION_DESCRIPTOR, + UNKNOWN }; } diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 7c6ce8be..1fca7cda 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -72,10 +72,17 @@ namespace io { handleCd(telegram); break; } + case message_type::UNKNOWN: + { + node_->log(LogLevel::DEBUG, "Unhandeled message received " + + std::string(telegram->message.begin(), + telegram->message.end())); + break; + } default: { node_->log(LogLevel::DEBUG, - "TelegramHandler received an unknown message to handle"); + "TelegramHandler received an invalid message to handle"); break; } } diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index b51e1927..5bf72ad7 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -2754,6 +2754,10 @@ namespace io { void MessageParser::parseNmea(const std::shared_ptr& telegram) { std::string message(telegram->message.begin(), telegram->message.end()); + /*node_->log( + LogLevel::DEBUG, + "The NMEA message contains " + std::to_string(message.size()) + + " bytes and is ready to be parsed. It reads: " + message);*/ boost::char_separator sep_2(",*", "", boost::keep_empty_tokens); boost::tokenizer> tokens(message, sep_2); std::vector body; From ca4c8a0610e48cf8a983ad7ca1fcd58999385269 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 21:00:11 +0100 Subject: [PATCH 027/170] Fix error response detection --- .../communication/async_manager.hpp | 14 ++++++++------ .../communication/telegram_handler.cpp | 6 +----- .../parsers/message_parser.cpp | 1 - 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index c2b17b0b..d62022ba 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -366,16 +366,15 @@ namespace io { resync(); break; } - case ERROR_SYNC_BYTE_3: + case RESPONSE_SYNC_BYTE_3: { - if (telegram_->type == - message_type::ERROR_RESPONSE) + if (telegram_->type == message_type::RESPONSE) readString(); else resync(); break; } - case RESPONSE_SYNC_BYTE_3: + case RESPONSE_SYNC_BYTE_3a: { if (telegram_->type == message_type::RESPONSE) readString(); @@ -383,11 +382,14 @@ namespace io { resync(); break; } - case RESPONSE_SYNC_BYTE_3a: + case ERROR_SYNC_BYTE_3: { if (telegram_->type == message_type::RESPONSE) + { + telegram_->type = + message_type::ERROR_RESPONSE; readString(); - else + } else resync(); break; } diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 1fca7cda..9cf05c8e 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -74,7 +74,7 @@ namespace io { } case message_type::UNKNOWN: { - node_->log(LogLevel::DEBUG, "Unhandeled message received " + + node_->log(LogLevel::DEBUG, "Unhandeled message received: " + std::string(telegram->message.begin(), telegram->message.end())); break; @@ -95,10 +95,6 @@ namespace io { void TelegramHandler::handleNmea(const std::shared_ptr& telegram) { - /*node_->log( - LogLevel::DEBUG, - "The NMEA message contains " + std::to_string(block_in_string.size()) + - " bytes and is ready to be parsed. It reads: " + *tokens.begin());*/ messageParser_.parseNmea(telegram); } diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 5bf72ad7..2e091e86 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -2197,7 +2197,6 @@ namespace io { case EXT_SENSOR_MEAS: { - bool hasImuMeas = false; if (!ExtSensorMeasParser(node_, telegram->message.begin(), telegram->message.end(), last_extsensmeas_, From 8ddd3f0e41cd5117df7d2f8f03fb03a586d295c8 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 22:03:33 +0100 Subject: [PATCH 028/170] Fix handling of INS NMEA talker ID --- CMakeLists.txt | 9 ++ .../communication/async_manager.hpp | 2 +- .../parsers/message_parser.hpp | 5 +- .../communication/communication_core.cpp | 2 +- .../parsers/message_parser.cpp | 111 +++++++----------- 5 files changed, 57 insertions(+), 72 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ea7c818..e59b4e9b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,15 @@ project(septentrio_gnss_driver) ## Compile as C++14, supported in ROS Melodic and newer add_compile_options(-std=c++14) +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) +message(STATUS "Setting build type to Release as none was specified.") +set(CMAKE_BUILD_TYPE "Release" CACHE + STRING "Choose the type of build." FORCE) +# Set the possible values of build type for cmake-gui +set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS + "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index d62022ba..7d352480 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -360,7 +360,7 @@ namespace io { } case NMEA_INS_SYNC_BYTE_3: { - if (telegram_->type == message_type::NMEA) + if (telegram_->type == message_type::NMEA_INS) readString(); else resync(); diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index 84b20cab..b991cd28 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -192,9 +192,8 @@ namespace io { Timestamp recvTimestamp_; std::unordered_map nmeaMap_{ - {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPST", 1}, {"$INST", 1}, - {"$GPRMC", 2}, {"$INRMC", 2}, {"$GPGSA", 3}, {"$INGSA", 3}, - {"$GAGSV", 4}, {"$INGSV", 4}}; + {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPRMC", 1}, {"$INRMC", 1}, + {"$GPGSA", 2}, {"$INGSA", 2}, {"$GAGSV", 3}, {"$INGSV", 3}}; /** * @brief Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 5640eecd..911a609d 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -449,7 +449,7 @@ namespace io { // Setting up NMEA streams { - send("snti, GP\x0D"); + // send("snti, GP\x0D"); std::stringstream blocks; if (settings_->publish_gpgga) diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 2e091e86..5f029497 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -1834,8 +1834,7 @@ namespace io { if (!PVTCartesianParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PVTCartesian"); + node_->log(LogLevel::ERROR, "parse error in PVTCartesian"); break; } msg.header.frame_id = settings_->frame_id; @@ -1856,8 +1855,7 @@ namespace io { if (!PVTGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_pvtgeodetic_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PVTGeodetic"); + node_->log(LogLevel::ERROR, "parse error in PVTGeodetic"); break; } last_pvtgeodetic_.header.frame_id = settings_->frame_id; @@ -1880,8 +1878,7 @@ namespace io { if (!BaseVectorCartParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in BaseVectorCart"); + node_->log(LogLevel::ERROR, "parse error in BaseVectorCart"); break; } msg.header.frame_id = settings_->frame_id; @@ -1903,8 +1900,7 @@ namespace io { if (!BaseVectorGeodParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in BaseVectorGeod"); + node_->log(LogLevel::ERROR, "parse error in BaseVectorGeod"); break; } msg.header.frame_id = settings_->frame_id; @@ -1926,8 +1922,7 @@ namespace io { if (!PosCovCartesianParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PosCovCartesian"); + node_->log(LogLevel::ERROR, "parse error in PosCovCartesian"); break; } msg.header.frame_id = settings_->frame_id; @@ -1948,8 +1943,7 @@ namespace io { if (!PosCovGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_poscovgeodetic_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in PosCovGeodetic"); + node_->log(LogLevel::ERROR, "parse error in PosCovGeodetic"); break; } last_poscovgeodetic_.header.frame_id = settings_->frame_id; @@ -1972,8 +1966,7 @@ namespace io { telegram->message.end(), last_atteuler_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in AttEuler"); + node_->log(LogLevel::ERROR, "parse error in AttEuler"); break; } last_atteuler_.header.frame_id = settings_->frame_id; @@ -1996,8 +1989,7 @@ namespace io { telegram->message.end(), last_attcoveuler_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in AttCovEuler"); + node_->log(LogLevel::ERROR, "parse error in AttCovEuler"); break; } last_attcoveuler_.header.frame_id = settings_->frame_id; @@ -2021,8 +2013,7 @@ namespace io { telegram->message.end(), last_insnavcart_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in INSNavCart"); + node_->log(LogLevel::ERROR, "parse error in INSNavCart"); break; } if (settings_->ins_use_poi) @@ -2051,8 +2042,7 @@ namespace io { telegram->message.end(), last_insnavgeod_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in INSNavGeod"); + node_->log(LogLevel::ERROR, "parse error in INSNavGeod"); break; } if (settings_->ins_use_poi) @@ -2088,8 +2078,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in IMUSetup"); + node_->log(LogLevel::ERROR, "parse error in IMUSetup"); break; } msg.header.frame_id = settings_->vehicle_frame_id; @@ -2113,8 +2102,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in VelSensorSetup"); + node_->log(LogLevel::ERROR, "parse error in VelSensorSetup"); break; } msg.header.frame_id = settings_->vehicle_frame_id; @@ -2139,9 +2127,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log( - LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtEventINSNavCart"); + node_->log(LogLevel::ERROR, "parse error in ExtEventINSNavCart"); break; } if (settings_->ins_use_poi) @@ -2171,9 +2157,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log( - LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtEventINSNavGeod"); + node_->log(LogLevel::ERROR, "parse error in ExtEventINSNavGeod"); break; } if (settings_->ins_use_poi) @@ -2203,8 +2187,7 @@ namespace io { settings_->use_ros_axis_orientation, hasImuMeas)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ExtSensorMeas"); + node_->log(LogLevel::ERROR, "parse error in ExtSensorMeas"); break; } last_extsensmeas_.header.frame_id = settings_->imu_frame_id; @@ -2241,8 +2224,7 @@ namespace io { if (!ChannelStatusParser(node_, telegram->message.begin(), telegram->message.end(), last_channelstatus_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ChannelStatus"); + node_->log(LogLevel::ERROR, "parse error in ChannelStatus"); break; } break; @@ -2253,8 +2235,7 @@ namespace io { if (!MeasEpochParser(node_, telegram->message.begin(), telegram->message.end(), last_measepoch_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in MeasEpoch"); + node_->log(LogLevel::ERROR, "parse error in MeasEpoch"); break; } last_measepoch_.header.frame_id = settings_->frame_id; @@ -2271,8 +2252,7 @@ namespace io { if (!DOPParser(node_, telegram->message.begin(), telegram->message.end(), last_dop_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in DOP"); + node_->log(LogLevel::ERROR, "parse error in DOP"); break; } break; @@ -2283,8 +2263,7 @@ namespace io { if (!VelCovGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_velcovgeodetic_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in VelCovGeodetic"); + node_->log(LogLevel::ERROR, "parse error in VelCovGeodetic"); break; } last_velcovgeodetic_.header.frame_id = settings_->frame_id; @@ -2311,8 +2290,7 @@ namespace io { if (!ReceiverStatusParser(node_, telegram->message.begin(), telegram->message.end(), last_receiverstatus_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverStatus"); + node_->log(LogLevel::ERROR, "parse error in ReceiverStatus"); break; } break; @@ -2323,8 +2301,7 @@ namespace io { if (!QualityIndParser(node_, telegram->message.begin(), telegram->message.end(), last_qualityind_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in QualityInd"); + node_->log(LogLevel::ERROR, "parse error in QualityInd"); break; } break; @@ -2334,10 +2311,12 @@ namespace io { if (!ReceiverSetupParser(node_, telegram->message.begin(), telegram->message.end(), last_receiversetup_)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverSetup"); + node_->log(LogLevel::ERROR, "parse error in ReceiverSetup"); break; } + node_->log(LogLevel::DEBUG, + "receiver setup firmware: " + last_receiversetup_.rx_version); + static int32_t ins_major = 1; static int32_t ins_minor = 3; static int32_t ins_patch = 2; @@ -2356,9 +2335,7 @@ namespace io { } if (major_minor_patch.size() < 3) { - node_->log( - LogLevel::ERROR, - "septentrio_gnss_driver: parse error of firmware version."); + node_->log(LogLevel::ERROR, "parse error of firmware version."); } else { if ((settings_->septentrio_receiver_type == "ins") || @@ -2416,8 +2393,7 @@ namespace io { if (!ReceiverTimeParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, - "septentrio_gnss_driver: parse error in ReceiverTime"); + node_->log(LogLevel::ERROR, "parse error in ReceiverTime"); break; } current_leap_seconds_ = msg.delta_ls; @@ -2435,6 +2411,22 @@ namespace io { void processjointMessages() { /* + + case 1: + { + TimeReferenceMsg msg; + Timestamp time_obj = timestampSBF( + telegram->message, true); // We need the GPS time, hence + msg.time_ref = timestampToRos(time_obj); + msg.source = "GPST"; + // Wait as long as necessary (only when reading from SBF/PCAP file) + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } + publish("/gpst", msg); + break; + } case evDiagnosticArray: { DiagnosticArrayMsg msg; @@ -2802,21 +2794,6 @@ namespace io { break; } case 1: - { - TimeReferenceMsg msg; - Timestamp time_obj = timestampSBF( - telegram->message, true); // We need the GPS time, hence - msg.time_ref = timestampToRos(time_obj); - msg.source = "GPST"; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpst", msg); - break; - } - case 2: { // Create NmeaSentence struct to pass to GprmcParser::parseASCII NMEASentence rmc_message(id, body); @@ -2842,7 +2819,7 @@ namespace io { publish("/gprmc", msg); break; } - case 3: + case 2: { // Create NmeaSentence struct to pass to GpgsaParser::parseASCII NMEASentence gsa_message(id, body); From 6be5aa1aa22a8dc61e1cc4bc60e6247428e63947 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 15 Jan 2023 23:42:35 +0100 Subject: [PATCH 029/170] Fix reset main connection on exit hang --- .../communication/telegram_handler.hpp | 2 +- .../communication/communication_core.cpp | 4 ++++ .../communication/telegram_handler.cpp | 18 +++--------------- .../parsers/message_parser.cpp | 5 +++++ 4 files changed, 13 insertions(+), 16 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 8684ddc1..2fa06b25 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -120,7 +120,7 @@ namespace io { //! Returns the connection descriptor void resetWaitforMainCd() { - cdCtr_ = 0; + cdCtr_ = 1; // Cd is sent twice only after startup mainConnectionDescriptor_ = std::string(); } diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 911a609d..598d94a2 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -269,6 +269,8 @@ namespace io { send("lif, Identification \x0D"); } + node_->log(LogLevel::INFO, "Setting up Rx."); + std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand( settings_->polling_period_pvt); @@ -846,7 +848,9 @@ namespace io { nmeaActivated_ = true; } } + node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); + node_->log(LogLevel::INFO, "Setup complete."); } void CommunicationCore::sendVelocity(const std::string& velNmea) diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 9cf05c8e..276b57fb 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -117,13 +117,7 @@ namespace io { " bytes and reads:\n " + block_in_string); } - try - { - responseSemaphore_.notify(); - } catch (std::exception& e) - { - node_->log(LogLevel::DEBUG, "handleResponse " + std::string(e.what())); - } + responseSemaphore_.notify(); } void TelegramHandler::handleCd(const std::shared_ptr& telegram) @@ -138,14 +132,8 @@ namespace io { { node_->log(LogLevel::INFO, "The connection descriptor is " + mainConnectionDescriptor_); - try - { - cdSemaphore_.notify(); - } catch (std::exception& e) - { - node_->log(LogLevel::DEBUG, - "handleCd cd " + std::string(e.what())); - } + + cdSemaphore_.notify(); } } } diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 5f029497..3ed31ec0 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -1825,6 +1825,11 @@ namespace io { */ void MessageParser::parseSbf(const std::shared_ptr& telegram) { + /*node_->log(LogLevel::DEBUG, + "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + + " made up of " + std::to_string(telegram->message.size()) + + " bytes...");*/ + switch (telegram->sbfId) { case PVT_CARTESIAN: // Position and velocity in XYZ From ef93fd0d779a74ea9d6b969d1d71ad93ccb7c381 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 16 Jan 2023 18:12:54 +0100 Subject: [PATCH 030/170] Add file readers --- CMakeLists.txt | 3 +- .../abstraction/typedefs.hpp | 2 +- .../communication/communication_core.hpp | 27 - .../communication/io.hpp | 115 +++- .../communication/pcap_reader.hpp | 118 ----- .../communication/telegram_handler.hpp | 9 +- .../parsers/message_parser.hpp | 22 +- .../communication/communication_core.cpp | 501 +++++++----------- .../communication/pcap_reader.cpp | 192 ------- .../parsers/message_parser.cpp | 53 +- 10 files changed, 332 insertions(+), 710 deletions(-) delete mode 100644 include/septentrio_gnss_driver/communication/pcap_reader.hpp delete mode 100644 src/septentrio_gnss_driver/communication/pcap_reader.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e59b4e9b..b061b45c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,8 @@ else () set(libpcap_FOUND TRUE) endif () +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") + ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html @@ -187,7 +189,6 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_node src/septentrio_gnss_driver/communication/communication_core.cpp - src/septentrio_gnss_driver/communication/pcap_reader.cpp src/septentrio_gnss_driver/communication/telegram_handler.cpp src/septentrio_gnss_driver/crc/crc.cpp src/septentrio_gnss_driver/node/main.cpp diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index a80bd511..80141bd2 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -178,7 +178,7 @@ class ROSaicNodeBase virtual ~ROSaicNodeBase() {} - Settings* getSettings() { return &settings_; } + Settings* settings() { return &settings_; } void registerSubscriber() { diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 605a8550..319aa5ce 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -137,33 +137,6 @@ namespace io { */ std::string resetMainConnection(); - /** - * @brief Sets up the stage for SBF file reading - * @param[in] file_name The name of (or path to) the SBF file, e.g. "xyz.sbf" - */ - void prepareSBFFileReading(std::string file_name); - - /** - * @brief Sets up the stage for PCAP file reading - * @param[in] file_name The path to PCAP file, e.g. "/tmp/capture.sbf" - */ - void preparePCAPFileReading(std::string file_name); - - /** - * @brief Initializes SBF file reading and reads SBF file by repeatedly - * calling read_callback_() - * @param[in] file_name The name of (or path to) the SBF file, e.g. "xyz.sbf" - */ - void initializeSBFFileReading(std::string file_name); - - /** - * @brief Initializes PCAP file reading and reads PCAP file by repeatedly - * calling read_callback_() - * @param[in] file_name The name of (or path to) the PCAP file, e.g. - * "/tmp/capture.pcap" - */ - void initializePCAPFileReading(std::string file_name); - void processTelegrams(); /** diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index b49a07fc..df26b34a 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -34,11 +34,15 @@ #include // Linux +#include #include // Boost #include +// pcap +#include + // ROSaic #include @@ -67,13 +71,13 @@ namespace io { { boost::asio::ip::tcp::resolver resolver(ioService_); boost::asio::ip::tcp::resolver::query query( - node_->getSettings()->tcp_ip, node_->getSettings()->tcp_port); + node_->settings()->tcp_ip, node_->settings()->tcp_port); endpointIterator = resolver.resolve(query); } catch (std::runtime_error& e) { node_->log(LogLevel::ERROR, - "Could not resolve " + node_->getSettings()->tcp_ip + - " on port " + node_->getSettings()->tcp_port + ": " + + "Could not resolve " + node_->settings()->tcp_ip + + " on port " + node_->settings()->tcp_port + ": " + e.what()); return false; } @@ -81,8 +85,8 @@ namespace io { stream_.reset(new boost::asio::ip::tcp::socket(ioService_)); node_->log(LogLevel::INFO, "Connecting to tcp://" + - node_->getSettings()->tcp_ip + ":" + - node_->getSettings()->tcp_port + "..."); + node_->settings()->tcp_ip + ":" + + node_->settings()->tcp_port + "..."); try { @@ -115,8 +119,8 @@ namespace io { struct SerialIo { SerialIo(ROSaicNodeBase* node) : - node_(node), flowcontrol_(node->getSettings()->hw_flow_control), - baudrate_(node->getSettings()->baudrate) + node_(node), flowcontrol_(node->settings()->hw_flow_control), + baudrate_(node->settings()->baudrate) { stream_.reset(new boost::asio::serial_port(ioService_)); } @@ -140,9 +144,9 @@ namespace io { { node_->log(LogLevel::INFO, "Connecting serially to device" + - node_->getSettings()->device + + node_->settings()->device + ", targeted baudrate: " + - std::to_string(node_->getSettings()->baudrate)); + std::to_string(node_->settings()->baudrate)); stream_->open(port_); opened = true; } catch (const boost::system::system_error& err) @@ -304,4 +308,97 @@ namespace io { boost::asio::io_service ioService_; std::unique_ptr stream_; }; + + class SbfFileIo + { + public: + SbfFileIo(ROSaicNodeBase* node) : node_(node) {} + + ~SbfFileIo() { stream_->close(); } + + void close() { stream_->close(); } + + [[nodiscard]] bool connect() + { + node_->log(LogLevel::INFO, "Opening SBF file stream" + + node_->settings()->device + "..."); + + int fd = open(node_->settings()->device.c_str(), O_RDONLY); + if (fd == -1) + { + node_->log(LogLevel::ERROR, "open SBF file failed."); + return false; + } + stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); + + try + { + stream_->assign(fd); + + } catch (std::runtime_error& e) + { + node_->log(LogLevel::ERROR, "assigning SBF file failed due to " + + std::string(e.what())); + return false; + } + return true; + } + + private: + ROSaicNodeBase* node_; + std::array errBuff_; + pcap_t* pcap_; + + public: + boost::asio::io_service ioService_; + std::unique_ptr stream_; + }; + + class PcapFileIo + { + public: + PcapFileIo(ROSaicNodeBase* node) : node_(node) {} + + ~PcapFileIo() + { + pcap_close(pcap_); + stream_->close(); + } + + void close() + { + pcap_close(pcap_); + stream_->close(); + } + + [[nodiscard]] bool connect() + { + node_->log(LogLevel::INFO, "Opening pcap file stream" + + node_->settings()->device + "..."); + + stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); + + pcap_ = pcap_open_offline(node_->settings()->device.c_str(), + errBuff_.data()); + stream_->assign(pcap_get_selectable_fd(pcap_)); + + try + { + + } catch (std::runtime_error& e) + { + return false; + } + return true; + } + + private: + ROSaicNodeBase* node_; + std::array errBuff_; + pcap_t* pcap_; + + public: + boost::asio::io_service ioService_; + std::unique_ptr stream_; + }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/pcap_reader.hpp b/include/septentrio_gnss_driver/communication/pcap_reader.hpp deleted file mode 100644 index ebad8b09..00000000 --- a/include/septentrio_gnss_driver/communication/pcap_reader.hpp +++ /dev/null @@ -1,118 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#pragma once - -#include -#include -#include -#include - -/** - * @file pcap_reader.hpp - * @date 07/05/2021 - * @author Ashwin A Nayar - * - * @brief Declares a class for handling pcap files. - */ - -namespace pcapReader { - - using buffer_t = std::vector; - - //! Read operation status - enum ReadResult - { - /// Data read successfully - READ_SUCCESS = 0, - READ_INSUFFICIENT_DATA = 1, - READ_TIMEOUT = 2, - READ_INTERRUPTED = 3, - READ_ERROR = -1, - /// Unable to parse data, parsing error - READ_PARSE_FAILED = -2 - - }; - - /** - * @class PcapDevice - * @brief Class for handling a pcap file - */ - class PcapDevice - { - public: - static const size_t BUFFSIZE = 100; - - /** - * @brief Constructor for PcapDevice - * @param[out] buffer Buffer to write read raw data to - */ - explicit PcapDevice(ROSaicNodeBase* node, buffer_t& buffer); - - /** - * @brief Try to open a pcap file - * @param[in] device Path to pcap file - * @return True if success, false otherwise - */ - bool connect(const char* device); - - /** - * @brief Close connected file - */ - void disconnect(); - - /** - * @brief Check if file is open and healthy - * @return True if file is open, false otherwise - */ - bool isConnected() const; - - /** - * @brief Attempt to read a packet and store data to buffer - * @return Result of read operation - */ - ReadResult read(); - - //! Destructor for PcapDevice - ~PcapDevice(); - - private: - //! Pointer to the node - ROSaicNodeBase* node_; - //! Reference to raw data buffer to write to - buffer_t& m_dataBuff; - //! File handle to pcap file - pcap_t* m_device{nullptr}; - bpf_program m_pktFilter{}; - char m_errBuff[BUFFSIZE]{}; - char* m_deviceName; - buffer_t m_lastPkt; - }; -} // namespace pcapReader diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 2fa06b25..b43a4515 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -120,7 +120,13 @@ namespace io { //! Returns the connection descriptor void resetWaitforMainCd() { - cdCtr_ = 1; // Cd is sent twice only after startup + // Cd is sent twice only after startup + if (init_ == false) + { + cdCtr_ = 0; + init_ = true; + } else + cdCtr_ = 1; mainConnectionDescriptor_ = std::string(); } @@ -146,6 +152,7 @@ namespace io { //! MessageParser parser MessageParser messageParser_; + bool init_ = false; uint8_t cdCtr_ = 0; Semaphore cdSemaphore_; Semaphore responseSemaphore_; diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index b991cd28..73c187cf 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -145,7 +145,7 @@ namespace io { * @param[in] node Pointer to the node) */ MessageParser(ROSaicNodeBase* node) : - node_(node), settings_(node->getSettings()), unix_time_(0) + node_(node), settings_(node->settings()), unix_time_(0) { } @@ -289,13 +289,13 @@ namespace io { * @brief "Callback" function when constructing NavSatFix messages * @return A smart pointer to the ROS message NavSatFix just created */ - NavSatFixMsg NavSatFixCallback(); + NavSatFixMsg assembleNavSatFix(); /** * @brief "Callback" function when constructing GPSFix messages * @return A smart pointer to the ROS message GPSFix just created */ - GPSFixMsg GPSFixCallback(); + GPSFixMsg assembleGPSFix(); /** * @brief "Callback" function when constructing PoseWithCovarianceStamped @@ -303,7 +303,7 @@ namespace io { * @return A smart pointer to the ROS message PoseWithCovarianceStamped just * created */ - PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback(); + PoseWithCovarianceStampedMsg assemblePoseWithCovarianceStamped(); /** * @brief "Callback" function when constructing @@ -311,7 +311,7 @@ namespace io { * @return A ROS message * DiagnosticArrayMsg just created */ - DiagnosticArrayMsg DiagnosticArrayCallback(); + DiagnosticArrayMsg assembleDiagnosticArray(); /** * @brief "Callback" function when constructing @@ -319,7 +319,7 @@ namespace io { * @return A ROS message * ImuMsg just created */ - ImuMsg ImuCallback(); + ImuMsg assmembleImu(); /** * @brief "Callback" function when constructing @@ -327,7 +327,7 @@ namespace io { * @return A ROS message * LocalizationMsg just created */ - LocalizationMsg LocalizationUtmCallback(); + LocalizationMsg assembleLocalizationUtm(); /** * @brief "Callback" function when constructing @@ -335,7 +335,7 @@ namespace io { * @return A ROS message * LocalizationMsg just created */ - LocalizationMsg LocalizationEcefCallback(); + LocalizationMsg assembleLocalizationEcef(); /** * @brief function to fill twist part of LocalizationMsg @@ -344,8 +344,8 @@ namespace io { * @param[in] yaw yaw [rad] * @param[inout] msg LocalizationMsg to be filled */ - void fillLocalizationMsgTwist(double roll, double pitch, double yaw, - LocalizationMsg& msg); + void assembleLocalizationMsgTwist(double roll, double pitch, double yaw, + LocalizationMsg& msg); /** * @brief "Callback" function when constructing @@ -354,7 +354,7 @@ namespace io { * @return A ROS message * TwistWithCovarianceStampedMsg just created */ - TwistWithCovarianceStampedMsg TwistCallback(bool fromIns = false); + TwistWithCovarianceStampedMsg assembleTwist(bool fromIns = false); /** * @brief Waits according to time when reading from file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 598d94a2..d918ae8f 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -34,7 +34,6 @@ // Boost includes #include #include -#include static const int16_t ANGLE_MAX = 180; static const int16_t ANGLE_MIN = -180; @@ -60,7 +59,7 @@ static const int8_t POSSTD_DEV_MAX = 100; namespace io { CommunicationCore::CommunicationCore(ROSaicNodeBase* node) : - node_(node), settings_(node->getSettings()), telegramHandler_(node), + node_(node), settings_(node->settings()), telegramHandler_(node), running_(true) { running_ = true; @@ -196,9 +195,8 @@ namespace io { { settings_->read_from_sbf_log = true; settings_->use_gnss_time = true; - // connectionThread_ = boost::thread( - // boost::bind(&CommunicationCore::prepareSBFFileReading, this, - // match[2])); + settings_->device = match[2]; + manager_.reset(new AsyncManager(node_, &telegramQueue_)); } else if (boost::regex_match( settings_->device, match, @@ -206,10 +204,8 @@ namespace io { { settings_->read_from_pcap = true; settings_->use_gnss_time = true; - // connectionThread_ = boost::thread( - // boost::bind(&CommunicationCore::preparePCAPFileReading, this, - // match[2])); - + settings_->device = match[2]; + manager_.reset(new AsyncManager(node_, &telegramQueue_)); } else if (boost::regex_match(settings_->device, match, boost::regex("(serial):(.+)"))) { @@ -308,177 +304,6 @@ namespace io { send(ss.str()); } - // Setting up SBF blocks with rx_period_pvt - { - std::stringstream blocks; - if (settings_->use_gnss_time) - { - blocks << " +ReceiverTime"; - } - if (settings_->publish_pvtcartesian) - { - blocks << " +PVTCartesian"; - } - if (settings_->publish_pvtgeodetic || settings_->publish_twist || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +PVTGeodetic"; - } - if (settings_->publish_basevectorcart) - { - blocks << " +BaseVectorCart"; - } - if (settings_->publish_basevectorgeod) - { - blocks << " +BaseVectorGeod"; - } - if (settings_->publish_poscovcartesian) - { - blocks << " +PosCovCartesian"; - } - if (settings_->publish_poscovgeodetic || - (settings_->publish_navsatfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +PosCovGeodetic"; - } - if (settings_->publish_velcovgeodetic || settings_->publish_twist || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +VelCovGeodetic"; - } - if (settings_->publish_atteuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +AttEuler"; - } - if (settings_->publish_attcoveuler || - (settings_->publish_gpsfix && - (settings_->septentrio_receiver_type == "gnss")) || - (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) - { - blocks << " +AttCovEuler"; - } - if (settings_->publish_measepoch || settings_->publish_gpsfix) - { - blocks << " +MeasEpoch"; - } - if (settings_->publish_gpsfix) - { - blocks << " +ChannelStatus +DOP"; - } - // Setting SBF output of Rx depending on the receiver type - // If INS then... - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_insnavcart || - settings_->publish_localization_ecef || - settings_->publish_tf_ecef) - { - blocks << " +INSNavCart"; - } - if (settings_->publish_insnavgeod || settings_->publish_navsatfix || - settings_->publish_gpsfix || settings_->publish_pose || - settings_->publish_imu || settings_->publish_localization || - settings_->publish_tf || settings_->publish_twist || - settings_->publish_localization_ecef || - settings_->publish_tf_ecef) - { - blocks << " +INSNavGeod"; - } - if (settings_->publish_exteventinsnavgeod) - { - blocks << " +ExtEventINSNavGeod"; - } - if (settings_->publish_exteventinsnavcart) - { - blocks << " +ExtEventINSNavCart"; - } - if (settings_->publish_extsensormeas || settings_->publish_imu) - { - blocks << " +ExtSensorMeas"; - } - } - std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " - << mainConnectionDescriptor_ << "," << blocks.str() << ", " - << pvt_interval << "\x0D"; - send(ss.str()); - ++stream; - } - // Setting up SBF blocks with rx_period_rest - { - std::stringstream blocks; - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->publish_imusetup) - { - blocks << " +IMUSetup"; - } - if (settings_->publish_velsensorsetup) - { - blocks << " +VelSensorSetup"; - } - } - if (settings_->publish_diagnostics) - { - blocks << " +ReceiverStatus +QualityInd"; - } - - blocks << " +ReceiverSetup"; - - std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " - << mainConnectionDescriptor_ << "," << blocks.str() << ", " - << rest_interval << "\x0D"; - send(ss.str()); - ++stream; - } - - // Setting up NMEA streams - { - // send("snti, GP\x0D"); - - std::stringstream blocks; - if (settings_->publish_gpgga) - { - blocks << " +GGA"; - } - if (settings_->publish_gprmc) - { - blocks << " +RMC"; - } - if (settings_->publish_gpgsa) - { - blocks << " +GSA"; - } - if (settings_->publish_gpgsv) - { - blocks << " +GSV"; - } - - std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " - << mainConnectionDescriptor_ << "," << blocks.str() << ", " - << pvt_interval << "\x0D"; - send(ss.str()); - ++stream; - } - if ((settings_->septentrio_receiver_type == "ins") || settings_->ins_in_gnss_mode) { @@ -822,6 +647,178 @@ namespace io { } } + // Setting up SBF blocks with rx_period_rest + { + std::stringstream blocks; + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->publish_imusetup) + { + blocks << " +IMUSetup"; + } + if (settings_->publish_velsensorsetup) + { + blocks << " +VelSensorSetup"; + } + } + if (settings_->publish_diagnostics) + { + blocks << " +ReceiverStatus +QualityInd"; + } + + blocks << " +ReceiverSetup"; + + std::stringstream ss; + ss << "sso, Stream" << std::to_string(stream) << ", " + << mainConnectionDescriptor_ << "," << blocks.str() << ", " + << rest_interval << "\x0D"; + send(ss.str()); + ++stream; + } + + // Setting up NMEA streams + { + // send("snti, GP\x0D"); + + std::stringstream blocks; + if (settings_->publish_gpgga) + { + blocks << " +GGA"; + } + if (settings_->publish_gprmc) + { + blocks << " +RMC"; + } + if (settings_->publish_gpgsa) + { + blocks << " +GSA"; + } + if (settings_->publish_gpgsv) + { + blocks << " +GSV"; + } + + std::stringstream ss; + ss << "sno, Stream" << std::to_string(stream) << ", " + << mainConnectionDescriptor_ << "," << blocks.str() << ", " + << pvt_interval << "\x0D"; + send(ss.str()); + ++stream; + } + + // Setting up SBF blocks with rx_period_pvt + { + std::stringstream blocks; + if (settings_->use_gnss_time) + { + blocks << " +ReceiverTime"; + } + if (settings_->publish_pvtcartesian) + { + blocks << " +PVTCartesian"; + } + if (settings_->publish_pvtgeodetic || settings_->publish_twist || + (settings_->publish_navsatfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +PVTGeodetic"; + } + if (settings_->publish_basevectorcart) + { + blocks << " +BaseVectorCart"; + } + if (settings_->publish_basevectorgeod) + { + blocks << " +BaseVectorGeod"; + } + if (settings_->publish_poscovcartesian) + { + blocks << " +PosCovCartesian"; + } + if (settings_->publish_poscovgeodetic || + (settings_->publish_navsatfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +PosCovGeodetic"; + } + if (settings_->publish_velcovgeodetic || settings_->publish_twist || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +VelCovGeodetic"; + } + if (settings_->publish_atteuler || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +AttEuler"; + } + if (settings_->publish_attcoveuler || + (settings_->publish_gpsfix && + (settings_->septentrio_receiver_type == "gnss")) || + (settings_->publish_pose && + (settings_->septentrio_receiver_type == "gnss"))) + { + blocks << " +AttCovEuler"; + } + if (settings_->publish_measepoch || settings_->publish_gpsfix) + { + blocks << " +MeasEpoch"; + } + if (settings_->publish_gpsfix) + { + blocks << " +ChannelStatus +DOP"; + } + // Setting SBF output of Rx depending on the receiver type + // If INS then... + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->publish_insnavcart || + settings_->publish_localization_ecef || + settings_->publish_tf_ecef) + { + blocks << " +INSNavCart"; + } + if (settings_->publish_insnavgeod || settings_->publish_navsatfix || + settings_->publish_gpsfix || settings_->publish_pose || + settings_->publish_imu || settings_->publish_localization || + settings_->publish_tf || settings_->publish_twist || + settings_->publish_localization_ecef || + settings_->publish_tf_ecef) + { + blocks << " +INSNavGeod"; + } + if (settings_->publish_exteventinsnavgeod) + { + blocks << " +ExtEventINSNavGeod"; + } + if (settings_->publish_exteventinsnavcart) + { + blocks << " +ExtEventINSNavCart"; + } + if (settings_->publish_extsensormeas || settings_->publish_imu) + { + blocks << " +ExtSensorMeas"; + } + } + std::stringstream ss; + ss << "sso, Stream" << std::to_string(stream) << ", " + << mainConnectionDescriptor_ << "," << blocks.str() << ", " + << pvt_interval << "\x0D"; + send(ss.str()); + ++stream; + } + if (settings_->septentrio_receiver_type == "ins") { if (!settings_->ins_vsm_ip_server_id.empty()) @@ -869,150 +866,6 @@ namespace io { return telegramHandler_.getMainCd(); } - void CommunicationCore::prepareSBFFileReading(std::string file_name) - { - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from" << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializeSBFFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "CommunicationCore::initializeSBFFileReading() failed for SBF File" - << file_name << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } - } - - void CommunicationCore::preparePCAPFileReading(std::string file_name) - { - try - { - std::stringstream ss; - ss << "Setting up everything needed to read from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - initializePCAPFileReading(file_name); - } catch (std::runtime_error& e) - { - std::stringstream ss; - ss << "CommunicationCore::initializePCAPFileReading() failed for SBF File " - << file_name << " due to: " << e.what(); - node_->log(LogLevel::ERROR, ss.str()); - } - } - - void CommunicationCore::initializeSBFFileReading(std::string file_name) - { - /*node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method.."); - std::size_t buffer_size = 8192; - uint8_t* to_be_parsed; - to_be_parsed = new uint8_t[buffer_size]; - std::ifstream bin_file(file_name, std::ios::binary); - std::vector vec_buf; - if (bin_file.good()) - { - /* Reads binary data using streambuffer iterators. - Copies all SBF file content into bin_data. */ - /*std::vector v_buf((std::istreambuf_iterator(bin_file)), - (std::istreambuf_iterator())); - vec_buf = v_buf; - bin_file.close(); - } - else - { - throw std::runtime_error("I could not find your file. Or it is corrupted."); - } - // The spec now guarantees that vectors store their elements contiguously. - to_be_parsed = vec_buf.data(); - std::stringstream ss; - ss << "Opened and copied over from " << file_name; - node_->log(LogLevel::DEBUG, ss.str()); - - while (running_) // Loop will stop if we are done reading the SBF file - { - try - { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be parsed - being " + buffer_size); handlers_.readCallback(node_->getTime(), to_be_parsed, - buffer_size); } catch (std::size_t& parsing_failed_here) - { - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; - } - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; - } - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; - } - to_be_parsed = to_be_parsed + buffer_size; - } - node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method..");*/ - } - - void CommunicationCore::initializePCAPFileReading(std::string file_name) - { - /*node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() - method.."); pcapReader::buffer_t vec_buf; pcapReader::PcapDevice - device(node_, vec_buf); - - if (!device.connect(file_name.c_str())) - { - node_->log(LogLevel::ERROR, - "Unable to find file or either it is corrupted"); - return; - } - - node_->log(LogLevel::INFO, "Reading ..."); - while (device.isConnected() && device.read() == pcapReader::READ_SUCCESS) - ; - device.disconnect(); - - std::size_t buffer_size = pcapReader::PcapDevice::BUFFSIZE; - uint8_t* to_be_parsed = new uint8_t[buffer_size]; - to_be_parsed = vec_buf.data(); - - while (running_) // Loop will stop if we are done reading the SBF file - { - try - { - node_->log( - LogLevel::DEBUG, - "Calling read_callback_() method, with number of bytes to be - parsed being " + buffer_size); handlers_.readCallback(node_->getTime(), - to_be_parsed, buffer_size); } catch (std::size_t& parsing_failed_here) - { - if (to_be_parsed - vec_buf.data() >= - vec_buf.size() * sizeof(uint8_t)) - { - break; - } - if (!parsing_failed_here) - parsing_failed_here = 1; - - to_be_parsed = to_be_parsed + parsing_failed_here; - node_->log(LogLevel::DEBUG, - "Parsing_failed_here is " + parsing_failed_here); - continue; - } - if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t)) - { - break; - } - to_be_parsed = to_be_parsed + buffer_size; - } - node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() - method..");*/ - } - void CommunicationCore::processTelegrams() { while (running_) diff --git a/src/septentrio_gnss_driver/communication/pcap_reader.cpp b/src/septentrio_gnss_driver/communication/pcap_reader.cpp deleted file mode 100644 index 87f77e60..00000000 --- a/src/septentrio_gnss_driver/communication/pcap_reader.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// ***************************************************************************** -// -// © Copyright 2020, Septentrio NV/SA. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// ***************************************************************************** - -#include "septentrio_gnss_driver/communication/pcap_reader.hpp" - -#include -#include -#include -#include -#include -#include - -/** - * @file pcap_reader.cpp - * @date 07/05/2021 - * @author Ashwin A Nayar - * - * @brief Implements auxiliary reader object for handling pcap files. - * - * Functions include connecting to the file, reading the contents to the - * specified data buffer and graceful exit. - */ - -namespace pcapReader { - - PcapDevice::PcapDevice(ROSaicNodeBase* node, buffer_t& buffer) : - node_(node), m_dataBuff{buffer} - { - } - - PcapDevice::~PcapDevice() { disconnect(); } - - bool PcapDevice::connect(const char* device) - { - if (isConnected()) - return true; - // Try to open pcap file - if ((m_device = pcap_open_offline(device, m_errBuff)) == nullptr) - return false; - - m_deviceName = (char*)device; - // Try to compile filter program - if (pcap_compile(m_device, &m_pktFilter, "tcp dst port 3001", 1, - PCAP_NETMASK_UNKNOWN) != 0) - return false; - - node_->log(LogLevel::INFO, "Connected to" + std::string(m_deviceName)); - return true; - } - - void PcapDevice::disconnect() - { - if (!isConnected()) - return; - - pcap_close(m_device); - m_device = nullptr; - node_->log(LogLevel::INFO, "Disconnected from " + std::string(m_deviceName)); - } - - bool PcapDevice::isConnected() const { return m_device; } - - ReadResult PcapDevice::read() - { - if (!isConnected()) - return READ_ERROR; - - struct pcap_pkthdr* header; - const u_char* pktData; - int result; - - result = pcap_next_ex(m_device, &header, &pktData); - - if (result >= 0) - { - auto ipHdr = - reinterpret_cast(pktData + sizeof(struct ethhdr)); - uint32_t ipHdrLen = ipHdr->ihl * 4u; - - switch (ipHdr->protocol) - { - case 6: - { - if (header->len == 54) - { - return READ_SUCCESS; - } - bool storePkt = true; - - if (!m_lastPkt.empty()) - { - auto tcpHdr = reinterpret_cast( - pktData + ipHdrLen + sizeof(struct ethhdr)); - - auto lastIpHdr = reinterpret_cast(&(m_lastPkt[0])); - uint32_t lastIpHdrLen = lastIpHdr->ihl * 4u; - auto lastTcpHdr = reinterpret_cast( - &(m_lastPkt[0]) + lastIpHdrLen); - uint16_t lastLen = - ntohs(static_cast(lastIpHdr->tot_len)); - uint16_t newLen = ntohs(static_cast(ipHdr->tot_len)); - uint32_t lastSeq = ntohl(lastTcpHdr->seq); - uint32_t newSeq = ntohl(tcpHdr->seq); - - if (newSeq != lastSeq) - { - uint32_t dataOffset = lastTcpHdr->doff * 4; - m_dataBuff.insert(m_dataBuff.end(), - m_lastPkt.begin() + lastIpHdrLen + - dataOffset, - m_lastPkt.end()); - } else if (newLen <= lastLen) - { - storePkt = false; - } - } - - if (storePkt) - { - m_lastPkt.clear(); - m_lastPkt.insert(m_lastPkt.end(), - pktData + sizeof(struct ethhdr), - pktData + header->len); - } - break; - } - - default: - { - node_->log(LogLevel::ERROR, - "Skipping protocol: " + std::to_string(result)); - return READ_ERROR; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - return READ_SUCCESS; - } else if (result == -2) - { - node_->log(LogLevel::INFO, - "Done reading from " + std::string(m_deviceName)); - if (!m_lastPkt.empty()) - { - auto lastIpHdr = reinterpret_cast(&(m_lastPkt[0])); - uint32_t ipHdrLength = lastIpHdr->ihl * 4u; - - auto lastTcpHdr = - reinterpret_cast(&(m_lastPkt[0]) + ipHdrLength); - uint32_t dataOffset = lastTcpHdr->doff * 4u; - - m_dataBuff.insert(m_dataBuff.end(), - m_lastPkt.begin() + ipHdrLength + dataOffset, - m_lastPkt.end()); - - m_lastPkt.clear(); - } - disconnect(); - return READ_SUCCESS; - } else - { - node_->log(LogLevel::ERROR, "Error reading data"); - return READ_ERROR; - } - } -} // namespace pcapReader \ No newline at end of file diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 3ed31ec0..06a25028 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -54,7 +54,7 @@ using parsing_utilities::rad2deg; using parsing_utilities::square; namespace io { - PoseWithCovarianceStampedMsg MessageParser::PoseWithCovarianceStampedCallback() + PoseWithCovarianceStampedMsg MessageParser::assemblePoseWithCovarianceStamped() { PoseWithCovarianceStampedMsg msg; if (settings_->septentrio_receiver_type == "gnss") @@ -191,7 +191,7 @@ namespace io { return msg; }; - DiagnosticArrayMsg MessageParser::DiagnosticArrayCallback() + DiagnosticArrayMsg MessageParser::assembleDiagnosticArray() { DiagnosticArrayMsg msg; std::string serialnumber(last_receiversetup_.rx_serial_number); @@ -300,7 +300,7 @@ namespace io { return msg; }; - ImuMsg MessageParser::ImuCallback() + ImuMsg MessageParser::assmembleImu() { ImuMsg msg; @@ -415,7 +415,7 @@ namespace io { }; TwistWithCovarianceStampedMsg - MessageParser::TwistCallback(bool fromIns /* = false*/) + MessageParser::assembleTwist(bool fromIns /* = false*/) { TwistWithCovarianceStampedMsg msg; @@ -638,7 +638,7 @@ namespace io { * Angular velocity not available, thus according autocovariances are set to * -1.0. */ - LocalizationMsg MessageParser::LocalizationUtmCallback() + LocalizationMsg MessageParser::assembleLocalizationUtm() { LocalizationMsg msg; @@ -808,7 +808,7 @@ namespace io { msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); } - fillLocalizationMsgTwist(roll, pitch, yaw, msg); + assembleLocalizationMsgTwist(roll, pitch, yaw, msg); return msg; }; @@ -818,7 +818,7 @@ namespace io { * Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ - LocalizationMsg MessageParser::LocalizationEcefCallback() + LocalizationMsg MessageParser::assembleLocalizationEcef() { LocalizationMsg msg; @@ -966,13 +966,14 @@ namespace io { msg.pose.covariance[35] = -1.0; } - fillLocalizationMsgTwist(roll, pitch, yaw, msg); + assembleLocalizationMsgTwist(roll, pitch, yaw, msg); return msg; }; - void MessageParser::fillLocalizationMsgTwist(double roll, double pitch, - double yaw, LocalizationMsg& msg) + void MessageParser::assembleLocalizationMsgTwist(double roll, double pitch, + double yaw, + LocalizationMsg& msg) { Eigen::Matrix3d R_local_body = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); @@ -1093,7 +1094,7 @@ namespace io { * SignalInfo field of the PVTGeodetic block does not disclose it. For that, one * would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block. */ - NavSatFixMsg MessageParser::NavSatFixCallback() + NavSatFixMsg MessageParser::assembleNavSatFix() { NavSatFixMsg msg; uint16_t mask = 15; // We extract the first four bits using this mask. @@ -1288,7 +1289,7 @@ namespace io { * includes those "in search". In case certain values appear unphysical, please * consult the firmware, since those most likely refer to Do-Not-Use values. */ - GPSFixMsg MessageParser::GPSFixCallback() + GPSFixMsg MessageParser::assembleGPSFix() { GPSFixMsg msg; msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); @@ -1825,10 +1826,10 @@ namespace io { */ void MessageParser::parseSbf(const std::shared_ptr& telegram) { - /*node_->log(LogLevel::DEBUG, + node_->log(LogLevel::DEBUG, "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + " made up of " + std::to_string(telegram->message.size()) + - " bytes...");*/ + " bytes..."); switch (telegram->sbfId) { @@ -2069,7 +2070,7 @@ namespace io { publish("/insnavgeod", last_insnavgeod_); if (settings_->publish_twist) { - TwistWithCovarianceStampedMsg twist = TwistCallback(true); + TwistWithCovarianceStampedMsg twist = assembleTwist(true); publish("/twist_ins", twist); } break; @@ -2211,7 +2212,7 @@ namespace io { ImuMsg msg; try { - msg = ImuCallback(); + msg = assmembleImu(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, "ImuMsg: " + std::string(e.what())); @@ -2284,7 +2285,7 @@ namespace io { publish("/velcovgeodetic", last_velcovgeodetic_); if (settings_->publish_twist) { - TwistWithCovarianceStampedMsg twist = TwistCallback(); + TwistWithCovarianceStampedMsg twist = assembleTwist(); publish("/twist", twist); } break; @@ -2437,7 +2438,7 @@ namespace io { DiagnosticArrayMsg msg; try { - msg = DiagnosticArrayCallback(); + msg = assembleDiagnosticArray(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, @@ -2475,7 +2476,7 @@ namespace io { LocalizationMsg msg; try { - msg = LocalizationUtmCallback(); + msg = assembleLocalizationUtm(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, "LocalizationMsg: " + @@ -2500,7 +2501,7 @@ namespace io { LocalizationMsg msg; try { - msg = LocalizationEcefCallback(); + msg = assembleLocalizationEcef(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, "LocalizationMsg: " + @@ -2529,7 +2530,7 @@ namespace io { NavSatFixMsg msg; try { - msg = NavSatFixCallback(); + msg = assembleNavSatFix(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, @@ -2557,7 +2558,7 @@ namespace io { NavSatFixMsg msg; try { - msg = NavSatFixCallback(); + msg = assembleNavSatFix(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, @@ -2591,7 +2592,7 @@ namespace io { GPSFixMsg msg; try { - msg = GPSFixCallback(); + msg = assembleGPSFix(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, "GPSFixMsg: " + @@ -2627,7 +2628,7 @@ namespace io { GPSFixMsg msg; try { - msg = GPSFixCallback(); + msg = assembleGPSFix(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, "GPSFixMsg: " + @@ -2665,7 +2666,7 @@ namespace io { PoseWithCovarianceStampedMsg msg; try { - msg = PoseWithCovarianceStampedCallback(); + msg = assemblePoseWithCovarianceStamped(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, @@ -2695,7 +2696,7 @@ namespace io { PoseWithCovarianceStampedMsg msg; try { - msg = PoseWithCovarianceStampedCallback(); + msg = assemblePoseWithCovarianceStamped(); } catch (std::runtime_error& e) { node_->log(LogLevel::DEBUG, From 2c977e131b558fe0cd153c156447616231694f7c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 16 Jan 2023 18:23:27 +0100 Subject: [PATCH 031/170] Remove copy paste vars --- include/septentrio_gnss_driver/communication/io.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index df26b34a..11dc20af 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -346,8 +346,6 @@ namespace io { private: ROSaicNodeBase* node_; - std::array errBuff_; - pcap_t* pcap_; public: boost::asio::io_service ioService_; From e9d8f5259ec3e795fec82742d3fcfe0467f5c1e5 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 11:07:26 +0100 Subject: [PATCH 032/170] Move wait --- .../parsers/message_parser.hpp | 16 +- .../parsers/message_parser.cpp | 296 +++++++----------- 2 files changed, 127 insertions(+), 185 deletions(-) diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index 73c187cf..ff287364 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -166,15 +166,19 @@ namespace io { * @brief Publishing function * @param[in] topic String of topic * @param[in] msg ROS message to be published + * @param[in] time_obj message time to wait if reading from file */ template - void publish(const std::string& topic, const M& msg); + void publish(const std::string& topic, const M& msg, + const Timestamp& time_obj); /** * @brief Publishing function * @param[in] msg Localization message + * @param[in] time_obj message time to wait if reading from file */ - void publishTf(const LocalizationMsg& msg); + + void publishTf(const LocalizationMsg& msg, const Timestamp& time_obj); /** * @brief Pointer to the node @@ -300,18 +304,14 @@ namespace io { /** * @brief "Callback" function when constructing PoseWithCovarianceStamped * messages - * @return A smart pointer to the ROS message PoseWithCovarianceStamped just - * created */ - PoseWithCovarianceStampedMsg assemblePoseWithCovarianceStamped(); + void assemblePoseWithCovarianceStamped(const Timestamp& time_obj); /** * @brief "Callback" function when constructing * DiagnosticArrayMsg messages - * @return A ROS message - * DiagnosticArrayMsg just created */ - DiagnosticArrayMsg assembleDiagnosticArray(); + void assembleDiagnosticArray(const Timestamp& time_obj); /** * @brief "Callback" function when constructing diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 06a25028..ac9b1222 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -54,48 +54,16 @@ using parsing_utilities::rad2deg; using parsing_utilities::square; namespace io { - PoseWithCovarianceStampedMsg MessageParser::assemblePoseWithCovarianceStamped() + void MessageParser::assemblePoseWithCovarianceStamped(const Timestamp& time_obj) { PoseWithCovarianceStampedMsg msg; - if (settings_->septentrio_receiver_type == "gnss") - { - // Filling in the pose data - double yaw = 0.0; - if (validValue(last_atteuler_.heading)) - yaw = last_atteuler_.heading; - double pitch = 0.0; - if (validValue(last_atteuler_.pitch)) - pitch = last_atteuler_.pitch; - double roll = 0.0; - if (validValue(last_atteuler_.roll)) - roll = last_atteuler_.roll; - msg.pose.pose.orientation = convertEulerToQuaternionMsg( - deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); - msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); - msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); - msg.pose.pose.position.z = last_pvtgeodetic_.height; - // Filling in the covariance data in row-major order - msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon; - msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon; - msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt; - msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon; - msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat; - msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt; - msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; - msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; - msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; - msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll); - msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll); - msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch); - msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll); - msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); - } if (settings_->septentrio_receiver_type == "ins") { + if (!validValue(last_insnavgeod_.block_header.tow)) + return; + + msg.header = last_insnavgeod_.header; + msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude); msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude); msg.pose.pose.position.z = last_insnavgeod_.height; @@ -187,14 +155,71 @@ namespace io { msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov); } + } else + { + if ((!validValue(last_pvtgeodetic_.block_header.tow)) || + (last_pvtgeodetic_.block_header.tow != + last_atteuler_.block_header.tow) || + (last_pvtgeodetic_.block_header.tow != + last_poscovgeodetic_.block_header.tow) || + (last_pvtgeodetic_.block_header.tow != + last_attcoveuler_.block_header.tow)) + return; + + msg.header = last_pvtgeodetic_.header; + + // Filling in the pose data + double yaw = 0.0; + if (validValue(last_atteuler_.heading)) + yaw = last_atteuler_.heading; + double pitch = 0.0; + if (validValue(last_atteuler_.pitch)) + pitch = last_atteuler_.pitch; + double roll = 0.0; + if (validValue(last_atteuler_.roll)) + roll = last_atteuler_.roll; + msg.pose.pose.orientation = convertEulerToQuaternionMsg( + deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); + msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude); + msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude); + msg.pose.pose.position.z = last_pvtgeodetic_.height; + // Filling in the covariance data in row-major order + msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon; + msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon; + msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt; + msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon; + msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat; + msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt; + msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; + msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; + msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; + msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll); + msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll); + msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch); + msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch); + msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll); + msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); + msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); } - return msg; + publish("/pose", msg, time_obj); }; - DiagnosticArrayMsg MessageParser::assembleDiagnosticArray() + void MessageParser::assembleDiagnosticArray(const Timestamp& time_obj) { DiagnosticArrayMsg msg; - std::string serialnumber(last_receiversetup_.rx_serial_number); + if (!validValue(last_receiverstatus_.block_header.tow) || + (last_receiverstatus_.block_header.tow != + last_qualityind_.block_header.tow)) + return; + msg.header.stamp = timestampToRos(time_obj); + // TODO frame_id + std::string serialnumber; + if (validValue(last_receiversetup_.block_header.tow)) + serialnumber = last_receiversetup_.rx_serial_number; + else + serialnumber = "invalid"; DiagnosticStatusMsg gnss_status; // Constructing the "level of operation" field uint16_t indicators_type_mask = static_cast(255); @@ -297,7 +322,7 @@ namespace io { gnss_status.message = "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; msg.status.push_back(gnss_status); - return msg; + publish("/diagnostics", msg, time_obj); }; ImuMsg MessageParser::assmembleImu() @@ -1761,7 +1786,7 @@ namespace io { time_obj -= current_leap_seconds_ * secToNSec; } else { - time_obj = recvTimestamp_; + time_obj = recvTimestamp_; // TODO use timestamp from telegram !!! } return time_obj; } @@ -1770,12 +1795,17 @@ namespace io { * If GNSS time is used, Publishing is only done with valid leap seconds */ template - void MessageParser::publish(const std::string& topic, const M& msg) + void MessageParser::publish(const std::string& topic, const M& msg, + const Timestamp& time_obj) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || (settings_->use_gnss_time && (current_leap_seconds_ != -128))) { + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } node_->publishMessage(topic, msg); } else { @@ -1793,13 +1823,18 @@ namespace io { /** * If GNSS time is used, Publishing is only done with valid leap seconds */ - void MessageParser::publishTf(const LocalizationMsg& msg) + void MessageParser::publishTf(const LocalizationMsg& msg, + const Timestamp& time_obj) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || (settings_->use_gnss_time && (current_leap_seconds_ != -128) && (current_leap_seconds_ != 0))) { + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { + wait(time_obj); + } node_->publishTf(msg); } else { @@ -1826,6 +1861,11 @@ namespace io { */ void MessageParser::parseSbf(const std::shared_ptr& telegram) { + node_->log(LogLevel::ERROR, + "init tow " + std::to_string(last_pvtgeodetic_.block_header.tow) + + " wnc " + std::to_string(last_pvtgeodetic_.block_header.wnc) + + " ."); + node_->log(LogLevel::DEBUG, "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + " made up of " + std::to_string(telegram->message.size()) + @@ -1847,12 +1887,8 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pvtcartesian", msg); + + publish("/pvtcartesian", msg, time_obj); break; } case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame @@ -1868,13 +1904,8 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_pvtgeodetic) - publish("/pvtgeodetic", last_pvtgeodetic_); + publish("/pvtgeodetic", last_pvtgeodetic_, time_obj); break; } case BASE_VECTOR_CART: @@ -1891,12 +1922,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/basevectorcart", msg); + publish("/basevectorcart", msg, time_obj); break; } case BASE_VECTOR_GEOD: @@ -1913,12 +1939,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/basevectorgeod", msg); + publish("/basevectorgeod", msg, time_obj); break; } case POS_COV_CARTESIAN: @@ -1935,12 +1956,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/poscovcartesian", msg); + publish("/poscovcartesian", msg, time_obj); break; } case POS_COV_GEODETIC: @@ -1956,13 +1972,9 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_poscovgeodetic) - publish("/poscovgeodetic", last_poscovgeodetic_); + publish("/poscovgeodetic", last_poscovgeodetic_, + time_obj); break; } case ATT_EULER: @@ -1979,13 +1991,8 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_atteuler_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_atteuler) - publish("/atteuler", last_atteuler_); + publish("/atteuler", last_atteuler_, time_obj); break; } case ATT_COV_EULER: @@ -2002,13 +2009,8 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_attcoveuler_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_attcoveuler) - publish("/attcoveuler", last_attcoveuler_); + publish("/attcoveuler", last_attcoveuler_, time_obj); break; } case INS_NAV_CART: // Position, velocity and orientation in cartesian @@ -2032,12 +2034,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_insnavcart_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/insnavcart", last_insnavcart_); + publish("/insnavcart", last_insnavcart_, time_obj); break; } case INS_NAV_GEOD: // Position, velocity and orientation in geodetic @@ -2061,17 +2058,13 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_insnavgeod_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_insnavgeod) - publish("/insnavgeod", last_insnavgeod_); + publish("/insnavgeod", last_insnavgeod_, time_obj); if (settings_->publish_twist) { TwistWithCovarianceStampedMsg twist = assembleTwist(true); - publish("/twist_ins", twist); + publish("/twist_ins", twist, + time_obj); } break; } @@ -2091,12 +2084,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/imusetup", msg); + publish("/imusetup", msg, time_obj); break; } @@ -2115,12 +2103,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/velsensorsetup", msg); + publish("/velsensorsetup", msg, time_obj); break; } @@ -2146,12 +2129,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/exteventinsnavcart", msg); + publish("/exteventinsnavcart", msg, time_obj); break; } @@ -2176,12 +2154,7 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/exteventinsnavgeod", msg); + publish("/exteventinsnavgeod", msg, time_obj); break; } @@ -2200,13 +2173,9 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_extsensmeas_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_extsensormeas) - publish("/extsensormeas", last_extsensmeas_); + publish("/extsensormeas", last_extsensmeas_, + time_obj); if (settings_->publish_imu && hasImuMeas) { ImuMsg msg; @@ -2220,7 +2189,7 @@ namespace io { } msg.header.frame_id = settings_->imu_frame_id; msg.header.stamp = last_extsensmeas_.header.stamp; - publish("/imu", msg); + publish("/imu", msg, time_obj); } break; } @@ -2249,7 +2218,7 @@ namespace io { timestampSBF(telegram->message, settings_->use_gnss_time); last_measepoch_.header.stamp = timestampToRos(time_obj); if (settings_->publish_measepoch) - publish("/measepoch", last_measepoch_); + publish("/measepoch", last_measepoch_, time_obj); break; } case DOP: @@ -2276,17 +2245,13 @@ namespace io { Timestamp time_obj = timestampSBF(telegram->message, settings_->use_gnss_time); last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } if (settings_->publish_velcovgeodetic) - publish("/velcovgeodetic", last_velcovgeodetic_); + publish("/velcovgeodetic", last_velcovgeodetic_, + time_obj); if (settings_->publish_twist) { TwistWithCovarianceStampedMsg twist = assembleTwist(); - publish("/twist", twist); + publish("/twist", twist, time_obj); } break; } @@ -2790,13 +2755,8 @@ namespace io { "GpggaMsg: " + std::string(e.what())); break; } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgga", msg); + Timestamp time_obj = timestampFromRos(msg.header.stamp); + publish("/gpgga", msg, time_obj); break; } case 1: @@ -2816,13 +2776,8 @@ namespace io { "GprmcMsg: " + std::string(e.what())); break; } - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gprmc", msg); + Timestamp time_obj = timestampFromRos(msg.header.stamp); + publish("/gprmc", msg, time_obj); break; } case 2: @@ -2842,11 +2797,12 @@ namespace io { "GpgsaMsg: " + std::string(e.what())); break; } + Timestamp time_obj; if (settings_->use_gnss_time) { if (settings_->septentrio_receiver_type == "gnss") { - Timestamp time_obj; + time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, last_pvtgeodetic_.block_header.wnc, settings_->use_gnss_time); @@ -2854,7 +2810,6 @@ namespace io { } if (settings_->septentrio_receiver_type == "ins") { - Timestamp time_obj; time_obj = timestampSBF(last_insnavgeod_.block_header.tow, last_insnavgeod_.block_header.wnc, settings_->use_gnss_time); @@ -2862,13 +2817,7 @@ namespace io { } } else msg.header.stamp = timestampToRos(telegram->stamp); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsa", msg); + publish("/gpgsa", msg, time_obj); break; } case 4: @@ -2888,12 +2837,12 @@ namespace io { "GpgsvMsg: " + std::string(e.what())); break; } + Timestamp time_obj; if (settings_->use_gnss_time) { if (settings_->septentrio_receiver_type == "gnss") { - Timestamp time_obj; time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, last_pvtgeodetic_.block_header.wnc, settings_->use_gnss_time); @@ -2901,7 +2850,6 @@ namespace io { } if (settings_->septentrio_receiver_type == "ins") { - Timestamp time_obj; time_obj = timestampSBF(last_insnavgeod_.block_header.tow, last_insnavgeod_.block_header.wnc, settings_->use_gnss_time); @@ -2909,13 +2857,7 @@ namespace io { } } else msg.header.stamp = timestampToRos(telegram->stamp); - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - Timestamp time_obj = timestampFromRos(msg.header.stamp); - wait(time_obj); - } - publish("/gpgsv", msg); + publish("/gpgsv", msg, time_obj); break; } } From 312eea0f350def80be8486a0c1b33ddbf55862af Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 12:55:51 +0100 Subject: [PATCH 033/170] Add custom BlockHeader constructor --- .../abstraction/typedefs.hpp | 21 +++++++++++-------- .../plugin/BlockHeader.h | 8 +++++++ .../parsers/message_parser.cpp | 5 ----- 3 files changed, 20 insertions(+), 14 deletions(-) create mode 100644 include/septentrio_gnss_driver/plugin/BlockHeader.h diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 80141bd2..1183c707 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -192,12 +192,14 @@ class ROSaicNodeBase } /** - * @brief Gets an integer or unsigned integer value from the parameter server - * @param[in] name The key to be used in the parameter server's dictionary - * @param[out] val Storage for the retrieved value, of type U, which can be - * either unsigned int or int - * @param[in] defaultVal Value to use if the server doesn't contain this - * parameter + * @brief Gets an integer or unsigned integer value from the + * parameter server + * @param[in] name The key to be used in the parameter server's + * dictionary + * @param[out] val Storage for the retrieved value, of type U, which + * can be either unsigned int or int + * @param[in] defaultVal Value to use if the server doesn't contain + * this parameter */ bool getUint32Param(const std::string& name, uint32_t& val, uint32_t defaultVal) { @@ -213,10 +215,11 @@ class ROSaicNodeBase /** * @brief Gets parameter of type T from the parameter server - * @param[in] name The key to be used in the parameter server's dictionary + * @param[in] name The key to be used in the parameter server's + * dictionary * @param[out] val Storage for the retrieved value, of type T - * @param[in] defaultVal Value to use if the server doesn't contain this - * parameter + * @param[in] defaultVal Value to use if the server doesn't contain + * this parameter * @return True if it could be retrieved, false if not */ template diff --git a/include/septentrio_gnss_driver/plugin/BlockHeader.h b/include/septentrio_gnss_driver/plugin/BlockHeader.h new file mode 100644 index 00000000..9a269329 --- /dev/null +++ b/include/septentrio_gnss_driver/plugin/BlockHeader.h @@ -0,0 +1,8 @@ +// Custom constructor macro to initalize SBF msgs with do_not_use values in block +// header +#define SEPTENTRIO_GNSS_DRIVER_MESSAGE_BLOCKHEADER_PLUGIN_CONSTRUCTOR \ + BlockHeader_() : \ + sync_1(0x24), sync_2(0x40), crc(0), id(0), revision(0), length(0), \ + tow(4294967295UL), wnc(65535) \ + { \ + } diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index ac9b1222..665c4420 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -1861,11 +1861,6 @@ namespace io { */ void MessageParser::parseSbf(const std::shared_ptr& telegram) { - node_->log(LogLevel::ERROR, - "init tow " + std::to_string(last_pvtgeodetic_.block_header.tow) + - " wnc " + std::to_string(last_pvtgeodetic_.block_header.wnc) + - " ."); - node_->log(LogLevel::DEBUG, "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + " made up of " + std::to_string(telegram->message.size()) + From e9121b34db26bdb591c8a86492f0abc9a3d5f9aa Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 15:08:02 +0100 Subject: [PATCH 034/170] Add units to come msg definitions --- msg/AttCovEuler.msg | 12 ++++----- msg/AttEuler.msg | 12 ++++----- msg/BlockHeader.msg | 4 +-- msg/ExtSensorMeas.msg | 24 ++++++++--------- msg/IMUSetup.msg | 12 ++++----- msg/INSNavCart.msg | 62 +++++++++++++++++++++---------------------- 6 files changed, 63 insertions(+), 63 deletions(-) diff --git a/msg/AttCovEuler.msg b/msg/AttCovEuler.msg index 6b5e4a9a..64ef1119 100644 --- a/msg/AttCovEuler.msg +++ b/msg/AttCovEuler.msg @@ -6,9 +6,9 @@ std_msgs/Header header BlockHeader block_header uint8 error -float32 cov_headhead -float32 cov_pitchpitch -float32 cov_rollroll -float32 cov_headpitch -float32 cov_headroll -float32 cov_pitchroll +float32 cov_headhead # deg^2 +float32 cov_pitchpitch # deg^2 +float32 cov_rollroll # deg^2 +float32 cov_headpitch # deg^2 +float32 cov_headroll # deg^2 +float32 cov_pitchroll # deg^2 diff --git a/msg/AttEuler.msg b/msg/AttEuler.msg index 33857a4f..320f7280 100644 --- a/msg/AttEuler.msg +++ b/msg/AttEuler.msg @@ -8,9 +8,9 @@ BlockHeader block_header uint8 nr_sv uint8 error uint16 mode -float32 heading -float32 pitch -float32 roll -float32 pitch_dot -float32 roll_dot -float32 heading_dot +float32 heading # deg +float32 pitch # deg +float32 roll # deg +float32 pitch_dot # deg/s +float32 roll_dot # deg/s +float32 heading_dot # deg/s diff --git a/msg/BlockHeader.msg b/msg/BlockHeader.msg index d299dfd3..e4af0bf4 100644 --- a/msg/BlockHeader.msg +++ b/msg/BlockHeader.msg @@ -7,5 +7,5 @@ uint16 id uint8 revision uint16 length -uint32 tow -uint16 wnc \ No newline at end of file +uint32 tow # ms since week start +uint16 wnc # weeks since Jan 06, 1980 at 00:00:00 UTC \ No newline at end of file diff --git a/msg/ExtSensorMeas.msg b/msg/ExtSensorMeas.msg index 9dece5fe..6ed338bb 100644 --- a/msg/ExtSensorMeas.msg +++ b/msg/ExtSensorMeas.msg @@ -16,22 +16,22 @@ uint8[] type uint8[] obs_info #ExtSensorMeasAcceleration -float64 acceleration_x -float64 acceleration_y -float64 acceleration_z +float64 acceleration_x # m/s^2 +float64 acceleration_y # m/s^2 +float64 acceleration_z # m/s^2 #ExtSensorMeasAngularRate -float64 angular_rate_x -float64 angular_rate_y -float64 angular_rate_z +float64 angular_rate_x # deg/s +float64 angular_rate_y # deg/s +float64 angular_rate_z # deg/s #ExtSensorMeasVelocity -float32 velocity_x -float32 velocity_y -float32 velocity_z -float32 std_dev_x -float32 std_dev_y -float32 std_dev_z +float32 velocity_x # m/s +float32 velocity_y # m/s +float32 velocity_z # m/s +float32 std_dev_x # m/s +float32 std_dev_y # m/s +float32 std_dev_z # m/s #ExtSensorMeasInfo float32 sensor_temperature # deg C diff --git a/msg/IMUSetup.msg b/msg/IMUSetup.msg index d19a5865..71d9bae3 100644 --- a/msg/IMUSetup.msg +++ b/msg/IMUSetup.msg @@ -7,9 +7,9 @@ std_msgs/Header header BlockHeader block_header uint8 serial_port -float32 ant_lever_arm_x -float32 ant_lever_arm_y -float32 ant_lever_arm_z -float32 theta_x -float32 theta_y -float32 theta_z \ No newline at end of file +float32 ant_lever_arm_x # m +float32 ant_lever_arm_y # m +float32 ant_lever_arm_z # m +float32 theta_x # deg +float32 theta_y # deg +float32 theta_z # deg \ No newline at end of file diff --git a/msg/INSNavCart.msg b/msg/INSNavCart.msg index ec921161..bb8db689 100644 --- a/msg/INSNavCart.msg +++ b/msg/INSNavCart.msg @@ -9,60 +9,60 @@ BlockHeader block_header uint8 gnss_mode uint8 error uint16 info -uint16 gnss_age -float64 x -float64 y -float64 z -uint16 accuracy -uint16 latency -uint8 datum +uint16 gnss_age # 0.01 s +float64 x # m +float64 y # m +float64 z # m +uint16 accuracy # 0.01 m +uint16 latency # 0.1 ms +uint8 datum #uint8 reserved uint16 sb_list # INSNavCartPosStdDev sub-block definition: # If the Position StdDev field is 1 then this sub block is available: -float32 x_std_dev -float32 y_std_dev -float32 z_std_dev +float32 x_std_dev # m +float32 y_std_dev # m +float32 z_std_dev # m # INSNavCartPosCov sub-block definition: # If the Position Cov field is 1 then this sub block is available: -float32 xy_cov -float32 xz_cov -float32 yz_cov +float32 xy_cov # m^2 +float32 xz_cov # m^2 +float32 yz_cov # m^2 # INSNavCartAtt sub-block definition: # If the Attitude field is 1 then this sub block is available: -float32 heading -float32 pitch -float32 roll +float32 heading # deg +float32 pitch # deg +float32 roll # deg # INSNavCartAttStdDev sub-block definition: # If the Attitude StdDev field is 1 then this sub block is available: -float32 heading_std_dev -float32 pitch_std_dev -float32 roll_std_dev +float32 heading_std_dev # deg +float32 pitch_std_dev # deg +float32 roll_std_dev # deg # INSNavCartAttCov sub-block definition: # If the Attitude Cov field is 1 then this sub block is available: -float32 heading_pitch_cov -float32 heading_roll_cov -float32 pitch_roll_cov +float32 heading_pitch_cov # deg^2 +float32 heading_roll_cov # deg^2 +float32 pitch_roll_cov # deg^2 # INSNavCartVel sub-block definition: # If the Velocity field is 1 then this sub block is available: -float32 vx -float32 vy -float32 vz +float32 vx # m/s +float32 vy # m/S +float32 vz # m/s # INSNavCartVelStdDev sub-block definition: # If the Velocity StdDev field is 1 then this sub block is available: -float32 vx_std_dev -float32 vy_std_dev -float32 vz_std_dev +float32 vx_std_dev # m/s +float32 vy_std_dev # m/s +float32 vz_std_dev # m/s # INSNavCartVelCov sub-block definition: # If the Velocity Cov field is 1 then this sub block is available: -float32 vx_vy_cov -float32 vx_vz_cov -float32 vy_vz_cov \ No newline at end of file +float32 vx_vy_cov # m^2/s^2 +float32 vx_vz_cov # m^2/s^2 +float32 vy_vz_cov # m^2/s^2 \ No newline at end of file From c3a6430f02dc5e9dd2b44454214a6cd04f56d802 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 17:28:18 +0100 Subject: [PATCH 035/170] Add assembled messages, to be tested --- .../abstraction/typedefs.hpp | 4 +- .../communication/settings.hpp | 2 +- .../parsers/message_parser.hpp | 26 +-- .../parsers/sbf_blocks.hpp | 25 ++- .../parsers/message_parser.cpp | 163 ++++++++++++------ 5 files changed, 141 insertions(+), 79 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 1183c707..396588be 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -96,8 +96,8 @@ typedef geometry_msgs::Quaternion QuaternionMsg; typedef geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg; typedef geometry_msgs::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg; typedef geometry_msgs::TransformStamped TransformStampedMsg; -typedef gps_common::GPSFix GPSFixMsg; -typedef gps_common::GPSStatus GPSStatusMsg; +typedef gps_common::GPSFix GpsFixMsg; +typedef gps_common::GPSStatus GpsStatusMsg; typedef sensor_msgs::NavSatFix NavSatFixMsg; typedef sensor_msgs::NavSatStatus NavSatStatusMsg; typedef sensor_msgs::TimeReference TimeReferenceMsg; diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 03011c96..d55a24f7 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -239,7 +239,7 @@ struct Settings bool publish_gpst; //! Whether or not to publish the NavSatFixMsg message bool publish_navsatfix; - //! Whether or not to publish the GPSFixMsg message + //! Whether or not to publish the GpsFixMsg message bool publish_gpsfix; //! Whether or not to publish the PoseWithCovarianceStampedMsg message bool publish_pose; diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index ff287364..a83045e3 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -291,25 +291,27 @@ namespace io { /** * @brief "Callback" function when constructing NavSatFix messages - * @return A smart pointer to the ROS message NavSatFix just created + * @param[in] time_obj time of message */ - NavSatFixMsg assembleNavSatFix(); + void assembleNavSatFix(const Timestamp& time_obj); /** * @brief "Callback" function when constructing GPSFix messages - * @return A smart pointer to the ROS message GPSFix just created + * @param[in] time_obj time of message */ - GPSFixMsg assembleGPSFix(); + void assembleGpsFix(const Timestamp& time_obj); /** * @brief "Callback" function when constructing PoseWithCovarianceStamped * messages + * @param[in] time_obj time of message */ void assemblePoseWithCovarianceStamped(const Timestamp& time_obj); /** * @brief "Callback" function when constructing * DiagnosticArrayMsg messages + * @param[in] time_obj time of message */ void assembleDiagnosticArray(const Timestamp& time_obj); @@ -324,18 +326,16 @@ namespace io { /** * @brief "Callback" function when constructing * LocalizationMsg messages in UTM - * @return A ROS message - * LocalizationMsg just created + * @param[in] time_obj time of message */ - LocalizationMsg assembleLocalizationUtm(); + void assembleLocalizationUtm(const Timestamp& time_obj); /** * @brief "Callback" function when constructing * LocalizationMsg messages in ECEF - * @return A ROS message - * LocalizationMsg just created + * @param[in] time_obj time of message */ - LocalizationMsg assembleLocalizationEcef(); + void assembleLocalizationEcef(const Timestamp& time_obj); /** * @brief function to fill twist part of LocalizationMsg @@ -350,14 +350,14 @@ namespace io { /** * @brief "Callback" function when constructing * TwistWithCovarianceStampedMsg messages + * @param[in] time_obj time of message * @param[in] fromIns Wether to contruct message from INS data - * @return A ROS message - * TwistWithCovarianceStampedMsg just created */ - TwistWithCovarianceStampedMsg assembleTwist(bool fromIns = false); + void assembleTwist(const Timestamp& time_obj, bool fromIns = false); /** * @brief Waits according to time when reading from file + * @param[in] time_obj wait until time */ void wait(Timestamp time_obj); diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 0933f78d..5ee7f73b 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -269,7 +269,7 @@ namespace qi = boost::spirit::qi; /** * validValue - * @brief Check if value is not set to Do-Not-Use -2e10 + * @brief Check if value is not set to Do-Not-Use */ template bool validValue(T s) @@ -294,19 +294,30 @@ bool validValue(T s) /** * setDoNotUse - * @brief Sets scalar to Do-Not-Use value -2e10 + * @brief Sets scalar to Do-Not-Use value */ -template -void setDoNotUse(T& s) +template +void setDoNotUse(Val& s) { - static_assert(std::is_same::value || std::is_same::value); - if (std::is_same::value) + static_assert( + std::is_same::value || std::is_same::value || + std::is_same::value || std::is_same::value || + std::is_same::value); + + if (std::is_same::value) + { + s = 65535; + } else if (std::is_same::value) + { + s = 4294967295ul; + } else if (std::is_same::value) { s = -2e10f; - } else if (std::is_same::value) + } else if (std::is_same::value) { s = -2e10; } + // TODO add more } /** diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index 665c4420..f2c08660 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -337,11 +337,12 @@ namespace io { msg.angular_velocity.y = last_extsensmeas_.angular_rate_y; msg.angular_velocity.z = last_extsensmeas_.angular_rate_z; - bool valid_orientation = true; + bool valid_orientation = false; if (settings_->septentrio_receiver_type == "ins") { if (validValue(last_insnavgeod_.block_header.tow)) { + // INS tow and extsens meas tow have the same time scale Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow, last_extsensmeas_.block_header.wnc, true); @@ -420,9 +421,6 @@ namespace io { { valid_orientation = false; } - } else - { - valid_orientation = false; } if (!valid_orientation) @@ -439,9 +437,11 @@ namespace io { return msg; }; - TwistWithCovarianceStampedMsg - MessageParser::assembleTwist(bool fromIns /* = false*/) + void MessageParser::assembleTwist(const Timestamp& time_obj, + bool fromIns /* = false*/) { + if (!settings_->publish_twist) + return; TwistWithCovarianceStampedMsg msg; if (fromIns) @@ -557,6 +557,10 @@ namespace io { } else { + if ((!validValue(last_pvtgeodetic_.block_header.tow)) || + (last_pvtgeodetic_.block_header.tow != + last_velcovgeodetic_.block_header.tow)) + return; msg.header = last_pvtgeodetic_.header; if (last_pvtgeodetic_.error == 0) @@ -654,7 +658,7 @@ namespace io { msg.header.frame_id = "navigation"; - return msg; + publish("/twist_ins", msg, time_obj); }; /** @@ -663,8 +667,11 @@ namespace io { * Angular velocity not available, thus according autocovariances are set to * -1.0. */ - LocalizationMsg MessageParser::assembleLocalizationUtm() + void MessageParser::assembleLocalizationUtm(const Timestamp& time_obj) { + if (!settings_->publish_localization) + return; + LocalizationMsg msg; int zone; @@ -835,7 +842,7 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); - return msg; + publish("/localization", msg, time_obj); }; /** @@ -843,8 +850,15 @@ namespace io { * Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ - LocalizationMsg MessageParser::assembleLocalizationEcef() + void MessageParser::assembleLocalizationEcef(const Timestamp& time_obj) { + if (!settings_->publish_localization_ecef) + return; + + if ((!validValue(last_insnavcart_.block_header.tow)) || + (last_insnavcart_.block_header.tow != last_insnavgeod_.block_header.tow)) + return; + LocalizationMsg msg; msg.header.frame_id = "ecef"; @@ -993,7 +1007,7 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); - return msg; + publish("/localization_ecef", msg, time_obj); }; void MessageParser::assembleLocalizationMsgTwist(double roll, double pitch, @@ -1119,12 +1133,20 @@ namespace io { * SignalInfo field of the PVTGeodetic block does not disclose it. For that, one * would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block. */ - NavSatFixMsg MessageParser::assembleNavSatFix() + void MessageParser::assembleNavSatFix(const Timestamp& time_obj) { + if (!settings_->publish_navsatfix) + return; + NavSatFixMsg msg; uint16_t mask = 15; // We extract the first four bits using this mask. if (settings_->septentrio_receiver_type == "gnss") { + if ((!validValue(last_pvtgeodetic_.block_header.tow)) || + (last_pvtgeodetic_.block_header.tow != + last_poscovgeodetic_.block_header.tow)) + return; + uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; switch (type_of_pvt) { @@ -1199,11 +1221,13 @@ namespace io { msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; - return msg; - } - - if (settings_->septentrio_receiver_type == "ins") + } else if (settings_->septentrio_receiver_type == "ins") { + if ((!validValue(last_insnavgeod_.block_header.tow)) || + (last_insnavgeod_.block_header.tow != + last_pvtgeodetic_.block_header.tow)) + return; + NavSatFixMsg msg; uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; switch (type_of_pvt) @@ -1290,7 +1314,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - return msg; + publish("/navsatfix", msg, time_obj); }; /** @@ -1314,9 +1338,34 @@ namespace io { * includes those "in search". In case certain values appear unphysical, please * consult the firmware, since those most likely refer to Do-Not-Use values. */ - GPSFixMsg MessageParser::assembleGPSFix() + void MessageParser::assembleGpsFix(const Timestamp& time_obj) { - GPSFixMsg msg; + if (!settings_->publish_gpsfix) + return; + + if (settings_->septentrio_receiver_type == "gnss") + { + if (!validValue(last_measepoch_.block_header.tow) || + !validValue(last_channelstatus_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_pvtgeodetic_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_poscovgeodetic_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_atteuler_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_attcoveuler_.block_header.tow)) + return; + } else if (settings_->septentrio_receiver_type == "ins") + { + if (!validValue(last_measepoch_.block_header.tow) || + !validValue(last_channelstatus_.block_header.tow) || + (last_measepoch_.block_header.tow != + last_insnavgeod_.block_header.tow)) + return; + } + + GpsFixMsg msg; msg.status.satellites_used = static_cast(last_pvtgeodetic_.nr_sv); // MeasEpoch Processing @@ -1447,13 +1496,13 @@ namespace io { { case evNoPVT: { - msg.status.status = GPSStatusMsg::STATUS_NO_FIX; + msg.status.status = GpsStatusMsg::STATUS_NO_FIX; break; } case evStandAlone: case evFixed: { - msg.status.status = GPSStatusMsg::STATUS_FIX; + msg.status.status = GpsStatusMsg::STATUS_FIX; break; } case evDGPS: @@ -1463,7 +1512,7 @@ namespace io { case evMovingBaseRTKFloat: case evPPP: { - msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; + msg.status.status = GpsStatusMsg::STATUS_GBAS_FIX; break; } case evSBAS: @@ -1473,10 +1522,10 @@ namespace io { if (reference_id == 131 || reference_id == 133 || reference_id == 135 || reference_id == 135) { - msg.status.status = GPSStatusMsg::STATUS_WAAS_FIX; + msg.status.status = GpsStatusMsg::STATUS_WAAS_FIX; } else { - msg.status.status = GPSStatusMsg::STATUS_SBAS_FIX; + msg.status.status = GpsStatusMsg::STATUS_SBAS_FIX; } break; } @@ -1490,11 +1539,11 @@ namespace io { } // Doppler is not used when calculating the velocities of, say, // mosaic-x5, hence: - msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; + msg.status.motion_source = GpsStatusMsg::SOURCE_POINTS; // Doppler is not used when calculating the orientation of, say, // mosaic-x5, hence: - msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; - msg.status.position_source = GPSStatusMsg::SOURCE_GPS; + msg.status.orientation_source = GpsStatusMsg::SOURCE_POINTS; + msg.status.position_source = GpsStatusMsg::SOURCE_GPS; msg.latitude = rad2deg(last_pvtgeodetic_.latitude); msg.longitude = rad2deg(last_pvtgeodetic_.longitude); msg.altitude = last_pvtgeodetic_.height; @@ -1584,9 +1633,7 @@ namespace io { msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt; msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt; msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; - } - - if (settings_->septentrio_receiver_type == "ins") + } else if (settings_->septentrio_receiver_type == "ins") { // PVT Status Analysis uint16_t status_mask = @@ -1597,13 +1644,13 @@ namespace io { { case evNoPVT: { - msg.status.status = GPSStatusMsg::STATUS_NO_FIX; + msg.status.status = GpsStatusMsg::STATUS_NO_FIX; break; } case evStandAlone: case evFixed: { - msg.status.status = GPSStatusMsg::STATUS_FIX; + msg.status.status = GpsStatusMsg::STATUS_FIX; break; } case evDGPS: @@ -1613,7 +1660,7 @@ namespace io { case evMovingBaseRTKFloat: case evPPP: { - msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX; + msg.status.status = GpsStatusMsg::STATUS_GBAS_FIX; break; } case evSBAS: @@ -1627,11 +1674,11 @@ namespace io { } // Doppler is not used when calculating the velocities of, say, // mosaic-x5, hence: - msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS; + msg.status.motion_source = GpsStatusMsg::SOURCE_POINTS; // Doppler is not used when calculating the orientation of, say, // mosaic-x5, hence: - msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS; - msg.status.position_source = GPSStatusMsg::SOURCE_GPS; + msg.status.orientation_source = GpsStatusMsg::SOURCE_POINTS; + msg.status.position_source = GpsStatusMsg::SOURCE_GPS; msg.latitude = rad2deg(last_insnavgeod_.latitude); msg.longitude = rad2deg(last_insnavgeod_.longitude); msg.altitude = last_insnavgeod_.height; @@ -1747,7 +1794,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - return msg; + publish("/gpsfix", msg, time_obj); }; Timestamp MessageParser::timestampSBF(const std::vector& data, @@ -1901,6 +1948,9 @@ namespace io { last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); if (settings_->publish_pvtgeodetic) publish("/pvtgeodetic", last_pvtgeodetic_, time_obj); + assembleTwist(time_obj); + assembleNavSatFix(time_obj); + assembleGpsFix(time_obj); break; } case BASE_VECTOR_CART: @@ -1970,6 +2020,9 @@ namespace io { if (settings_->publish_poscovgeodetic) publish("/poscovgeodetic", last_poscovgeodetic_, time_obj); + assembleNavSatFix(time_obj); + assembleGpsFix(time_obj); + assembleGpsFix(time_obj); break; } case ATT_EULER: @@ -1988,6 +2041,7 @@ namespace io { last_atteuler_.header.stamp = timestampToRos(time_obj); if (settings_->publish_atteuler) publish("/atteuler", last_atteuler_, time_obj); + assembleGpsFix(time_obj); break; } case ATT_COV_EULER: @@ -2006,6 +2060,7 @@ namespace io { last_attcoveuler_.header.stamp = timestampToRos(time_obj); if (settings_->publish_attcoveuler) publish("/attcoveuler", last_attcoveuler_, time_obj); + assembleGpsFix(time_obj); break; } case INS_NAV_CART: // Position, velocity and orientation in cartesian @@ -2030,6 +2085,7 @@ namespace io { timestampSBF(telegram->message, settings_->use_gnss_time); last_insnavcart_.header.stamp = timestampToRos(time_obj); publish("/insnavcart", last_insnavcart_, time_obj); + assembleLocalizationEcef(time_obj); break; } case INS_NAV_GEOD: // Position, velocity and orientation in geodetic @@ -2055,12 +2111,10 @@ namespace io { last_insnavgeod_.header.stamp = timestampToRos(time_obj); if (settings_->publish_insnavgeod) publish("/insnavgeod", last_insnavgeod_, time_obj); - if (settings_->publish_twist) - { - TwistWithCovarianceStampedMsg twist = assembleTwist(true); - publish("/twist_ins", twist, - time_obj); - } + assembleLocalizationUtm(time_obj); + assembleLocalizationEcef(time_obj); + assembleTwist(time_obj, true); + assembleGpsFix(time_obj); break; } @@ -2214,6 +2268,7 @@ namespace io { last_measepoch_.header.stamp = timestampToRos(time_obj); if (settings_->publish_measepoch) publish("/measepoch", last_measepoch_, time_obj); + assembleGpsFix(time_obj); break; } case DOP: @@ -2243,11 +2298,7 @@ namespace io { if (settings_->publish_velcovgeodetic) publish("/velcovgeodetic", last_velcovgeodetic_, time_obj); - if (settings_->publish_twist) - { - TwistWithCovarianceStampedMsg twist = assembleTwist(); - publish("/twist", twist, time_obj); - } + assembleTwist(time_obj); break; } case RECEIVER_STATUS: @@ -2549,13 +2600,13 @@ namespace io { { case evGPSFix: { - GPSFixMsg msg; + GpsFixMsg msg; try { - msg = assembleGPSFix(); + msg = assembleGpsFix(); } catch (std::runtime_error& e) { - node_->log(LogLevel::DEBUG, "GPSFixMsg: " + + node_->log(LogLevel::DEBUG, "GpsFixMsg: " + std::string(e.what())); break; } msg.header.frame_id = settings_->frame_id; @@ -2577,7 +2628,7 @@ namespace io { { wait(time_obj); } - publish("/gpsfix", msg); + publish("/gpsfix", msg); break; } } @@ -2585,13 +2636,13 @@ namespace io { { case evINSGPSFix: { - GPSFixMsg msg; + GpsFixMsg msg; try { - msg = assembleGPSFix(); + msg = assembleGpsFix(); } catch (std::runtime_error& e) { - node_->log(LogLevel::DEBUG, "GPSFixMsg: " + + node_->log(LogLevel::DEBUG, "GpsFixMsg: " + std::string(e.what())); break; } if (settings_->ins_use_poi) @@ -2615,7 +2666,7 @@ namespace io { { wait(time_obj); } - publish("/gpsfix", msg); + publish("/gpsfix", msg); break; } } From 8f85fadfb006446a510a17206ddcfeaab56a2e6a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 21:55:15 +0100 Subject: [PATCH 036/170] Change class privacy --- .../septentrio_gnss_driver/communication/async_manager.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 7d352480..e64dfa89 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -107,6 +107,11 @@ namespace io { template class AsyncManager : public AsyncManagerBase { + friend class TcpIo; + friend class SerialIo; + friend class SbfFileIo; + friend class PcapFileIo; + public: /** * @brief Class constructor From 721872eda2eeb88ea7dc4c705cc97fe2e8c9b997 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 22:08:37 +0100 Subject: [PATCH 037/170] Update changelog --- CHANGELOG.rst | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index a9ffcd47..7742d5ed 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,8 +4,10 @@ Changelog for package septentrio_gnss_driver 1.2.4 (upcoming) ------------------ -* New Features - * Output of localization and tf in ECEF frame +* Improvements + * Rework IO core and message handling -> reduce system load +* Preliminary Features + * Output of localization and tf in ECEF frame, feedback welcome 1.2.3 (2022-11-09) ------------------ From 0eaf9d49f05212dd86be10d24a796b3f91f4f4bb Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 23:00:46 +0100 Subject: [PATCH 038/170] Change timestamp code --- .../communication/async_manager.hpp | 5 - .../communication/io.hpp | 3 +- .../parsers/message_parser.hpp | 14 +- .../parsers/message_parser.cpp | 153 +++++++++--------- 4 files changed, 84 insertions(+), 91 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index e64dfa89..7d352480 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -107,11 +107,6 @@ namespace io { template class AsyncManager : public AsyncManagerBase { - friend class TcpIo; - friend class SerialIo; - friend class SbfFileIo; - friend class PcapFileIo; - public: /** * @brief Class constructor diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 11dc20af..71129ac9 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -116,8 +116,9 @@ namespace io { std::unique_ptr stream_; }; - struct SerialIo + class SerialIo { + public: SerialIo(ROSaicNodeBase* node) : node_(node), flowcontrol_(node->settings()->hw_flow_control), baudrate_(node->settings()->baudrate) diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index a83045e3..4a567fef 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -186,15 +186,13 @@ namespace io { ROSaicNodeBase* node_; /** - * @brief Pointe to settings struct + * @brief Pointer to settings struct */ Settings* settings_; /** - * @brief Timestamp of receiving buffer + * @brief Map of NMEA messgae IDs and uint8_t */ - Timestamp recvTimestamp_; - std::unordered_map nmeaMap_{ {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPRMC", 1}, {"$INRMC", 1}, {"$GPGSA", 2}, {"$INGSA", 2}, {"$GAGSV", 3}, {"$INGSV", 3}}; @@ -371,12 +369,10 @@ namespace io { * This is either done using the TOW as transmitted with the SBF block (if * "use_gnss" is true), or using the current time. * @param[in] data Pointer to the buffer - * @param[in] use_gnss_time If true, the TOW as transmitted with the SBF - * block is used, otherwise the current time * @return Timestamp object containing seconds and nanoseconds since last * epoch */ - Timestamp timestampSBF(const std::vector& data, bool use_gnss_time); + Timestamp timestampSBF(const std::vector& data); /** * @brief Calculates the timestamp, in the Unix Epoch time format @@ -386,11 +382,9 @@ namespace io { * the beginning of the current GPS week as transmitted by the SBF block * @param[in] wnc (Week Number Counter) counts the number of complete weeks * elapsed since January 6, 1980 - * @param[in] use_gnss If true, the TOW as transmitted with the SBF block is - * used, otherwise the current time * @return Timestamp object containing seconds and nanoseconds since last * epoch */ - Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time); + Timestamp timestampSBF(uint32_t tow, uint16_t wnc); }; } // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index f2c08660..a1f29366 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -343,13 +343,10 @@ namespace io { if (validValue(last_insnavgeod_.block_header.tow)) { // INS tow and extsens meas tow have the same time scale - Timestamp tsImu = - timestampSBF(last_extsensmeas_.block_header.tow, - last_extsensmeas_.block_header.wnc, true); - Timestamp tsIns = - timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - true); // Filling in the orientation data + Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow, + last_extsensmeas_.block_header.wnc); + Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc); static int64_t maxDt = (settings_->polling_period_pvt == 0) ? 10000000 @@ -1797,13 +1794,12 @@ namespace io { publish("/gpsfix", msg, time_obj); }; - Timestamp MessageParser::timestampSBF(const std::vector& data, - bool use_gnss_time) + Timestamp MessageParser::timestampSBF(const std::vector& data) { uint32_t tow = parsing_utilities::getTow(data.data()); uint16_t wnc = parsing_utilities::getWnc(data.data()); - return timestampSBF(tow, wnc, use_gnss_time); + return timestampSBF(tow, wnc); } /// If the current time shall be employed, it is calculated via the time(NULL) @@ -1811,30 +1807,25 @@ namespace io { /// (2020), the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the /// settings_->leap_seconds ROSaic parameter accordingly as soon as the /// next leap second is inserted into the UTC time. - Timestamp MessageParser::timestampSBF(uint32_t tow, uint16_t wnc, - bool use_gnss_time) + Timestamp MessageParser::timestampSBF(uint32_t tow, uint16_t wnc) { Timestamp time_obj; - if (use_gnss_time) - { - // conversion from GPS time of week and week number to UTC taking leap - // seconds into account - static uint64_t secToNSec = 1000000000; - static uint64_t mSec2NSec = 1000000; - static uint64_t nsOfGpsStart = - 315964800 * - secToNSec; // GPS week counter starts at 1980-01-06 which is - // 315964800 seconds since Unix epoch (1970-01-01 UTC) - static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec; - - time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek; - - if (current_leap_seconds_ != -128) - time_obj -= current_leap_seconds_ * secToNSec; - } else - { - time_obj = recvTimestamp_; // TODO use timestamp from telegram !!! - } + + // conversion from GPS time of week and week number to UTC taking leap + // seconds into account + static uint64_t secToNSec = 1000000000; + static uint64_t mSec2NSec = 1000000; + static uint64_t nsOfGpsStart = + 315964800 * + secToNSec; // GPS week counter starts at 1980-01-06 which is + // 315964800 seconds since Unix epoch (1970-01-01 UTC) + static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec; + + time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek; + + if (current_leap_seconds_ != -128) + time_obj -= current_leap_seconds_ * secToNSec; + return time_obj; } @@ -1926,8 +1917,9 @@ namespace io { break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/pvtcartesian", msg, time_obj); @@ -1943,8 +1935,9 @@ namespace io { break; } last_pvtgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); if (settings_->publish_pvtgeodetic) publish("/pvtgeodetic", last_pvtgeodetic_, time_obj); @@ -1964,8 +1957,9 @@ namespace io { break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/basevectorcart", msg, time_obj); break; @@ -1981,8 +1975,9 @@ namespace io { break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/basevectorgeod", msg, time_obj); break; @@ -1998,8 +1993,9 @@ namespace io { break; } msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/poscovcartesian", msg, time_obj); break; @@ -2014,8 +2010,9 @@ namespace io { break; } last_poscovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); if (settings_->publish_poscovgeodetic) publish("/poscovgeodetic", last_poscovgeodetic_, @@ -2036,8 +2033,9 @@ namespace io { break; } last_atteuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_atteuler_.header.stamp = timestampToRos(time_obj); if (settings_->publish_atteuler) publish("/atteuler", last_atteuler_, time_obj); @@ -2055,8 +2053,9 @@ namespace io { break; } last_attcoveuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_attcoveuler_.header.stamp = timestampToRos(time_obj); if (settings_->publish_attcoveuler) publish("/attcoveuler", last_attcoveuler_, time_obj); @@ -2081,8 +2080,9 @@ namespace io { { last_insnavcart_.header.frame_id = settings_->frame_id; } - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_insnavcart_.header.stamp = timestampToRos(time_obj); publish("/insnavcart", last_insnavcart_, time_obj); assembleLocalizationEcef(time_obj); @@ -2106,8 +2106,9 @@ namespace io { { last_insnavgeod_.header.frame_id = settings_->frame_id; } - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_insnavgeod_.header.stamp = timestampToRos(time_obj); if (settings_->publish_insnavgeod) publish("/insnavgeod", last_insnavgeod_, time_obj); @@ -2130,8 +2131,9 @@ namespace io { break; } msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/imusetup", msg, time_obj); break; @@ -2149,8 +2151,9 @@ namespace io { break; } msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/velsensorsetup", msg, time_obj); break; @@ -2175,8 +2178,9 @@ namespace io { { msg.header.frame_id = settings_->frame_id; } - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/exteventinsnavcart", msg, time_obj); break; @@ -2200,8 +2204,9 @@ namespace io { { msg.header.frame_id = settings_->frame_id; } - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; msg.header.stamp = timestampToRos(time_obj); publish("/exteventinsnavgeod", msg, time_obj); break; @@ -2219,8 +2224,9 @@ namespace io { break; } last_extsensmeas_.header.frame_id = settings_->imu_frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_extsensmeas_.header.stamp = timestampToRos(time_obj); if (settings_->publish_extsensormeas) publish("/extsensormeas", last_extsensmeas_, @@ -2263,8 +2269,9 @@ namespace io { break; } last_measepoch_.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_measepoch_.header.stamp = timestampToRos(time_obj); if (settings_->publish_measepoch) publish("/measepoch", last_measepoch_, time_obj); @@ -2292,8 +2299,9 @@ namespace io { break; } last_velcovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = - timestampSBF(telegram->message, settings_->use_gnss_time); + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); if (settings_->publish_velcovgeodetic) publish("/velcovgeodetic", last_velcovgeodetic_, @@ -2848,17 +2856,14 @@ namespace io { { if (settings_->septentrio_receiver_type == "gnss") { - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); + last_pvtgeodetic_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } if (settings_->septentrio_receiver_type == "ins") { time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); + last_insnavgeod_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } } else @@ -2890,15 +2895,13 @@ namespace io { if (settings_->septentrio_receiver_type == "gnss") { time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc, - settings_->use_gnss_time); + last_pvtgeodetic_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } if (settings_->septentrio_receiver_type == "ins") { time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc, - settings_->use_gnss_time); + last_insnavgeod_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } } else From 0a16d363d4b0e8f3f991d109ae3fbe0222152072 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 23:15:40 +0100 Subject: [PATCH 039/170] Add namespace to enum --- .../abstraction/typedefs.hpp | 30 ++-- .../communication/async_manager.hpp | 76 ++++---- .../communication/io.hpp | 65 +++---- .../communication/telegram.hpp | 6 +- .../parsers/sbf_blocks.hpp | 168 +++++++++--------- .../communication/communication_core.cpp | 42 ++--- .../communication/telegram_handler.cpp | 26 +-- .../node/rosaic_node.cpp | 62 +++---- .../parsers/message_parser.cpp | 90 +++++----- 9 files changed, 285 insertions(+), 280 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 396588be..dc1d8b08 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -158,14 +158,16 @@ inline Timestamp timestampFromRos(const TimestampRos& tsr) { return tsr.toNSec() /** * @brief Log level for ROS logging */ -enum LogLevel -{ - DEBUG, - INFO, - WARN, - ERROR, - FATAL -}; +namespace log_level { + enum LogLevel + { + DEBUG, + INFO, + WARN, + ERROR, + FATAL + }; +} /** * @class ROSaicNodeBase @@ -233,23 +235,23 @@ class ROSaicNodeBase * @param[in] logLevel Log level * @param[in] s String to log */ - void log(LogLevel logLevel, const std::string& s) + void log(log_level::LogLevel logLevel, const std::string& s) { switch (logLevel) { - case LogLevel::DEBUG: + case log_level::DEBUG: ROS_DEBUG_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::INFO: + case log_level::INFO: ROS_INFO_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::WARN: + case log_level::WARN: ROS_WARN_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::ERROR: + case log_level::ERROR: ROS_ERROR_STREAM(ros::this_node::getName() << ": " << s); break; - case LogLevel::FATAL: + case log_level::FATAL: ROS_FATAL_STREAM(ros::this_node::getName() << ": " << s); break; default: diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 7d352480..f4d4b60a 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -158,7 +158,7 @@ namespace io { node_(node), ioInterface_(node), telegramQueue_(telegramQueue) { - node_->log(LogLevel::DEBUG, "AsyncManager created."); + node_->log(log_level::DEBUG, "AsyncManager created."); } template @@ -166,11 +166,11 @@ namespace io { { running_ = false; close(); - node_->log(LogLevel::DEBUG, "AsyncManager shutting down threads"); + node_->log(log_level::DEBUG, "AsyncManager shutting down threads"); ioInterface_.ioService_.stop(); ioThread_.join(); watchdogThread_.join(); - node_->log(LogLevel::DEBUG, "AsyncManager threads stopped"); + node_->log(log_level::DEBUG, "AsyncManager threads stopped"); } template @@ -193,7 +193,7 @@ namespace io { { if (cmd.size() == 0) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "AsyncManager message size to be sent to the Rx would be 0"); return; } @@ -220,7 +220,7 @@ namespace io { void AsyncManager::runIoService() { ioInterface_.ioService_.run(); - node_->log(LogLevel::DEBUG, "AsyncManager ioService terminated."); + node_->log(log_level::DEBUG, "AsyncManager ioService terminated."); } template @@ -232,7 +232,7 @@ namespace io { if (running_ && ioInterface_.ioService_.stopped()) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "AsyncManager connection lost. Trying to reconnect."); ioInterface_.ioService_.reset(); ioThread_.join(); @@ -252,12 +252,12 @@ namespace io { if (!ec) { // Prints the data that was sent - node_->log(LogLevel::DEBUG, "AsyncManager sent the following " + - std::to_string(cmd.size()) + - " bytes to the Rx: " + cmd); + node_->log(log_level::DEBUG, "AsyncManager sent the following " + + std::to_string(cmd.size()) + + " bytes to the Rx: " + cmd); } else { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "AsyncManager was unable to send the following " + std::to_string(cmd.size()) + " bytes to the Rx: " + cmd); @@ -300,7 +300,7 @@ namespace io { { case 0: { - telegram_->type = message_type::UNKNOWN; + telegram_->type = telegram_type::UNKNOWN; readUnknown(); break; } @@ -310,25 +310,25 @@ namespace io { { case SBF_SYNC_BYTE_2: { - telegram_->type = message_type::SBF; + telegram_->type = telegram_type::SBF; readSbfHeader(); break; } case NMEA_SYNC_BYTE_2: { - telegram_->type = message_type::NMEA; + telegram_->type = telegram_type::NMEA; readSync<2>(); break; } case NMEA_INS_SYNC_BYTE_2: { - telegram_->type = message_type::NMEA_INS; + telegram_->type = telegram_type::NMEA_INS; readSync<2>(); break; } case RESPONSE_SYNC_BYTE_2: { - telegram_->type = message_type::RESPONSE; + telegram_->type = telegram_type::RESPONSE; readSync<2>(); break; } @@ -337,7 +337,7 @@ namespace io { std::stringstream ss; ss << std::hex << currByte; node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager sync byte 2 read fault, should never come here.. Received byte was " + ss.str()); resync(); @@ -352,7 +352,7 @@ namespace io { { case NMEA_SYNC_BYTE_3: { - if (telegram_->type == message_type::NMEA) + if (telegram_->type == telegram_type::NMEA) readString(); else resync(); @@ -360,7 +360,7 @@ namespace io { } case NMEA_INS_SYNC_BYTE_3: { - if (telegram_->type == message_type::NMEA_INS) + if (telegram_->type == telegram_type::NMEA_INS) readString(); else resync(); @@ -368,7 +368,7 @@ namespace io { } case RESPONSE_SYNC_BYTE_3: { - if (telegram_->type == message_type::RESPONSE) + if (telegram_->type == telegram_type::RESPONSE) readString(); else resync(); @@ -376,7 +376,7 @@ namespace io { } case RESPONSE_SYNC_BYTE_3a: { - if (telegram_->type == message_type::RESPONSE) + if (telegram_->type == telegram_type::RESPONSE) readString(); else resync(); @@ -384,10 +384,10 @@ namespace io { } case ERROR_SYNC_BYTE_3: { - if (telegram_->type == message_type::RESPONSE) + if (telegram_->type == telegram_type::RESPONSE) { telegram_->type = - message_type::ERROR_RESPONSE; + telegram_type::ERROR_RESPONSE; readString(); } else resync(); @@ -398,7 +398,7 @@ namespace io { std::stringstream ss; ss << std::hex << currByte; node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager sync byte 3 read fault, should never come here. Received byte was " + ss.str()); resync(); @@ -410,7 +410,7 @@ namespace io { default: { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager sync read fault, should never come here."); resync(); break; @@ -420,14 +420,14 @@ namespace io { } else { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager sync read fault, wrong number of bytes read: " + std::to_string(numBytes)); resync(); } } else { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "AsyncManager sync read error: " + ec.message()); resync(); } @@ -452,7 +452,7 @@ namespace io { if (length > MAX_SBF_SIZE) { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager SBF header read fault, length of block exceeds " + std::to_string(MAX_SBF_SIZE) + ": " + std::to_string(length)); @@ -461,14 +461,14 @@ namespace io { } else { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager SBF header read fault, wrong number of bytes read: " + std::to_string(numBytes)); resync(); } } else { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "AsyncManager SBF header read error: " + ec.message()); resync(); @@ -497,19 +497,19 @@ namespace io { telegramQueue_->push(telegram_); } else - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "AsyncManager crc failed for SBF " + std::to_string(telegram_->sbfId) + "."); } else { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager SBF read fault, wrong number of bytes read: " + std::to_string(numBytes)); } } else { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "AsyncManager SBF read error: " + ec.message()); } resync(); @@ -541,7 +541,7 @@ namespace io { if (numBytes == 1) { telegram_->message.push_back(buf_[0]); - /*node_->log(LogLevel::DEBUG, + /*node_->log(log_level::DEBUG, "Buffer: " + std::string(telegram_->message.begin(), telegram_->message.end()));*/ @@ -554,7 +554,7 @@ namespace io { telegram_->message[0] = buf_[0]; telegram_->stamp = node_->getTime(); node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager string read fault, sync 1 found."); readSync<1>(); break; @@ -566,7 +566,7 @@ namespace io { telegramQueue_->push(telegram_); else node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "LF wo CR: " + std::string(telegram_->message.begin(), telegram_->message.end())); @@ -575,7 +575,7 @@ namespace io { } case CONNECTION_DESCRIPTOR_FOOTER: { - telegram_->type = message_type::CONNECTION_DESCRIPTOR; + telegram_->type = telegram_type::CONNECTION_DESCRIPTOR; telegramQueue_->push(telegram_); resync(); break; @@ -589,14 +589,14 @@ namespace io { } else { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "AsyncManager string read fault, wrong number of bytes read: " + std::to_string(numBytes)); resync(); } } else { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "AsyncManager string read error: " + ec.message()); resync(); } diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 71129ac9..00813941 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -75,7 +75,7 @@ namespace io { endpointIterator = resolver.resolve(query); } catch (std::runtime_error& e) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "Could not resolve " + node_->settings()->tcp_ip + " on port " + node_->settings()->tcp_port + ": " + e.what()); @@ -84,9 +84,9 @@ namespace io { stream_.reset(new boost::asio::ip::tcp::socket(ioService_)); - node_->log(LogLevel::INFO, "Connecting to tcp://" + - node_->settings()->tcp_ip + ":" + - node_->settings()->tcp_port + "..."); + node_->log(log_level::INFO, "Connecting to tcp://" + + node_->settings()->tcp_ip + ":" + + node_->settings()->tcp_port + "..."); try { @@ -94,12 +94,12 @@ namespace io { stream_->set_option(boost::asio::ip::tcp::no_delay(true)); - node_->log(LogLevel::INFO, + node_->log(log_level::INFO, "Connected to " + endpointIterator->host_name() + ":" + endpointIterator->service_name() + "."); } catch (std::runtime_error& e) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "Could not connect to " + endpointIterator->host_name() + ": " + endpointIterator->service_name() + ": " + e.what()); @@ -143,7 +143,7 @@ namespace io { { try { - node_->log(LogLevel::INFO, + node_->log(log_level::INFO, "Connecting serially to device" + node_->settings()->device + ", targeted baudrate: " + @@ -152,7 +152,7 @@ namespace io { opened = true; } catch (const boost::system::system_error& err) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "SerialCoket: Could not open serial port " + port_ + ". Error: " + err.what() + ". Will retry every second."); @@ -208,10 +208,10 @@ namespace io { [[nodiscard]] bool setBaudrate() { // Setting the baudrate, incrementally.. - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "Gradually increasing the baudrate to the desired value..."); boost::asio::serial_port_base::baud_rate current_baudrate; - node_->log(LogLevel::DEBUG, "Initiated current_baudrate object..."); + node_->log(log_level::DEBUG, "Initiated current_baudrate object..."); try { stream_->get_option(current_baudrate); // Note that this sets @@ -223,10 +223,10 @@ namespace io { } catch (boost::system::system_error& e) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "get_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); + node_->log(log_level::INFO, "Additional info about error is " + + boost::diagnostic_information(e)); /* boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. @@ -239,7 +239,7 @@ namespace io { // Gradually increase the baudrate to the desired value // The desired baudrate can be lower or larger than the // current baudrate; the for loop takes care of both scenarios. - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "Current baudrate is " + std::to_string(current_baudrate.value())); for (uint8_t i = 0; i < baudrates.size(); i++) @@ -261,10 +261,11 @@ namespace io { } catch (boost::system::system_error& e) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "set_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); + node_->log(log_level::INFO, + "Additional info about error is " + + boost::diagnostic_information(e)); return false; } using namespace std::chrono_literals; @@ -276,10 +277,11 @@ namespace io { } catch (boost::system::system_error& e) { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "get_option failed due to " + std::string(e.what())); - node_->log(LogLevel::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); + node_->log(log_level::INFO, + "Additional info about error is " + + boost::diagnostic_information(e)); /* boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. @@ -289,13 +291,14 @@ namespace io { */ return false; } - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "Set ASIO baudrate to " + std::to_string(current_baudrate.value())); } - node_->log(LogLevel::INFO, "Set ASIO baudrate to " + - std::to_string(current_baudrate.value()) + - ", leaving InitializeSerial() method"); + node_->log(log_level::INFO, + "Set ASIO baudrate to " + + std::to_string(current_baudrate.value()) + + ", leaving InitializeSerial() method"); return true; } @@ -321,13 +324,13 @@ namespace io { [[nodiscard]] bool connect() { - node_->log(LogLevel::INFO, "Opening SBF file stream" + - node_->settings()->device + "..."); + node_->log(log_level::INFO, "Opening SBF file stream" + + node_->settings()->device + "..."); int fd = open(node_->settings()->device.c_str(), O_RDONLY); if (fd == -1) { - node_->log(LogLevel::ERROR, "open SBF file failed."); + node_->log(log_level::ERROR, "open SBF file failed."); return false; } stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); @@ -338,8 +341,8 @@ namespace io { } catch (std::runtime_error& e) { - node_->log(LogLevel::ERROR, "assigning SBF file failed due to " + - std::string(e.what())); + node_->log(log_level::ERROR, "assigning SBF file failed due to " + + std::string(e.what())); return false; } return true; @@ -372,8 +375,8 @@ namespace io { [[nodiscard]] bool connect() { - node_->log(LogLevel::INFO, "Opening pcap file stream" + - node_->settings()->device + "..."); + node_->log(log_level::INFO, "Opening pcap file stream" + + node_->settings()->device + "..."); stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 37e43e62..87671de6 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -86,7 +86,7 @@ static const uint8_t CONNECTION_DESCRIPTOR_FOOTER = 0x3E; static const uint16_t SBF_HEADER_SIZE = 8; static const uint16_t MAX_SBF_SIZE = 65535; -namespace message_type { +namespace telegram_type { enum TelegramType { EMPTY, @@ -103,12 +103,12 @@ namespace message_type { struct Telegram { Timestamp stamp; - message_type::TelegramType type; + telegram_type::TelegramType type; uint16_t sbfId; std::vector message; Telegram() : - stamp(0), type(message_type::EMPTY), sbfId(0), + stamp(0), type(telegram_type::EMPTY), sbfId(0), message(std::vector(3)) { message.reserve(MAX_SBF_SIZE); diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 5ee7f73b..a63a3c49 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -386,13 +386,13 @@ bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header) qiLittleEndianParser(it, block_header.sync_1); if (block_header.sync_1 != SBF_SYNC_1) { - node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 1."); + node->log(log_level::ERROR, "Parse error: Wrong sync byte 1."); return false; } qiLittleEndianParser(it, block_header.sync_2); if (block_header.sync_2 != SBF_SYNC_2) { - node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 2."); + node->log(log_level::ERROR, "Parse error: Wrong sync byte 2."); return false; } qiLittleEndianParser(it, block_header.crc); @@ -438,8 +438,8 @@ bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, qiLittleEndianParser(it, msg.n2); if (msg.n2 > MAXSB_CHANNELSTATEINFO) { - node->log(LogLevel::ERROR, "Parse error: Too many ChannelStateInfo " + - std::to_string(msg.n2)); + node->log(log_level::ERROR, "Parse error: Too many ChannelStateInfo " + + std::to_string(msg.n2)); return false; } qiLittleEndianParser(it, msg.rx_channel); @@ -464,14 +464,14 @@ bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& m return false; if (msg.block_header.id != 4013) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_CHANNELSATINFO) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many ChannelSatInfo " + std::to_string(msg.n)); return false; } @@ -486,7 +486,7 @@ bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& m } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -504,8 +504,8 @@ bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg) return false; if (msg.block_header.id != 4001) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.nr_sv); @@ -523,7 +523,7 @@ bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg) qiLittleEndianParser(it, msg.vpl); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -573,8 +573,8 @@ bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, std::advance(it, sb1_length - 20); // skip padding if (msg.n2 > MAXSB_MEASEPOCH_T2) { - node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType2 " + - std::to_string(msg.n2)); + node->log(log_level::ERROR, "Parse error: Too many MeasEpochChannelType2 " + + std::to_string(msg.n2)); return false; } msg.type2.resize(msg.n2); @@ -596,15 +596,15 @@ bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) return false; if (msg.block_header.id != 4027) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_MEASEPOCH_T1) { - node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType1 " + - std::to_string(msg.n)); + node->log(log_level::ERROR, "Parse error: Too many MeasEpochChannelType1 " + + std::to_string(msg.n)); return false; } qiLittleEndianParser(it, msg.sb1_length); @@ -622,7 +622,7 @@ bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -639,8 +639,8 @@ bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& m return false; if (msg.block_header.id != 5902) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } std::advance(it, 2); // reserved @@ -679,7 +679,7 @@ bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& m } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -696,8 +696,8 @@ bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& return false; if (msg.block_header.id != 5914) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.utc_year); @@ -710,7 +710,7 @@ bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& qiLittleEndianParser(it, msg.sync_level); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -727,8 +727,8 @@ bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& return false; if (msg.block_header.id != 4006) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -765,7 +765,7 @@ bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -782,8 +782,8 @@ bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& ms return false; if (msg.block_header.id != 4007) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -820,7 +820,7 @@ bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& ms } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -838,8 +838,8 @@ bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, return false; if (msg.block_header.id != 5938) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.nr_sv); @@ -865,7 +865,7 @@ bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -883,8 +883,8 @@ bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& ms return false; if (msg.block_header.id != 5939) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } ++it; // reserved @@ -904,7 +904,7 @@ bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& ms } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -947,14 +947,14 @@ bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 4043) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_NBVECTORINFO) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many VectorInfoCart " + std::to_string(msg.n)); return false; } @@ -966,7 +966,7 @@ bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1009,14 +1009,14 @@ bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 4028) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > MAXSB_NBVECTORINFO) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many VectorInfoGeod " + std::to_string(msg.n)); return false; } @@ -1028,7 +1028,7 @@ bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1046,8 +1046,8 @@ bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, return false; if ((msg.block_header.id != 4225) && (msg.block_header.id != 4229)) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.gnss_mode); @@ -1166,7 +1166,7 @@ bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1184,8 +1184,8 @@ bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 5905) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1202,7 +1202,7 @@ bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_zb); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1220,8 +1220,8 @@ bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 5906) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1238,7 +1238,7 @@ bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_hb); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1256,8 +1256,8 @@ bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 5907) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1274,7 +1274,7 @@ bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_vzdt); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1292,8 +1292,8 @@ bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 5908) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.mode); @@ -1310,7 +1310,7 @@ bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, qiLittleEndianParser(it, msg.cov_vudt); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1327,14 +1327,14 @@ bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg) return false; if (msg.block_header.id != 4082) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); if (msg.n > 40) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many indicators " + std::to_string(msg.n)); return false; } @@ -1347,7 +1347,7 @@ bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg) } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1378,8 +1378,8 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& return false; if (msg.block_header.id != 4014) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.cpu_load); @@ -1390,7 +1390,7 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& qiLittleEndianParser(it, msg.n); if (msg.n > 18) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Too many AGCState " + std::to_string(msg.n)); return false; } @@ -1404,7 +1404,7 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1421,8 +1421,8 @@ bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& return false; if (msg.block_header.id != 5914) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.utc_year); @@ -1435,7 +1435,7 @@ bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& qiLittleEndianParser(it, msg.sync_level); if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1453,8 +1453,8 @@ bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, return false; if ((msg.block_header.id != 4226) && (msg.block_header.id != 4230)) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.gnss_mode); @@ -1574,7 +1574,7 @@ bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1592,8 +1592,8 @@ bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, return false; if (msg.block_header.id != 4224) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } ++it; // reserved @@ -1612,7 +1612,7 @@ bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1630,8 +1630,8 @@ bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 4244) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } ++it; // reserved @@ -1646,7 +1646,7 @@ bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } return true; @@ -1665,15 +1665,15 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, return false; if (msg.block_header.id != 4050) { - node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + - std::to_string(msg.block_header.id)); + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); return false; } qiLittleEndianParser(it, msg.n); qiLittleEndianParser(it, msg.sb_length); if (msg.sb_length != 28) { - node->log(LogLevel::ERROR, + node->log(log_level::ERROR, "Parse error: Wrong sb_length " + std::to_string(msg.sb_length)); return false; } @@ -1776,7 +1776,7 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, default: { node->log( - LogLevel::ERROR, + log_level::ERROR, "Unknown external sensor measurement type in SBF ExtSensorMeas."); std::advance(it, 24); break; @@ -1785,7 +1785,7 @@ bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, } if (it > itEnd) { - node->log(LogLevel::ERROR, "Parse error: iterator past end."); + node->log(log_level::ERROR, "Parse error: iterator past end."); return false; } hasImuMeas = hasAcc && hasOmega; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index d918ae8f..897f631a 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -131,9 +131,9 @@ namespace io { void CommunicationCore::connect() { - node_->log(LogLevel::DEBUG, "Called connect() method"); + node_->log(log_level::DEBUG, "Called connect() method"); node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "Started timer for calling connect() method until connection succeeds"); boost::asio::io_service io; @@ -160,17 +160,17 @@ namespace io { // and sets all its necessary corrections-related parameters if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { - node_->log(LogLevel::DEBUG, "Configure Rx."); + node_->log(log_level::DEBUG, "Configure Rx."); configureRx(); } - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "Successully connected. Leaving connect() method"); } [[nodiscard]] bool CommunicationCore::initializeIo() { - node_->log(LogLevel::DEBUG, "Called initializeIo() method"); + node_->log(log_level::DEBUG, "Called initializeIo() method"); boost::smatch match; // In fact: smatch is a typedef of match_results if (boost::regex_match(settings_->device, match, @@ -212,17 +212,17 @@ namespace io { std::string proto(match[2]); std::stringstream ss; ss << "Searching for serial port" << proto; - node_->log(LogLevel::DEBUG, ss.str()); + node_->log(log_level::DEBUG, ss.str()); settings_->device = proto; manager_.reset(new AsyncManager(node_, &telegramQueue_)); } else { std::stringstream ss; ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; - node_->log(LogLevel::ERROR, ss.str()); + node_->log(log_level::ERROR, ss.str()); return false; } - node_->log(LogLevel::DEBUG, "Leaving initializeIo() method"); + node_->log(log_level::DEBUG, "Leaving initializeIo() method"); return true; } @@ -235,11 +235,11 @@ namespace io { //! enter command mode via "SSSSSSSSSS". void CommunicationCore::configureRx() { - node_->log(LogLevel::DEBUG, "Called configureRx() method"); + node_->log(log_level::DEBUG, "Called configureRx() method"); if (!initializedIo_) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "Called configureRx() method but IO is not initialized."); return; } @@ -265,7 +265,7 @@ namespace io { send("lif, Identification \x0D"); } - node_->log(LogLevel::INFO, "Setting up Rx."); + node_->log(log_level::INFO, "Setting up Rx."); std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand( settings_->polling_period_pvt); @@ -467,7 +467,7 @@ namespace io { send(ss.str()); } else { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "Please specify a valid parameter for heading and pitch"); } } @@ -498,7 +498,7 @@ namespace io { } else { node_->log( - LogLevel::ERROR, + log_level::ERROR, "Please specify a correct value for IMU orientation angles"); } } @@ -524,7 +524,7 @@ namespace io { } else { node_->log( - LogLevel::ERROR, + log_level::ERROR, "Please specify a correct value for x, y and z in the config file under ant_lever_arm"); } } @@ -550,7 +550,7 @@ namespace io { } else { node_->log( - LogLevel::ERROR, + log_level::ERROR, "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm"); } } @@ -576,7 +576,7 @@ namespace io { } else { node_->log( - LogLevel::ERROR, + log_level::ERROR, "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vsm_lever_arm"); } } @@ -620,7 +620,7 @@ namespace io { send(ss.str()); } else { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "Invalid mode specified for ins_initial_heading."); } } @@ -641,7 +641,7 @@ namespace io { send(ss.str()); } else { - node_->log(LogLevel::ERROR, + node_->log(log_level::ERROR, "Please specify a valid AttStsDev and PosStdDev"); } } @@ -846,8 +846,8 @@ namespace io { } } - node_->log(LogLevel::DEBUG, "Leaving configureRx() method"); - node_->log(LogLevel::INFO, "Setup complete."); + node_->log(log_level::DEBUG, "Leaving configureRx() method"); + node_->log(log_level::INFO, "Setup complete."); } void CommunicationCore::sendVelocity(const std::string& velNmea) @@ -872,7 +872,7 @@ namespace io { { std::shared_ptr telegram; telegramQueue_.pop(telegram); - if (telegram->type != message_type::EMPTY) + if (telegram->type != telegram_type::EMPTY) telegramHandler_.handleTelegram(telegram); } } diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 276b57fb..4c98b897 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -42,46 +42,46 @@ namespace io { { switch (telegram->type) { - case message_type::SBF: + case telegram_type::SBF: { handleSbf(telegram); break; } - case message_type::NMEA: + case telegram_type::NMEA: { handleNmea(telegram); break; } - case message_type::NMEA_INS: + case telegram_type::NMEA_INS: { handleNmea(telegram); break; } - case message_type::RESPONSE: + case telegram_type::RESPONSE: { handleResponse(telegram); break; } - case message_type::ERROR_RESPONSE: + case telegram_type::ERROR_RESPONSE: { handleResponse(telegram); break; } - case message_type::CONNECTION_DESCRIPTOR: + case telegram_type::CONNECTION_DESCRIPTOR: { handleCd(telegram); break; } - case message_type::UNKNOWN: + case telegram_type::UNKNOWN: { - node_->log(LogLevel::DEBUG, "Unhandeled message received: " + + node_->log(log_level::DEBUG, "Unhandeled message received: " + std::string(telegram->message.begin(), telegram->message.end())); break; } default: { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "TelegramHandler received an invalid message to handle"); break; } @@ -103,16 +103,16 @@ namespace io { std::string block_in_string(telegram->message.begin(), telegram->message.end()); - if (telegram->type == message_type::ERROR_RESPONSE) + if (telegram->type == telegram_type::ERROR_RESPONSE) { node_->log( - LogLevel::ERROR, + log_level::ERROR, "Invalid command just sent to the Rx! The Rx's response contains " + std::to_string(block_in_string.size()) + " bytes and reads:\n " + block_in_string); } else { - node_->log(LogLevel::DEBUG, "The Rx's response contains " + + node_->log(log_level::DEBUG, "The Rx's response contains " + std::to_string(block_in_string.size()) + " bytes and reads:\n " + block_in_string); @@ -130,7 +130,7 @@ namespace io { ++cdCtr_; if (cdCtr_ == 2) { - node_->log(LogLevel::INFO, "The connection descriptor is " + + node_->log(log_level::INFO, "The connection descriptor is " + mainConnectionDescriptor_); cdSemaphore_.notify(); diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 7b67b8a2..3aa79d05 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -50,7 +50,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) ros::console::notifyLoggerLevelsChanged(); } - this->log(LogLevel::DEBUG, "Called ROSaicNode() constructor.."); + this->log(log_level::DEBUG, "Called ROSaicNode() constructor.."); tfListener_.reset(new tf2_ros::TransformListener(tfBuffer_)); @@ -61,7 +61,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) // Initializes Connection IO_.connect(); - this->log(LogLevel::DEBUG, "Leaving ROSaicNode() constructor.."); + this->log(log_level::DEBUG, "Leaving ROSaicNode() constructor.."); } bool rosaic_node::ROSaicNode::getROSParams() @@ -92,7 +92,7 @@ bool rosaic_node::ROSaicNode::getROSParams() (settings_.septentrio_receiver_type == "ins") || (settings_.septentrio_receiver_type == "ins_in_gnss_mode"))) { - this->log(LogLevel::FATAL, "Unkown septentrio_receiver_type " + + this->log(log_level::FATAL, "Unkown septentrio_receiver_type " + settings_.septentrio_receiver_type + " use either gnss or ins."); return false; @@ -111,7 +111,7 @@ bool rosaic_node::ROSaicNode::getROSParams() settings_.septentrio_receiver_type == "ins"))) { this->log( - LogLevel::FATAL, + log_level::FATAL, "Please specify a valid polling period for PVT-related SBF blocks and NMEA messages."); return false; } @@ -121,7 +121,7 @@ bool rosaic_node::ROSaicNode::getROSParams() settings_.septentrio_receiver_type == "ins"))) { this->log( - LogLevel::FATAL, + log_level::FATAL, "Please specify a valid polling period for PVT-unrelated SBF blocks and NMEA messages."); return false; } @@ -174,7 +174,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.publish_tf && settings_.publish_tf_ecef) { this->log( - LogLevel::WARN, + log_level::WARN, "Only one of the tfs may be published at once, just activating tf in ECEF "); settings_.publish_tf = false; } @@ -330,28 +330,28 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.publish_atteuler) { this->log( - LogLevel::WARN, + log_level::WARN, "Pitch angle output by topic /atteuler is a tilt angle rotated by " + std::to_string(settings_.heading_offset) + "."); } if (settings_.publish_pose && (settings_.septentrio_receiver_type == "gnss")) { this->log( - LogLevel::WARN, + log_level::WARN, "Pitch angle output by topic /pose is a tilt angle rotated by " + std::to_string(settings_.heading_offset) + "."); } } - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "IMU roll offset: " + std::to_string(settings_.theta_x)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "IMU pitch offset: " + std::to_string(settings_.theta_y)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "IMU yaw offset: " + std::to_string(settings_.theta_z)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "Ant heading offset: " + std::to_string(settings_.heading_offset)); - this->log(LogLevel::DEBUG, + this->log(log_level::DEBUG, "Ant pitch offset: " + std::to_string(settings_.pitch_offset)); // ins_initial_heading param @@ -367,7 +367,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.publish_tf && !settings_.ins_use_poi) { this->log( - LogLevel::ERROR, + log_level::ERROR, "If tf shall be published, ins_use_poi has to be set to true! It is set automatically to true."); settings_.ins_use_poi = true; } @@ -470,28 +470,28 @@ bool rosaic_node::ROSaicNode::getROSParams() param("ntrip_settings/mode", tempString, std::string("")); if (tempString != "") this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/mode has been removed, see README under section rtk_settings."); param("ntrip_settings/caster", tempString, std::string("")); if (tempString != "") this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/caster has been removed, see README under section rtk_settings."); param("ntrip_settings/rx_has_internet", tempBool, false); if (tempBool) this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/rx_has_internet has been removed, see README under section rtk_settings."); param("ntrip_settings/rx_input_corrections_tcp", tempInt, 0); if (tempInt != 0) this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/rx_input_corrections_tcp has been removed, see README under section rtk_settings."); param("ntrip_settings/rx_input_corrections_serial", tempString, std::string("")); if (tempString != "") this->log( - LogLevel::WARN, + log_level::WARN, "Deprecation warning: parameter ntrip_settings/rx_input_corrections_serial has been removed, see README under section rtk_settings."); } @@ -500,7 +500,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (!settings_.multi_antenna) { this->log( - LogLevel::WARN, + log_level::WARN, "AttEuler needs multi-antenna receiver. Multi-antenna setting automatically activated. Deactivate publishing of AttEuler if multi-antenna operation is not available."); settings_.multi_antenna = true; } @@ -515,7 +515,7 @@ bool rosaic_node::ROSaicNode::getROSParams() ins_use_vsm = ((settings_.ins_vsm_ros_source == "odometry") || (settings_.ins_vsm_ros_source == "twist")); if (!settings_.ins_vsm_ros_source.empty() && !ins_use_vsm) - this->log(LogLevel::ERROR, "unknown ins_vsm/ros/source " + + this->log(log_level::ERROR, "unknown ins_vsm/ros/source " + settings_.ins_vsm_ros_source + " -> VSM input will not be used!"); param("ins_vsm/ip_server/id", settings_.ins_vsm_ip_server_id, @@ -528,7 +528,7 @@ bool rosaic_node::ROSaicNode::getROSParams() param("ins_vsm/ip_server/keep_open", settings_.ins_vsm_ip_server_keep_open, true); this->log( - LogLevel::INFO, + log_level::INFO, "external velocity sensor measurements via ip_server are used."); } @@ -540,7 +540,7 @@ bool rosaic_node::ROSaicNode::getROSParams() static_cast(115200)); param("ins_vsm/serial/keep_open", settings_.ins_vsm_serial_keep_open, true); - this->log(LogLevel::INFO, + this->log(log_level::INFO, "external velocity sensor measurements via serial are used."); } @@ -554,7 +554,7 @@ bool rosaic_node::ROSaicNode::getROSParams() { ins_use_vsm = false; this->log( - LogLevel::ERROR, + log_level::ERROR, "all elements of ins_vsm/ros/config have been set to false -> VSM input will not be used!"); } else { @@ -567,7 +567,7 @@ bool rosaic_node::ROSaicNode::getROSParams() if (settings_.ins_vsm_ros_variances.size() != 3) { this->log( - LogLevel::ERROR, + log_level::ERROR, "ins_vsm/ros/variances has to be of size 3 for var_x, var_y, and var_z -> VSM input will not be used!"); ins_use_vsm = false; settings_.ins_vsm_ros_source = ""; @@ -580,7 +580,7 @@ bool rosaic_node::ROSaicNode::getROSParams() (settings_.ins_vsm_ros_variances[i] <= 0.0)) { this->log( - LogLevel::ERROR, + log_level::ERROR, "ins_vsm/ros/config of element " + std::to_string(i) + " has been set to be used but its variance is not > 0.0 -> its VSM input will not be used!"); @@ -595,7 +595,7 @@ bool rosaic_node::ROSaicNode::getROSParams() ins_use_vsm = false; settings_.ins_vsm_ros_source = ""; this->log( - LogLevel::ERROR, + log_level::ERROR, "all elements of ins_vsm/ros/config have been set to false due to invalid covariances -> VSM input will not be used!"); } } @@ -604,12 +604,12 @@ bool rosaic_node::ROSaicNode::getROSParams() { settings_.ins_vsm_ros_source = ""; this->log( - LogLevel::ERROR, + log_level::ERROR, "ins_vsm/ros/config has to be of size 3 to signal wether to use v_x, v_y, and v_z -> VSM input will not be used!"); } if (ins_use_vsm) { - this->log(LogLevel::INFO, "ins_vsm/ros/source " + + this->log(log_level::INFO, "ins_vsm/ros/source " + settings_.ins_vsm_ros_source + " will be used."); registerSubscriber(); @@ -617,7 +617,7 @@ bool rosaic_node::ROSaicNode::getROSParams() } // To be implemented: RTCM, raw data settings, PPP, SBAS ... - this->log(LogLevel::DEBUG, "Finished getROSParams() method"); + this->log(log_level::DEBUG, "Finished getROSParams() method"); return true; } @@ -647,7 +647,7 @@ void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, found = true; } catch (const tf2::TransformException& ex) { - this->log(LogLevel::WARN, "Waiting for transform from " + sourceFrame + + this->log(log_level::WARN, "Waiting for transform from " + sourceFrame + " to " + targetFrame + ": " + ex.what() + "."); found = false; diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index a1f29366..c53e8085 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -1176,7 +1176,7 @@ namespace io { default: { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "PVTGeodetic's Mode field contains an invalid type of PVT solution."); break; } @@ -1258,7 +1258,7 @@ namespace io { default: { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "INSNavGeod's Mode field contains an invalid type of PVT solution."); break; } @@ -1529,7 +1529,7 @@ namespace io { default: { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "PVTGeodetic's Mode field contains an invalid type of PVT solution."); break; } @@ -1664,7 +1664,7 @@ namespace io { default: { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "INSNavGeod's Mode field contains an invalid type of PVT solution."); break; } @@ -1853,7 +1853,7 @@ namespace io { available yet.");*/ if (settings_->read_from_sbf_log || settings_->read_from_pcap) node_->log( - LogLevel::WARN, + log_level::WARN, "No leap seconds were set and none were received from log yet."); } } @@ -1877,11 +1877,11 @@ namespace io { } else { node_->log( - LogLevel::DEBUG, + log_level::DEBUG, "Not publishing tf with GNSS time because no leap seconds are available yet."); if (settings_->read_from_sbf_log || settings_->read_from_pcap) node_->log( - LogLevel::WARN, + log_level::WARN, "No leap seconds were set and none were received from log yet."); } } @@ -1899,7 +1899,7 @@ namespace io { */ void MessageParser::parseSbf(const std::shared_ptr& telegram) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + " made up of " + std::to_string(telegram->message.size()) + " bytes..."); @@ -1913,7 +1913,7 @@ namespace io { if (!PVTCartesianParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, "parse error in PVTCartesian"); + node_->log(log_level::ERROR, "parse error in PVTCartesian"); break; } msg.header.frame_id = settings_->frame_id; @@ -1931,7 +1931,7 @@ namespace io { if (!PVTGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_pvtgeodetic_)) { - node_->log(LogLevel::ERROR, "parse error in PVTGeodetic"); + node_->log(log_level::ERROR, "parse error in PVTGeodetic"); break; } last_pvtgeodetic_.header.frame_id = settings_->frame_id; @@ -1953,7 +1953,7 @@ namespace io { if (!BaseVectorCartParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, "parse error in BaseVectorCart"); + node_->log(log_level::ERROR, "parse error in BaseVectorCart"); break; } msg.header.frame_id = settings_->frame_id; @@ -1971,7 +1971,7 @@ namespace io { if (!BaseVectorGeodParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, "parse error in BaseVectorGeod"); + node_->log(log_level::ERROR, "parse error in BaseVectorGeod"); break; } msg.header.frame_id = settings_->frame_id; @@ -1989,7 +1989,7 @@ namespace io { if (!PosCovCartesianParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, "parse error in PosCovCartesian"); + node_->log(log_level::ERROR, "parse error in PosCovCartesian"); break; } msg.header.frame_id = settings_->frame_id; @@ -2006,7 +2006,7 @@ namespace io { if (!PosCovGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_poscovgeodetic_)) { - node_->log(LogLevel::ERROR, "parse error in PosCovGeodetic"); + node_->log(log_level::ERROR, "parse error in PosCovGeodetic"); break; } last_poscovgeodetic_.header.frame_id = settings_->frame_id; @@ -2029,7 +2029,7 @@ namespace io { telegram->message.end(), last_atteuler_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in AttEuler"); + node_->log(log_level::ERROR, "parse error in AttEuler"); break; } last_atteuler_.header.frame_id = settings_->frame_id; @@ -2049,7 +2049,7 @@ namespace io { telegram->message.end(), last_attcoveuler_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in AttCovEuler"); + node_->log(log_level::ERROR, "parse error in AttCovEuler"); break; } last_attcoveuler_.header.frame_id = settings_->frame_id; @@ -2070,7 +2070,7 @@ namespace io { telegram->message.end(), last_insnavcart_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in INSNavCart"); + node_->log(log_level::ERROR, "parse error in INSNavCart"); break; } if (settings_->ins_use_poi) @@ -2096,7 +2096,7 @@ namespace io { telegram->message.end(), last_insnavgeod_, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in INSNavGeod"); + node_->log(log_level::ERROR, "parse error in INSNavGeod"); break; } if (settings_->ins_use_poi) @@ -2127,7 +2127,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in IMUSetup"); + node_->log(log_level::ERROR, "parse error in IMUSetup"); break; } msg.header.frame_id = settings_->vehicle_frame_id; @@ -2147,7 +2147,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in VelSensorSetup"); + node_->log(log_level::ERROR, "parse error in VelSensorSetup"); break; } msg.header.frame_id = settings_->vehicle_frame_id; @@ -2168,7 +2168,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in ExtEventINSNavCart"); + node_->log(log_level::ERROR, "parse error in ExtEventINSNavCart"); break; } if (settings_->ins_use_poi) @@ -2194,7 +2194,7 @@ namespace io { telegram->message.end(), msg, settings_->use_ros_axis_orientation)) { - node_->log(LogLevel::ERROR, "parse error in ExtEventINSNavGeod"); + node_->log(log_level::ERROR, "parse error in ExtEventINSNavGeod"); break; } if (settings_->ins_use_poi) @@ -2220,7 +2220,7 @@ namespace io { settings_->use_ros_axis_orientation, hasImuMeas)) { - node_->log(LogLevel::ERROR, "parse error in ExtSensorMeas"); + node_->log(log_level::ERROR, "parse error in ExtSensorMeas"); break; } last_extsensmeas_.header.frame_id = settings_->imu_frame_id; @@ -2239,7 +2239,7 @@ namespace io { msg = assmembleImu(); } catch (std::runtime_error& e) { - node_->log(LogLevel::DEBUG, "ImuMsg: " + std::string(e.what())); + node_->log(log_level::DEBUG, "ImuMsg: " + std::string(e.what())); break; } msg.header.frame_id = settings_->imu_frame_id; @@ -2254,7 +2254,7 @@ namespace io { if (!ChannelStatusParser(node_, telegram->message.begin(), telegram->message.end(), last_channelstatus_)) { - node_->log(LogLevel::ERROR, "parse error in ChannelStatus"); + node_->log(log_level::ERROR, "parse error in ChannelStatus"); break; } break; @@ -2265,7 +2265,7 @@ namespace io { if (!MeasEpochParser(node_, telegram->message.begin(), telegram->message.end(), last_measepoch_)) { - node_->log(LogLevel::ERROR, "parse error in MeasEpoch"); + node_->log(log_level::ERROR, "parse error in MeasEpoch"); break; } last_measepoch_.header.frame_id = settings_->frame_id; @@ -2284,7 +2284,7 @@ namespace io { if (!DOPParser(node_, telegram->message.begin(), telegram->message.end(), last_dop_)) { - node_->log(LogLevel::ERROR, "parse error in DOP"); + node_->log(log_level::ERROR, "parse error in DOP"); break; } break; @@ -2295,7 +2295,7 @@ namespace io { if (!VelCovGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_velcovgeodetic_)) { - node_->log(LogLevel::ERROR, "parse error in VelCovGeodetic"); + node_->log(log_level::ERROR, "parse error in VelCovGeodetic"); break; } last_velcovgeodetic_.header.frame_id = settings_->frame_id; @@ -2315,7 +2315,7 @@ namespace io { if (!ReceiverStatusParser(node_, telegram->message.begin(), telegram->message.end(), last_receiverstatus_)) { - node_->log(LogLevel::ERROR, "parse error in ReceiverStatus"); + node_->log(log_level::ERROR, "parse error in ReceiverStatus"); break; } break; @@ -2326,7 +2326,7 @@ namespace io { if (!QualityIndParser(node_, telegram->message.begin(), telegram->message.end(), last_qualityind_)) { - node_->log(LogLevel::ERROR, "parse error in QualityInd"); + node_->log(log_level::ERROR, "parse error in QualityInd"); break; } break; @@ -2336,10 +2336,10 @@ namespace io { if (!ReceiverSetupParser(node_, telegram->message.begin(), telegram->message.end(), last_receiversetup_)) { - node_->log(LogLevel::ERROR, "parse error in ReceiverSetup"); + node_->log(log_level::ERROR, "parse error in ReceiverSetup"); break; } - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "receiver setup firmware: " + last_receiversetup_.rx_version); static int32_t ins_major = 1; @@ -2360,7 +2360,7 @@ namespace io { } if (major_minor_patch.size() < 3) { - node_->log(LogLevel::ERROR, "parse error of firmware version."); + node_->log(log_level::ERROR, "parse error of firmware version."); } else { if ((settings_->septentrio_receiver_type == "ins") || @@ -2374,7 +2374,7 @@ namespace io { (major_minor_patch[2] < ins_patch))) { node_->log( - LogLevel::WARN, + log_level::WARN, "INS receiver has firmware version: " + last_receiversetup_.rx_version + ", which does not support all features. Please update to at least " + @@ -2387,7 +2387,7 @@ namespace io { if (major_minor_patch[0] < 3) { node_->log( - LogLevel::WARN, + log_level::WARN, "INS receiver seems to be used as GNSS. Some settings may trigger warnings or errors. Consider using 'ins_in_gnss_mode' as receiver type."); } else if ((major_minor_patch[0] < gnss_major) || ((major_minor_patch[0] == gnss_major) && @@ -2397,7 +2397,7 @@ namespace io { (major_minor_patch[2] < gnss_patch))) { node_->log( - LogLevel::WARN, + log_level::WARN, "GNSS receiver has firmware version: " + last_receiversetup_.rx_version + ", which may not support all features. Please update to at least " + @@ -2405,7 +2405,7 @@ namespace io { std::to_string(gnss_minor) + "." + std::to_string(gnss_patch) + " or consult README."); } else - node_->log(LogLevel::ERROR, "gnss"); + node_->log(log_level::ERROR, "gnss"); } } @@ -2418,7 +2418,7 @@ namespace io { if (!ReceiverTimeParser(node_, telegram->message.begin(), telegram->message.end(), msg)) { - node_->log(LogLevel::ERROR, "parse error in ReceiverTime"); + node_->log(log_level::ERROR, "parse error in ReceiverTime"); break; } current_leap_seconds_ = msg.delta_ls; @@ -2426,7 +2426,7 @@ namespace io { } default: { - node_->log(LogLevel::DEBUG, "unknown SBF block received."); + node_->log(log_level::DEBUG, "unknown SBF block received."); break; } // Many more to be implemented... @@ -2755,7 +2755,7 @@ namespace io { std::stringstream ss; ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; - node_->log(LogLevel::DEBUG, ss.str()); + node_->log(log_level::DEBUG, ss.str()); std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); } @@ -2805,7 +2805,7 @@ namespace io { telegram->stamp); } catch (ParseException& e) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "GpggaMsg: " + std::string(e.what())); break; } @@ -2826,7 +2826,7 @@ namespace io { telegram->stamp); } catch (ParseException& e) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "GprmcMsg: " + std::string(e.what())); break; } @@ -2847,7 +2847,7 @@ namespace io { node_->getTime()); } catch (ParseException& e) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "GpgsaMsg: " + std::string(e.what())); break; } @@ -2884,7 +2884,7 @@ namespace io { node_->getTime()); } catch (ParseException& e) { - node_->log(LogLevel::DEBUG, + node_->log(log_level::DEBUG, "GpgsvMsg: " + std::string(e.what())); break; } @@ -2912,7 +2912,7 @@ namespace io { } } else { - node_->log(LogLevel::DEBUG, "Unknown NMEA message: " + body[0]); + node_->log(log_level::DEBUG, "Unknown NMEA message: " + body[0]); } } From d327a1e0cf0d84543b374b039351c7764eaa982f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 17 Jan 2023 23:32:11 +0100 Subject: [PATCH 040/170] Change parsing utilities and crc to get message --- .../communication/async_manager.hpp | 6 ++--- include/septentrio_gnss_driver/crc/crc.hpp | 2 +- .../parsers/message_parser.hpp | 2 +- .../parsers/parsing_utilities.hpp | 10 ++++---- src/septentrio_gnss_driver/crc/crc.cpp | 8 +++---- .../parsers/message_parser.cpp | 6 ++--- .../parsers/parsing_utilities.cpp | 24 ++++++++++++++----- 7 files changed, 35 insertions(+), 23 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index f4d4b60a..2c0671a3 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -448,7 +448,7 @@ namespace io { if (numBytes == (SBF_HEADER_SIZE - 2)) { uint16_t length = - parsing_utilities::getLength(telegram_->message.data()); + parsing_utilities::getLength(telegram_->message); if (length > MAX_SBF_SIZE) { node_->log( @@ -490,10 +490,10 @@ namespace io { { if (numBytes == (length - SBF_HEADER_SIZE)) { - if (crc::isValid(telegram_->message.data())) + if (crc::isValid(telegram_->message)) { telegram_->sbfId = - parsing_utilities::getId(telegram_->message.data()); + parsing_utilities::getId(telegram_->message); telegramQueue_->push(telegram_); } else diff --git a/include/septentrio_gnss_driver/crc/crc.hpp b/include/septentrio_gnss_driver/crc/crc.hpp index ffacc61c..d0ad4ec7 100644 --- a/include/septentrio_gnss_driver/crc/crc.hpp +++ b/include/septentrio_gnss_driver/crc/crc.hpp @@ -61,6 +61,6 @@ namespace crc { * @param block The SBF block that we are interested in * @return True if the CRC check of the SBFBlock has passed, false otherwise */ - bool isValid(const uint8_t* block); + bool isValid(const std::vector& message); } // namespace crc diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/parsers/message_parser.hpp index 4a567fef..bbd6ad60 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/parsers/message_parser.hpp @@ -372,7 +372,7 @@ namespace io { * @return Timestamp object containing seconds and nanoseconds since last * epoch */ - Timestamp timestampSBF(const std::vector& data); + Timestamp timestampSBF(const std::vector& message); /** * @brief Calculates the timestamp, in the Unix Epoch time format diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index cd405e22..98d09ea4 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -371,7 +371,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message CRC */ - uint16_t getCrc(const uint8_t* buffer); + uint16_t getCrc(const std::vector& message); /** * @brief Get the ID of the SBF message @@ -379,7 +379,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message ID */ - uint16_t getId(const uint8_t* buffer); + uint16_t getId(const std::vector& message); /** * @brief Get the length of the SBF message @@ -387,7 +387,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message length */ - uint16_t getLength(const uint8_t* buffer); + uint16_t getLength(const std::vector& message); /** * @brief Get the time of week in ms of the SBF message @@ -395,7 +395,7 @@ namespace parsing_utilities { * @param[in] buffer A pointer to a buffer containing an SBF message * @return SBF time of week in ms */ - uint32_t getTow(const uint8_t* buffer); + uint32_t getTow(const std::vector& message); /** * @brief Get the GPS week counter of the SBF message @@ -403,5 +403,5 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF GPS week counter */ - uint16_t getWnc(const uint8_t* buffer); + uint16_t getWnc(const std::vector& message); } // namespace parsing_utilities \ No newline at end of file diff --git a/src/septentrio_gnss_driver/crc/crc.cpp b/src/septentrio_gnss_driver/crc/crc.cpp index 8b794e2d..e3085a17 100644 --- a/src/septentrio_gnss_driver/crc/crc.cpp +++ b/src/septentrio_gnss_driver/crc/crc.cpp @@ -68,15 +68,15 @@ namespace crc { return crc; } - bool isValid(const uint8_t* block) + bool isValid(const std::vector& message) { // We need all of the message except for the first 4 bytes (Sync and CRC), // i.e. we start at the address of ID. - uint16_t length = parsing_utilities::getLength(block); + uint16_t length = parsing_utilities::getLength(message); if (length > 4) { - uint16_t crc = compute16CCITT(block + 4, length - 4); - return (crc == parsing_utilities::getCrc(block)); + uint16_t crc = compute16CCITT(message.data() + 4, length - 4); + return (crc == parsing_utilities::getCrc(message)); } else { return false; diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/parsers/message_parser.cpp index c53e8085..f346c294 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/parsers/message_parser.cpp @@ -1794,10 +1794,10 @@ namespace io { publish("/gpsfix", msg, time_obj); }; - Timestamp MessageParser::timestampSBF(const std::vector& data) + Timestamp MessageParser::timestampSBF(const std::vector& message) { - uint32_t tow = parsing_utilities::getTow(data.data()); - uint16_t wnc = parsing_utilities::getWnc(data.data()); + uint32_t tow = parsing_utilities::getTow(message); + uint16_t wnc = parsing_utilities::getWnc(message); return timestampSBF(tow, wnc); } diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index 37aa2e02..5925ea7e 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -417,22 +417,34 @@ namespace parsing_utilities { return "min" + std::to_string(period_user / 60000); } - uint16_t getCrc(const uint8_t* buffer) { return parseUInt16(buffer + 2); } + uint16_t getCrc(const std::vector& message) + { + return parseUInt16(message.data() + 2); + } - uint16_t getId(const uint8_t* buffer) + uint16_t getId(const std::vector& message) { // Defines bit mask.. // Highest three bits are for revision and rest for block number static uint16_t mask = 8191; // Bitwise AND gives us all but highest 3 bits set to zero, rest unchanged - return parseUInt16(buffer + 4) & mask; + return parseUInt16(message.data() + 4) & mask; } - uint16_t getLength(const uint8_t* buffer) { return parseUInt16(buffer + 6); } + uint16_t getLength(const std::vector& message) + { + return parseUInt16(message.data() + 6); + } - uint32_t getTow(const uint8_t* buffer) { return parseUInt32(buffer + 8); } + uint32_t getTow(const std::vector& message) + { + return parseUInt32(message.data() + 8); + } - uint16_t getWnc(const uint8_t* buffer) { return parseUInt16(buffer + 12); } + uint16_t getWnc(const std::vector& message) + { + return parseUInt16(message.data() + 12); + } } // namespace parsing_utilities From 76d27495a1aa4f3a9bdecec52a4821ed76d4d664 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 08:30:30 +0100 Subject: [PATCH 041/170] Rename message handler again --- CMakeLists.txt | 2 +- .../message_handler.hpp} | 8 ++-- .../communication/telegram_handler.hpp | 8 ++-- .../message_handler.cpp} | 44 +++++++++---------- .../communication/telegram_handler.cpp | 4 +- 5 files changed, 33 insertions(+), 33 deletions(-) rename include/septentrio_gnss_driver/{parsers/message_parser.hpp => communication/message_handler.hpp} (98%) rename src/septentrio_gnss_driver/{parsers/message_parser.cpp => communication/message_handler.cpp} (98%) diff --git a/CMakeLists.txt b/CMakeLists.txt index b061b45c..cc81e342 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,11 +189,11 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_node src/septentrio_gnss_driver/communication/communication_core.cpp + src/septentrio_gnss_driver/communication/message_handler.cpp src/septentrio_gnss_driver/communication/telegram_handler.cpp src/septentrio_gnss_driver/crc/crc.cpp src/septentrio_gnss_driver/node/main.cpp src/septentrio_gnss_driver/node/rosaic_node.cpp - src/septentrio_gnss_driver/parsers/message_parser.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp diff --git a/include/septentrio_gnss_driver/parsers/message_parser.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp similarity index 98% rename from include/septentrio_gnss_driver/parsers/message_parser.hpp rename to include/septentrio_gnss_driver/communication/message_handler.hpp index bbd6ad60..63eaedd6 100644 --- a/include/septentrio_gnss_driver/parsers/message_parser.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -129,14 +129,14 @@ enum SbfId namespace io { /** - * @class MessageParser + * @class MessageHandler * @brief Can search buffer for messages, read/parse them, and so on */ - class MessageParser + class MessageHandler { public: /** - * @brief Constructor of the MessageParser class + * @brief Constructor of the MessageHandler class * * One can always provide a non-const value where a const one was expected. * The const-ness of the argument just means the function promises not to @@ -144,7 +144,7 @@ namespace io { * no other C++ cast is capable of removing it (not even reinterpret_cast) * @param[in] node Pointer to the node) */ - MessageParser(ROSaicNodeBase* node) : + MessageHandler(ROSaicNodeBase* node) : node_(node), settings_(node->settings()), unix_time_(0) { } diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index b43a4515..923bfe8b 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -63,8 +63,8 @@ // ROSaic includes #include +#include #include -#include /** * @file telegram_handler.hpp @@ -110,7 +110,7 @@ namespace io { { public: - TelegramHandler(ROSaicNodeBase* node) : node_(node), messageParser_(node) {} + TelegramHandler(ROSaicNodeBase* node) : node_(node), messageHandler_(node) {} /** * @brief Called every time a telegram is received @@ -149,8 +149,8 @@ namespace io { //! Pointer to Node ROSaicNodeBase* node_; - //! MessageParser parser - MessageParser messageParser_; + //! MessageHandler parser + MessageHandler messageHandler_; bool init_ = false; uint8_t cdCtr_ = 0; diff --git a/src/septentrio_gnss_driver/parsers/message_parser.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp similarity index 98% rename from src/septentrio_gnss_driver/parsers/message_parser.cpp rename to src/septentrio_gnss_driver/communication/message_handler.cpp index f346c294..672f665a 100644 --- a/src/septentrio_gnss_driver/parsers/message_parser.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include /** @@ -54,7 +54,7 @@ using parsing_utilities::rad2deg; using parsing_utilities::square; namespace io { - void MessageParser::assemblePoseWithCovarianceStamped(const Timestamp& time_obj) + void MessageHandler::assemblePoseWithCovarianceStamped(const Timestamp& time_obj) { PoseWithCovarianceStampedMsg msg; if (settings_->septentrio_receiver_type == "ins") @@ -206,7 +206,7 @@ namespace io { publish("/pose", msg, time_obj); }; - void MessageParser::assembleDiagnosticArray(const Timestamp& time_obj) + void MessageHandler::assembleDiagnosticArray(const Timestamp& time_obj) { DiagnosticArrayMsg msg; if (!validValue(last_receiverstatus_.block_header.tow) || @@ -325,7 +325,7 @@ namespace io { publish("/diagnostics", msg, time_obj); }; - ImuMsg MessageParser::assmembleImu() + ImuMsg MessageHandler::assmembleImu() { ImuMsg msg; @@ -434,8 +434,8 @@ namespace io { return msg; }; - void MessageParser::assembleTwist(const Timestamp& time_obj, - bool fromIns /* = false*/) + void MessageHandler::assembleTwist(const Timestamp& time_obj, + bool fromIns /* = false*/) { if (!settings_->publish_twist) return; @@ -664,7 +664,7 @@ namespace io { * Angular velocity not available, thus according autocovariances are set to * -1.0. */ - void MessageParser::assembleLocalizationUtm(const Timestamp& time_obj) + void MessageHandler::assembleLocalizationUtm(const Timestamp& time_obj) { if (!settings_->publish_localization) return; @@ -847,7 +847,7 @@ namespace io { * Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ - void MessageParser::assembleLocalizationEcef(const Timestamp& time_obj) + void MessageHandler::assembleLocalizationEcef(const Timestamp& time_obj) { if (!settings_->publish_localization_ecef) return; @@ -1007,9 +1007,9 @@ namespace io { publish("/localization_ecef", msg, time_obj); }; - void MessageParser::assembleLocalizationMsgTwist(double roll, double pitch, - double yaw, - LocalizationMsg& msg) + void MessageHandler::assembleLocalizationMsgTwist(double roll, double pitch, + double yaw, + LocalizationMsg& msg) { Eigen::Matrix3d R_local_body = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); @@ -1130,7 +1130,7 @@ namespace io { * SignalInfo field of the PVTGeodetic block does not disclose it. For that, one * would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block. */ - void MessageParser::assembleNavSatFix(const Timestamp& time_obj) + void MessageHandler::assembleNavSatFix(const Timestamp& time_obj) { if (!settings_->publish_navsatfix) return; @@ -1335,7 +1335,7 @@ namespace io { * includes those "in search". In case certain values appear unphysical, please * consult the firmware, since those most likely refer to Do-Not-Use values. */ - void MessageParser::assembleGpsFix(const Timestamp& time_obj) + void MessageHandler::assembleGpsFix(const Timestamp& time_obj) { if (!settings_->publish_gpsfix) return; @@ -1794,7 +1794,7 @@ namespace io { publish("/gpsfix", msg, time_obj); }; - Timestamp MessageParser::timestampSBF(const std::vector& message) + Timestamp MessageHandler::timestampSBF(const std::vector& message) { uint32_t tow = parsing_utilities::getTow(message); uint16_t wnc = parsing_utilities::getWnc(message); @@ -1807,7 +1807,7 @@ namespace io { /// (2020), the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the /// settings_->leap_seconds ROSaic parameter accordingly as soon as the /// next leap second is inserted into the UTC time. - Timestamp MessageParser::timestampSBF(uint32_t tow, uint16_t wnc) + Timestamp MessageHandler::timestampSBF(uint32_t tow, uint16_t wnc) { Timestamp time_obj; @@ -1833,8 +1833,8 @@ namespace io { * If GNSS time is used, Publishing is only done with valid leap seconds */ template - void MessageParser::publish(const std::string& topic, const M& msg, - const Timestamp& time_obj) + void MessageHandler::publish(const std::string& topic, const M& msg, + const Timestamp& time_obj) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -1861,8 +1861,8 @@ namespace io { /** * If GNSS time is used, Publishing is only done with valid leap seconds */ - void MessageParser::publishTf(const LocalizationMsg& msg, - const Timestamp& time_obj) + void MessageHandler::publishTf(const LocalizationMsg& msg, + const Timestamp& time_obj) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -1897,7 +1897,7 @@ namespace io { * allowed e.g. for GGA seems to be 89 on a mosaic-x5. Luckily, when parsing we * do not care since we just search for \\. */ - void MessageParser::parseSbf(const std::shared_ptr& telegram) + void MessageHandler::parseSbf(const std::shared_ptr& telegram) { node_->log(log_level::DEBUG, "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + @@ -2743,7 +2743,7 @@ namespace io { }*/ } - void MessageParser::wait(Timestamp time_obj) + void MessageHandler::wait(Timestamp time_obj) { Timestamp unix_old = unix_time_; unix_time_ = time_obj; @@ -2767,7 +2767,7 @@ namespace io { current_leap_seconds_ = settings_->leap_seconds; } - void MessageParser::parseNmea(const std::shared_ptr& telegram) + void MessageHandler::parseNmea(const std::shared_ptr& telegram) { std::string message(telegram->message.begin(), telegram->message.end()); /*node_->log( diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 4c98b897..a2107206 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -90,12 +90,12 @@ namespace io { void TelegramHandler::handleSbf(const std::shared_ptr& telegram) { - messageParser_.parseSbf(telegram); + messageHandler_.parseSbf(telegram); } void TelegramHandler::handleNmea(const std::shared_ptr& telegram) { - messageParser_.parseNmea(telegram); + messageHandler_.parseNmea(telegram); } void TelegramHandler::handleResponse(const std::shared_ptr& telegram) From 9f07a618ecdaa2052b9fcc34a199b6a995cedcfa Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 09:11:19 +0100 Subject: [PATCH 042/170] Refactor header assembly --- .../communication/message_handler.hpp | 34 +-- .../communication/message_handler.cpp | 275 +++++++----------- 2 files changed, 126 insertions(+), 183 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 63eaedd6..49cf7854 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -162,23 +162,29 @@ namespace io { void parseNmea(const std::shared_ptr& telegram); private: + /** + * @brief Header assembling + * @param[in] frameId String of frame ID + * @param[in] telegram telegram from which the msg was assembled + * @param[in] msg ROS msg for which the header is assembled + */ + template + void assembleHeader(const std::string& frameId, + const std::shared_ptr& telegram, T& msg); /** * @brief Publishing function * @param[in] topic String of topic * @param[in] msg ROS message to be published - * @param[in] time_obj message time to wait if reading from file */ template - void publish(const std::string& topic, const M& msg, - const Timestamp& time_obj); + void publish(const std::string& topic, const M& msg); /** * @brief Publishing function * @param[in] msg Localization message - * @param[in] time_obj message time to wait if reading from file */ - void publishTf(const LocalizationMsg& msg, const Timestamp& time_obj); + void publishTf(const LocalizationMsg& msg); /** * @brief Pointer to the node @@ -289,22 +295,19 @@ namespace io { /** * @brief "Callback" function when constructing NavSatFix messages - * @param[in] time_obj time of message */ - void assembleNavSatFix(const Timestamp& time_obj); + void assembleNavSatFix(); /** * @brief "Callback" function when constructing GPSFix messages - * @param[in] time_obj time of message */ - void assembleGpsFix(const Timestamp& time_obj); + void assembleGpsFix(); /** * @brief "Callback" function when constructing PoseWithCovarianceStamped * messages - * @param[in] time_obj time of message */ - void assemblePoseWithCovarianceStamped(const Timestamp& time_obj); + void assemblePoseWithCovarianceStamped(); /** * @brief "Callback" function when constructing @@ -324,16 +327,14 @@ namespace io { /** * @brief "Callback" function when constructing * LocalizationMsg messages in UTM - * @param[in] time_obj time of message */ - void assembleLocalizationUtm(const Timestamp& time_obj); + void assembleLocalizationUtm(); /** * @brief "Callback" function when constructing * LocalizationMsg messages in ECEF - * @param[in] time_obj time of message */ - void assembleLocalizationEcef(const Timestamp& time_obj); + void assembleLocalizationEcef(); /** * @brief function to fill twist part of LocalizationMsg @@ -348,10 +349,9 @@ namespace io { /** * @brief "Callback" function when constructing * TwistWithCovarianceStampedMsg messages - * @param[in] time_obj time of message * @param[in] fromIns Wether to contruct message from INS data */ - void assembleTwist(const Timestamp& time_obj, bool fromIns = false); + void assembleTwist(bool fromIns = false); /** * @brief Waits according to time when reading from file diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 672f665a..b2fb45a9 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -54,7 +54,7 @@ using parsing_utilities::rad2deg; using parsing_utilities::square; namespace io { - void MessageHandler::assemblePoseWithCovarianceStamped(const Timestamp& time_obj) + void MessageHandler::assemblePoseWithCovarianceStamped() { PoseWithCovarianceStampedMsg msg; if (settings_->septentrio_receiver_type == "ins") @@ -203,7 +203,7 @@ namespace io { msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); } - publish("/pose", msg, time_obj); + publish("/pose", msg); }; void MessageHandler::assembleDiagnosticArray(const Timestamp& time_obj) @@ -322,7 +322,7 @@ namespace io { gnss_status.message = "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; msg.status.push_back(gnss_status); - publish("/diagnostics", msg, time_obj); + publish("/diagnostics", msg); }; ImuMsg MessageHandler::assmembleImu() @@ -434,8 +434,7 @@ namespace io { return msg; }; - void MessageHandler::assembleTwist(const Timestamp& time_obj, - bool fromIns /* = false*/) + void MessageHandler::assembleTwist(bool fromIns /* = false*/) { if (!settings_->publish_twist) return; @@ -655,7 +654,7 @@ namespace io { msg.header.frame_id = "navigation"; - publish("/twist_ins", msg, time_obj); + publish("/twist_ins", msg); }; /** @@ -664,7 +663,7 @@ namespace io { * Angular velocity not available, thus according autocovariances are set to * -1.0. */ - void MessageHandler::assembleLocalizationUtm(const Timestamp& time_obj) + void MessageHandler::assembleLocalizationUtm() { if (!settings_->publish_localization) return; @@ -839,7 +838,7 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); - publish("/localization", msg, time_obj); + publish("/localization", msg); }; /** @@ -847,7 +846,7 @@ namespace io { * Linear velocity of twist in body frame as per msg definition. Angular * velocity not available, thus according autocovariances are set to -1.0. */ - void MessageHandler::assembleLocalizationEcef(const Timestamp& time_obj) + void MessageHandler::assembleLocalizationEcef() { if (!settings_->publish_localization_ecef) return; @@ -1004,7 +1003,7 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); - publish("/localization_ecef", msg, time_obj); + publish("/localization_ecef", msg); }; void MessageHandler::assembleLocalizationMsgTwist(double roll, double pitch, @@ -1130,7 +1129,7 @@ namespace io { * SignalInfo field of the PVTGeodetic block does not disclose it. For that, one * would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block. */ - void MessageHandler::assembleNavSatFix(const Timestamp& time_obj) + void MessageHandler::assembleNavSatFix() { if (!settings_->publish_navsatfix) return; @@ -1311,7 +1310,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - publish("/navsatfix", msg, time_obj); + publish("/navsatfix", msg); }; /** @@ -1335,7 +1334,7 @@ namespace io { * includes those "in search". In case certain values appear unphysical, please * consult the firmware, since those most likely refer to Do-Not-Use values. */ - void MessageHandler::assembleGpsFix(const Timestamp& time_obj) + void MessageHandler::assembleGpsFix() { if (!settings_->publish_gpsfix) return; @@ -1791,9 +1790,22 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - publish("/gpsfix", msg, time_obj); + publish("/gpsfix", msg); }; + template + void MessageHandler::assembleHeader(const std::string& frameId, + const std::shared_ptr& telegram, + T& msg) + { + Timestamp time_obj = settings_->use_gnss_time + ? timestampSBF(telegram->message) + : telegram->stamp; + msg.header.frame_id = frameId; + + msg.header.stamp = timestampToRos(time_obj); + } + Timestamp MessageHandler::timestampSBF(const std::vector& message) { uint32_t tow = parsing_utilities::getTow(message); @@ -1833,8 +1845,7 @@ namespace io { * If GNSS time is used, Publishing is only done with valid leap seconds */ template - void MessageHandler::publish(const std::string& topic, const M& msg, - const Timestamp& time_obj) + void MessageHandler::publish(const std::string& topic, const M& msg) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -1842,7 +1853,7 @@ namespace io { { if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - wait(time_obj); + wait(timestampFromRos(msg.header.stamp)); } node_->publishMessage(topic, msg); } else @@ -1861,8 +1872,7 @@ namespace io { /** * If GNSS time is used, Publishing is only done with valid leap seconds */ - void MessageHandler::publishTf(const LocalizationMsg& msg, - const Timestamp& time_obj) + void MessageHandler::publishTf(const LocalizationMsg& msg) { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || @@ -1871,7 +1881,7 @@ namespace io { { if (settings_->read_from_sbf_log || settings_->read_from_pcap) { - wait(time_obj); + wait(timestampFromRos(msg.header.stamp)); } node_->publishTf(msg); } else @@ -1916,13 +1926,8 @@ namespace io { node_->log(log_level::ERROR, "parse error in PVTCartesian"); break; } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - - publish("/pvtcartesian", msg, time_obj); + assembleHeader(settings_->frame_id, telegram, msg); + publish("/pvtcartesian", msg); break; } case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame @@ -1934,16 +1939,12 @@ namespace io { node_->log(log_level::ERROR, "parse error in PVTGeodetic"); break; } - last_pvtgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_pvtgeodetic_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->frame_id, telegram, last_pvtgeodetic_); if (settings_->publish_pvtgeodetic) - publish("/pvtgeodetic", last_pvtgeodetic_, time_obj); - assembleTwist(time_obj); - assembleNavSatFix(time_obj); - assembleGpsFix(time_obj); + publish("/pvtgeodetic", last_pvtgeodetic_); + assembleTwist(); + assembleNavSatFix(); + assembleGpsFix(); break; } case BASE_VECTOR_CART: @@ -1956,12 +1957,8 @@ namespace io { node_->log(log_level::ERROR, "parse error in BaseVectorCart"); break; } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/basevectorcart", msg, time_obj); + assembleHeader(settings_->frame_id, telegram, msg); + publish("/basevectorcart", msg); break; } case BASE_VECTOR_GEOD: @@ -1974,12 +1971,8 @@ namespace io { node_->log(log_level::ERROR, "parse error in BaseVectorGeod"); break; } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/basevectorgeod", msg, time_obj); + assembleHeader(settings_->frame_id, telegram, msg); + publish("/basevectorgeod", msg); break; } case POS_COV_CARTESIAN: @@ -1992,12 +1985,8 @@ namespace io { node_->log(log_level::ERROR, "parse error in PosCovCartesian"); break; } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/poscovcartesian", msg, time_obj); + assembleHeader(settings_->frame_id, telegram, msg); + publish("/poscovcartesian", msg); break; } case POS_COV_GEODETIC: @@ -2009,17 +1998,12 @@ namespace io { node_->log(log_level::ERROR, "parse error in PosCovGeodetic"); break; } - last_poscovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_poscovgeodetic_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->frame_id, telegram, last_poscovgeodetic_); if (settings_->publish_poscovgeodetic) - publish("/poscovgeodetic", last_poscovgeodetic_, - time_obj); - assembleNavSatFix(time_obj); - assembleGpsFix(time_obj); - assembleGpsFix(time_obj); + publish("/poscovgeodetic", last_poscovgeodetic_); + assembleNavSatFix(); + assembleGpsFix(); + assembleGpsFix(); break; } case ATT_EULER: @@ -2032,14 +2016,10 @@ namespace io { node_->log(log_level::ERROR, "parse error in AttEuler"); break; } - last_atteuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_atteuler_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->frame_id, telegram, last_atteuler_); if (settings_->publish_atteuler) - publish("/atteuler", last_atteuler_, time_obj); - assembleGpsFix(time_obj); + publish("/atteuler", last_atteuler_); + assembleGpsFix(); break; } case ATT_COV_EULER: @@ -2052,14 +2032,10 @@ namespace io { node_->log(log_level::ERROR, "parse error in AttCovEuler"); break; } - last_attcoveuler_.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_attcoveuler_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->frame_id, telegram, last_attcoveuler_); if (settings_->publish_attcoveuler) - publish("/attcoveuler", last_attcoveuler_, time_obj); - assembleGpsFix(time_obj); + publish("/attcoveuler", last_attcoveuler_); + assembleGpsFix(); break; } case INS_NAV_CART: // Position, velocity and orientation in cartesian @@ -2073,19 +2049,17 @@ namespace io { node_->log(log_level::ERROR, "parse error in INSNavCart"); break; } + std::string frame_id; if (settings_->ins_use_poi) { - last_insnavcart_.header.frame_id = settings_->poi_frame_id; + frame_id = settings_->poi_frame_id; } else { - last_insnavcart_.header.frame_id = settings_->frame_id; + frame_id = settings_->frame_id; } - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_insnavcart_.header.stamp = timestampToRos(time_obj); - publish("/insnavcart", last_insnavcart_, time_obj); - assembleLocalizationEcef(time_obj); + assembleHeader(frame_id, telegram, last_insnavcart_); + publish("/insnavcart", last_insnavcart_); + assembleLocalizationEcef(); break; } case INS_NAV_GEOD: // Position, velocity and orientation in geodetic @@ -2099,23 +2073,21 @@ namespace io { node_->log(log_level::ERROR, "parse error in INSNavGeod"); break; } + std::string frame_id; if (settings_->ins_use_poi) { - last_insnavgeod_.header.frame_id = settings_->poi_frame_id; + frame_id = settings_->poi_frame_id; } else { - last_insnavgeod_.header.frame_id = settings_->frame_id; + frame_id = settings_->frame_id; } - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_insnavgeod_.header.stamp = timestampToRos(time_obj); + assembleHeader(frame_id, telegram, last_insnavgeod_); if (settings_->publish_insnavgeod) - publish("/insnavgeod", last_insnavgeod_, time_obj); - assembleLocalizationUtm(time_obj); - assembleLocalizationEcef(time_obj); - assembleTwist(time_obj, true); - assembleGpsFix(time_obj); + publish("/insnavgeod", last_insnavgeod_); + assembleLocalizationUtm(); + assembleLocalizationEcef(); + assembleTwist(true); + assembleGpsFix(); break; } @@ -2130,12 +2102,8 @@ namespace io { node_->log(log_level::ERROR, "parse error in IMUSetup"); break; } - msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/imusetup", msg, time_obj); + assembleHeader(settings_->vehicle_frame_id, telegram, msg); + publish("/imusetup", msg); break; } @@ -2150,12 +2118,8 @@ namespace io { node_->log(log_level::ERROR, "parse error in VelSensorSetup"); break; } - msg.header.frame_id = settings_->vehicle_frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/velsensorsetup", msg, time_obj); + assembleHeader(settings_->vehicle_frame_id, telegram, msg); + publish("/velsensorsetup", msg); break; } @@ -2171,18 +2135,16 @@ namespace io { node_->log(log_level::ERROR, "parse error in ExtEventINSNavCart"); break; } + std::string frame_id; if (settings_->ins_use_poi) { - msg.header.frame_id = settings_->poi_frame_id; + frame_id = settings_->poi_frame_id; } else { - msg.header.frame_id = settings_->frame_id; + frame_id = settings_->frame_id; } - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/exteventinsnavcart", msg, time_obj); + assembleHeader(frame_id, telegram, msg); + publish("/exteventinsnavcart", msg); break; } @@ -2197,18 +2159,16 @@ namespace io { node_->log(log_level::ERROR, "parse error in ExtEventINSNavGeod"); break; } + std::string frame_id; if (settings_->ins_use_poi) { - msg.header.frame_id = settings_->poi_frame_id; + frame_id = settings_->poi_frame_id; } else { - msg.header.frame_id = settings_->frame_id; + frame_id = settings_->frame_id; } - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - msg.header.stamp = timestampToRos(time_obj); - publish("/exteventinsnavgeod", msg, time_obj); + assembleHeader(frame_id, telegram, msg); + publish("/exteventinsnavgeod", msg); break; } @@ -2223,14 +2183,9 @@ namespace io { node_->log(log_level::ERROR, "parse error in ExtSensorMeas"); break; } - last_extsensmeas_.header.frame_id = settings_->imu_frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_extsensmeas_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->imu_frame_id, telegram, last_extsensmeas_); if (settings_->publish_extsensormeas) - publish("/extsensormeas", last_extsensmeas_, - time_obj); + publish("/extsensormeas", last_extsensmeas_); if (settings_->publish_imu && hasImuMeas) { ImuMsg msg; @@ -2242,15 +2197,13 @@ namespace io { node_->log(log_level::DEBUG, "ImuMsg: " + std::string(e.what())); break; } - msg.header.frame_id = settings_->imu_frame_id; - msg.header.stamp = last_extsensmeas_.header.stamp; - publish("/imu", msg, time_obj); + assembleHeader(settings_->imu_frame_id, telegram, msg); + publish("/imu", msg); } break; } case CHANNEL_STATUS: { - if (!ChannelStatusParser(node_, telegram->message.begin(), telegram->message.end(), last_channelstatus_)) { @@ -2268,14 +2221,10 @@ namespace io { node_->log(log_level::ERROR, "parse error in MeasEpoch"); break; } - last_measepoch_.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_measepoch_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->frame_id, telegram, last_measepoch_); if (settings_->publish_measepoch) - publish("/measepoch", last_measepoch_, time_obj); - assembleGpsFix(time_obj); + publish("/measepoch", last_measepoch_); + assembleGpsFix(); break; } case DOP: @@ -2298,20 +2247,14 @@ namespace io { node_->log(log_level::ERROR, "parse error in VelCovGeodetic"); break; } - last_velcovgeodetic_.header.frame_id = settings_->frame_id; - Timestamp time_obj = settings_->use_gnss_time - ? timestampSBF(telegram->message) - : telegram->stamp; - last_velcovgeodetic_.header.stamp = timestampToRos(time_obj); + assembleHeader(settings_->frame_id, telegram, last_velcovgeodetic_); if (settings_->publish_velcovgeodetic) - publish("/velcovgeodetic", last_velcovgeodetic_, - time_obj); - assembleTwist(time_obj); + publish("/velcovgeodetic", last_velcovgeodetic_); + assembleTwist(); break; } case RECEIVER_STATUS: { - if (!ReceiverStatusParser(node_, telegram->message.begin(), telegram->message.end(), last_receiverstatus_)) { @@ -2809,8 +2752,7 @@ namespace io { "GpggaMsg: " + std::string(e.what())); break; } - Timestamp time_obj = timestampFromRos(msg.header.stamp); - publish("/gpgga", msg, time_obj); + publish("/gpgga", msg); break; } case 1: @@ -2830,8 +2772,7 @@ namespace io { "GprmcMsg: " + std::string(e.what())); break; } - Timestamp time_obj = timestampFromRos(msg.header.stamp); - publish("/gprmc", msg, time_obj); + publish("/gprmc", msg); break; } case 2: @@ -2851,24 +2792,25 @@ namespace io { "GpgsaMsg: " + std::string(e.what())); break; } - Timestamp time_obj; if (settings_->use_gnss_time) { if (settings_->septentrio_receiver_type == "gnss") { - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc); + Timestamp time_obj = + timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } if (settings_->septentrio_receiver_type == "ins") { - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc); + Timestamp time_obj = + timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } } else msg.header.stamp = timestampToRos(telegram->stamp); - publish("/gpgsa", msg, time_obj); + publish("/gpgsa", msg); break; } case 4: @@ -2888,25 +2830,26 @@ namespace io { "GpgsvMsg: " + std::string(e.what())); break; } - Timestamp time_obj; if (settings_->use_gnss_time) { if (settings_->septentrio_receiver_type == "gnss") { - time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, - last_pvtgeodetic_.block_header.wnc); + Timestamp time_obj = + timestampSBF(last_pvtgeodetic_.block_header.tow, + last_pvtgeodetic_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } if (settings_->septentrio_receiver_type == "ins") { - time_obj = timestampSBF(last_insnavgeod_.block_header.tow, - last_insnavgeod_.block_header.wnc); + Timestamp time_obj = + timestampSBF(last_insnavgeod_.block_header.tow, + last_insnavgeod_.block_header.wnc); msg.header.stamp = timestampToRos(time_obj); } } else msg.header.stamp = timestampToRos(telegram->stamp); - publish("/gpgsv", msg, time_obj); + publish("/gpgsv", msg); break; } } From 1964295b1f0ef3a0c7bc5e49e14323323637d058 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 09:14:53 +0100 Subject: [PATCH 043/170] Move SBF ID handling --- .../communication/async_manager.hpp | 7 +++--- .../communication/telegram.hpp | 4 +--- .../communication/message_handler.cpp | 24 +++++++------------ 3 files changed, 12 insertions(+), 23 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 2c0671a3..702afd58 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -492,14 +492,13 @@ namespace io { { if (crc::isValid(telegram_->message)) { - telegram_->sbfId = - parsing_utilities::getId(telegram_->message); - telegramQueue_->push(telegram_); } else node_->log(log_level::DEBUG, "AsyncManager crc failed for SBF " + - std::to_string(telegram_->sbfId) + "."); + std::to_string(parsing_utilities::getId( + telegram_->message)) + + "."); } else { node_->log( diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 87671de6..b2fa5a7f 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -104,12 +104,10 @@ struct Telegram { Timestamp stamp; telegram_type::TelegramType type; - uint16_t sbfId; std::vector message; Telegram() : - stamp(0), type(telegram_type::EMPTY), sbfId(0), - message(std::vector(3)) + stamp(0), type(telegram_type::EMPTY), message(std::vector(3)) { message.reserve(MAX_SBF_SIZE); } diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index b2fb45a9..78b2f9e9 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1896,25 +1896,17 @@ namespace io { } } - /** - * Note that putting the default in the definition's argument list instead of the - * declaration's is an added extra that is not available for function templates, - * hence no search = false here. Also note that the SBF block header part of the - * SBF-echoing ROS messages have ID fields that only show the block number as - * found in the firmware (e.g. 4007 for PVTGeodetic), without the revision - * number. NMEA 0183 messages are at most 82 characters long in principle, but - * most Septentrio Rxs by default increase precision on lat/lon s.t. the maximum - * allowed e.g. for GGA seems to be 89 on a mosaic-x5. Luckily, when parsing we - * do not care since we just search for \\. - */ void MessageHandler::parseSbf(const std::shared_ptr& telegram) { - node_->log(log_level::DEBUG, - "ROSaic reading SBF block " + std::to_string(telegram->sbfId) + - " made up of " + std::to_string(telegram->message.size()) + - " bytes..."); - switch (telegram->sbfId) + uint16_t sbfId = parsing_utilities::getId(telegram->message); + + node_->log(log_level::DEBUG, "ROSaic reading SBF block " + + std::to_string(sbfId) + " made up of " + + std::to_string(telegram->message.size()) + + " bytes..."); + + switch (sbfId) { case PVT_CARTESIAN: // Position and velocity in XYZ { From ba414848331355e76079418e4339b3b6a445cc42 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 09:53:45 +0100 Subject: [PATCH 044/170] Make settings access const --- .../abstraction/typedefs.hpp | 2 +- .../communication/communication_core.hpp | 2 +- .../communication/message_handler.hpp | 2 +- .../communication/settings.hpp | 16 +++- .../communication/communication_core.cpp | 74 ++++++------------- .../node/rosaic_node.cpp | 55 ++++++++++++-- 6 files changed, 89 insertions(+), 62 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index dc1d8b08..4d9cd280 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -180,7 +180,7 @@ class ROSaicNodeBase virtual ~ROSaicNodeBase() {} - Settings* settings() { return &settings_; } + const Settings* settings() { return &settings_; } void registerSubscriber() { diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 319aa5ce..29b5d2eb 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -148,7 +148,7 @@ namespace io { //! Pointer to Node ROSaicNodeBase* node_; //! Settings - Settings* settings_; + const Settings* settings_; //! TelegramQueue TelegramQueue telegramQueue_; //! TelegramHandler diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 49cf7854..18071c24 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -194,7 +194,7 @@ namespace io { /** * @brief Pointer to settings struct */ - Settings* settings_; + const Settings* settings_; /** * @brief Map of NMEA messgae IDs and uint8_t diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index d55a24f7..005c9226 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -99,17 +99,31 @@ struct RtkSettings std::vector serial; }; +namespace device_type { + enum DeviceType + { + TCP, + SERIAL, + SBF_FILE, + PCAP_FILE + }; +} // namespace device_type + //! Settings struct struct Settings { //! Set logger level to DEBUG bool activate_debug_log; - //! Device port + //! Device std::string device; + //! Device type + device_type::DeviceType device_type; //! TCP IP std::string tcp_ip; //! TCP port std::string tcp_port; + //! Filename + std::string file_name; //! Username for login std::string login_user; //! Password for login diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 897f631a..65777d30 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -171,58 +171,35 @@ namespace io { [[nodiscard]] bool CommunicationCore::initializeIo() { node_->log(log_level::DEBUG, "Called initializeIo() method"); - boost::smatch match; - // In fact: smatch is a typedef of match_results - if (boost::regex_match(settings_->device, match, - boost::regex("(tcp)://(.+):(\\d+)"))) - // C++ needs \\d instead of \d: Both mean decimal. - // Note that regex_match can be used with a smatch object to store - // results, or without. In any case, true is returned if and only if it - // matches the !complete! string. + switch (settings_->device_type) + { + case device_type::TCP: { - // The first sub_match (index 0) contained in a match_result always - // represents the full match within a target sequence made by a - // regex, and subsequent sub_matches represent sub-expression matches - // corresponding in sequence to the left parenthesis delimiting the - // sub-expression in the regex, i.e. $n Perl is equivalent to m[n] in - // boost regex. - settings_->tcp_ip = match[2]; - settings_->tcp_port = match[3]; manager_.reset(new AsyncManager(node_, &telegramQueue_)); - } else if (boost::regex_match( - settings_->device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) + break; + } + case device_type::SERIAL: + { + manager_.reset(new AsyncManager(node_, &telegramQueue_)); + break; + } + case device_type::SBF_FILE: { - settings_->read_from_sbf_log = true; - settings_->use_gnss_time = true; - settings_->device = match[2]; manager_.reset(new AsyncManager(node_, &telegramQueue_)); - - } else if (boost::regex_match( - settings_->device, match, - boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) + break; + } + case device_type::PCAP_FILE: { - settings_->read_from_pcap = true; - settings_->use_gnss_time = true; - settings_->device = match[2]; manager_.reset(new AsyncManager(node_, &telegramQueue_)); - } else if (boost::regex_match(settings_->device, match, - boost::regex("(serial):(.+)"))) - { - std::string proto(match[2]); - std::stringstream ss; - ss << "Searching for serial port" << proto; - node_->log(log_level::DEBUG, ss.str()); - settings_->device = proto; - manager_.reset(new AsyncManager(node_, &telegramQueue_)); - } else + break; + } + default: { - std::stringstream ss; - ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; - node_->log(log_level::ERROR, ss.str()); + node_->log(log_level::DEBUG, "Unsupported device."); return false; + break; + } } - node_->log(log_level::DEBUG, "Leaving initializeIo() method"); return true; } @@ -297,9 +274,6 @@ namespace io { // not ellipsoidal height) { std::stringstream ss; - // WGS84 is equivalent to Default and kept for backwards compatibility - if (settings_->datum == "Default") - settings_->datum = "WGS84"; ss << "sgd, " << settings_->datum << "\x0D"; send(ss.str()); } @@ -389,8 +363,8 @@ namespace io { { { std::stringstream ss; - // In case this IP server was used before, old configuration is - // lost of course. + // In case this IP server was used before, old + // configuration is lost of course. ss << "siss, " << ip_server.id << ", " << std::to_string(ip_server.port) << ", TCP2Way \x0D"; send(ss.str()); @@ -858,8 +832,8 @@ namespace io { std::string CommunicationCore::resetMainConnection() { - // Escape sequence (escape from correction mode), ensuring that we can send - // our real commands afterwards... + // Escape sequence (escape from correction mode), ensuring that we + // can send our real commands afterwards... std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); telegramHandler_.resetWaitforMainCd(); manager_.get()->send(cmd); diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 3aa79d05..7cd5626f 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -93,8 +93,8 @@ bool rosaic_node::ROSaicNode::getROSParams() (settings_.septentrio_receiver_type == "ins_in_gnss_mode"))) { this->log(log_level::FATAL, "Unkown septentrio_receiver_type " + - settings_.septentrio_receiver_type + - " use either gnss or ins."); + settings_.septentrio_receiver_type + + " use either gnss or ins."); return false; } @@ -181,6 +181,9 @@ bool rosaic_node::ROSaicNode::getROSParams() // Datum and marker-to-ARP offset param("datum", settings_.datum, std::string("Default")); + // WGS84 is equivalent to Default and kept for backwards compatibility + if (settings_.datum == "Default") + settings_.datum = "WGS84"; param("ant_type", settings_.ant_type, std::string("Unknown")); param("ant_aux1_type", settings_.ant_aux1_type, std::string("Unknown")); param("ant_serial_nr", settings_.ant_serial_nr, std::string()); @@ -516,8 +519,8 @@ bool rosaic_node::ROSaicNode::getROSParams() (settings_.ins_vsm_ros_source == "twist")); if (!settings_.ins_vsm_ros_source.empty() && !ins_use_vsm) this->log(log_level::ERROR, "unknown ins_vsm/ros/source " + - settings_.ins_vsm_ros_source + - " -> VSM input will not be used!"); + settings_.ins_vsm_ros_source + + " -> VSM input will not be used!"); param("ins_vsm/ip_server/id", settings_.ins_vsm_ip_server_id, std::string("")); if (!settings_.ins_vsm_ip_server_id.empty()) @@ -610,12 +613,48 @@ bool rosaic_node::ROSaicNode::getROSParams() if (ins_use_vsm) { this->log(log_level::INFO, "ins_vsm/ros/source " + - settings_.ins_vsm_ros_source + - " will be used."); + settings_.ins_vsm_ros_source + + " will be used."); registerSubscriber(); } } + boost::smatch match; + if (boost::regex_match(settings_.device, match, + boost::regex("(tcp)://(.+):(\\d+)"))) + { + settings_.tcp_ip = match[2]; + settings_.tcp_port = match[3]; + settings_.device_type = device_type::TCP; + } else if (boost::regex_match(settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) + { + settings_.read_from_sbf_log = true; + settings_.use_gnss_time = true; + settings_.device = match[2]; + settings_.device_type = device_type::SBF_FILE; + } else if (boost::regex_match( + settings_.device, match, + boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)"))) + { + settings_.read_from_pcap = true; + settings_.use_gnss_time = true; + settings_.device = match[2]; + settings_.device_type = device_type::PCAP_FILE; + } else if (boost::regex_match(settings_.device, match, + boost::regex("(serial):(.+)"))) + { + std::string proto(match[2]); + settings_.device_type = device_type::SERIAL; + settings_.device = proto; + } else + { + std::stringstream ss; + ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; + this->log(log_level::ERROR, ss.str()); + return false; + } + // To be implemented: RTCM, raw data settings, PPP, SBAS ... this->log(log_level::DEBUG, "Finished getROSParams() method"); return true; @@ -648,8 +687,8 @@ void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, } catch (const tf2::TransformException& ex) { this->log(log_level::WARN, "Waiting for transform from " + sourceFrame + - " to " + targetFrame + ": " + ex.what() + - "."); + " to " + targetFrame + ": " + ex.what() + + "."); found = false; } } From 9563ff67b4186837d021a46195a5647743290d2c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 11:23:36 +0100 Subject: [PATCH 045/170] Add const specifiers to functions --- .../abstraction/typedefs.hpp | 32 ++++++++++------ .../communication/async_manager.hpp | 2 + .../communication/io.hpp | 21 ++++++----- .../communication/message_handler.hpp | 10 ++--- .../communication/telegram.hpp | 37 ++++++++++++++++++- .../node/rosaic_node.hpp | 6 +-- .../parsers/parser_base_class.hpp | 19 +--------- .../communication/communication_core.cpp | 2 +- .../communication/message_handler.cpp | 10 ++--- .../communication/telegram_handler.cpp | 12 +++--- .../node/rosaic_node.cpp | 6 +-- 11 files changed, 92 insertions(+), 65 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 4d9cd280..5d66c49d 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -180,17 +180,24 @@ class ROSaicNodeBase virtual ~ROSaicNodeBase() {} - const Settings* settings() { return &settings_; } + const Settings* settings() const { return &settings_; } void registerSubscriber() { - ros::NodeHandle nh; - if (settings_.ins_vsm_ros_source == "odometry") - odometrySubscriber_ = nh.subscribe( - "odometry_vsm", 10, &ROSaicNodeBase::callbackOdometry, this); - else if (settings_.ins_vsm_ros_source == "twist") - twistSubscriber_ = nh.subscribe( - "twist_vsm", 10, &ROSaicNodeBase::callbackTwist, this); + try + { + ros::NodeHandle nh; + if (settings_.ins_vsm_ros_source == "odometry") + odometrySubscriber_ = nh.subscribe( + "odometry_vsm", 10, &ROSaicNodeBase::callbackOdometry, this); + else if (settings_.ins_vsm_ros_source == "twist") + twistSubscriber_ = nh.subscribe( + "twist_vsm", 10, &ROSaicNodeBase::callbackTwist, this); + } catch (const std::runtime_error& ex) + { + this->log(log_level::ERROR, "Subscriber initalization failed due to: " + + std::string(ex.what()) + "."); + } } /** @@ -203,7 +210,8 @@ class ROSaicNodeBase * @param[in] defaultVal Value to use if the server doesn't contain * this parameter */ - bool getUint32Param(const std::string& name, uint32_t& val, uint32_t defaultVal) + bool getUint32Param(const std::string& name, uint32_t& val, + uint32_t defaultVal) const { int32_t tempVal; if ((!pNh_->getParam(name, tempVal)) || (tempVal < 0)) @@ -225,7 +233,7 @@ class ROSaicNodeBase * @return True if it could be retrieved, false if not */ template - bool param(const std::string& name, T& val, const T& defaultVal) + bool param(const std::string& name, T& val, const T& defaultVal) const { return pNh_->param(name, val, defaultVal); }; @@ -235,7 +243,7 @@ class ROSaicNodeBase * @param[in] logLevel Log level * @param[in] s String to log */ - void log(log_level::LogLevel logLevel, const std::string& s) + void log(log_level::LogLevel logLevel, const std::string& s) const { switch (logLevel) { @@ -263,7 +271,7 @@ class ROSaicNodeBase * @brief Gets current timestamp * @return Timestamp */ - Timestamp getTime() { return ros::Time::now().toNSec(); } + Timestamp getTime() const { return ros::Time::now().toNSec(); } /** * @brief Publishing function diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 702afd58..ae5b2419 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -519,6 +519,7 @@ namespace io { void AsyncManager::readUnknown() { telegram_->message.resize(1); + telegram_->message.reserve(256); readStringElements(); } @@ -526,6 +527,7 @@ namespace io { void AsyncManager::readString() { telegram_->message.resize(3); + telegram_->message.reserve(256); readStringElements(); } diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 00813941..da898377 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -333,10 +333,10 @@ namespace io { node_->log(log_level::ERROR, "open SBF file failed."); return false; } - stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); try { + stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); stream_->assign(fd); } catch (std::runtime_error& e) @@ -375,20 +375,21 @@ namespace io { [[nodiscard]] bool connect() { - node_->log(log_level::INFO, "Opening pcap file stream" + - node_->settings()->device + "..."); - - stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); - - pcap_ = pcap_open_offline(node_->settings()->device.c_str(), - errBuff_.data()); - stream_->assign(pcap_get_selectable_fd(pcap_)); - try { + node_->log(log_level::INFO, "Opening pcap file stream" + + node_->settings()->device + "..."); + + stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); + + pcap_ = pcap_open_offline(node_->settings()->device.c_str(), + errBuff_.data()); + stream_->assign(pcap_get_selectable_fd(pcap_)); } catch (std::runtime_error& e) { + node_->log(log_level::ERROR, "assigning PCAP file failed due to " + + std::string(e.what())); return false; } return true; diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 18071c24..c4ed27e5 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -170,7 +170,7 @@ namespace io { */ template void assembleHeader(const std::string& frameId, - const std::shared_ptr& telegram, T& msg); + const std::shared_ptr& telegram, T& msg) const; /** * @brief Publishing function * @param[in] topic String of topic @@ -322,7 +322,7 @@ namespace io { * @return A ROS message * ImuMsg just created */ - ImuMsg assmembleImu(); + ImuMsg assmembleImu() const; /** * @brief "Callback" function when constructing @@ -344,7 +344,7 @@ namespace io { * @param[inout] msg LocalizationMsg to be filled */ void assembleLocalizationMsgTwist(double roll, double pitch, double yaw, - LocalizationMsg& msg); + LocalizationMsg& msg) const; /** * @brief "Callback" function when constructing @@ -372,7 +372,7 @@ namespace io { * @return Timestamp object containing seconds and nanoseconds since last * epoch */ - Timestamp timestampSBF(const std::vector& message); + Timestamp timestampSBF(const std::vector& message) const; /** * @brief Calculates the timestamp, in the Unix Epoch time format @@ -385,6 +385,6 @@ namespace io { * @return Timestamp object containing seconds and nanoseconds since last * epoch */ - Timestamp timestampSBF(uint32_t tow, uint16_t wnc); + Timestamp timestampSBF(uint32_t tow, uint16_t wnc) const; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index b2fa5a7f..343e6052 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -106,10 +106,43 @@ struct Telegram telegram_type::TelegramType type; std::vector message; - Telegram() : + Telegram() noexcept : stamp(0), type(telegram_type::EMPTY), message(std::vector(3)) { - message.reserve(MAX_SBF_SIZE); + } + + ~Telegram() {} + + Telegram(const Telegram& other) noexcept : + stamp(other.stamp), type(other.type), message(other.message) + { + } + + Telegram(Telegram&& other) noexcept : + stamp(other.stamp), type(other.type), message(other.message) + { + } + + Telegram& operator=(const Telegram& other) noexcept + { + if (this != &other) + { + this->stamp = other.stamp; + this->type = other.type; + this->message = other.message; + } + return *this; + } + + Telegram& operator=(Telegram&& other) noexcept + { + if (this != &other) + { + this->stamp = other.stamp; + this->type = other.type; + this->message = other.message; + } + return *this; } }; diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp index e4aa3135..6af7fad0 100644 --- a/include/septentrio_gnss_driver/node/rosaic_node.hpp +++ b/include/septentrio_gnss_driver/node/rosaic_node.hpp @@ -104,7 +104,7 @@ namespace rosaic_node { * @param[in] isIns wether recevier is an INS * @return wether the period is valid */ - bool validPeriod(uint32_t period, bool isIns); + bool validPeriod(uint32_t period, bool isIns) const; /** * @brief Gets transforms from tf2 * @param[in] targetFrame traget frame id @@ -113,7 +113,7 @@ namespace rosaic_node { */ void getTransform(const std::string& targetFrame, const std::string& sourceFrame, - TransformStampedMsg& T_s_t); + TransformStampedMsg& T_s_t) const; /** * @brief Gets Euler angles from quaternion message * @param[in] qm quaternion message @@ -122,7 +122,7 @@ namespace rosaic_node { * @param[out] yaw yaw angle */ void getRPY(const QuaternionMsg& qm, double& roll, double& pitch, - double& yaw); + double& yaw) const; void sendVelocity(const std::string& velNmea); diff --git a/include/septentrio_gnss_driver/parsers/parser_base_class.hpp b/include/septentrio_gnss_driver/parsers/parser_base_class.hpp index 82b0c696..a5e57b08 100644 --- a/include/septentrio_gnss_driver/parsers/parser_base_class.hpp +++ b/include/septentrio_gnss_driver/parsers/parser_base_class.hpp @@ -46,8 +46,7 @@ * @brief Base class for parsing NMEA messages and SBF blocks * * Subclasses that parse NMEA messages should implement - * ParseASCII(const NMEASentence&); subclasses that parse SBF blocks - * should implement ParseBinary(const SBFBlock&). The base class is implemented + * ParseASCII(const NMEASentence&); The base class is implemented * as a template, which is a simple and yet very powerful tool in C++. The * simple idea is to pass data type as a parameter so that we don’t need to * write the same code for different data types. Like function templates, class @@ -96,22 +95,6 @@ class BaseParser */ virtual const std::string getMessageID() const = 0; - /** - * @brief Converts bin_msg into a ROS message pointer (e.g. nmea_msgs::GpggaPtr) - * and returns it - * - * The returned value should not be NULL. ParseException will be thrown - * if there are any issues parsing the block. - * - * @param[in] bin_msg The message to convert, of type const SBFStructT - * @return A valid ROS message pointer - */ - template - T parseBinary(const SBFStructT& bin_msg) noexcept(false) - { - throw ParseException("ParseBinary not implemented."); - }; - /** * @brief Converts an NMEA sentence - both standardized and proprietary ones - * into a ROS message pointer (e.g. nmea_msgs::GpggaPtr) and returns it diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 65777d30..d180e941 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -221,8 +221,8 @@ namespace io { return; } + uint8_t stream = 1; // Determining communication mode: TCP vs USB/Serial - unsigned stream = 1; boost::smatch match; boost::regex_match(settings_->device, match, boost::regex("(tcp)://(.+):(\\d+)")); diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 78b2f9e9..5839ec08 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -325,7 +325,7 @@ namespace io { publish("/diagnostics", msg); }; - ImuMsg MessageHandler::assmembleImu() + ImuMsg MessageHandler::assmembleImu() const { ImuMsg msg; @@ -1008,7 +1008,7 @@ namespace io { void MessageHandler::assembleLocalizationMsgTwist(double roll, double pitch, double yaw, - LocalizationMsg& msg) + LocalizationMsg& msg) const { Eigen::Matrix3d R_local_body = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); @@ -1796,7 +1796,7 @@ namespace io { template void MessageHandler::assembleHeader(const std::string& frameId, const std::shared_ptr& telegram, - T& msg) + T& msg) const { Timestamp time_obj = settings_->use_gnss_time ? timestampSBF(telegram->message) @@ -1806,7 +1806,7 @@ namespace io { msg.header.stamp = timestampToRos(time_obj); } - Timestamp MessageHandler::timestampSBF(const std::vector& message) + Timestamp MessageHandler::timestampSBF(const std::vector& message) const { uint32_t tow = parsing_utilities::getTow(message); uint16_t wnc = parsing_utilities::getWnc(message); @@ -1819,7 +1819,7 @@ namespace io { /// (2020), the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the /// settings_->leap_seconds ROSaic parameter accordingly as soon as the /// next leap second is inserted into the UTC time. - Timestamp MessageHandler::timestampSBF(uint32_t tow, uint16_t wnc) + Timestamp MessageHandler::timestampSBF(uint32_t tow, uint16_t wnc) const { Timestamp time_obj; diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index a2107206..1d749f36 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -75,8 +75,8 @@ namespace io { case telegram_type::UNKNOWN: { node_->log(log_level::DEBUG, "Unhandeled message received: " + - std::string(telegram->message.begin(), - telegram->message.end())); + std::string(telegram->message.begin(), + telegram->message.end())); break; } default: @@ -113,9 +113,9 @@ namespace io { } else { node_->log(log_level::DEBUG, "The Rx's response contains " + - std::to_string(block_in_string.size()) + - " bytes and reads:\n " + - block_in_string); + std::to_string(block_in_string.size()) + + " bytes and reads:\n " + + block_in_string); } responseSemaphore_.notify(); } @@ -131,7 +131,7 @@ namespace io { if (cdCtr_ == 2) { node_->log(log_level::INFO, "The connection descriptor is " + - mainConnectionDescriptor_); + mainConnectionDescriptor_); cdSemaphore_.notify(); } diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 7cd5626f..9986fd20 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -660,7 +660,7 @@ bool rosaic_node::ROSaicNode::getROSParams() return true; } -bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns) +bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns) const { return ((period == 0) || ((period == 5 && isIns)) || (period == 10) || (period == 20) || (period == 40) || (period == 50) || (period == 100) || @@ -673,7 +673,7 @@ bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns) void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, const std::string& sourceFrame, - TransformStampedMsg& T_s_t) + TransformStampedMsg& T_s_t) const { bool found = false; while (!found) @@ -695,7 +695,7 @@ void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, } void rosaic_node::ROSaicNode::getRPY(const QuaternionMsg& qm, double& roll, - double& pitch, double& yaw) + double& pitch, double& yaw) const { Eigen::Quaterniond q(qm.w, qm.x, qm.y, qm.z); Eigen::Quaterniond::RotationMatrixType C = q.matrix(); From c5d12a8f152e068e1f5634fc7a85b769443055c1 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 12:35:38 +0100 Subject: [PATCH 046/170] Add nodiscard attribute to functions --- .../communication/communication_core.hpp | 2 +- .../communication/telegram_handler.hpp | 2 +- .../node/rosaic_node.hpp | 4 +- .../parsers/parsing_utilities.hpp | 70 +++++----- .../parsers/sbf_blocks.hpp | 125 ++++++++++-------- .../parsers/string_utilities.hpp | 16 +-- .../node/rosaic_node.cpp | 5 +- .../parsers/nmea_parsers/gpgsa.cpp | 35 ++++- .../parsers/parsing_utilities.cpp | 84 ++++++------ .../parsers/string_utilities.cpp | 20 +-- 10 files changed, 204 insertions(+), 159 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 29b5d2eb..56ee8eaf 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -129,7 +129,7 @@ namespace io { * @brief Initializes the I/O handling * * @return Wether connection was successful */ - bool initializeIo(); + [[nodiscard]] bool initializeIo(); /** * @brief Reset main connection so it can receive commands diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 923bfe8b..e236be13 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -131,7 +131,7 @@ namespace io { } //! Returns the connection descriptor - std::string getMainCd() + [[nodiscard]] std::string getMainCd() { cdSemaphore_.wait(); return mainConnectionDescriptor_; diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp index 6af7fad0..3fbd98ae 100644 --- a/include/septentrio_gnss_driver/node/rosaic_node.hpp +++ b/include/septentrio_gnss_driver/node/rosaic_node.hpp @@ -97,14 +97,14 @@ namespace rosaic_node { * * The other ROSaic parameters are specified via the command line. */ - bool getROSParams(); + [[nodiscard]] bool getROSParams(); /** * @brief Checks if the period has a valid value * @param[in] period period [ms] * @param[in] isIns wether recevier is an INS * @return wether the period is valid */ - bool validPeriod(uint32_t period, bool isIns) const; + [[nodiscard]] bool validPeriod(uint32_t period, bool isIns) const; /** * @brief Gets transforms from tf2 * @param[in] targetFrame traget frame id diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index 98d09ea4..b4e95e62 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -57,7 +57,7 @@ namespace parsing_utilities { * Square value **********************************************************************/ template - inline T square(T val) + [[nodiscard]] inline T square(T val) { return val * val; } @@ -66,7 +66,7 @@ namespace parsing_utilities { * Convert degrees to radians **********************************************************************/ template - inline T deg2rad(T deg) + [[nodiscard]] inline T deg2rad(T deg) { return deg * boost::math::constants::degree(); } @@ -75,7 +75,7 @@ namespace parsing_utilities { * Convert degrees^2 to radians^2 **********************************************************************/ template - inline T deg2radSq(T deg) + [[nodiscard]] inline T deg2radSq(T deg) { return deg * boost::math::constants::degree() * boost::math::constants::degree(); @@ -85,7 +85,7 @@ namespace parsing_utilities { * Convert radians to degree **********************************************************************/ template - inline T rad2deg(T rad) + [[nodiscard]] inline T rad2deg(T rad) { return rad * boost::math::constants::radian(); } @@ -93,7 +93,7 @@ namespace parsing_utilities { /*********************************************************************** * Convert Euler angles to rotation matrix **********************************************************************/ - inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw) + [[nodiscard]] inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw) { Eigen::Matrix3d M; double sa, ca, sb, cb, sc, cc; @@ -113,14 +113,14 @@ namespace parsing_utilities { * @brief Wraps an angle between -180 and 180 degrees * @param[in] angle The angle to be wrapped */ - double wrapAngle180to180(double angle); + [[nodiscard]] double wrapAngle180to180(double angle); /** * @brief Converts an 8-byte-buffer into a double * @param[in] buffer A pointer to a buffer containing 8 bytes of data * @return The double extracted from the data in the buffer */ - double parseDouble(const uint8_t* buffer); + [[nodiscard]] double parseDouble(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a floating point number of type @@ -134,14 +134,14 @@ namespace parsing_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool parseDouble(const std::string& string, double& value); + [[nodiscard]] bool parseDouble(const std::string& string, double& value); /** * @brief Converts a 4-byte-buffer into a float * @param[in] buffer A pointer to a buffer containing 4 bytes of data * @return The float extracted from the data in the buffer */ - float parseFloat(const uint8_t* buffer); + [[nodiscard]] float parseFloat(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a floating point number of type @@ -155,14 +155,14 @@ namespace parsing_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool parseFloat(const std::string& string, float& value); + [[nodiscard]] bool parseFloat(const std::string& string, float& value); /** * @brief Converts a 2-byte-buffer into a signed 16-bit integer * @param[in] buffer A pointer to a buffer containing 2 bytes of data * @return The int16_t value extracted from the data in the buffer */ - int16_t parseInt16(const uint8_t* buffer); + [[nodiscard]] int16_t parseInt16(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a integer number of type @@ -178,14 +178,14 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10); + [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10); /** * @brief Converts a 4-byte-buffer into a signed 32-bit integer * @param[in] buffer A pointer to a buffer containing 4 bytes of data * @return The int32_t value extracted from the data in the buffer */ - int32_t parseInt32(const uint8_t* buffer); + [[nodiscard]] int32_t parseInt32(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a integer number of type @@ -201,7 +201,7 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10); + [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -217,14 +217,14 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10); /** * @brief Converts a 2-byte-buffer into an unsigned 16-bit integer * @param[in] buffer A pointer to a buffer containing 2 bytes of data * @return The uint16_t value extracted from the data in the buffer */ - uint16_t parseUInt16(const uint8_t* buffer); + [[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -240,14 +240,14 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10); /** * @brief Converts a 4-byte-buffer into an unsigned 32-bit integer * @param[in] buffer A pointer to a buffer containing 4 bytes of data * @return The uint32_t value extracted from the data in the buffer */ - uint32_t parseUInt32(const uint8_t* buffer); + [[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -263,7 +263,7 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10); /** * @brief Converts UTC time from the without-colon-delimiter format to the @@ -272,7 +272,7 @@ namespace parsing_utilities { * format * @return Represents UTC time in the number-of-seconds-since-midnight format */ - double convertUTCDoubleToSeconds(double utc_double); + [[nodiscard]] double convertUTCDoubleToSeconds(double utc_double); /** * @brief Converts UTC time from the without-colon-delimiter format to Unix Epoch @@ -284,7 +284,7 @@ namespace parsing_utilities { * format * @return The time_t variable representing Unix Epoch time */ - std::time_t convertUTCtoUnix(double utc_double); + [[nodiscard]] std::time_t convertUTCtoUnix(double utc_double); /** * @brief Converts latitude or longitude from the DMS notation (in the @@ -295,7 +295,7 @@ namespace parsing_utilities { * without-colon-delimiter format) * @return Represents latitude or longitude in the pure degree notation */ - double convertDMSToDegrees(double dms); + [[nodiscard]] double convertDMSToDegrees(double dms); /** * @brief Transforms Euler angles to a quaternion @@ -304,7 +304,7 @@ namespace parsing_utilities { * @param[in] roll Roll about the new y-axis [rad] * @return quaternion */ - Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, + [[nodiscard]] Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, double yaw); /** @@ -314,14 +314,14 @@ namespace parsing_utilities { * @param[in] roll Roll about the new x-axis [rad] * @return ROS message representing a quaternion */ - QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw); + [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw); /** * @brief Convert Eigen quaternion to a QuaternionMsg * @param[in] q Eigen quaternion * @return ROS message representing a quaternion */ - QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q); + [[nodiscard]] QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q); /** * @brief Generates the quaternion to rotate from local ENU to ECEF @@ -329,7 +329,7 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return quaternion */ - Eigen::Quaterniond q_enu_ecef(double lat, double lon); + [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon); /** * @brief Generates the quaternion to rotate from local NED to ECEF @@ -337,7 +337,7 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return rotation matrix */ - Eigen::Quaterniond q_ned_ecef(double lat, double lon); + [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon); /** * @brief Generates the matrix to rotate from local ENU to ECEF @@ -345,7 +345,7 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return rotation matrix */ - Eigen::Matrix3d R_enu_ecef(double lat, double lon); + [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon); /** * @brief Generates the matrix to rotate from local NED to ECEF @@ -353,7 +353,7 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return rotation matrix */ - Eigen::Matrix3d R_ned_ecef(double lat, double lon); + [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon); /** * @brief Transforms the input polling period [milliseconds] into a std::string @@ -363,7 +363,7 @@ namespace parsing_utilities { * @return Number to be appended to either msec, sec or min when sending commands * to the Rx */ - std::string convertUserPeriodToRxCommand(uint32_t period_user); + [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user); /** * @brief Get the CRC of the SBF message @@ -371,7 +371,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message CRC */ - uint16_t getCrc(const std::vector& message); + [[nodiscard]] uint16_t getCrc(const std::vector& message); /** * @brief Get the ID of the SBF message @@ -379,7 +379,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message ID */ - uint16_t getId(const std::vector& message); + [[nodiscard]] uint16_t getId(const std::vector& message); /** * @brief Get the length of the SBF message @@ -387,7 +387,7 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF message length */ - uint16_t getLength(const std::vector& message); + [[nodiscard]] uint16_t getLength(const std::vector& message); /** * @brief Get the time of week in ms of the SBF message @@ -395,7 +395,7 @@ namespace parsing_utilities { * @param[in] buffer A pointer to a buffer containing an SBF message * @return SBF time of week in ms */ - uint32_t getTow(const std::vector& message); + [[nodiscard]] uint32_t getTow(const std::vector& message); /** * @brief Get the GPS week counter of the SBF message @@ -403,5 +403,5 @@ namespace parsing_utilities { * @param buffer A pointer to a buffer containing an SBF message * @return SBF GPS week counter */ - uint16_t getWnc(const std::vector& message); + [[nodiscard]] uint16_t getWnc(const std::vector& message); } // namespace parsing_utilities \ No newline at end of file diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index a63a3c49..74867546 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -272,7 +272,7 @@ namespace qi = boost::spirit::qi; * @brief Check if value is not set to Do-Not-Use */ template -bool validValue(T s) +[[nodiscard]] bool validValue(T s) { static_assert(std::is_same::value || std::is_same::value || @@ -325,7 +325,7 @@ void setDoNotUse(Val& s) * @brief Qi little endian parsers for numeric values */ template -bool qiLittleEndianParser(It& it, Val& val) +void qiLittleEndianParser(It& it, Val& val) { static_assert( std::is_same::value || std::is_same::value || @@ -336,28 +336,28 @@ bool qiLittleEndianParser(It& it, Val& val) if (std::is_same::value) { - return qi::parse(it, it + 1, qi::char_, val); + qi::parse(it, it + 1, qi::char_, val); } else if (std::is_same::value) { - return qi::parse(it, it + 1, qi::byte_, val); + qi::parse(it, it + 1, qi::byte_, val); } else if ((std::is_same::value) || (std::is_same::value)) { - return qi::parse(it, it + 2, qi::little_word, val); + qi::parse(it, it + 2, qi::little_word, val); } else if ((std::is_same::value) || (std::is_same::value)) { - return qi::parse(it, it + 4, qi::little_dword, val); + qi::parse(it, it + 4, qi::little_dword, val); } else if ((std::is_same::value) || (std::is_same::value)) { - return qi::parse(it, it + 8, qi::little_qword, val); + qi::parse(it, it + 8, qi::little_qword, val); } else if (std::is_same::value) { - return qi::parse(it, it + 4, qi::little_bin_float, val); + qi::parse(it, it + 4, qi::little_bin_float, val); } else if (std::is_same::value) { - return qi::parse(it, it + 8, qi::little_bin_double, val); + qi::parse(it, it + 8, qi::little_bin_double, val); } } @@ -366,14 +366,12 @@ bool qiLittleEndianParser(It& it, Val& val) * @brief Qi parser for char array to string */ template -bool qiCharsToStringParser(It& it, std::string& val, std::size_t num) +void qiCharsToStringParser(It& it, std::string& val, std::size_t num) { - bool success = false; val.clear(); - success = qi::parse(it, it + num, qi::repeat(num)[qi::char_], val); + qi::parse(it, it + num, qi::repeat(num)[qi::char_], val); // remove string termination characters '\0' val.erase(std::remove(val.begin(), val.end(), '\0'), val.end()); - return success; } /** @@ -381,7 +379,7 @@ bool qiCharsToStringParser(It& it, std::string& val, std::size_t num) * @brief Qi based parser for the SBF block "BlockHeader" plus receiver time stamp */ template -bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header) +[[nodiscard]] bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header) { qiLittleEndianParser(it, block_header.sync_1); if (block_header.sync_1 != SBF_SYNC_1) @@ -426,8 +424,9 @@ void ChannelStateInfoParser(It& it, ChannelStateInfo& msg, uint8_t sb2_length) * @brief Qi based parser or the SBF sub-block "ChannelSatInfo" */ template -bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, - uint8_t sb1_length, uint8_t sb2_length) +[[nodiscard]] bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, + ChannelSatInfo& msg, uint8_t sb1_length, + uint8_t sb2_length) { qiLittleEndianParser(it, msg.sv_id); qiLittleEndianParser(it, msg.freq_nr); @@ -458,7 +457,8 @@ bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, * @brief Qi based parser for the SBF block "ChannelStatus" */ template -bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& msg) +[[nodiscard]] bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, + ChannelStatus& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -497,7 +497,7 @@ bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& m * @brief Qi based parser for the SBF block "DOP" */ template -bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg) +[[nodiscard]] bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) @@ -554,9 +554,10 @@ void MeasEpochChannelType2Parser(It& it, MeasEpochChannelType2Msg& msg, * @brief Qi based parser for the SBF sub-block "MeasEpochChannelType1" */ template -bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, - MeasEpochChannelType1Msg& msg, uint8_t sb1_length, - uint8_t sb2_length) +[[nodiscard]] bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, + MeasEpochChannelType1Msg& msg, + uint8_t sb1_length, + uint8_t sb2_length) { qiLittleEndianParser(it, msg.rx_channel); qiLittleEndianParser(it, msg.type); @@ -590,7 +591,8 @@ bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, * @brief Qi based parser for the SBF block "MeasEpoch" */ template -bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) +[[nodiscard]] bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, + MeasEpochMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -633,7 +635,8 @@ bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg) * @brief Qi based parser for the SBF block "ReceiverSetup" */ template -bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& msg) +[[nodiscard]] bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverSetup& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -690,7 +693,8 @@ bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& m * @brief Struct for the SBF block "ReceiverTime" */ template -bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& msg) +[[nodiscard]] bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverTimeMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -721,7 +725,8 @@ bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& * @brief Qi based parser for the SBF block "PVTCartesian" */ template -bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& msg) +[[nodiscard]] bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, + PVTCartesianMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -776,7 +781,8 @@ bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& * @brief Qi based parser for the SBF block "PVTGeodetic" */ template -bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& msg) +[[nodiscard]] bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, + PVTGeodeticMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -831,8 +837,8 @@ bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& ms * @brief Qi based parser for the SBF block "AttEuler" */ template -bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, + AttEulerMsg& msg, bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -876,8 +882,9 @@ bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, * @brief Qi based parser for the SBF block "AttCovEuler" */ template -bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, + AttCovEulerMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -940,8 +947,8 @@ void VectorInfoCartParser(It& it, VectorInfoCartMsg& msg, uint8_t sb_length) * @brief Qi based parser for the SBF block "BaseVectorCart" */ template -bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, - BaseVectorCartMsg& msg) +[[nodiscard]] bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd, + BaseVectorCartMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1002,8 +1009,8 @@ void VectorInfoGeodParser(It& it, VectorInfoGeodMsg& msg, uint8_t sb_length) * @brief Qi based parser for the SBF block "BaseVectorGeod" */ template -bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, - BaseVectorGeodMsg& msg) +[[nodiscard]] bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, + BaseVectorGeodMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1039,8 +1046,9 @@ bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "INSNavCart" */ template -bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, + INSNavCartMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1177,8 +1185,8 @@ bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, * @brief Qi based parser for the SBF block "PosCovCartesian" */ template -bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, - PosCovCartesianMsg& msg) +[[nodiscard]] bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, + PosCovCartesianMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1213,8 +1221,8 @@ bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "PosCovGeodetic" */ template -bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, - PosCovGeodeticMsg& msg) +[[nodiscard]] bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, + PosCovGeodeticMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1249,8 +1257,8 @@ bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "VelCovCartesian" */ template -bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, - VelCovCartesianMsg& msg) +[[nodiscard]] bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, + VelCovCartesianMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1285,8 +1293,8 @@ bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "VelCovGeodetic" */ template -bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, - VelCovGeodeticMsg& msg) +[[nodiscard]] bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, + VelCovGeodeticMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1321,7 +1329,8 @@ bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, * @brief @brief Qi based parser for the SBF block "QualityInd" */ template -bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg) +[[nodiscard]] bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, + QualityInd& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1372,7 +1381,8 @@ void AgcStateParser(It it, AgcState& msg, uint8_t sb_length) * @brief Struct for the SBF block "ReceiverStatus" */ template -bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& msg) +[[nodiscard]] bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverStatus& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1415,7 +1425,8 @@ bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& * @brief Struct for the SBF block "ReceiverTime" */ template -bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& msg) +[[nodiscard]] bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, + ReceiverTimeMsg& msg) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1446,8 +1457,9 @@ bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& * @brief Qi based parser for the SBF block "INSNavGeod" */ template -bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, + INSNavGeodMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1585,8 +1597,8 @@ bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, * @brief Qi based parser for the SBF block "IMUSetup" */ template -bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, - bool use_ros_axis_orientation) +[[nodiscard]] bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, + IMUSetupMsg& msg, bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1623,8 +1635,9 @@ bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, * @brief Qi based parser for the SBF block "VelSensorSetup" */ template -bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, - VelSensorSetupMsg& msg, bool use_ros_axis_orientation) +[[nodiscard]] bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, + VelSensorSetupMsg& msg, + bool use_ros_axis_orientation) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; @@ -1657,9 +1670,9 @@ bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, * @brief Qi based parser for the SBF block "ExtSensorMeas" */ template -bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, - ExtSensorMeasMsg& msg, bool use_ros_axis_orientation, - bool& hasImuMeas) +[[nodiscard]] bool +ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg, + bool use_ros_axis_orientation, bool& hasImuMeas) { if (!BlockHeaderParser(node, it, msg.block_header)) return false; diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.hpp b/include/septentrio_gnss_driver/parsers/string_utilities.hpp index df694cde..f7625a04 100644 --- a/include/septentrio_gnss_driver/parsers/string_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/string_utilities.hpp @@ -57,7 +57,7 @@ namespace string_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool toDouble(const std::string& string, double& value); + [[nodiscard]] bool toDouble(const std::string& string, double& value); /** * @brief Interprets the contents of "string" as a floating point number of type @@ -71,7 +71,7 @@ namespace string_utilities { * floating point number found in "string" * @return True if all went fine, false if not */ - bool toFloat(const std::string& string, float& value); + [[nodiscard]] bool toFloat(const std::string& string, float& value); /** * @brief Interprets the contents of "string" as a floating point number of @@ -86,7 +86,7 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return True if all went fine, false if not */ - bool toInt32(const std::string& string, int32_t& value, int32_t base = 10); + [[nodiscard]] bool toInt32(const std::string& string, int32_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a floating point number of @@ -101,7 +101,7 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return True if all went fine, false if not */ - bool toUInt32(const std::string& string, uint32_t& value, int32_t base = 10); + [[nodiscard]] bool toUInt32(const std::string& string, uint32_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a floating point number of @@ -115,7 +115,7 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return The value found in "string" */ - int8_t toInt8(const std::string& string, int8_t& value, int32_t base = 10); + [[nodiscard]] int8_t toInt8(const std::string& string, int8_t& value, int32_t base = 10); /** * @brief Interprets the contents of "string" as a floating point number of @@ -129,19 +129,19 @@ namespace string_utilities { * @param[in] base The conversion assumes this base, here: decimal * @return The value found in "string" */ - uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base = 10); + [[nodiscard]] uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base = 10); /** * @brief Trims decimal places to three * @param[in] num The double who shall be trimmed * @return The string */ - std::string trimDecimalPlaces(double num); + [[nodiscard]] std::string trimDecimalPlaces(double num); /** * @brief Checks if a string contains spaces * @param[in] str the string to be analyzed * @return true if string contains space */ - bool containsSpace(const std::string str); + [[nodiscard]] bool containsSpace(const std::string str); } // namespace string_utilities \ No newline at end of file diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 9986fd20..e1b9273a 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -64,7 +64,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) this->log(log_level::DEBUG, "Leaving ROSaicNode() constructor.."); } -bool rosaic_node::ROSaicNode::getROSParams() +[[nodiscard]] bool rosaic_node::ROSaicNode::getROSParams() { param("use_gnss_time", settings_.use_gnss_time, true); param("frame_id", settings_.frame_id, (std::string) "gnss"); @@ -660,7 +660,8 @@ bool rosaic_node::ROSaicNode::getROSParams() return true; } -bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns) const +[[nodiscard]] bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, + bool isIns) const { return ((period == 0) || ((period == 5 && isIns)) || (period == 10) || (period == 20) || (period == 40) || (period == 50) || (period == 100) || diff --git a/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp b/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp index 0e548757..e8396f5f 100644 --- a/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp +++ b/src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp @@ -69,7 +69,12 @@ GpgsaMsg GpgsaParser::parseASCII(const NMEASentence& sentence, msg.header.frame_id = frame_id; msg.message_id = sentence.get_body()[0]; msg.auto_manual_mode = sentence.get_body()[1]; - parsing_utilities::parseUInt8(sentence.get_body()[2], msg.fix_mode); + if (!parsing_utilities::parseUInt8(sentence.get_body()[2], msg.fix_mode)) + { + std::stringstream error; + error << "GPGSA fix_mode parsing error."; + throw ParseException(error.str()); + } // Words 3-14 of the sentence are SV PRNs. Copying only the non-null strings.. // 0 is the character needed to fill the new character space, in case 12 (first // argument) is larger than sv_ids. @@ -81,14 +86,34 @@ GpgsaMsg GpgsaParser::parseASCII(const NMEASentence& sentence, { if (!id->empty()) { - parsing_utilities::parseUInt8(*id, msg.sv_ids[n_svs]); + if (!parsing_utilities::parseUInt8(*id, msg.sv_ids[n_svs])) + { + std::stringstream error; + error << "GPGSA sv_ids parsing error."; + throw ParseException(error.str()); + } ++n_svs; } } msg.sv_ids.resize(n_svs); - parsing_utilities::parseFloat(sentence.get_body()[15], msg.pdop); - parsing_utilities::parseFloat(sentence.get_body()[16], msg.hdop); - parsing_utilities::parseFloat(sentence.get_body()[17], msg.vdop); + if (!parsing_utilities::parseFloat(sentence.get_body()[15], msg.pdop)) + { + std::stringstream error; + error << "GPGSA pdop parsing error."; + throw ParseException(error.str()); + } + if (!parsing_utilities::parseFloat(sentence.get_body()[16], msg.hdop)) + { + std::stringstream error; + error << "GPGSA hdop parsing error."; + throw ParseException(error.str()); + } + if (!parsing_utilities::parseFloat(sentence.get_body()[17], msg.vdop)) + { + std::stringstream error; + error << "GPGSA vdop parsing error."; + throw ParseException(error.str()); + } return msg; } \ No newline at end of file diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index 5925ea7e..88743e1f 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -46,7 +46,7 @@ namespace parsing_utilities { namespace qi = boost::spirit::qi; - double wrapAngle180to180(double angle) + [[nodiscard]] double wrapAngle180to180(double angle) { while (angle > 180.0) { @@ -59,7 +59,7 @@ namespace parsing_utilities { return angle; } - double parseDouble(const uint8_t* buffer) + [[nodiscard]] double parseDouble(const uint8_t* buffer) { double val; qi::parse(buffer, buffer + 8, qi::little_bin_double, val); @@ -71,12 +71,12 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseDouble(const std::string& string, double& value) + [[nodiscard]] bool parseDouble(const std::string& string, double& value) { return string_utilities::toDouble(string, value) || string.empty(); } - float parseFloat(const uint8_t* buffer) + [[nodiscard]] float parseFloat(const uint8_t* buffer) { float val; qi::parse(buffer, buffer + 4, qi::little_bin_float, val); @@ -88,7 +88,7 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseFloat(const std::string& string, float& value) + [[nodiscard]] bool parseFloat(const std::string& string, float& value) { return string_utilities::toFloat(string, value) || string.empty(); } @@ -100,7 +100,7 @@ namespace parsing_utilities { * reinterpret_cast(&x). Recall: data_type *var_name = reinterpret_cast * (pointer_variable) converts the pointer type, no return type */ - int16_t parseInt16(const uint8_t* buffer) + [[nodiscard]] int16_t parseInt16(const uint8_t* buffer) { int16_t val; qi::parse(buffer, buffer + 2, qi::little_word, val); @@ -112,7 +112,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseInt16(const std::string& string, int16_t& value, int32_t base) + [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, + int32_t base) { value = 0; if (string.empty()) @@ -132,7 +133,7 @@ namespace parsing_utilities { return false; } - int32_t parseInt32(const uint8_t* buffer) + [[nodiscard]] int32_t parseInt32(const uint8_t* buffer) { int32_t val; qi::parse(buffer, buffer + 4, qi::little_dword, val); @@ -144,7 +145,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseInt32(const std::string& string, int32_t& value, int32_t base) + [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, + int32_t base) { return string_utilities::toInt32(string, value, base) || string.empty(); } @@ -154,7 +156,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseUInt8(const std::string& string, uint8_t& value, int32_t base) + [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, + int32_t base) { value = 0; if (string.empty()) @@ -173,7 +176,7 @@ namespace parsing_utilities { return false; } - uint16_t parseUInt16(const uint8_t* buffer) + [[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer) { uint16_t val; qi::parse(buffer, buffer + 2, qi::little_word, val); @@ -185,7 +188,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseUInt16(const std::string& string, uint16_t& value, int32_t base) + [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, + int32_t base) { value = 0; if (string.empty()) @@ -204,7 +208,7 @@ namespace parsing_utilities { return false; } - uint32_t parseUInt32(const uint8_t* buffer) + [[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer) { uint32_t val; qi::parse(buffer, buffer + 4, qi::little_dword, val); @@ -216,7 +220,8 @@ namespace parsing_utilities { * exist within "string", and returns true if the latter two tests are negative * or when the string is empty, false otherwise. */ - bool parseUInt32(const std::string& string, uint32_t& value, int32_t base) + [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, + int32_t base) { return string_utilities::toUInt32(string, value, base) || string.empty(); } @@ -226,14 +231,13 @@ namespace parsing_utilities { * in both the without-colon-delimiter and the number-of-seconds-since-midnight * formats. */ - double convertUTCDoubleToSeconds(double utc_double) + [[nodiscard]] double convertUTCDoubleToSeconds(double utc_double) { uint32_t hours = static_cast(utc_double) / 10000; uint32_t minutes = (static_cast(utc_double) - hours * 10000) / 100; - double seconds = - utc_double - static_cast(hours * 10000 + minutes * 100); - seconds += static_cast(hours * 3600 + minutes * 60); - return seconds; + + return utc_double - static_cast(hours * 10000 + minutes * 100) + + static_cast(hours * 3600 + minutes * 60); } /** @@ -241,12 +245,11 @@ namespace parsing_utilities { * into 60 seconds (of arc). Use of the degrees-minutes-seconds system is also * called DMS notation. */ - double convertDMSToDegrees(double dms) + [[nodiscard]] double convertDMSToDegrees(double dms) { uint32_t whole_degrees = static_cast(dms) / 100; double minutes = dms - static_cast(whole_degrees * 100); - double degrees = static_cast(whole_degrees) + minutes / 60.0; - return degrees; + return static_cast(whole_degrees) + minutes / 60.0; } /** @@ -265,7 +268,7 @@ namespace parsing_utilities { * "header.stamp.sec" field of ROS messages, while "header.stamp.nsec" is taken * care of separately. */ - time_t convertUTCtoUnix(double utc_double) + [[nodiscard]] time_t convertUTCtoUnix(double utc_double) { time_t time_now = time(0); struct tm* timeinfo; @@ -295,18 +298,15 @@ namespace parsing_utilities { */ // Inverse of gmtime, the latter converts time_t (Unix time) to tm (UTC time) - time_t date = timegm(timeinfo); - - // ROS_DEBUG("Since 1970/01/01 %jd seconds have passed.\n", (intmax_t) date); - return date; + return timegm(timeinfo); } //! The rotational sequence convention we adopt here (and Septentrio receivers' //! pitch, roll, yaw definition too) is the yaw-pitch-roll sequence, i.e. the //! 3-2-1 sequence: The body first does yaw around the z-axis, then pitches //! around the new y-axis and finally rolls around the new x-axis. - Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, - double yaw) + [[nodiscard]] Eigen::Quaterniond + convertEulerToQuaternion(double roll, double pitch, double yaw) { double cy = std::cos(yaw * 0.5); double sy = std::sin(yaw * 0.5); @@ -320,7 +320,8 @@ namespace parsing_utilities { cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy); } - QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q) + [[nodiscard]] QuaternionMsg + quaternionToQuaternionMsg(const Eigen::Quaterniond& q) { QuaternionMsg qm; @@ -332,12 +333,13 @@ namespace parsing_utilities { return qm; } - QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw) + [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, + double pitch, double yaw) { return quaternionToQuaternionMsg(convertEulerToQuaternion(roll, pitch, yaw)); } - Eigen::Quaterniond q_enu_ecef(double lat, double lon) + [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon) { static double pihalf = boost::math::constants::pi() / 2.0; double sr = sin((pihalf - lat) / 2.0); @@ -348,7 +350,7 @@ namespace parsing_utilities { return Eigen::Quaterniond(cr * cy, sr * cy, sr * sy, cr * sy); } - Eigen::Quaterniond q_ned_ecef(double lat, double lon) + [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon) { static double pihalf = boost::math::constants::pi() / 2.0; double sp = sin((-lat - pihalf) / 2.0); @@ -359,7 +361,7 @@ namespace parsing_utilities { return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy); } - Eigen::Matrix3d R_enu_ecef(double lat, double lon) + [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon) { Eigen::Matrix3d R; @@ -381,7 +383,7 @@ namespace parsing_utilities { return R; } - Eigen::Matrix3d R_ned_ecef(double lat, double lon) + [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon) { Eigen::Matrix3d R; @@ -403,7 +405,7 @@ namespace parsing_utilities { return R; } - std::string convertUserPeriodToRxCommand(uint32_t period_user) + [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user) { std::string cmd; @@ -417,12 +419,12 @@ namespace parsing_utilities { return "min" + std::to_string(period_user / 60000); } - uint16_t getCrc(const std::vector& message) + [[nodiscard]] uint16_t getCrc(const std::vector& message) { return parseUInt16(message.data() + 2); } - uint16_t getId(const std::vector& message) + [[nodiscard]] uint16_t getId(const std::vector& message) { // Defines bit mask.. // Highest three bits are for revision and rest for block number @@ -432,17 +434,17 @@ namespace parsing_utilities { return parseUInt16(message.data() + 4) & mask; } - uint16_t getLength(const std::vector& message) + [[nodiscard]] uint16_t getLength(const std::vector& message) { return parseUInt16(message.data() + 6); } - uint32_t getTow(const std::vector& message) + [[nodiscard]] uint32_t getTow(const std::vector& message) { return parseUInt32(message.data() + 8); } - uint16_t getWnc(const std::vector& message) + [[nodiscard]] uint16_t getWnc(const std::vector& message) { return parseUInt16(message.data() + 12); } diff --git a/src/septentrio_gnss_driver/parsers/string_utilities.cpp b/src/septentrio_gnss_driver/parsers/string_utilities.cpp index a3d8d8ae..e38b55b9 100644 --- a/src/septentrio_gnss_driver/parsers/string_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/string_utilities.cpp @@ -50,7 +50,7 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toDouble(const std::string& string, double& value) + [[nodiscard]] bool toDouble(const std::string& string, double& value) { if (string.empty()) { @@ -76,7 +76,7 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toFloat(const std::string& string, float& value) + [[nodiscard]] bool toFloat(const std::string& string, float& value) { if (string.empty()) { @@ -101,7 +101,8 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toInt32(const std::string& string, int32_t& value, int32_t base) + [[nodiscard]] bool toInt32(const std::string& string, int32_t& value, + int32_t base) { if (string.empty()) { @@ -132,7 +133,8 @@ namespace string_utilities { * exist within "string", and returns true if the latter two tests are negative * and the string is non-empty, false otherwise. */ - bool toUInt32(const std::string& string, uint32_t& value, int32_t base) + [[nodiscard]] bool toUInt32(const std::string& string, uint32_t& value, + int32_t base) { if (string.empty()) { @@ -160,7 +162,8 @@ namespace string_utilities { /** * Not used as of now.. */ - int8_t toInt8(const std::string& string, int8_t& value, int32_t base) + [[nodiscard]] int8_t toInt8(const std::string& string, int8_t& value, + int32_t base) { char* end; errno = 0; @@ -173,7 +176,8 @@ namespace string_utilities { /** * Not used as of now.. */ - uint8_t toUInt8(const std::string& string, uint8_t& value, int32_t base) + [[nodiscard]] uint8_t toUInt8(const std::string& string, uint8_t& value, + int32_t base) { char* end; errno = 0; @@ -183,7 +187,7 @@ namespace string_utilities { return true; } - std::string trimDecimalPlaces(double num) + [[nodiscard]] std::string trimDecimalPlaces(double num) { num = std::round(num * 1000); num = num / 1000; @@ -194,7 +198,7 @@ namespace string_utilities { return ss.str(); } - bool containsSpace(const std::string str) + [[nodiscard]] bool containsSpace(const std::string str) { for (size_t i = 0; i < str.size(); ++i) { From b426b2993c7e8153db20037e53194db4007ce8a0 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 12:36:32 +0100 Subject: [PATCH 047/170] Add nodiscard attribute to functions --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cc81e342..4c74a296 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(septentrio_gnss_driver) ## Compile as C++14, supported in ROS Melodic and newer -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) message(STATUS "Setting build type to Release as none was specified.") From 032c66404ed3beae5afae246fd9eac9d55c34267 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 12:36:48 +0100 Subject: [PATCH 048/170] Add nodiscard attribute to functions --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c74a296..5b1af046 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0.2) project(septentrio_gnss_driver) -## Compile as C++14, supported in ROS Melodic and newer +## Compile as C++17, supported in ROS Melodic and newer add_compile_options(-std=c++17) if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) From 03e17136bbbf4f1c6e44952a4a7fea662cddd6fd Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 14:17:11 +0100 Subject: [PATCH 049/170] Add units to remaining msgs --- .../parsers/sbf_blocks.hpp | 2 +- msg/INSNavGeod.msg | 62 +++++++++---------- msg/MeasEpoch.msg | 2 +- msg/MeasEpochChannelType1.msg | 12 ++-- msg/MeasEpochChannelType2.msg | 8 +-- msg/PVTCartesian.msg | 30 ++++----- msg/PVTGeodetic.msg | 30 ++++----- msg/PosCovCartesian.msg | 20 +++--- msg/PosCovGeodetic.msg | 20 +++--- msg/ReceiverTime.msg | 14 ++--- msg/VelCovCartesian.msg | 20 +++--- msg/VelSensorSetup.msg | 6 +- 12 files changed, 113 insertions(+), 113 deletions(-) diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 74867546..cccc26cd 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -1758,7 +1758,7 @@ ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg } case 3: { - qi::parse(it, it + 2, qi::little_word, msg.sensor_temperature); + qiLittleEndianParser(it, msg.sensor_temperature); msg.sensor_temperature /= 100.0f; std::advance(it, 22); // reserved break; diff --git a/msg/INSNavGeod.msg b/msg/INSNavGeod.msg index 71f2e11d..9bb7e2b4 100644 --- a/msg/INSNavGeod.msg +++ b/msg/INSNavGeod.msg @@ -9,61 +9,61 @@ BlockHeader block_header uint8 gnss_mode uint8 error uint16 info -uint16 gnss_age -float64 latitude -float64 longitude -float64 height -float32 undulation -uint16 accuracy -uint16 latency +uint16 gnss_age # 0.01 s +float64 latitude # rad +float64 longitude # rad +float64 height # m (ellipsoidal) +float32 undulation # m +uint16 accuracy # 0.01 m +uint16 latency # 0.0001 s uint8 datum #uint8 reserved uint16 sb_list # INSNavGeodPosStdDev sub-block definition: # If the Position StdDev field is 1 then this sub block is available: -float32 latitude_std_dev -float32 longitude_std_dev -float32 height_std_dev +float32 latitude_std_dev # m +float32 longitude_std_dev # m +float32 height_std_dev # m # INSNavGeodPosCov sub-block definition: # If the Position Cov field is 1 then this sub block is available: -float32 latitude_longitude_cov -float32 latitude_height_cov -float32 longitude_height_cov +float32 latitude_longitude_cov # m^2 +float32 latitude_height_cov # m^2 +float32 longitude_height_cov # m^2 # INSNavGeodAtt sub-block definition: # If the Attitude field is 1 then this sub block is available: -float32 heading -float32 pitch -float32 roll +float32 heading # deg +float32 pitch # deg +float32 roll # deg # INSNavGeodAttStdDev sub-block definition: # If the Attitude StdDev field is 1 then this sub block is available: -float32 heading_std_dev -float32 pitch_std_dev -float32 roll_std_dev +float32 heading_std_dev # deg +float32 pitch_std_dev # deg +float32 roll_std_dev # deg # INSNavGeodAttCov sub-block definition: # If the Attitude Cov field is 1 then this sub block is available: -float32 heading_pitch_cov -float32 heading_roll_cov -float32 pitch_roll_cov +float32 heading_pitch_cov # deg^2 +float32 heading_roll_cov # deg^2 +float32 pitch_roll_cov # deg^2 # INSNavGeodVel sub-block definition: # If the Velocity field is 1 then this sub block is available: -float32 ve -float32 vn -float32 vu +float32 ve # m/s +float32 vn # m/s +float32 vu # m/s # INSNavGeodVelStdDev sub-block definition: # If the Velocity StdDev field is 1 then this sub block is available: -float32 ve_std_dev -float32 vn_std_dev -float32 vu_std_dev +float32 ve_std_dev # m/s +float32 vn_std_dev # m/s +float32 vu_std_dev # m/s # INSNavGeodVelCov sub-block definition: # If the Velocity Cov field is 1 then this sub block is available: -float32 ve_vn_cov -float32 ve_vu_cov -float32 vn_vu_cov +float32 ve_vn_cov # m^2/s^2 +float32 ve_vu_cov # m^2/s^2 +float32 vn_vu_cov # m^2/s^2 diff --git a/msg/MeasEpoch.msg b/msg/MeasEpoch.msg index 7ce4175f..b0e3d522 100644 --- a/msg/MeasEpoch.msg +++ b/msg/MeasEpoch.msg @@ -9,6 +9,6 @@ uint8 n uint8 sb1_length uint8 sb2_length uint8 common_flags -uint8 cum_clk_jumps +uint8 cum_clk_jumps # 0.001 s MeasEpochChannelType1[] type1 \ No newline at end of file diff --git a/msg/MeasEpochChannelType1.msg b/msg/MeasEpochChannelType1.msg index 00e6c68b..01dad32f 100644 --- a/msg/MeasEpochChannelType1.msg +++ b/msg/MeasEpochChannelType1.msg @@ -3,12 +3,12 @@ uint8 rx_channel uint8 type uint8 sv_id -uint8 misc -uint32 code_lsb -int32 doppler -uint16 carrier_lsb -int8 carrier_msb -uint8 cn0 +uint8 misc # 4294967.296 m +uint32 code_lsb # 0.001 m +int32 doppler # 0.0001 Hz +uint16 carrier_lsb # 0.001 cycles +int8 carrier_msb # 65.536 cycles +uint8 cn0 # 0.25 dB-Hz uint16 lock_time uint8 obs_info uint8 n2 diff --git a/msg/MeasEpochChannelType2.msg b/msg/MeasEpochChannelType2.msg index f79b781d..f3b41939 100644 --- a/msg/MeasEpochChannelType2.msg +++ b/msg/MeasEpochChannelType2.msg @@ -1,10 +1,10 @@ # MeasEpochChannelType2 block uint8 type -uint8 lock_time -uint8 cn0 -uint8 offsets_msb -int8 carrier_msb +uint8 lock_time # s +uint8 cn0 # 0.25 dB-Hz +uint8 offsets_msb # 65.536 m or 65.536 Hz +int8 carrier_msb # 65.536 cycles uint8 obs_info uint16 code_offset_lsb uint16 carrier_lsb diff --git a/msg/PVTCartesian.msg b/msg/PVTCartesian.msg index 526ebe8a..9a48aa81 100644 --- a/msg/PVTCartesian.msg +++ b/msg/PVTCartesian.msg @@ -7,27 +7,27 @@ BlockHeader block_header uint8 mode uint8 error -float64 x -float64 y -float64 z -float32 undulation -float32 vx -float32 vy -float32 vz -float32 cog -float64 rx_clk_bias -float32 rx_clk_drift +float64 x # m +float64 y # m +float64 z # m +float32 undulation # m +float32 vx # m/s +float32 vy # m/s +float32 vz # m/s +float32 cog # deg +float64 rx_clk_bias # ms +float32 rx_clk_drift # ppm uint8 time_system uint8 datum uint8 nr_sv uint8 wa_corr_info uint16 reference_id -uint16 mean_corr_age +uint16 mean_corr_age # 0.01s uint32 signal_info uint8 alert_flag uint8 nr_bases -uint16 ppp_info -uint16 latency -uint16 h_accuracy -uint16 v_accuracy +uint16 ppp_info # s +uint16 latency # 0.0001 s +uint16 h_accuracy # 0.01 m +uint16 v_accuracy # 0.01 m uint8 misc diff --git a/msg/PVTGeodetic.msg b/msg/PVTGeodetic.msg index 6c174ff5..78c6e4c1 100644 --- a/msg/PVTGeodetic.msg +++ b/msg/PVTGeodetic.msg @@ -7,27 +7,27 @@ BlockHeader block_header uint8 mode uint8 error -float64 latitude -float64 longitude -float64 height -float32 undulation -float32 vn -float32 ve -float32 vu -float32 cog -float64 rx_clk_bias -float32 rx_clk_drift +float64 latitude # rad +float64 longitude # rad +float64 height # m (ellipsoidal) +float32 undulation # m +float32 vn # m/s +float32 ve # m/s +float32 vu # m/s +float32 cog # deg +float64 rx_clk_bias # ms +float32 rx_clk_drift # ppm uint8 time_system uint8 datum uint8 nr_sv uint8 wa_corr_info uint16 reference_id -uint16 mean_corr_age +uint16 mean_corr_age # 0.01s uint32 signal_info uint8 alert_flag uint8 nr_bases -uint16 ppp_info -uint16 latency -uint16 h_accuracy -uint16 v_accuracy +uint16 ppp_info # s +uint16 latency # 0.0001 s +uint16 h_accuracy # 0.01 m +uint16 v_accuracy # 0.01 m uint8 misc \ No newline at end of file diff --git a/msg/PosCovCartesian.msg b/msg/PosCovCartesian.msg index 08ce8e3b..3b517687 100644 --- a/msg/PosCovCartesian.msg +++ b/msg/PosCovCartesian.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_xx -float32 cov_yy -float32 cov_zz -float32 cov_bb -float32 cov_xy -float32 cov_xz -float32 cov_xb -float32 cov_yz -float32 cov_yb -float32 cov_zb \ No newline at end of file +float32 cov_xx # m^2 +float32 cov_yy # m^2 +float32 cov_zz # m^2 +float32 cov_bb # m^2 +float32 cov_xy # m^2 +float32 cov_xz # m^2 +float32 cov_xb # m^2 +float32 cov_yz # m^2 +float32 cov_yb # m^2 +float32 cov_zb # m^2 \ No newline at end of file diff --git a/msg/PosCovGeodetic.msg b/msg/PosCovGeodetic.msg index 8f9f8c16..2d69400e 100644 --- a/msg/PosCovGeodetic.msg +++ b/msg/PosCovGeodetic.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_latlat -float32 cov_lonlon -float32 cov_hgthgt -float32 cov_bb -float32 cov_latlon -float32 cov_lathgt -float32 cov_latb -float32 cov_lonhgt -float32 cov_lonb -float32 cov_hb \ No newline at end of file +float32 cov_latlat # m^2 +float32 cov_lonlon # m^2 +float32 cov_hgthgt # m^2 +float32 cov_bb # m^2 +float32 cov_latlon # m^2 +float32 cov_lathgt # m^2 +float32 cov_latb # m^2 +float32 cov_lonhgt # m^2 +float32 cov_lonb # m^2 +float32 cov_hb # m^2 \ No newline at end of file diff --git a/msg/ReceiverTime.msg b/msg/ReceiverTime.msg index 8566f056..d2667190 100644 --- a/msg/ReceiverTime.msg +++ b/msg/ReceiverTime.msg @@ -5,11 +5,11 @@ std_msgs/Header header # SBF block header including time header BlockHeader block_header -int8 utc_year -int8 utc_month -int8 utc_day -int8 utc_hour -int8 utc_min -int8 utc_second -int8 delta_ls +int8 utc_year # year +int8 utc_month # month +int8 utc_day # day +int8 utc_hour # hour +int8 utc_min # minute +int8 utc_second # s +int8 delta_ls # s uint8 sync_level \ No newline at end of file diff --git a/msg/VelCovCartesian.msg b/msg/VelCovCartesian.msg index 35ada65d..5cb77ea1 100644 --- a/msg/VelCovCartesian.msg +++ b/msg/VelCovCartesian.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_vxvx -float32 cov_vyvy -float32 cov_vzvz -float32 cov_dtdt -float32 cov_vxvy -float32 cov_vxvz -float32 cov_vxdt -float32 cov_vyvz -float32 cov_vydt -float32 cov_vzdt \ No newline at end of file +float32 cov_vxvx # m^2/s^2 +float32 cov_vyvy # m^2/s^2 +float32 cov_vzvz # m^2/s^2 +float32 cov_dtdt # m^2/s^2 +float32 cov_vxvy # m^2/s^2 +float32 cov_vxvz # m^2/s^2 +float32 cov_vxdt # m^2/s^2 +float32 cov_vyvz # m^2/s^2 +float32 cov_vydt # m^2/s^2 +float32 cov_vzdt # m^2/s^2 \ No newline at end of file diff --git a/msg/VelSensorSetup.msg b/msg/VelSensorSetup.msg index f673096b..42de4c29 100644 --- a/msg/VelSensorSetup.msg +++ b/msg/VelSensorSetup.msg @@ -7,6 +7,6 @@ std_msgs/Header header BlockHeader block_header uint8 port -float32 lever_arm_x -float32 lever_arm_y -float32 lever_arm_z \ No newline at end of file +float32 lever_arm_x # m +float32 lever_arm_y # m +float32 lever_arm_z # m \ No newline at end of file From 3e12524f4a0bf1a1635b35b316b082fa4477fffc Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 14:44:22 +0100 Subject: [PATCH 050/170] Fix extsensor parser --- .../septentrio_gnss_driver/parsers/sbf_blocks.hpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index cccc26cd..8ceca632 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -1758,8 +1758,12 @@ ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg } case 3: { - qiLittleEndianParser(it, msg.sensor_temperature); - msg.sensor_temperature /= 100.0f; + int16_t temp; + qiLittleEndianParser(it, temp); + if (temp != -32768) + msg.sensor_temperature = temp / 100.0f; + else + msg.sensor_temperature = std::numeric_limits::quiet_NaN(); std::advance(it, 22); // reserved break; } @@ -1789,8 +1793,9 @@ ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg default: { node->log( - log_level::ERROR, - "Unknown external sensor measurement type in SBF ExtSensorMeas."); + log_level::DEBUG, + "Unknown external sensor measurement type in SBF ExtSensorMeas: " + + std::to_string(msg.type[i])); std::advance(it, 24); break; } From 91c45b461a2835ba7f627c1ac2b1335723314a74 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 14:45:02 +0100 Subject: [PATCH 051/170] Add processing latency correction for ROS timestamps --- .../communication/message_handler.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 5839ec08..742b4d53 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1803,6 +1803,17 @@ namespace io { : telegram->stamp; msg.header.frame_id = frameId; + if constexpr (std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value) + { + if (!settings_->use_gnss_time) + { + time_obj -= msg.latency * 100000ul; // from 0.0001 s to ns + } + } + msg.header.stamp = timestampToRos(time_obj); } From 0d72fe00c83ae4b7f549b9a8bacf08ddcb35b21f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 15:26:25 +0100 Subject: [PATCH 052/170] Set do-not-use for temp to NaN --- include/septentrio_gnss_driver/parsers/sbf_blocks.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 8ceca632..5f0ed115 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -1707,7 +1707,7 @@ ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg msg.std_dev_y = std::numeric_limits::quiet_NaN(); msg.std_dev_z = std::numeric_limits::quiet_NaN(); - msg.sensor_temperature = -32768.0f; // do not use value + msg.sensor_temperature = std::numeric_limits::quiet_NaN(); msg.zero_velocity_flag = std::numeric_limits::quiet_NaN(); msg.source.resize(msg.n); From 416c86db7a4f0e50f8fef23a9202beeddf7c134b Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 18:20:22 +0100 Subject: [PATCH 053/170] Add UDP client, WIP --- .../communication/io.hpp | 158 ++++++++++++++++++ .../communication/telegram.hpp | 5 +- 2 files changed, 161 insertions(+), 2 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index da898377..f7da1c2a 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -45,6 +45,7 @@ // ROSaic #include +#include //! Possible baudrates for the Rx const static std::array baudrates = { @@ -54,6 +55,163 @@ const static std::array baudrates = { namespace io { + class UdpClient + { + public: + UdpClient(ROSaicNodeBase* node, int16_t port, TelegramQueue* telegramQueue) : + node_(node), running_(true), port_(port), telegramQueue_(telegramQueue) + { + connect(); + watchdogThread_ = + std::thread(boost::bind(&UdpClient::runWatchdog, this)); + } + + ~UdpClient() + { + running_ = false; + + node_->log(log_level::INFO, "UDP client shutting down threads"); + ioService_.stop(); + ioThread_.join(); + watchdogThread_.join(); + node_->log(log_level::INFO, " UDP client threads stopped"); + } + + private: + void connect() + { + socket_.reset(new boost::asio::ip::udp::socket( + ioService_, + boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), port_))); + + asyncReceive(); + + ioThread_ = std::thread(boost::bind(&UdpClient::runIoService, this)); + + node_->log(log_level::INFO, + "Listening on UDP port " + std::to_string(port_)); + } + + void asyncReceive() + { + std::shared_ptr telegram(new Telegram(MAX_SBF_SIZE)); + + socket_->async_receive_from( + boost::asio::buffer(telegram->message, MAX_SBF_SIZE), eP_, + boost::bind(&UdpClient::handleReceive, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred, telegram)); + } + + void handleReceive(const boost::system::error_code& error, + size_t bytes_recvd, std::shared_ptr telegram) + { + telegram->stamp = node_->getTime(); + + if (!error && (bytes_recvd > 0)) + { + if (bytes_recvd > 2) + { + telegram->message.resize(bytes_recvd); + if (telegram->message[0] == SYNC_BYTE_1) + { + if (telegram->message[1] == SBF_SYNC_BYTE_2) + { + if (crc::isValid(telegram->message)) + { + telegram->type == telegram_type::SBF; + telegramQueue_->push(telegram); + } else + node_->log( + log_level::DEBUG, + "AsyncManager crc failed for SBF " + + std::to_string(parsing_utilities::getId( + telegram->message)) + + "."); + } + } else if ((telegram->message[1] == NMEA_SYNC_BYTE_2) && + (telegram->message[2] == NMEA_SYNC_BYTE_3)) + { + // TODO check \r\n + telegram->type = telegram_type::NMEA; + telegramQueue_->push(telegram); + + } else if ((telegram->message[1] == NMEA_INS_SYNC_BYTE_2) && + (telegram->message[2] == NMEA_INS_SYNC_BYTE_3)) + { + // TODO check \r\n + telegram->type = telegram_type::NMEA_INS; + telegramQueue_->push(telegram); + } else if (telegram->message[1] == RESPONSE_SYNC_BYTE_2) + { + // TODO determine if response is sent via UDP + if ((telegram->message[2] == RESPONSE_SYNC_BYTE_3) || + (telegram->message[2] == RESPONSE_SYNC_BYTE_3a)) + { + // TODO check \r\n + telegram->type = telegram_type::RESPONSE; + telegramQueue_->push(telegram); + } else if (telegram->message[2] == ERROR_SYNC_BYTE_3) + { + // TODO check \r\n + telegram->type = telegram_type::ERROR_RESPONSE; + telegramQueue_->push(telegram); + } else + { + // TODO fault + } + } else + { + // TODO generic string or cd? determine if these are sent via + // UDP + } + } + + } else + { + node_->log(log_level::ERROR, + "UDP client receive error: " + error.message()); + } + + asyncReceive(); + } + + void runIoService() + { + ioService_.run(); + node_->log(log_level::INFO, "UDP client ioService terminated."); + } + + void runWatchdog() + { + while (running_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + if (running_ && ioService_.stopped()) + { + node_->log(log_level::ERROR, + "UDP client connection lost. Trying to reconnect."); + ioService_.reset(); + ioThread_.join(); + connect(); + } + } + } + + private: + //! Pointer to the node + ROSaicNodeBase* node_; + std::atomic running_; + int16_t port_; + boost::asio::io_service ioService_; + std::thread ioThread_; + std::thread watchdogThread_; + boost::asio::ip::udp::endpoint eP_; + std::unique_ptr socket_; + TelegramQueue* telegramQueue_; + }; + class TcpIo { public: diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 343e6052..50d58a13 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -106,8 +106,9 @@ struct Telegram telegram_type::TelegramType type; std::vector message; - Telegram() noexcept : - stamp(0), type(telegram_type::EMPTY), message(std::vector(3)) + Telegram(size_t preallocate = 3) noexcept : + stamp(0), type(telegram_type::EMPTY), + message(std::vector(preallocate)) { } From 5f7468a51ffbe5a632f06beb3fd4555e2786d1a5 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Jan 2023 23:13:50 +0100 Subject: [PATCH 054/170] Add test code for UDP, WIP --- .../communication/communication_core.hpp | 2 ++ .../septentrio_gnss_driver/communication/io.hpp | 3 +++ .../communication/communication_core.cpp | 15 +++++++++------ 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 56ee8eaf..696cee9e 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -161,6 +161,8 @@ namespace io { //! This declaration is deliberately stream-independent (Serial or TCP). std::unique_ptr manager_; + std::unique_ptr udpClient_; + bool nmeaActivated_ = false; //! Indicator for threads to run diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index f7da1c2a..0d1febe2 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -113,6 +113,9 @@ namespace io { if (bytes_recvd > 2) { telegram->message.resize(bytes_recvd); + node_->log(log_level::INFO, + "Buffer: " + std::string(telegram->message.begin(), + telegram->message.end())); if (telegram->message[0] == SYNC_BYTE_1) { if (telegram->message[1] == SBF_SYNC_BYTE_2) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index d180e941..662e0e56 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -176,6 +176,7 @@ namespace io { case device_type::TCP: { manager_.reset(new AsyncManager(node_, &telegramQueue_)); + udpClient_.reset(new UdpClient(node_, 28785, &telegramQueue_)); break; } case device_type::SERIAL: @@ -621,6 +622,8 @@ namespace io { } } + // TODO UDP + send("siss, IPS1, 28785, UDP, 172.18.10.219\x0D"); // Setting up SBF blocks with rx_period_rest { std::stringstream blocks; @@ -644,8 +647,8 @@ namespace io { std::stringstream ss; ss << "sso, Stream" << std::to_string(stream) << ", " - << mainConnectionDescriptor_ << "," << blocks.str() << ", " - << rest_interval << "\x0D"; + << "IPS1" + << "," << blocks.str() << ", " << rest_interval << "\x0D"; send(ss.str()); ++stream; } @@ -674,8 +677,8 @@ namespace io { std::stringstream ss; ss << "sno, Stream" << std::to_string(stream) << ", " - << mainConnectionDescriptor_ << "," << blocks.str() << ", " - << pvt_interval << "\x0D"; + << "IPS1" + << "," << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); ++stream; } @@ -787,8 +790,8 @@ namespace io { } std::stringstream ss; ss << "sso, Stream" << std::to_string(stream) << ", " - << mainConnectionDescriptor_ << "," << blocks.str() << ", " - << pvt_interval << "\x0D"; + << "IPS1" + << "," << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); ++stream; } From 4a0bafbf477da0aa042709e58ede5c6e4f9e9e47 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 19 Jan 2023 13:41:11 +0100 Subject: [PATCH 055/170] Fix UDP message identification logic --- .../communication/io.hpp | 57 +++++++++---------- .../communication/telegram.hpp | 2 +- .../communication/communication_core.cpp | 21 ++++--- .../communication/message_handler.cpp | 8 +-- 4 files changed, 42 insertions(+), 46 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 0d1febe2..6a0b574b 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -113,16 +113,16 @@ namespace io { if (bytes_recvd > 2) { telegram->message.resize(bytes_recvd); - node_->log(log_level::INFO, + /*node_->log(log_level::DEBUG, "Buffer: " + std::string(telegram->message.begin(), - telegram->message.end())); + telegram->message.end()));*/ if (telegram->message[0] == SYNC_BYTE_1) { if (telegram->message[1] == SBF_SYNC_BYTE_2) { if (crc::isValid(telegram->message)) { - telegram->type == telegram_type::SBF; + telegram->type = telegram_type::SBF; telegramQueue_->push(telegram); } else node_->log( @@ -131,42 +131,39 @@ namespace io { std::to_string(parsing_utilities::getId( telegram->message)) + "."); - } - } else if ((telegram->message[1] == NMEA_SYNC_BYTE_2) && - (telegram->message[2] == NMEA_SYNC_BYTE_3)) - { - // TODO check \r\n - telegram->type = telegram_type::NMEA; - telegramQueue_->push(telegram); - - } else if ((telegram->message[1] == NMEA_INS_SYNC_BYTE_2) && - (telegram->message[2] == NMEA_INS_SYNC_BYTE_3)) - { - // TODO check \r\n - telegram->type = telegram_type::NMEA_INS; - telegramQueue_->push(telegram); - } else if (telegram->message[1] == RESPONSE_SYNC_BYTE_2) - { - // TODO determine if response is sent via UDP - if ((telegram->message[2] == RESPONSE_SYNC_BYTE_3) || - (telegram->message[2] == RESPONSE_SYNC_BYTE_3a)) + } else if ((telegram->message[1] == NMEA_SYNC_BYTE_2) && + (telegram->message[2] == NMEA_SYNC_BYTE_3) && + (telegram->message[telegram->message.size() - + 2] == CR) && + (telegram->message[telegram->message.size() - + 1] == LF)) { - // TODO check \r\n - telegram->type = telegram_type::RESPONSE; + telegram->type = telegram_type::NMEA; telegramQueue_->push(telegram); - } else if (telegram->message[2] == ERROR_SYNC_BYTE_3) + + } else if ((telegram->message[1] == NMEA_INS_SYNC_BYTE_2) && + (telegram->message[2] == NMEA_INS_SYNC_BYTE_3) && + (telegram->message[telegram->message.size() - + 2] == CR) && + (telegram->message[telegram->message.size() - + 1] == LF)) { - // TODO check \r\n - telegram->type = telegram_type::ERROR_RESPONSE; + telegram->type = telegram_type::NMEA_INS; telegramQueue_->push(telegram); } else { - // TODO fault + node_->log(log_level::DEBUG, + "head: " + + std::string(std::string( + telegram->message.begin(), + telegram->message.begin() + 2))); } } else { - // TODO generic string or cd? determine if these are sent via - // UDP + node_->log(log_level::DEBUG, + "head: " + std::string(std::string( + telegram->message.begin(), + telegram->message.begin() + 2))); } } diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 50d58a13..8e6f3b09 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -159,7 +159,7 @@ class ConcurrentQueue private: std::queue queue_; std::condition_variable cond_; - std::mutex mtx_; + mutable std::mutex mtx_; }; template diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 662e0e56..0f742c4c 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -623,8 +623,10 @@ namespace io { } // TODO UDP - send("siss, IPS1, 28785, UDP, 172.18.10.219\x0D"); - // Setting up SBF blocks with rx_period_rest + send("siss, IPS1, 28785, UDP, 10.255.255.200\x0D"); + std::string port = "IPS1"; + port = mainConnectionDescriptor_; + // Setting up SBF blocks with rx_period_rest { std::stringstream blocks; if (settings_->septentrio_receiver_type == "ins") @@ -646,9 +648,8 @@ namespace io { blocks << " +ReceiverSetup"; std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " - << "IPS1" - << "," << blocks.str() << ", " << rest_interval << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " << port << "," + << blocks.str() << ", " << rest_interval << "\x0D"; send(ss.str()); ++stream; } @@ -676,9 +677,8 @@ namespace io { } std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " - << "IPS1" - << "," << blocks.str() << ", " << pvt_interval << "\x0D"; + ss << "sno, Stream" << std::to_string(stream) << ", " << port << "," + << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); ++stream; } @@ -789,9 +789,8 @@ namespace io { } } std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " - << "IPS1" - << "," << blocks.str() << ", " << pvt_interval << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " << port << "," + << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); ++stream; } diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 742b4d53..0bbfb519 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1912,10 +1912,10 @@ namespace io { uint16_t sbfId = parsing_utilities::getId(telegram->message); - node_->log(log_level::DEBUG, "ROSaic reading SBF block " + - std::to_string(sbfId) + " made up of " + - std::to_string(telegram->message.size()) + - " bytes..."); + /*node_->log(log_level::DEBUG, "ROSaic reading SBF block " + + std::to_string(sbfId) + " made up of " + + std::to_string(telegram->message.size()) + + " bytes...");*/ switch (sbfId) { From 749f97ea5595ff4ab055ce581e37e7e857dabe7d Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 19 Jan 2023 14:03:07 +0100 Subject: [PATCH 056/170] Prepare communication for UDP option (still inactive) --- .../communication/communication_core.hpp | 4 ++- .../communication/communication_core.cpp | 31 ++++++++++--------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 696cee9e..fc51f6f5 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -169,6 +169,8 @@ namespace io { std::atomic running_; //! Main communication port - std::string mainConnectionDescriptor_; + std::string mainConnectionPort_; + // Port for receiving data streams + std::string streamPort_; }; } // namespace io diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 0f742c4c..ba68bceb 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -73,7 +73,7 @@ namespace io { if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { resetMainConnection(); - send("sdio, " + mainConnectionDescriptor_ + ", auto, none\x0D"); + send("sdio, " + streamPort_ + ", auto, none\x0D"); for (auto ntrip : settings_->rtk_settings.ntrip) { if (!ntrip.id.empty() && !ntrip.keep_open) @@ -176,7 +176,8 @@ namespace io { case device_type::TCP: { manager_.reset(new AsyncManager(node_, &telegramQueue_)); - udpClient_.reset(new UdpClient(node_, 28785, &telegramQueue_)); + // udpClient_.reset(new UdpClient(node_, 28785, &telegramQueue_)); //TODO + // UDP break; } case device_type::SERIAL: @@ -228,14 +229,16 @@ namespace io { boost::regex_match(settings_->device, match, boost::regex("(tcp)://(.+):(\\d+)")); std::string proto(match[1]); - mainConnectionDescriptor_ = resetMainConnection(); + mainConnectionPort_ = resetMainConnection(); + // streamPort_ = "IPS1"; // TODO UDP + streamPort_ = mainConnectionPort_; if (proto == "tcp") { - // mainConnectionDescriptor_ = manager_->getConnectionDescriptor(); + // mainConnectionPort_ = manager_->getConnectionDescriptor(); } else { // TODO check if rx_serial_portcan be removed - mainConnectionDescriptor_ = settings_->rx_serial_port; + mainConnectionPort_ = settings_->rx_serial_port; // After booting, the Rx sends the characters "x?" to all ports, which // could potentially mingle with our first command. Hence send a // safeguard command "lif", whose potentially false processing is @@ -623,9 +626,7 @@ namespace io { } // TODO UDP - send("siss, IPS1, 28785, UDP, 10.255.255.200\x0D"); - std::string port = "IPS1"; - port = mainConnectionDescriptor_; + // send("siss, IPS1, 28785, UDP, 10.255.255.200\x0D"); // Setting up SBF blocks with rx_period_rest { std::stringstream blocks; @@ -648,8 +649,8 @@ namespace io { blocks << " +ReceiverSetup"; std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << port << "," - << blocks.str() << ", " << rest_interval << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " << streamPort_ + << "," << blocks.str() << ", " << rest_interval << "\x0D"; send(ss.str()); ++stream; } @@ -677,8 +678,8 @@ namespace io { } std::stringstream ss; - ss << "sno, Stream" << std::to_string(stream) << ", " << port << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; + ss << "sno, Stream" << std::to_string(stream) << ", " << streamPort_ + << "," << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); ++stream; } @@ -789,8 +790,8 @@ namespace io { } } std::stringstream ss; - ss << "sso, Stream" << std::to_string(stream) << ", " << port << "," - << blocks.str() << ", " << pvt_interval << "\x0D"; + ss << "sso, Stream" << std::to_string(stream) << ", " << streamPort_ + << "," << blocks.str() << ", " << pvt_interval << "\x0D"; send(ss.str()); ++stream; } @@ -816,7 +817,7 @@ namespace io { (settings_->ins_vsm_ros_source == "twist")) { std::string s; - s = "sdio, " + mainConnectionDescriptor_ + ", NMEA, +NMEA +SBF\x0D"; + s = "sdio, " + mainConnectionPort_ + ", NMEA, +NMEA +SBF\x0D"; send(s); nmeaActivated_ = true; } From 4b08ff880d14b22e16107125a1149f065e8c6d12 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 19 Jan 2023 14:04:44 +0100 Subject: [PATCH 057/170] Add NMEA talker ID setting to ensure reception --- .../communication/communication_core.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index ba68bceb..0c17f68c 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -657,7 +657,10 @@ namespace io { // Setting up NMEA streams { - // send("snti, GP\x0D"); + if (settings_->septentrio_receiver_type == "ins") + send("snti, IN\x0D"); + else + send("snti, GP\x0D"); std::stringstream blocks; if (settings_->publish_gpgga) From d0466f7a2bc4c8d7e5a17c0e0df5ff6f4dedcde5 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 19 Jan 2023 14:12:35 +0100 Subject: [PATCH 058/170] Fix talker ID setting for INS --- src/septentrio_gnss_driver/communication/communication_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 0c17f68c..2f679e72 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -658,7 +658,7 @@ namespace io { // Setting up NMEA streams { if (settings_->septentrio_receiver_type == "ins") - send("snti, IN\x0D"); + send("snti, auto\x0D"); else send("snti, GP\x0D"); From 3aa5c175ecf97a533049c65c1ef5748ed2cdcfa7 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 20 Jan 2023 00:07:13 +0100 Subject: [PATCH 059/170] Fix serial connection --- include/septentrio_gnss_driver/communication/io.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 6a0b574b..13fdfc00 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -302,16 +302,16 @@ namespace io { try { node_->log(log_level::INFO, - "Connecting serially to device" + + "Connecting serially to device " + node_->settings()->device + ", targeted baudrate: " + std::to_string(node_->settings()->baudrate)); - stream_->open(port_); + stream_->open(node_->settings()->device); opened = true; } catch (const boost::system::system_error& err) { node_->log(log_level::ERROR, - "SerialCoket: Could not open serial port " + port_ + + "Could not open serial port " + node_->settings()->device + ". Error: " + err.what() + ". Will retry every second."); @@ -463,7 +463,6 @@ namespace io { private: ROSaicNodeBase* node_; std::string flowcontrol_; - std::string port_; uint32_t baudrate_; public: From 0c6d85587f2bf25ed81f48ea3ad400853b8a17cc Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 20 Jan 2023 08:42:59 +0100 Subject: [PATCH 060/170] Send port reset only once --- .../communication/telegram_handler.hpp | 14 +------------- .../communication/communication_core.cpp | 4 +++- .../communication/telegram_handler.cpp | 14 +++++--------- 3 files changed, 9 insertions(+), 23 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index e236be13..6d45d603 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -118,17 +118,7 @@ namespace io { void handleTelegram(const std::shared_ptr& telegram); //! Returns the connection descriptor - void resetWaitforMainCd() - { - // Cd is sent twice only after startup - if (init_ == false) - { - cdCtr_ = 0; - init_ = true; - } else - cdCtr_ = 1; - mainConnectionDescriptor_ = std::string(); - } + void resetWaitforMainCd() { mainConnectionDescriptor_ = std::string(); } //! Returns the connection descriptor [[nodiscard]] std::string getMainCd() @@ -152,8 +142,6 @@ namespace io { //! MessageHandler parser MessageHandler messageHandler_; - bool init_ = false; - uint8_t cdCtr_ = 0; Semaphore cdSemaphore_; Semaphore responseSemaphore_; std::string mainConnectionDescriptor_ = std::string(); diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 2f679e72..df553351 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -230,6 +230,8 @@ namespace io { boost::regex("(tcp)://(.+):(\\d+)")); std::string proto(match[1]); mainConnectionPort_ = resetMainConnection(); + node_->log(log_level::INFO, + "The connection descriptor is " + mainConnectionPort_); // streamPort_ = "IPS1"; // TODO UDP streamPort_ = mainConnectionPort_; if (proto == "tcp") @@ -840,7 +842,7 @@ namespace io { { // Escape sequence (escape from correction mode), ensuring that we // can send our real commands afterwards... - std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D"); + std::string cmd("\x0DSSSSSSSSSS\x0D\x0D"); telegramHandler_.resetWaitforMainCd(); manager_.get()->send(cmd); return telegramHandler_.getMainCd(); diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 1d749f36..755d2afc 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -122,19 +122,15 @@ namespace io { void TelegramHandler::handleCd(const std::shared_ptr& telegram) { - if (cdCtr_ < 2) + node_->log(log_level::DEBUG, + "handleCd: " + std::string(telegram->message.begin(), + telegram->message.end())); + if (telegram->message.back() == CONNECTION_DESCRIPTOR_FOOTER) { mainConnectionDescriptor_ = std::string(telegram->message.begin(), telegram->message.end() - 1); - ++cdCtr_; - if (cdCtr_ == 2) - { - node_->log(log_level::INFO, "The connection descriptor is " + - mainConnectionDescriptor_); - - cdSemaphore_.notify(); - } + cdSemaphore_.notify(); } } } // namespace io \ No newline at end of file From eac90f055e45fb23b9714650e8272634c8ac8c97 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 21 Jan 2023 23:54:38 +0100 Subject: [PATCH 061/170] notify semaphores in destructor --- .../communication/telegram_handler.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 6d45d603..7d6b6a90 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -112,6 +112,12 @@ namespace io { public: TelegramHandler(ROSaicNodeBase* node) : node_(node), messageHandler_(node) {} + ~TelegramHandler() + { + cdSemaphore_.notify(); + responseSemaphore_.notify(); + } + /** * @brief Called every time a telegram is received */ From 989b60c4bf3dae2646543c993502c2e624f54077 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 22 Jan 2023 01:05:24 +0100 Subject: [PATCH 062/170] Refactor ioservice --- .../communication/async_manager.hpp | 17 ++++----- .../communication/io.hpp | 36 +++++++++---------- .../communication/communication_core.cpp | 2 +- 3 files changed, 28 insertions(+), 27 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index ae5b2419..40229133 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -138,6 +138,7 @@ namespace io { //! Pointer to the node ROSaicNodeBase* node_; + std::shared_ptr ioService_; IoType ioInterface_; std::atomic running_; std::thread ioThread_; @@ -155,8 +156,8 @@ namespace io { template AsyncManager::AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue) : - node_(node), - ioInterface_(node), telegramQueue_(telegramQueue) + node_(node), ioService_(new boost::asio::io_service), + ioInterface_(node, ioService_), telegramQueue_(telegramQueue) { node_->log(log_level::DEBUG, "AsyncManager created."); } @@ -167,7 +168,7 @@ namespace io { running_ = false; close(); node_->log(log_level::DEBUG, "AsyncManager shutting down threads"); - ioInterface_.ioService_.stop(); + ioService_->stop(); ioThread_.join(); watchdogThread_.join(); node_->log(log_level::DEBUG, "AsyncManager threads stopped"); @@ -198,7 +199,7 @@ namespace io { return; } - ioInterface_.ioService_.post( + ioService_->post( boost::bind(&AsyncManager::write, this, cmd)); } @@ -213,13 +214,13 @@ namespace io { template void AsyncManager::close() { - ioInterface_.ioService_.post([this]() { ioInterface_.close(); }); + ioService_->post([this]() { ioInterface_.close(); }); } template void AsyncManager::runIoService() { - ioInterface_.ioService_.run(); + ioService_->run(); node_->log(log_level::DEBUG, "AsyncManager ioService terminated."); } @@ -230,11 +231,11 @@ namespace io { { std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - if (running_ && ioInterface_.ioService_.stopped()) + if (running_ && ioService_->stopped()) { node_->log(log_level::DEBUG, "AsyncManager connection lost. Trying to reconnect."); - ioInterface_.ioService_.reset(); + ioService_->reset(); ioThread_.join(); while (!ioInterface_.connect()) std::this_thread::sleep_for(std::chrono::milliseconds(1000)); diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 13fdfc00..d2567c14 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -215,7 +215,7 @@ namespace io { class TcpIo { public: - TcpIo(ROSaicNodeBase* node) : node_(node) {} + TcpIo(ROSaicNodeBase* node, std::shared_ptr ioService) : node_(node), ioService_(ioService) {} ~TcpIo() { stream_->close(); } @@ -227,7 +227,7 @@ namespace io { try { - boost::asio::ip::tcp::resolver resolver(ioService_); + boost::asio::ip::tcp::resolver resolver(*ioService_); boost::asio::ip::tcp::resolver::query query( node_->settings()->tcp_ip, node_->settings()->tcp_port); endpointIterator = resolver.resolve(query); @@ -240,7 +240,7 @@ namespace io { return false; } - stream_.reset(new boost::asio::ip::tcp::socket(ioService_)); + stream_.reset(new boost::asio::ip::tcp::socket(*ioService_)); node_->log(log_level::INFO, "Connecting to tcp://" + node_->settings()->tcp_ip + ":" + @@ -268,20 +268,20 @@ namespace io { private: ROSaicNodeBase* node_; - + std::shared_ptr ioService_; + public: - boost::asio::io_service ioService_; std::unique_ptr stream_; }; class SerialIo { public: - SerialIo(ROSaicNodeBase* node) : - node_(node), flowcontrol_(node->settings()->hw_flow_control), + SerialIo(ROSaicNodeBase* node, std::shared_ptr ioService) : + node_(node), ioService_(ioService), flowcontrol_(node->settings()->hw_flow_control), baudrate_(node->settings()->baudrate) { - stream_.reset(new boost::asio::serial_port(ioService_)); + stream_.reset(new boost::asio::serial_port(*ioService_)); } ~SerialIo() { stream_->close(); } @@ -327,8 +327,8 @@ namespace io { stream_->set_option(boost::asio::serial_port_base::character_size(8)); stream_->set_option(boost::asio::serial_port_base::stop_bits( boost::asio::serial_port_base::stop_bits::one)); - stream_->set_option(boost::asio::serial_port_base::flow_control( - boost::asio::serial_port_base::flow_control::none)); + /*stream_->set_option(boost::asio::serial_port_base::flow_control( + boost::asio::serial_port_base::flow_control::none));*/ int fd = stream_->native_handle(); termios tio; @@ -462,18 +462,18 @@ namespace io { private: ROSaicNodeBase* node_; + std::shared_ptr ioService_; std::string flowcontrol_; uint32_t baudrate_; public: - boost::asio::io_service ioService_; std::unique_ptr stream_; }; class SbfFileIo { public: - SbfFileIo(ROSaicNodeBase* node) : node_(node) {} + SbfFileIo(ROSaicNodeBase* node, std::shared_ptr ioService) : node_(node), ioService_(ioService) {} ~SbfFileIo() { stream_->close(); } @@ -493,7 +493,7 @@ namespace io { try { - stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); + stream_.reset(new boost::asio::posix::stream_descriptor(*ioService_)); stream_->assign(fd); } catch (std::runtime_error& e) @@ -507,16 +507,16 @@ namespace io { private: ROSaicNodeBase* node_; - + std::shared_ptr ioService_; + public: - boost::asio::io_service ioService_; std::unique_ptr stream_; }; class PcapFileIo { public: - PcapFileIo(ROSaicNodeBase* node) : node_(node) {} + PcapFileIo(ROSaicNodeBase* node, std::shared_ptr ioService) : node_(node), ioService_(ioService) {} ~PcapFileIo() { @@ -537,7 +537,7 @@ namespace io { node_->log(log_level::INFO, "Opening pcap file stream" + node_->settings()->device + "..."); - stream_.reset(new boost::asio::posix::stream_descriptor(ioService_)); + stream_.reset(new boost::asio::posix::stream_descriptor(*ioService_)); pcap_ = pcap_open_offline(node_->settings()->device.c_str(), errBuff_.data()); @@ -554,11 +554,11 @@ namespace io { private: ROSaicNodeBase* node_; + std::shared_ptr ioService_; std::array errBuff_; pcap_t* pcap_; public: - boost::asio::io_service ioService_; std::unique_ptr stream_; }; } // namespace io \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index df553351..e796536d 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -239,7 +239,7 @@ namespace io { // mainConnectionPort_ = manager_->getConnectionDescriptor(); } else { - // TODO check if rx_serial_portcan be removed + // TODO check if rx_serial_port can be removed mainConnectionPort_ = settings_->rx_serial_port; // After booting, the Rx sends the characters "x?" to all ports, which // could potentially mingle with our first command. Hence send a From d0c4075e49674275bfd56ed60a632876cc98fc97 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 22 Jan 2023 11:38:22 +0100 Subject: [PATCH 063/170] Replace flow control setup --- .../communication/io.hpp | 30 +++++++------------ .../communication/telegram_handler.hpp | 6 ++++ .../communication/communication_core.cpp | 3 +- .../communication/message_handler.cpp | 3 +- 4 files changed, 19 insertions(+), 23 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index d2567c14..29415d04 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -327,35 +327,21 @@ namespace io { stream_->set_option(boost::asio::serial_port_base::character_size(8)); stream_->set_option(boost::asio::serial_port_base::stop_bits( boost::asio::serial_port_base::stop_bits::one)); - /*stream_->set_option(boost::asio::serial_port_base::flow_control( - boost::asio::serial_port_base::flow_control::none));*/ - - int fd = stream_->native_handle(); - termios tio; - // Get terminal attribute, follows the syntax - // int tcgetattr(int fd, struct termios *termios_p); - tcgetattr(fd, &tio); // Hardware flow control settings if (flowcontrol_ == "RTS|CTS") { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag |= CRTSCTS; + stream_->set_option(boost::asio::serial_port_base::flow_control( + boost::asio::serial_port_base::flow_control::hardware)); } else { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag &= ~CRTSCTS; + stream_->set_option(boost::asio::serial_port_base::flow_control( + boost::asio::serial_port_base::flow_control::none)); } - // Setting serial port to "raw" mode to prevent EOF exit.. - cfmakeraw(&tio); - - // Commit settings, syntax is - // int tcsetattr(int fd, int optional_actions, const struct termios - // *termios_p); - tcsetattr(fd, TCSANOW, &tio); + // Set low latency + int fd = stream_->native_handle(); struct serial_struct serialInfo; - ioctl(fd, TIOCGSERIAL, &serialInfo); serialInfo.flags |= ASYNC_LOW_LATENCY; ioctl(fd, TIOCSSERIAL, &serialInfo); @@ -457,6 +443,10 @@ namespace io { "Set ASIO baudrate to " + std::to_string(current_baudrate.value()) + ", leaving InitializeSerial() method"); + + // clear io + ::tcflush(stream_->native_handle(), TCIOFLUSH); + return true; } diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 7d6b6a90..3a84cd7a 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -118,6 +118,12 @@ namespace io { responseSemaphore_.notify(); } + void clearSemaphores() + { + cdSemaphore_.notify(); + responseSemaphore_.notify(); + } + /** * @brief Called every time a telegram is received */ diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index e796536d..61a6b803 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -70,6 +70,7 @@ namespace io { CommunicationCore::~CommunicationCore() { + telegramHandler_.clearSemaphores(); if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { resetMainConnection(); @@ -245,7 +246,7 @@ namespace io { // could potentially mingle with our first command. Hence send a // safeguard command "lif", whose potentially false processing is // harmless. - send("lif, Identification \x0D"); + //send("lif, Identification \x0D"); } node_->log(log_level::INFO, "Setting up Rx."); diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 0bbfb519..0c8eac2f 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2350,8 +2350,7 @@ namespace io { std::to_string(gnss_major) + "." + std::to_string(gnss_minor) + "." + std::to_string(gnss_patch) + " or consult README."); - } else - node_->log(log_level::ERROR, "gnss"); + } } } From a55c8d9b22d4cd0b74757121f6964f104177ae9f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 22 Jan 2023 23:50:53 +0100 Subject: [PATCH 064/170] Fix reconnection logic --- CHANGELOG.rst | 2 ++ .../communication/async_manager.hpp | 19 +++++++++---------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 7742d5ed..5aad3416 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,6 +4,8 @@ Changelog for package septentrio_gnss_driver 1.2.4 (upcoming) ------------------ +* New Features + * Reconnect after connection interruption * Improvements * Rework IO core and message handling -> reduce system load * Preliminary Features diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 40229133..6da98148 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -185,7 +185,6 @@ namespace io { } receive(); - watchdogThread_ = std::thread(std::bind(&AsyncManager::runWatchdog, this)); return true; } @@ -207,8 +206,11 @@ namespace io { void AsyncManager::receive() { resync(); - ioThread_ = std::thread( - std::thread(std::bind(&AsyncManager::runIoService, this))); + ioThread_ = + std::thread(std::bind(&AsyncManager::runIoService, this)); + if (!watchdogThread_.joinable()) + watchdogThread_ = + std::thread(std::bind(&AsyncManager::runWatchdog, this)); } template @@ -229,11 +231,10 @@ namespace io { { while (running_) { - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); if (running_ && ioService_->stopped()) { - node_->log(log_level::DEBUG, + node_->log(log_level::ERROR, "AsyncManager connection lost. Trying to reconnect."); ioService_->reset(); ioThread_.join(); @@ -430,7 +431,6 @@ namespace io { { node_->log(log_level::DEBUG, "AsyncManager sync read error: " + ec.message()); - resync(); } }); } @@ -472,7 +472,6 @@ namespace io { node_->log(log_level::DEBUG, "AsyncManager SBF header read error: " + ec.message()); - resync(); } }); } @@ -507,12 +506,13 @@ namespace io { "AsyncManager SBF read fault, wrong number of bytes read: " + std::to_string(numBytes)); } + resync(); } else { node_->log(log_level::DEBUG, "AsyncManager SBF read error: " + ec.message()); } - resync(); + }); } @@ -600,7 +600,6 @@ namespace io { { node_->log(log_level::DEBUG, "AsyncManager string read error: " + ec.message()); - resync(); } }); } From d0acf3a5de5fb8103be23adeb5a7e7a2812d8a97 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 08:49:09 +0100 Subject: [PATCH 065/170] Fix leap seconds for timestamp if reading from file --- CHANGELOG.rst | 2 +- .../communication/message_handler.hpp | 14 ++++-- .../communication/communication_core.cpp | 2 +- .../communication/message_handler.cpp | 46 +++++++++++-------- .../node/rosaic_node.cpp | 2 +- 5 files changed, 39 insertions(+), 27 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5aad3416..069a3613 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,7 +5,7 @@ Changelog for package septentrio_gnss_driver 1.2.4 (upcoming) ------------------ * New Features - * Reconnect after connection interruption + * Recovery from connection interruption * Improvements * Rework IO core and message handling -> reduce system load * Preliminary Features diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index c4ed27e5..d6810978 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -137,16 +137,20 @@ namespace io { public: /** * @brief Constructor of the MessageHandler class - * - * One can always provide a non-const value where a const one was expected. - * The const-ness of the argument just means the function promises not to - * change it.. Recall: static_cast by the way can remove or add const-ness, - * no other C++ cast is capable of removing it (not even reinterpret_cast) * @param[in] node Pointer to the node) */ MessageHandler(ROSaicNodeBase* node) : node_(node), settings_(node->settings()), unix_time_(0) { + if (settings_->leap_seconds != -128) + current_leap_seconds_ = settings_->leap_seconds; + } + + void setLeapSeconds() + { + // set leap seconds to paramter if reading from file + if (settings_->read_from_sbf_log || settings_->read_from_pcap) + current_leap_seconds_ = settings_->leap_seconds; } /** diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 61a6b803..c506b3ab 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -246,7 +246,7 @@ namespace io { // could potentially mingle with our first command. Hence send a // safeguard command "lif", whose potentially false processing is // harmless. - //send("lif, Identification \x0D"); + // send("lif, Identification \x0D"); } node_->log(log_level::INFO, "Setting up Rx."); diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 0c8eac2f..823764ce 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1848,6 +1848,7 @@ namespace io { if (current_leap_seconds_ != -128) time_obj -= current_leap_seconds_ * secToNSec; + // else: warn? return time_obj; } @@ -1869,14 +1870,16 @@ namespace io { node_->publishMessage(topic, msg); } else { - /*node_->log( - LogLevel::DEBUG, - "Not publishing message with GNSS time because no leap seconds are - available yet.");*/ + node_->log( + log_level::DEBUG, + "Not publishing message with GNSS time because no leap seconds are available yet."); if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { node_->log( log_level::WARN, "No leap seconds were set and none were received from log yet."); + setLeapSeconds(); + } } } @@ -1887,8 +1890,7 @@ namespace io { { // TODO: maybe publish only if wnc and tow is valid? if (!settings_->use_gnss_time || - (settings_->use_gnss_time && (current_leap_seconds_ != -128) && - (current_leap_seconds_ != 0))) + (settings_->use_gnss_time && (current_leap_seconds_ != -128))) { if (settings_->read_from_sbf_log || settings_->read_from_pcap) { @@ -1901,9 +1903,12 @@ namespace io { log_level::DEBUG, "Not publishing tf with GNSS time because no leap seconds are available yet."); if (settings_->read_from_sbf_log || settings_->read_from_pcap) + { node_->log( log_level::WARN, - "No leap seconds were set and none were received from log yet."); + "No leap seconds were set and none were received from log yet. "); + setLeapSeconds(); + }; } } @@ -2371,7 +2376,8 @@ namespace io { } default: { - node_->log(log_level::DEBUG, "unknown SBF block received."); + node_->log(log_level::DEBUG, "unhandled SBF block " + + std::to_string(sbfId) + " received."); break; } // Many more to be implemented... @@ -2692,24 +2698,26 @@ namespace io { { Timestamp unix_old = unix_time_; unix_time_ = time_obj; - if ((unix_old != 0) && (unix_time_ != unix_old)) + if ((unix_old != 0) && (unix_time_ > unix_old)) { - if (unix_time_ > unix_old) - { - auto sleep_nsec = unix_time_ - unix_old; + auto sleep_nsec = unix_time_ - unix_old; + if (sleep_nsec < 10000000000) + { std::stringstream ss; ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; node_->log(log_level::DEBUG, ss.str()); - - std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); + } else + { + std::stringstream ss; + ss << "Delta t too large for watiting: " << sleep_nsec / 1000000 + << " milliseconds... reducing it to 5 ms."; + node_->log(log_level::WARN, ss.str()); + sleep_nsec = 5000000; } - } - // set leap seconds to paramter only if it was not set otherwise (by - // ReceiverTime) - if (current_leap_seconds_ == -128) - current_leap_seconds_ = settings_->leap_seconds; + std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); + } } void MessageHandler::parseNmea(const std::shared_ptr& telegram) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index e1b9273a..d8489047 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -76,7 +76,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("local_frame_id", settings_.local_frame_id, (std::string) "odom"); param("insert_local_frame", settings_.insert_local_frame, false); param("lock_utm_zone", settings_.lock_utm_zone, true); - param("leap_seconds", settings_.leap_seconds, -128); + param("leap_seconds", settings_.leap_seconds, 27); // Communication parameters param("device", settings_.device, std::string("/dev/ttyACM0")); From f71a8436a62a14f3d3805f06f7a105180849798c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 08:51:37 +0100 Subject: [PATCH 066/170] Add USB serial by ID info to README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 114389d3..8930a4e9 100644 --- a/README.md +++ b/README.md @@ -322,7 +322,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi Connectivity Specs + `device`: location of device connection - + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyUSB0` + + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. + Regarding the file path, ROS_HOME=\`pwd\` in front of `roslaunch septentrio...` might be useful to specify that the node should be started using the executable's directory as its working-directory. From 6caf69e289f7551c0ab66179d097d0544417040d Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 14:27:09 +0100 Subject: [PATCH 067/170] Change invalid timestamp handling for reading from file --- .../communication/async_manager.hpp | 33 ++++++++++++------- .../communication/message_handler.cpp | 19 ++++------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 6da98148..148829cb 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -156,8 +156,9 @@ namespace io { template AsyncManager::AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue) : - node_(node), ioService_(new boost::asio::io_service), - ioInterface_(node, ioService_), telegramQueue_(telegramQueue) + node_(node), + ioService_(new boost::asio::io_service), ioInterface_(node, ioService_), + telegramQueue_(telegramQueue) { node_->log(log_level::DEBUG, "AsyncManager created."); } @@ -198,8 +199,7 @@ namespace io { return; } - ioService_->post( - boost::bind(&AsyncManager::write, this, cmd)); + ioService_->post(boost::bind(&AsyncManager::write, this, cmd)); } template @@ -234,13 +234,23 @@ namespace io { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); if (running_ && ioService_->stopped()) { - node_->log(log_level::ERROR, - "AsyncManager connection lost. Trying to reconnect."); - ioService_->reset(); - ioThread_.join(); - while (!ioInterface_.connect()) - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - receive(); + if (node_->settings()->read_from_sbf_log || + node_->settings()->read_from_pcap) + { + node_->log( + log_level::INFO, + "AsyncManager finished reading file. Node will continue to publish queued messages."); + break; + } else + { + node_->log(log_level::ERROR, + "AsyncManager connection lost. Trying to reconnect."); + ioService_->reset(); + ioThread_.join(); + while (!ioInterface_.connect()) + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + receive(); + } } } } @@ -512,7 +522,6 @@ namespace io { node_->log(log_level::DEBUG, "AsyncManager SBF read error: " + ec.message()); } - }); } diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 823764ce..b7fb9b7a 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1822,6 +1822,9 @@ namespace io { uint32_t tow = parsing_utilities::getTow(message); uint16_t wnc = parsing_utilities::getWnc(message); + if (!validValue(tow) || !validValue(wnc)) + return 0; + return timestampSBF(tow, wnc); } @@ -2702,19 +2705,9 @@ namespace io { { auto sleep_nsec = unix_time_ - unix_old; - if (sleep_nsec < 10000000000) - { - std::stringstream ss; - ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; - node_->log(log_level::DEBUG, ss.str()); - } else - { - std::stringstream ss; - ss << "Delta t too large for watiting: " << sleep_nsec / 1000000 - << " milliseconds... reducing it to 5 ms."; - node_->log(log_level::WARN, ss.str()); - sleep_nsec = 5000000; - } + std::stringstream ss; + ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds..."; + node_->log(log_level::DEBUG, ss.str()); std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec)); } From 5427826cb75a3cfd5f956798e1947b688ed8eba7 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 15:03:58 +0100 Subject: [PATCH 068/170] Refine changelog --- CHANGELOG.rst | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 069a3613..97fdafb3 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,9 +7,12 @@ Changelog for package septentrio_gnss_driver * New Features * Recovery from connection interruption * Improvements - * Rework IO core and message handling -> reduce system load + * Rework IO core and message handling + * Unified stream processing + * Internal data queue + * Prevent message loss in file reading * Preliminary Features - * Output of localization and tf in ECEF frame, feedback welcome + * Output of localization and tf in ECEF frame, testing and feedback welcome 1.2.3 (2022-11-09) ------------------ From 16580b51a7bbe473c92bab2cb83d10c3828f9602 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 20:55:00 +0100 Subject: [PATCH 069/170] Add automiatic detection of serial port ID --- README.md | 2 -- .../communication/settings.hpp | 3 --- .../communication/communication_core.cpp | 13 ------------- src/septentrio_gnss_driver/node/rosaic_node.cpp | 1 - 4 files changed, 19 deletions(-) diff --git a/README.md b/README.md index 8930a4e9..ce73806a 100644 --- a/README.md +++ b/README.md @@ -84,7 +84,6 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: off login: @@ -332,7 +331,6 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + default: `tcp://192.168.3.1:28784 ` + `serial`: specifications for serial communication + `baudrate`: serial baud rate to be used in a serial connection. Ensure the provided rate is sufficient for the chosen SBF blocks. For example, activating MeasEpoch (also necessary for /gpsfix) may require up to almost 400 kBit/s. - + `rx_serial_port`: determines to which (virtual) serial port of the Rx we want to get connected to, e.g. USB1 or COM1 + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART HW flow control enabled or not + `off` to disable UART HW flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 005c9226..d6e4e981 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -135,9 +135,6 @@ struct Settings uint32_t baudrate; //! HW flow control std::string hw_flow_control; - //! In case of serial communication to Rx, rx_serial_port specifies Rx's - //! serial port connected to, e.g. USB1 or COM1 - std::string rx_serial_port; //! Datum to be used std::string datum; //! Polling period for PVT-related SBF blocks diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index c506b3ab..234c23d9 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -235,19 +235,6 @@ namespace io { "The connection descriptor is " + mainConnectionPort_); // streamPort_ = "IPS1"; // TODO UDP streamPort_ = mainConnectionPort_; - if (proto == "tcp") - { - // mainConnectionPort_ = manager_->getConnectionDescriptor(); - } else - { - // TODO check if rx_serial_port can be removed - mainConnectionPort_ = settings_->rx_serial_port; - // After booting, the Rx sends the characters "x?" to all ports, which - // could potentially mingle with our first command. Hence send a - // safeguard command "lif", whose potentially false processing is - // harmless. - // send("lif, Identification \x0D"); - } node_->log(log_level::INFO, "Setting up Rx."); diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index d8489047..f4d4a5d6 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -83,7 +83,6 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) getUint32Param("serial/baudrate", settings_.baudrate, static_cast(921600)); param("serial/hw_flow_control", settings_.hw_flow_control, std::string("off")); - param("serial/rx_serial_port", settings_.rx_serial_port, std::string("USB1")); param("login/user", settings_.login_user, std::string("")); param("login/password", settings_.login_password, std::string("")); settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list. From 39a3bfc1f451ac9abe8fccb36653b431666ea04c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 20:34:27 +0100 Subject: [PATCH 070/170] Update README on how to add new messages --- README.md | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index ce73806a..7fd6e458 100644 --- a/README.md +++ b/README.md @@ -659,16 +659,14 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr Is there an SBF or NMEA message that is not being addressed while being important to your application? If yes, follow these steps: 1. Find the log reference of interest in the publicly accessible, official documentation. Hence select the reference guide file, e.g. for mosaic-x5 in the [product support section for mosaic-X5](https://www.septentrio.com/en/support/mosaic/mosaic-x5), Chapter 4, of Septentrio's homepage. - 2. Add a new `.msg` file to the `septentrio_gnss_driver/msg` folder. - 3. SBF: Add the new struct definition to the `sbf_structs.hpp` file. - 4. Parsing/Processing the message/block: - - Both: Add a new include guard to let the compiler know about the existence of the header file (such as `septentrio_gnss_driver/PVTGeodetic.h`) that gets compiler-generated from the `.msg` file constructed in step 3. - - SBF: Extend the `NMEA_ID_Enum` enumeration in the `rx_message.hpp` file with a new entry. - - SBF: Extend the initialization of the `RxIDMap` map in the `rx_message.cpp` file with a new pair. - - SBF: Add a new callback function declaration, a new method, to the `io::RxMessage class` in the `rx_message.hpp` file. - - SBF: Add the latter's definition to the `rx_message.cpp` file. - - SBF: Add a new C++ "case" (part of the C++ switch-case structure) in the `rx_message.hpp` file. It should be modeled on the existing `evPVTGeodetic` case, e.g. one needs a static counter variable declaration. - - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `septentrio_gnss_driver/src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `septentrio_gnss_driver/include/septentrio_gnss_driver/parsers/nmea_parsers` folder. - 5. Create a new `publish/..` ROSaic parameter in the `septentrio_gnss_driver/config/rover.yaml` file, create a global boolean variable `publish_...` in the `septentrio_gnss_driver/src/septentrio_gnss_driver/node/rosaic_node.cpp` file, insert the publishing callback function to the C++ "multimap" `IO.handlers_.callbackmap_` - which is already storing all the others - in the `rosaic_node::ROSaicNode::defineMessages()` method in the same file and add an `extern bool publish_...;` line to the `septentrio_gnss_driver/include/septentrio_gnss_driver/node/rosaic_node.hpp` file. - 6. Modify the `septentrio_gnss_driver/CMakeLists.txt` file by adding a new entry to the `add_message_files` section. + 2. SBF: Add a new `.msg` file to the `../msg` folder. And modify the `../CMakeLists.txt` file by adding a new entry to the `add_message_files` section. + 3. Parsers: + - SBF: Add a parser to the `sbf_blocks.hpp` file. + - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `../src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `../include/septentrio_gnss_driver/parsers/nmea_parsers` folder. + 4. Processing the message/block: + - SBF: Extend the `SbfId` enumeration in the `message_handler.hpp` file with a new entry. + - SBF: Extend the SBF switch-case in `message_handler.cpp` file with a new case. + - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. + - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. + 5. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable in the `publish` struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file.
From aac86a23539ba3cfdafb98f5702d17e134f54513 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jan 2023 23:03:23 +0100 Subject: [PATCH 071/170] Add diagnostics and time ref msgs again --- README.md | 2 +- .../communication/message_handler.hpp | 19 +- .../communication/settings.hpp | 2 +- .../communication/communication_core.cpp | 3 +- .../communication/message_handler.cpp | 376 +++--------------- .../node/rosaic_node.cpp | 2 +- 6 files changed, 64 insertions(+), 340 deletions(-) diff --git a/README.md b/README.md index 7fd6e458..7efc4e7b 100644 --- a/README.md +++ b/README.md @@ -668,5 +668,5 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr - SBF: Extend the SBF switch-case in `message_handler.cpp` file with a new case. - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. - 5. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable in the `publish` struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. + 5. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file.
diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index d6810978..13f543ea 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -142,8 +142,6 @@ namespace io { MessageHandler(ROSaicNodeBase* node) : node_(node), settings_(node->settings()), unix_time_(0) { - if (settings_->leap_seconds != -128) - current_leap_seconds_ = settings_->leap_seconds; } void setLeapSeconds() @@ -295,7 +293,7 @@ namespace io { Timestamp unix_time_; //! Current leap seconds as received, do not use value is -128 - int8_t current_leap_seconds_ = -128; + int32_t current_leap_seconds_ = -128; /** * @brief "Callback" function when constructing NavSatFix messages @@ -316,17 +314,15 @@ namespace io { /** * @brief "Callback" function when constructing * DiagnosticArrayMsg messages - * @param[in] time_obj time of message + * @param[in] telegram telegram from which the msg was assembled */ - void assembleDiagnosticArray(const Timestamp& time_obj); + void assembleDiagnosticArray(const std::shared_ptr& telegram); /** * @brief "Callback" function when constructing * ImuMsg messages - * @return A ROS message - * ImuMsg just created */ - ImuMsg assmembleImu() const; + void assembleImu(); /** * @brief "Callback" function when constructing @@ -357,6 +353,13 @@ namespace io { */ void assembleTwist(bool fromIns = false); + /** + * @brief function when constructing + * TimeReferenceMsg messages + * @param[in] telegram telegram from which the msg was assembled + */ + void assembleTimeReference(const std::shared_ptr& telegram); + /** * @brief Waits according to time when reading from file * @param[in] time_obj wait until time diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index d6e4e981..3c0647ff 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -296,7 +296,7 @@ struct Settings //! Wether the UTM zone of the localization is locked bool lock_utm_zone; //! The number of leap seconds that have been inserted into the UTC time - int32_t leap_seconds; + int32_t leap_seconds = -128; //! Whether or not we are reading from an SBF file bool read_from_sbf_log = false; //! Whether or not we are reading from a PCAP file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 234c23d9..829435fc 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -680,7 +680,7 @@ namespace io { // Setting up SBF blocks with rx_period_pvt { std::stringstream blocks; - if (settings_->use_gnss_time) + if (settings_->use_gnss_time || settings_->publish_gpst) { blocks << " +ReceiverTime"; } @@ -689,6 +689,7 @@ namespace io { blocks << " +PVTCartesian"; } if (settings_->publish_pvtgeodetic || settings_->publish_twist || + settings_->publish_gpst || (settings_->publish_navsatfix && (settings_->septentrio_receiver_type == "gnss")) || (settings_->publish_gpsfix && diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index b7fb9b7a..3c7b9ab2 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -206,15 +206,14 @@ namespace io { publish("/pose", msg); }; - void MessageHandler::assembleDiagnosticArray(const Timestamp& time_obj) + void MessageHandler::assembleDiagnosticArray( + const std::shared_ptr& telegram) { DiagnosticArrayMsg msg; if (!validValue(last_receiverstatus_.block_header.tow) || (last_receiverstatus_.block_header.tow != last_qualityind_.block_header.tow)) return; - msg.header.stamp = timestampToRos(time_obj); - // TODO frame_id std::string serialnumber; if (validValue(last_receiversetup_.block_header.tow)) serialnumber = last_receiversetup_.rx_serial_number; @@ -322,13 +321,31 @@ namespace io { gnss_status.message = "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; msg.status.push_back(gnss_status); + std::string frame_id; + if (settings_->septentrio_receiver_type == "gnss") + { + frame_id = settings_->frame_id; + } + if (settings_->septentrio_receiver_type == "ins") + { + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + } + assembleHeader(frame_id, telegram, msg); publish("/diagnostics", msg); }; - ImuMsg MessageHandler::assmembleImu() const + void MessageHandler::assembleImu() { ImuMsg msg; + msg.header = last_extsensmeas_.header; + msg.linear_acceleration.x = last_extsensmeas_.acceleration_x; msg.linear_acceleration.y = last_extsensmeas_.acceleration_y; msg.linear_acceleration.z = last_extsensmeas_.acceleration_z; @@ -431,7 +448,7 @@ namespace io { msg.orientation_covariance[8] = -1.0; } - return msg; + publish("/imu", msg); }; void MessageHandler::assembleTwist(bool fromIns /* = false*/) @@ -1791,7 +1808,18 @@ namespace io { NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } publish("/gpsfix", msg); - }; + } + + void + MessageHandler::assembleTimeReference(const std::shared_ptr& telegram) + { + TimeReferenceMsg msg; + Timestamp time_obj = timestampSBF(telegram->message); + msg.time_ref = timestampToRos(time_obj); + msg.source = "GPST"; + assembleHeader(settings_->frame_id, telegram, msg); + publish("/gpst", msg); + } template void MessageHandler::assembleHeader(const std::string& frameId, @@ -1955,7 +1983,10 @@ namespace io { publish("/pvtgeodetic", last_pvtgeodetic_); assembleTwist(); assembleNavSatFix(); + assemblePoseWithCovarianceStamped(); assembleGpsFix(); + if (settings_->publish_gpst) + assembleTimeReference(telegram); break; } case BASE_VECTOR_CART: @@ -2013,7 +2044,7 @@ namespace io { if (settings_->publish_poscovgeodetic) publish("/poscovgeodetic", last_poscovgeodetic_); assembleNavSatFix(); - assembleGpsFix(); + assemblePoseWithCovarianceStamped(); assembleGpsFix(); break; } @@ -2030,6 +2061,7 @@ namespace io { assembleHeader(settings_->frame_id, telegram, last_atteuler_); if (settings_->publish_atteuler) publish("/atteuler", last_atteuler_); + assemblePoseWithCovarianceStamped(); assembleGpsFix(); break; } @@ -2046,6 +2078,7 @@ namespace io { assembleHeader(settings_->frame_id, telegram, last_attcoveuler_); if (settings_->publish_attcoveuler) publish("/attcoveuler", last_attcoveuler_); + assemblePoseWithCovarianceStamped(); assembleGpsFix(); break; } @@ -2098,6 +2131,8 @@ namespace io { assembleLocalizationUtm(); assembleLocalizationEcef(); assembleTwist(true); + assemblePoseWithCovarianceStamped(); + assembleNavSatFix(); assembleGpsFix(); break; } @@ -2199,17 +2234,7 @@ namespace io { publish("/extsensormeas", last_extsensmeas_); if (settings_->publish_imu && hasImuMeas) { - ImuMsg msg; - try - { - msg = assmembleImu(); - } catch (std::runtime_error& e) - { - node_->log(log_level::DEBUG, "ImuMsg: " + std::string(e.what())); - break; - } - assembleHeader(settings_->imu_frame_id, telegram, msg); - publish("/imu", msg); + assembleImu(); } break; } @@ -2221,6 +2246,7 @@ namespace io { node_->log(log_level::ERROR, "parse error in ChannelStatus"); break; } + assembleGpsFix(); break; } case MEAS_EPOCH: @@ -2247,6 +2273,7 @@ namespace io { node_->log(log_level::ERROR, "parse error in DOP"); break; } + assembleGpsFix(); break; } case VEL_COV_GEODETIC: @@ -2262,6 +2289,7 @@ namespace io { if (settings_->publish_velcovgeodetic) publish("/velcovgeodetic", last_velcovgeodetic_); assembleTwist(); + assembleGpsFix(); break; } case RECEIVER_STATUS: @@ -2272,6 +2300,7 @@ namespace io { node_->log(log_level::ERROR, "parse error in ReceiverStatus"); break; } + assembleDiagnosticArray(telegram); break; } case QUALITY_IND: @@ -2283,6 +2312,7 @@ namespace io { node_->log(log_level::ERROR, "parse error in QualityInd"); break; } + assembleDiagnosticArray(telegram); break; } case RECEIVER_SETUP: @@ -2387,316 +2417,6 @@ namespace io { } } - void processjointMessages() - { - /* - - case 1: - { - TimeReferenceMsg msg; - Timestamp time_obj = timestampSBF( - telegram->message, true); // We need the GPS time, hence - msg.time_ref = timestampToRos(time_obj); - msg.source = "GPST"; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpst", msg); - break; - } - case evDiagnosticArray: - { - DiagnosticArrayMsg msg; - try - { - msg = assembleDiagnosticArray(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "DiagnosticArrayMsg: " + std::string(e.what())); - break; - } - if (settings_->septentrio_receiver_type == "gnss") - { - msg.header.frame_id = settings_->frame_id; - } - if (settings_->septentrio_receiver_type == "ins") - { - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - } - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - receiverstatus_has_arrived_diagnostics_ = false; - qualityind_has_arrived_diagnostics_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/diagnostics", msg); - break; - } - case evLocalization: - { - LocalizationMsg msg; - try - { - msg = assembleLocalizationUtm(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + - std::string(e.what())); break; - } - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_localization) - publish("/localization", msg); - if (settings_->publish_tf) - publishTf(msg); - break; - } - case evLocalizationEcef: - { - LocalizationMsg msg; - try - { - msg = assembleLocalizationEcef(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "LocalizationMsg: " + - std::string(e.what())); break; - } - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_localization_ecef_ = false; - insnavgeod_has_arrived_localization_ecef_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP file) - if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - if (settings_->publish_localization_ecef) - publish("/localization_ecef", msg); - if (settings_->publish_tf_ecef) - publishTf(msg); - break; - } - - if (settings_->septentrio_receiver_type == "gnss") - { - case evNavSatFix: - { - NavSatFixMsg msg; - try - { - msg = assembleNavSatFix(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "NavSatFixMsg: " + std::string(e.what())); - break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_navsatfix_ = false; - poscovgeodetic_has_arrived_navsatfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP - file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/navsatfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSNavSatFix: - { - NavSatFixMsg msg; - try - { - msg = assembleNavSatFix(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "NavSatFixMsg: " + std::string(e.what())); - break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_navsatfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP - file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/navsatfix", msg); - break; - } - } - - if (settings_->septentrio_receiver_type == "gnss") - { - case evGPSFix: - { - GpsFixMsg msg; - try - { - msg = assembleGpsFix(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "GpsFixMsg: " + - std::string(e.what())); break; - } - msg.header.frame_id = settings_->frame_id; - msg.status.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - msg.status.header.stamp = timestampToRos(time_obj); - ++count_gpsfix_; - channelstatus_has_arrived_gpsfix_ = false; - measepoch_has_arrived_gpsfix_ = false; - dop_has_arrived_gpsfix_ = false; - pvtgeodetic_has_arrived_gpsfix_ = false; - poscovgeodetic_has_arrived_gpsfix_ = false; - velcovgeodetic_has_arrived_gpsfix_ = false; - atteuler_has_arrived_gpsfix_ = false; - attcoveuler_has_arrived_gpsfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP - file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpsfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSGPSFix: - { - GpsFixMsg msg; - try - { - msg = assembleGpsFix(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, "GpsFixMsg: " + - std::string(e.what())); break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - msg.status.header.frame_id = msg.header.frame_id; - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - msg.status.header.stamp = timestampToRos(time_obj); - ++count_gpsfix_; - channelstatus_has_arrived_gpsfix_ = false; - measepoch_has_arrived_gpsfix_ = false; - dop_has_arrived_gpsfix_ = false; - insnavgeod_has_arrived_gpsfix_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP - file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/gpsfix", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "gnss") - { - case evPoseWithCovarianceStamped: - { - PoseWithCovarianceStampedMsg msg; - try - { - msg = assemblePoseWithCovarianceStamped(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "PoseWithCovarianceStampedMsg: " + - std::string(e.what())); break; - } - msg.header.frame_id = settings_->frame_id; - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - pvtgeodetic_has_arrived_pose_ = false; - poscovgeodetic_has_arrived_pose_ = false; - atteuler_has_arrived_pose_ = false; - attcoveuler_has_arrived_pose_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP - file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pose", msg); - break; - } - } - if (settings_->septentrio_receiver_type == "ins") - { - case evINSPoseWithCovarianceStamped: - { - PoseWithCovarianceStampedMsg msg; - try - { - msg = assemblePoseWithCovarianceStamped(); - } catch (std::runtime_error& e) - { - node_->log(LogLevel::DEBUG, - "PoseWithCovarianceStampedMsg: " + - std::string(e.what())); break; - } - if (settings_->ins_use_poi) - { - msg.header.frame_id = settings_->poi_frame_id; - } else - { - msg.header.frame_id = settings_->frame_id; - } - Timestamp time_obj = timestampSBF(telegram->message, - settings_->use_gnss_time); msg.header.stamp = timestampToRos(time_obj); - insnavgeod_has_arrived_pose_ = false; - // Wait as long as necessary (only when reading from SBF/PCAP - file) if (settings_->read_from_sbf_log || settings_->read_from_pcap) - { - wait(time_obj); - } - publish("/pose", msg); - break; - } - }*/ - } - void MessageHandler::wait(Timestamp time_obj) { Timestamp unix_old = unix_time_; diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index f4d4a5d6..ae940bf7 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -76,7 +76,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("local_frame_id", settings_.local_frame_id, (std::string) "odom"); param("insert_local_frame", settings_.insert_local_frame, false); param("lock_utm_zone", settings_.lock_utm_zone, true); - param("leap_seconds", settings_.leap_seconds, 27); + param("leap_seconds", settings_.leap_seconds, -128); // Communication parameters param("device", settings_.device, std::string("/dev/ttyACM0")); From 5ddf5c862aa6241f472280a7f349a309ae0e6499 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 24 Jan 2023 16:57:53 +0100 Subject: [PATCH 072/170] Add option to bypass confugration of Rx --- CHANGELOG.rst | 1 + README.md | 15 ++++++++-- config/gnss.yaml | 2 ++ config/ins.yaml | 2 ++ config/rover.yaml | 2 ++ .../communication/communication_core.hpp | 5 ++++ .../communication/settings.hpp | 2 ++ .../communication/communication_core.cpp | 28 ++++++++++++------ .../node/rosaic_node.cpp | 29 ++++++++++++------- 9 files changed, 64 insertions(+), 22 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 97fdafb3..8788aa55 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,6 +6,7 @@ Changelog for package septentrio_gnss_driver ------------------ * New Features * Recovery from connection interruption + * Add option to bypass configuration of Rx * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 7efc4e7b..89f4c15f 100644 --- a/README.md +++ b/README.md @@ -85,6 +85,8 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo serial: baudrate: 921600 hw_flow_control: off + +configure_rx: true login: user: "" @@ -328,7 +330,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `tcp://host:port` format for TCP/IP connections + `28784` should be used as the default (command) port for TCP/IP connections. If another port is specified, the receiver needs to be (re-)configured via the Web Interface before ROSaic can be used. + An RNDIS IP interface is provided via USB, assigning the address `192.168.3.1` to the receiver. This should work on most modern Linux distributions. To verify successful connection, open a web browser to access the web interface of the receiver using the IP address `192.168.3.1`. - + default: `tcp://192.168.3.1:28784 ` + + default: `tcp://192.168.3.1:28784` + `serial`: specifications for serial communication + `baudrate`: serial baud rate to be used in a serial connection. Ensure the provided rate is sufficient for the chosen SBF blocks. For example, activating MeasEpoch (also necessary for /gpsfix) may require up to almost 400 kBit/s. + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART HW flow control enabled or not @@ -338,6 +340,15 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `user`: user name + `password`: password + +
+ Receiver Configuration + + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated. The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + default: true +
+ +
Receiver Type @@ -352,7 +363,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
- Frame ID + Frame IDs + `frame_id`: name of the ROS tf frame for the Rx, placed in the header of published GNSS messages. It corresponds to the frame of the main antenna. + In ROS, the [tf package](https://wiki.ros.org/tf) lets you keep track of multiple coordinate frames over time. The frame ID will be resolved by [`tf_prefix`](http://wiki.ros.org/geometry/CoordinateFrameConventions) if defined. If a ROS message has a header (all of those we publish do), the frame ID can be found via `rostopic echo /topic`, where `/topic` is the topic into which the message is being published. diff --git a/config/gnss.yaml b/config/gnss.yaml index e77e1a11..98516437 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -9,6 +9,8 @@ serial: rx_serial_port: USB1 hw_flow_control: "off" +configure_rx: true + login: user: "" password: "" diff --git a/config/ins.yaml b/config/ins.yaml index 95df4600..de204a62 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -9,6 +9,8 @@ serial: rx_serial_port: USB1 hw_flow_control: "off" +configure_rx: true + login: user: "" password: "" diff --git a/config/rover.yaml b/config/rover.yaml index f31b19f9..3e11b7a8 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -9,6 +9,8 @@ serial: rx_serial_port: USB1 hw_flow_control: off +configure_rx: true + login: user: "" password: "" diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index fc51f6f5..87851851 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -125,6 +125,11 @@ namespace io { void sendVelocity(const std::string& velNmea); private: + /** + * @brief Resets Rx settings + */ + void resetSettings(); + /** * @brief Initializes the I/O handling * * @return Wether connection was successful diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 3c0647ff..f958af8c 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -135,6 +135,8 @@ struct Settings uint32_t baudrate; //! HW flow control std::string hw_flow_control; + // Wether to configure Rx + bool configure_rx; //! Datum to be used std::string datum; //! Polling period for PVT-related SBF blocks diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 829435fc..4faab217 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -71,7 +71,19 @@ namespace io { CommunicationCore::~CommunicationCore() { telegramHandler_.clearSemaphores(); - if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) + + resetSettings(); + + running_ = false; + std::shared_ptr telegram(new Telegram); + telegramQueue_.push(telegram); + processingThread_.join(); + } + + void CommunicationCore::resetSettings() + { + if (settings_->configure_rx && !settings_->read_from_sbf_log && + !settings_->read_from_pcap) { resetMainConnection(); send("sdio, " + streamPort_ + ", auto, none\x0D"); @@ -121,13 +133,9 @@ namespace io { } } - send("logout \x0D"); + if (!settings_->login_user.empty() && !settings_->login_password.empty()) + send("logout \x0D"); } - - running_ = false; - std::shared_ptr telegram(new Telegram); - telegramQueue_.push(telegram); - processingThread_.join(); } void CommunicationCore::connect() @@ -162,9 +170,12 @@ namespace io { if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { node_->log(log_level::DEBUG, "Configure Rx."); - configureRx(); + if (settings_->configure_rx) + configureRx(); } + node_->log(log_level::INFO, "Setup complete."); + node_->log(log_level::DEBUG, "Successully connected. Leaving connect() method"); } @@ -818,7 +829,6 @@ namespace io { } node_->log(log_level::DEBUG, "Leaving configureRx() method"); - node_->log(log_level::INFO, "Setup complete."); } void CommunicationCore::sendVelocity(const std::string& velNmea) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index ae940bf7..4961f783 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -67,26 +67,33 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) [[nodiscard]] bool rosaic_node::ROSaicNode::getROSParams() { param("use_gnss_time", settings_.use_gnss_time, true); - param("frame_id", settings_.frame_id, (std::string) "gnss"); - param("imu_frame_id", settings_.imu_frame_id, (std::string) "imu"); - param("poi_frame_id", settings_.poi_frame_id, (std::string) "base_link"); - param("vsm_frame_id", settings_.vsm_frame_id, (std::string) "vsm"); - param("aux1_frame_id", settings_.aux1_frame_id, (std::string) "aux1"); + param("frame_id", settings_.frame_id, static_cast("gnss")); + param("imu_frame_id", settings_.imu_frame_id, static_cast("imu")); + param("poi_frame_id", settings_.poi_frame_id, + static_cast("base_link")); + param("vsm_frame_id", settings_.vsm_frame_id, static_cast("vsm")); + param("aux1_frame_id", settings_.aux1_frame_id, + static_cast("aux1")); param("vehicle_frame_id", settings_.vehicle_frame_id, settings_.poi_frame_id); - param("local_frame_id", settings_.local_frame_id, (std::string) "odom"); + param("local_frame_id", settings_.local_frame_id, + static_cast("odom")); param("insert_local_frame", settings_.insert_local_frame, false); param("lock_utm_zone", settings_.lock_utm_zone, true); param("leap_seconds", settings_.leap_seconds, -128); + param("configure_rx", settings_.configure_rx, true); + // Communication parameters - param("device", settings_.device, std::string("/dev/ttyACM0")); + param("device", settings_.device, static_cast("/dev/ttyACM0")); getUint32Param("serial/baudrate", settings_.baudrate, static_cast(921600)); - param("serial/hw_flow_control", settings_.hw_flow_control, std::string("off")); - param("login/user", settings_.login_user, std::string("")); - param("login/password", settings_.login_password, std::string("")); + param("serial/hw_flow_control", settings_.hw_flow_control, + static_cast("off")); + param("login/user", settings_.login_user, static_cast("")); + param("login/password", settings_.login_password, static_cast("")); settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list. - param("receiver_type", settings_.septentrio_receiver_type, std::string("gnss")); + param("receiver_type", settings_.septentrio_receiver_type, + static_cast("gnss")); if (!((settings_.septentrio_receiver_type == "gnss") || (settings_.septentrio_receiver_type == "ins") || (settings_.septentrio_receiver_type == "ins_in_gnss_mode"))) From 839b7e3ccb6ceeea09d5a20fcefb530f1e27a172 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 24 Jan 2023 16:57:53 +0100 Subject: [PATCH 073/170] Add option to bypass configuration of Rx --- CHANGELOG.rst | 1 + README.md | 19 +++++++++--- config/gnss.yaml | 2 ++ config/ins.yaml | 2 ++ config/rover.yaml | 2 ++ .../communication/communication_core.hpp | 5 ++++ .../communication/settings.hpp | 2 ++ .../communication/communication_core.cpp | 28 ++++++++++++------ .../node/rosaic_node.cpp | 29 ++++++++++++------- 9 files changed, 66 insertions(+), 24 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 97fdafb3..8788aa55 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,6 +6,7 @@ Changelog for package septentrio_gnss_driver ------------------ * New Features * Recovery from connection interruption + * Add option to bypass configuration of Rx * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 7efc4e7b..8538c244 100644 --- a/README.md +++ b/README.md @@ -85,6 +85,8 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo serial: baudrate: 921600 hw_flow_control: off + +configure_rx: true login: user: "" @@ -328,16 +330,25 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `tcp://host:port` format for TCP/IP connections + `28784` should be used as the default (command) port for TCP/IP connections. If another port is specified, the receiver needs to be (re-)configured via the Web Interface before ROSaic can be used. + An RNDIS IP interface is provided via USB, assigning the address `192.168.3.1` to the receiver. This should work on most modern Linux distributions. To verify successful connection, open a web browser to access the web interface of the receiver using the IP address `192.168.3.1`. - + default: `tcp://192.168.3.1:28784 ` + + default: `tcp://192.168.3.1:28784` + `serial`: specifications for serial communication + `baudrate`: serial baud rate to be used in a serial connection. Ensure the provided rate is sufficient for the chosen SBF blocks. For example, activating MeasEpoch (also necessary for /gpsfix) may require up to almost 400 kBit/s. - + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART HW flow control enabled or not - + `off` to disable UART HW flow control, `RTS|CTS` to enable it + + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password
+ +
+ Receiver Configuration + + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated. The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + default: true +
+ +
Receiver Type @@ -352,7 +363,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
- Frame ID + Frame IDs + `frame_id`: name of the ROS tf frame for the Rx, placed in the header of published GNSS messages. It corresponds to the frame of the main antenna. + In ROS, the [tf package](https://wiki.ros.org/tf) lets you keep track of multiple coordinate frames over time. The frame ID will be resolved by [`tf_prefix`](http://wiki.ros.org/geometry/CoordinateFrameConventions) if defined. If a ROS message has a header (all of those we publish do), the frame ID can be found via `rostopic echo /topic`, where `/topic` is the topic into which the message is being published. diff --git a/config/gnss.yaml b/config/gnss.yaml index e77e1a11..98516437 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -9,6 +9,8 @@ serial: rx_serial_port: USB1 hw_flow_control: "off" +configure_rx: true + login: user: "" password: "" diff --git a/config/ins.yaml b/config/ins.yaml index 95df4600..de204a62 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -9,6 +9,8 @@ serial: rx_serial_port: USB1 hw_flow_control: "off" +configure_rx: true + login: user: "" password: "" diff --git a/config/rover.yaml b/config/rover.yaml index f31b19f9..3e11b7a8 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -9,6 +9,8 @@ serial: rx_serial_port: USB1 hw_flow_control: off +configure_rx: true + login: user: "" password: "" diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index fc51f6f5..87851851 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -125,6 +125,11 @@ namespace io { void sendVelocity(const std::string& velNmea); private: + /** + * @brief Resets Rx settings + */ + void resetSettings(); + /** * @brief Initializes the I/O handling * * @return Wether connection was successful diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 3c0647ff..f958af8c 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -135,6 +135,8 @@ struct Settings uint32_t baudrate; //! HW flow control std::string hw_flow_control; + // Wether to configure Rx + bool configure_rx; //! Datum to be used std::string datum; //! Polling period for PVT-related SBF blocks diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 829435fc..4faab217 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -71,7 +71,19 @@ namespace io { CommunicationCore::~CommunicationCore() { telegramHandler_.clearSemaphores(); - if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) + + resetSettings(); + + running_ = false; + std::shared_ptr telegram(new Telegram); + telegramQueue_.push(telegram); + processingThread_.join(); + } + + void CommunicationCore::resetSettings() + { + if (settings_->configure_rx && !settings_->read_from_sbf_log && + !settings_->read_from_pcap) { resetMainConnection(); send("sdio, " + streamPort_ + ", auto, none\x0D"); @@ -121,13 +133,9 @@ namespace io { } } - send("logout \x0D"); + if (!settings_->login_user.empty() && !settings_->login_password.empty()) + send("logout \x0D"); } - - running_ = false; - std::shared_ptr telegram(new Telegram); - telegramQueue_.push(telegram); - processingThread_.join(); } void CommunicationCore::connect() @@ -162,9 +170,12 @@ namespace io { if (!settings_->read_from_sbf_log && !settings_->read_from_pcap) { node_->log(log_level::DEBUG, "Configure Rx."); - configureRx(); + if (settings_->configure_rx) + configureRx(); } + node_->log(log_level::INFO, "Setup complete."); + node_->log(log_level::DEBUG, "Successully connected. Leaving connect() method"); } @@ -818,7 +829,6 @@ namespace io { } node_->log(log_level::DEBUG, "Leaving configureRx() method"); - node_->log(log_level::INFO, "Setup complete."); } void CommunicationCore::sendVelocity(const std::string& velNmea) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index ae940bf7..4961f783 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -67,26 +67,33 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) [[nodiscard]] bool rosaic_node::ROSaicNode::getROSParams() { param("use_gnss_time", settings_.use_gnss_time, true); - param("frame_id", settings_.frame_id, (std::string) "gnss"); - param("imu_frame_id", settings_.imu_frame_id, (std::string) "imu"); - param("poi_frame_id", settings_.poi_frame_id, (std::string) "base_link"); - param("vsm_frame_id", settings_.vsm_frame_id, (std::string) "vsm"); - param("aux1_frame_id", settings_.aux1_frame_id, (std::string) "aux1"); + param("frame_id", settings_.frame_id, static_cast("gnss")); + param("imu_frame_id", settings_.imu_frame_id, static_cast("imu")); + param("poi_frame_id", settings_.poi_frame_id, + static_cast("base_link")); + param("vsm_frame_id", settings_.vsm_frame_id, static_cast("vsm")); + param("aux1_frame_id", settings_.aux1_frame_id, + static_cast("aux1")); param("vehicle_frame_id", settings_.vehicle_frame_id, settings_.poi_frame_id); - param("local_frame_id", settings_.local_frame_id, (std::string) "odom"); + param("local_frame_id", settings_.local_frame_id, + static_cast("odom")); param("insert_local_frame", settings_.insert_local_frame, false); param("lock_utm_zone", settings_.lock_utm_zone, true); param("leap_seconds", settings_.leap_seconds, -128); + param("configure_rx", settings_.configure_rx, true); + // Communication parameters - param("device", settings_.device, std::string("/dev/ttyACM0")); + param("device", settings_.device, static_cast("/dev/ttyACM0")); getUint32Param("serial/baudrate", settings_.baudrate, static_cast(921600)); - param("serial/hw_flow_control", settings_.hw_flow_control, std::string("off")); - param("login/user", settings_.login_user, std::string("")); - param("login/password", settings_.login_password, std::string("")); + param("serial/hw_flow_control", settings_.hw_flow_control, + static_cast("off")); + param("login/user", settings_.login_user, static_cast("")); + param("login/password", settings_.login_password, static_cast("")); settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list. - param("receiver_type", settings_.septentrio_receiver_type, std::string("gnss")); + param("receiver_type", settings_.septentrio_receiver_type, + static_cast("gnss")); if (!((settings_.septentrio_receiver_type == "gnss") || (settings_.septentrio_receiver_type == "ins") || (settings_.septentrio_receiver_type == "ins_in_gnss_mode"))) From 4f330d64cb5b3e6c63e69ec083f75fc28f1d8161 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 24 Jan 2023 19:41:50 +0100 Subject: [PATCH 074/170] Add small fixes and cleanup --- CMakeLists.txt | 2 -- README.md | 3 +-- config/gnss.yaml | 1 - config/ins.yaml | 1 - config/rover.yaml | 1 - include/septentrio_gnss_driver/abstraction/typedefs.hpp | 4 ++-- 6 files changed, 3 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b1af046..43fbb235 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,8 +49,6 @@ else () set(libpcap_FOUND TRUE) endif () -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") - ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html diff --git a/README.md b/README.md index 8538c244..98b40713 100644 --- a/README.md +++ b/README.md @@ -344,7 +344,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Receiver Configuration - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated. The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated. The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
@@ -660,7 +660,6 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr
Some Ideas - + Automatic Search: If the host address of the receiver is omitted in the `host:port` specification, the driver could automatically search and establish a connection on the specified port. + Equip ROSaic with an NTRIP client such that it can forward corrections to the receiver independently of `Data Link`.
diff --git a/config/gnss.yaml b/config/gnss.yaml index 98516437..0f5b9594 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -6,7 +6,6 @@ device: tcp://192.168.3.1:28784 serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: "off" configure_rx: true diff --git a/config/ins.yaml b/config/ins.yaml index de204a62..63f5b9f4 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -6,7 +6,6 @@ device: tcp://192.168.3.1:28784 serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: "off" configure_rx: true diff --git a/config/rover.yaml b/config/rover.yaml index 3e11b7a8..562727db 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -6,7 +6,6 @@ device: tcp://192.168.3.1:28784 serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: off configure_rx: true diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 5d66c49d..140051dc 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -167,7 +167,7 @@ namespace log_level { ERROR, FATAL }; -} +} // namespace log_level /** * @class ROSaicNodeBase @@ -195,7 +195,7 @@ class ROSaicNodeBase "twist_vsm", 10, &ROSaicNodeBase::callbackTwist, this); } catch (const std::runtime_error& ex) { - this->log(log_level::ERROR, "Subscriber initalization failed due to: " + + this->log(log_level::ERROR, "Subscriber initialization failed due to: " + std::string(ex.what()) + "."); } } From 47e7ad8e0dbffb0a8872c1c6d2467cd1e627fe94 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 24 Jan 2023 20:02:12 +0100 Subject: [PATCH 075/170] Replace log functions --- .../communication/io.hpp | 71 ++++++++++++------- 1 file changed, 46 insertions(+), 25 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 29415d04..4c9753f5 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -215,7 +215,12 @@ namespace io { class TcpIo { public: - TcpIo(ROSaicNodeBase* node, std::shared_ptr ioService) : node_(node), ioService_(ioService) {} + TcpIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService) + { + } ~TcpIo() { stream_->close(); } @@ -269,7 +274,7 @@ namespace io { private: ROSaicNodeBase* node_; std::shared_ptr ioService_; - + public: std::unique_ptr stream_; }; @@ -277,8 +282,10 @@ namespace io { class SerialIo { public: - SerialIo(ROSaicNodeBase* node, std::shared_ptr ioService) : - node_(node), ioService_(ioService), flowcontrol_(node->settings()->hw_flow_control), + SerialIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService), flowcontrol_(node->settings()->hw_flow_control), baudrate_(node->settings()->baudrate) { stream_.reset(new boost::asio::serial_port(*ioService_)); @@ -310,10 +317,10 @@ namespace io { opened = true; } catch (const boost::system::system_error& err) { - node_->log(log_level::ERROR, - "Could not open serial port " + node_->settings()->device + - ". Error: " + err.what() + - ". Will retry every second."); + node_->log(log_level::ERROR, "Could not open serial port " + + node_->settings()->device + + ". Error: " + err.what() + + ". Will retry every second."); using namespace std::chrono_literals; std::this_thread::sleep_for(1s); @@ -332,13 +339,13 @@ namespace io { if (flowcontrol_ == "RTS|CTS") { stream_->set_option(boost::asio::serial_port_base::flow_control( - boost::asio::serial_port_base::flow_control::hardware)); + boost::asio::serial_port_base::flow_control::hardware)); } else { stream_->set_option(boost::asio::serial_port_base::flow_control( - boost::asio::serial_port_base::flow_control::none)); + boost::asio::serial_port_base::flow_control::none)); } - + // Set low latency int fd = stream_->native_handle(); struct serial_struct serialInfo; @@ -367,10 +374,10 @@ namespace io { } catch (boost::system::system_error& e) { - node_->log(log_level::ERROR, - "get_option failed due to " + std::string(e.what())); + node_->log(log_level::ERROR, "get_option failed due to " + + static_cast(e.what())); node_->log(log_level::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); + static_cast(e.what())); /* boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. @@ -406,10 +413,11 @@ namespace io { { node_->log(log_level::ERROR, - "set_option failed due to " + std::string(e.what())); + "set_option failed due to " + + static_cast(e.what())); node_->log(log_level::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); + static_cast(e.what())); return false; } using namespace std::chrono_literals; @@ -422,10 +430,11 @@ namespace io { { node_->log(log_level::ERROR, - "get_option failed due to " + std::string(e.what())); + "get_option failed due to " + + static_cast(e.what())); node_->log(log_level::INFO, "Additional info about error is " + - boost::diagnostic_information(e)); + static_cast(e.what())); /* boost::system::error_code e_loop; do // Caution: Might cause infinite loop.. @@ -463,7 +472,12 @@ namespace io { class SbfFileIo { public: - SbfFileIo(ROSaicNodeBase* node, std::shared_ptr ioService) : node_(node), ioService_(ioService) {} + SbfFileIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService) + { + } ~SbfFileIo() { stream_->close(); } @@ -483,13 +497,14 @@ namespace io { try { - stream_.reset(new boost::asio::posix::stream_descriptor(*ioService_)); + stream_.reset( + new boost::asio::posix::stream_descriptor(*ioService_)); stream_->assign(fd); } catch (std::runtime_error& e) { node_->log(log_level::ERROR, "assigning SBF file failed due to " + - std::string(e.what())); + static_cast(e.what())); return false; } return true; @@ -498,7 +513,7 @@ namespace io { private: ROSaicNodeBase* node_; std::shared_ptr ioService_; - + public: std::unique_ptr stream_; }; @@ -506,7 +521,12 @@ namespace io { class PcapFileIo { public: - PcapFileIo(ROSaicNodeBase* node, std::shared_ptr ioService) : node_(node), ioService_(ioService) {} + PcapFileIo(ROSaicNodeBase* node, + std::shared_ptr ioService) : + node_(node), + ioService_(ioService) + { + } ~PcapFileIo() { @@ -527,7 +547,8 @@ namespace io { node_->log(log_level::INFO, "Opening pcap file stream" + node_->settings()->device + "..."); - stream_.reset(new boost::asio::posix::stream_descriptor(*ioService_)); + stream_.reset( + new boost::asio::posix::stream_descriptor(*ioService_)); pcap_ = pcap_open_offline(node_->settings()->device.c_str(), errBuff_.data()); @@ -536,7 +557,7 @@ namespace io { } catch (std::runtime_error& e) { node_->log(log_level::ERROR, "assigning PCAP file failed due to " + - std::string(e.what())); + static_cast(e.what())); return false; } return true; From d3afd9ef979133ef6a39b0db07794d485c286429 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 27 Jan 2023 08:23:48 +0100 Subject: [PATCH 076/170] Make vars const --- .../communication/message_handler.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 3c7b9ab2..efd8f583 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2326,12 +2326,12 @@ namespace io { node_->log(log_level::DEBUG, "receiver setup firmware: " + last_receiversetup_.rx_version); - static int32_t ins_major = 1; - static int32_t ins_minor = 3; - static int32_t ins_patch = 2; - static int32_t gnss_major = 4; - static int32_t gnss_minor = 10; - static int32_t gnss_patch = 0; + static const int32_t ins_major = 1; + static const int32_t ins_minor = 3; + static const int32_t ins_patch = 2; + static const int32_t gnss_major = 4; + static const int32_t gnss_minor = 10; + static const int32_t gnss_patch = 0; boost::tokenizer<> tok(last_receiversetup_.rx_version); boost::tokenizer<>::iterator it = tok.begin(); std::vector major_minor_patch; From 4abe85dbf0a2a67ac65887ce791c8b4f8257edef Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 31 Jan 2023 13:45:01 +0100 Subject: [PATCH 077/170] Fix navsatfix publishing --- .../communication/message_handler.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index efd8f583..0d70ff98 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1148,6 +1148,8 @@ namespace io { */ void MessageHandler::assembleNavSatFix() { + static auto last_ins_tow = last_insnavgeod_.block_header.tow; + if (!settings_->publish_navsatfix) return; @@ -1237,9 +1239,11 @@ namespace io { } else if (settings_->septentrio_receiver_type == "ins") { if ((!validValue(last_insnavgeod_.block_header.tow)) || - (last_insnavgeod_.block_header.tow != - last_pvtgeodetic_.block_header.tow)) + (last_insnavgeod_.block_header.tow == last_ins_tow)) + { return; + } + last_ins_tow = last_insnavgeod_.block_header.tow; NavSatFixMsg msg; uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; From 7f08b70e6383cb4757068b456642fad060ee3286 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 31 Jan 2023 13:58:21 +0100 Subject: [PATCH 078/170] Fix pose publishing rate --- .../communication/message_handler.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 0d70ff98..9fe1389d 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -56,11 +56,15 @@ using parsing_utilities::square; namespace io { void MessageHandler::assemblePoseWithCovarianceStamped() { + static auto last_ins_tow = last_insnavgeod_.block_header.tow; + PoseWithCovarianceStampedMsg msg; if (settings_->septentrio_receiver_type == "ins") { - if (!validValue(last_insnavgeod_.block_header.tow)) + if (!validValue(last_insnavgeod_.block_header.tow) || + (last_insnavgeod_.block_header.tow == last_ins_tow)) return; + last_ins_tow = last_insnavgeod_.block_header.tow; msg.header = last_insnavgeod_.header; From 39a6ded0b0986f2e8c8fb8820f88976db5c0ca99 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 31 Jan 2023 14:39:39 +0100 Subject: [PATCH 079/170] Add warn log for misconfiguration --- .../communication/telegram_handler.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 755d2afc..e878c144 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -110,6 +110,15 @@ namespace io { "Invalid command just sent to the Rx! The Rx's response contains " + std::to_string(block_in_string.size()) + " bytes and reads:\n " + block_in_string); + + if (block_in_string == + std::string( + "$R? setGNSSAttitude: Argument 'Source' is invalid!\r\n")) + { + node_->log( + log_level::WARN, + "Rx does not support dual antenna mode, set parameter multi_antenna to false and/or disable publishing of atteuler."); + } } else { node_->log(log_level::DEBUG, "The Rx's response contains " + From ec95a8091fdcebded14787835e904a36d0a241bf Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 1 Feb 2023 09:27:35 +0100 Subject: [PATCH 080/170] Add warning for configuring INS as GNSS --- .../communication/telegram_handler.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index e878c144..b1a2c8ad 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -119,6 +119,12 @@ namespace io { log_level::WARN, "Rx does not support dual antenna mode, set parameter multi_antenna to false and/or disable publishing of atteuler."); } + if (block_in_string.substr(0, 8) == "$R? sao,") + { + node_->log( + log_level::WARN, + "Rx does understand sao command, maybe an INS is used as GNSS. In this case set parameter receiver_type to ins_in_gnss_mode."); + } } else { node_->log(log_level::DEBUG, "The Rx's response contains " + From d2d9e33e59d1ed5dd0f25adb2e6212d94831fe5c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 1 Feb 2023 09:30:14 +0100 Subject: [PATCH 081/170] Update changelog --- CHANGELOG.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 8788aa55..38b7c08d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,7 +11,8 @@ Changelog for package septentrio_gnss_driver * Rework IO core and message handling * Unified stream processing * Internal data queue - * Prevent message loss in file reading + * Prevent message loss in file reading + * Add some explanatory warnings for parameter mismatches * Preliminary Features * Output of localization and tf in ECEF frame, testing and feedback welcome From 889707fe4862445214ce4d0bb0ffe8db9fd94600 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 1 Feb 2023 14:11:52 +0100 Subject: [PATCH 082/170] Refine README and fix compiled message logic --- README.md | 11 ++++++----- .../communication/communication_core.cpp | 5 +++-- .../communication/message_handler.cpp | 5 ++++- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 98b40713..b61e32b9 100644 --- a/README.md +++ b/README.md @@ -344,7 +344,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Receiver Configuration - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated. The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
@@ -549,7 +549,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + default: "" + `config`: Defines which measurements belonging to the respective axes are forwarded to the INS. In addition, non-holonomic constraints may be introduced for directions known to be restricted in movement. For example, a vehicle with Ackermann steering is limited in its sidewards and upwards movement. So, even if only motion in x-direction may be measured, zero-velocities for y and z may be sent. + default: [] - + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values inside the messaged are used. + + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values from inside the ROS messages are used. + default: false + `variances`: Variances of the respective axes. Only have to be set if `ins_vsm/ros/variances_by_parameter` is set to `true`. Values must be > 0.0, else measurements cannot not be used. + default: [] @@ -617,7 +617,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi A selection of NMEA sentences, the majority being standardized sentences, and proprietary SBF blocks is translated into ROS messages, partly generic and partly custom, and can be published at the discretion of the user into the following ROS topics. All published ROS messages, even custom ones, start with a ROS generic header [`std_msgs/Header.msg`](https://docs.ros.org/melodic/api/std_msgs/html/msg/Header.html), which includes the receiver time stamp as well as the frame ID, the latter being specified in the ROS parameter `frame_id`.
Available ROS Topics - + + `/gpgga`: publishes [`nmea_msgs/Gpgga.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgga.html) - converted from the NMEA sentence GGA. + `/gprmc`: publishes [`nmea_msgs/Gprmc.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gprmc.html) - converted from the NMEA sentence RMC. + `/gpgsa`: publishes [`nmea_msgs/Gpgsa.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsa.html) - converted from the NMEA sentence GSA. @@ -632,7 +632,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/velcovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/VelCovGeodetic.msg`, corresponding to SBF block `VelCovGeodetic` (GNSS case). + `/atteuler`: publishes custom ROS message `septentrio_gnss_driver/AttEuler.msg`, corresponding to SBF block `AttEuler`. + `/attcoveuler`: publishes custom ROS message `septentrio_gnss_driver/AttCovEuler.msg`, corresponding to the SBF block `AttCovEuler`. - + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its header, or - if `use_gnss_time` is set to `false` - from the systems's wall-clock time. + + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its block header. + `/navsatfix`: publishes generic ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`,`PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + The ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) can be fed directly into the [`navsat_transform_node`](https://docs.ros.org/melodic/api/robot_localization/html/navsat_transform_node.html) of the ROS navigation stack. + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). @@ -652,7 +652,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + The ROS message [`sensor_msgs/Imu.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + `/localization`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. - + `/localization_ecef`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + + `/localization_ecef`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF blocks `INSNavCart` and `INSNavGeod`. + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention.
@@ -679,4 +679,5 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. 5. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. + 6. Add SBF block or NMEA to data stream setup in `communication_core.cpp` (function `configureRx()`).
diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 4faab217..9ea9dce1 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -700,7 +700,8 @@ namespace io { blocks << " +PVTCartesian"; } if (settings_->publish_pvtgeodetic || settings_->publish_twist || - settings_->publish_gpst || + (settings_->publish_gpst && + (settings_->septentrio_receiver_type == "gnss")) || (settings_->publish_navsatfix && (settings_->septentrio_receiver_type == "gnss")) || (settings_->publish_gpsfix && @@ -777,7 +778,7 @@ namespace io { settings_->publish_imu || settings_->publish_localization || settings_->publish_tf || settings_->publish_twist || settings_->publish_localization_ecef || - settings_->publish_tf_ecef) + settings_->publish_tf_ecef || settings_->publish_gpst) { blocks << " +INSNavGeod"; } diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 9fe1389d..66326644 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1993,7 +1993,8 @@ namespace io { assembleNavSatFix(); assemblePoseWithCovarianceStamped(); assembleGpsFix(); - if (settings_->publish_gpst) + if (settings_->publish_gpst && + (settings_->septentrio_receiver_type == "gnss")) assembleTimeReference(telegram); break; } @@ -2142,6 +2143,8 @@ namespace io { assemblePoseWithCovarianceStamped(); assembleNavSatFix(); assembleGpsFix(); + if (settings_->publish_gpst) + assembleTimeReference(telegram); break; } From 60f94e1f8c5ce733237bea1ff6c8f739e3137583 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 1 Feb 2023 14:14:35 +0100 Subject: [PATCH 083/170] Update changelog --- CHANGELOG.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 38b7c08d..b00d8f3d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -12,7 +12,8 @@ Changelog for package septentrio_gnss_driver * Unified stream processing * Internal data queue * Prevent message loss in file reading - * Add some explanatory warnings for parameter mismatches + * Add some explanatory warnings for parameter mismatches + * Add units to message definitions * Preliminary Features * Output of localization and tf in ECEF frame, testing and feedback welcome From 36d0c48f1c75c6aaf9ec4734d5dc48054deeb77b Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 4 Feb 2023 00:30:32 +0100 Subject: [PATCH 084/170] Add OSNMA msg --- CHANGELOG.rst | 1 + CMakeLists.txt | 1 + README.md | 28 +++++++--- config/gnss.yaml | 4 ++ config/ins.yaml | 4 ++ .../abstraction/typedefs.hpp | 2 + .../communication/message_handler.hpp | 3 +- .../communication/settings.hpp | 14 ++++- .../parsers/sbf_blocks.hpp | 30 +++++++++++ msg/GALAuthStatus.msg | 14 +++++ .../communication/communication_core.cpp | 52 ++++++++++++++++--- .../communication/message_handler.cpp | 23 ++++++-- .../node/rosaic_node.cpp | 10 ++-- 13 files changed, 164 insertions(+), 22 deletions(-) create mode 100644 msg/GALAuthStatus.msg diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b00d8f3d..22c02db5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,6 +7,7 @@ Changelog for package septentrio_gnss_driver * New Features * Recovery from connection interruption * Add option to bypass configuration of Rx + * OSNMA * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/CMakeLists.txt b/CMakeLists.txt index 43fbb235..02ccfd45 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,7 @@ add_message_files( BaseVectorCart.msg BaseVectorGeod.msg BlockHeader.msg + GALAuthStatus.msg MeasEpoch.msg MeasEpochChannelType1.msg MeasEpochChannelType2.msg diff --git a/README.md b/README.md index b61e32b9..00b452a1 100644 --- a/README.md +++ b/README.md @@ -69,7 +69,9 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo + Besides the aforementioned config file `rover.yaml` containing all parameters, specialized launch files for GNSS `config/gnss.yaml` and INS `config/ins.yaml` respectively contain only the relevant parameters in each case. + The driver was developed and tested with firmware versions >= 4.10.0 for GNSS and >= 1.3.2 for INS. Receivers with older firmware versions are supported but some features may not be available. Known limitations are: * GNSS with firmware < 4.10.0 does not support IP over USB. + * GNSS with firmware < 4.12.1 does not support OSNMA. * INS with firmware < 1.3.2 does not support NTP. + * INS with firmware < 1.4 does not support OSNMA. * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. @@ -92,6 +94,10 @@ configure_rx: true user: "" password: "" + osnma: + mode: off + ntp_server: "" + frame_id: gnss imu_frame_id: imu @@ -341,14 +347,22 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `password`: password +
+ OSNMA + + + `osnma`: Configuration of the Open Service Navigation Message Authentication (OSNMA) feature. + + `mode`: Three operating modes are supported: `off` where OSNMA authentication is disabled, `loose` where satellites are included in the PVT if they are successfully authenticated or if their authentication status is unknown, and `strict` where only successfully-authenticated satellites are included in the PVT. In case of `strict` synchronization via NTP is mandatory. + + default: off + + `ntp_server`: In `strict` mode, OSNMA authentication requires the availability of external time information. In `loose` mode, this is optional but recommended for enhanced security. The receiver can connect to an NTP time server for this purpose. Options are `default` to let the receiver choose an NTP server or specify one like `pool.ntp.org` for example. + + default: "" +
+
Receiver Configuration + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
- -
Receiver Type @@ -623,6 +637,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/gpgsa`: publishes [`nmea_msgs/Gpgsa.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsa.html) - converted from the NMEA sentence GSA. + `/gpgsv`: publishes [`nmea_msgs/Gpgsv.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsv.html) - converted from the NMEA sentence GSV. + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. + + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. @@ -670,14 +685,15 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr Is there an SBF or NMEA message that is not being addressed while being important to your application? If yes, follow these steps: 1. Find the log reference of interest in the publicly accessible, official documentation. Hence select the reference guide file, e.g. for mosaic-x5 in the [product support section for mosaic-X5](https://www.septentrio.com/en/support/mosaic/mosaic-x5), Chapter 4, of Septentrio's homepage. 2. SBF: Add a new `.msg` file to the `../msg` folder. And modify the `../CMakeLists.txt` file by adding a new entry to the `add_message_files` section. - 3. Parsers: + 3. Add msg header and typedef to `typedefs.hpp`. + 4. Parsers: - SBF: Add a parser to the `sbf_blocks.hpp` file. - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `../src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `../include/septentrio_gnss_driver/parsers/nmea_parsers` folder. - 4. Processing the message/block: + 5. Processing the message/block: - SBF: Extend the `SbfId` enumeration in the `message_handler.hpp` file with a new entry. - SBF: Extend the SBF switch-case in `message_handler.cpp` file with a new case. - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. - 5. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. - 6. Add SBF block or NMEA to data stream setup in `communication_core.cpp` (function `configureRx()`). + 6. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. + 7. Add SBF block or NMEA to data stream setup in `communication_core.cpp` (function `configureRx()`).
diff --git a/config/gnss.yaml b/config/gnss.yaml index 0f5b9594..097b7074 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -14,6 +14,10 @@ login: user: "" password: "" +osnma: + mode: off + ntp_server: "" + frame_id: gnss aux1_frame_id: aux1 diff --git a/config/ins.yaml b/config/ins.yaml index 63f5b9f4..77a2d921 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -14,6 +14,10 @@ login: user: "" password: "" +osnma: + mode: off + ntp_server: "" + frame_id: gnss imu_frame_id: imu diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 140051dc..63c3aff2 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -108,6 +109,7 @@ typedef nav_msgs::Odometry LocalizationMsg; typedef septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg; typedef septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg; typedef septentrio_gnss_driver::BlockHeader BlockHeaderMsg; +typedef septentrio_gnss_driver::GALAuthStatus GalAuthStatusMsg; typedef septentrio_gnss_driver::MeasEpoch MeasEpochMsg; typedef septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg; typedef septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg; diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 13f543ea..de79e45a 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -123,7 +123,8 @@ enum SbfId IMU_SETUP = 4224, VEL_SENSOR_SETUP = 4244, EXT_SENSOR_MEAS = 4050, - RECEIVER_TIME = 5914 + RECEIVER_TIME = 5914, + GAL_AUTH_STATUS = 4245 }; namespace io { diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index f958af8c..8fe67a61 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -34,6 +34,14 @@ #include #include +struct Osnma +{ + //! OSNMA mode + std::string mode; + //! Server for NTP synchronization + std::string ntp_server; +}; + struct RtkNtrip { //! Id of the NTRIP port @@ -92,7 +100,7 @@ struct RtkSerial bool keep_open; }; -struct RtkSettings +struct Rtk { std::vector ntrip; std::vector ip_server; @@ -200,7 +208,9 @@ struct Settings //! Position deviation mask float pos_std_dev; //! RTK corrections settings - RtkSettings rtk_settings; + Rtk rtk; + //! OSNMA settings + Osnma osnma; //! Whether or not to publish the GGA message bool publish_gpgga; //! Whether or not to publish the RMC message diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 5f0ed115..2cacd85c 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -630,6 +630,36 @@ template return true; }; +/** + * @class GALAuthStatus + * @brief Qi based parser for the SBF block "GALAuthStatus" + */ +template +[[nodiscard]] bool GalAuthStatusParser(ROSaicNodeBase* node, It it, It itEnd, + GalAuthStatusMsg& msg) +{ + if (!BlockHeaderParser(node, it, msg.block_header)) + return false; + if (msg.block_header.id != 4245) + { + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); + return false; + } + qiLittleEndianParser(it, msg.osnma_status); + qiLittleEndianParser(it, msg.trusted_time_delta); + qiLittleEndianParser(it, msg.gal_active_mask); + qiLittleEndianParser(it, msg.gal_authentic_mask); + qiLittleEndianParser(it, msg.gps_active_mask); + qiLittleEndianParser(it, msg.gps_authentic_mask); + if (it > itEnd) + { + node->log(log_level::ERROR, "Parse error: iterator past end."); + return false; + } + return true; +}; + /** * ReceiverSetupParser * @brief Qi based parser for the SBF block "ReceiverSetup" diff --git a/msg/GALAuthStatus.msg b/msg/GALAuthStatus.msg new file mode 100644 index 00000000..5acd0c70 --- /dev/null +++ b/msg/GALAuthStatus.msg @@ -0,0 +1,14 @@ +# GALAuthStatus block +# Block_Number 4245 + +std_msgs/Header header + +# SBF block header including time header +BlockHeader block_header + +uint16 osnma_status +float32 trusted_time_delta # s +uint64 gal_active_mask +uint64 gal_authentic_mask +uint64 gps_active_mask +uint64 gps_authentic_mask \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 9ea9dce1..b3cb7918 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -87,14 +87,14 @@ namespace io { { resetMainConnection(); send("sdio, " + streamPort_ + ", auto, none\x0D"); - for (auto ntrip : settings_->rtk_settings.ntrip) + for (auto ntrip : settings_->rtk.ntrip) { if (!ntrip.id.empty() && !ntrip.keep_open) { send("snts, " + ntrip.id + ", off \x0D"); } } - for (auto ip_server : settings_->rtk_settings.ip_server) + for (auto ip_server : settings_->rtk.ip_server) { if (!ip_server.id.empty() && !ip_server.keep_open) { @@ -102,7 +102,7 @@ namespace io { send("siss, " + ip_server.id + ", 0\x0D"); } } - for (auto serial : settings_->rtk_settings.serial) + for (auto serial : settings_->rtk.serial) { if (!serial.port.empty() && !serial.keep_open) { @@ -132,6 +132,20 @@ namespace io { ", auto, none\x0D"); } } + if (settings_->osnma.mode == "loose" || + settings_->osnma.mode == "strict") + { + std::stringstream ss; + ss << "sou, off \x0D"; + send(ss.str()); + + if (!settings_->osnma.ntp_server.empty()) + { + std::stringstream ss; + ss << "snc, off \x0D"; + send(ss.str()); + } + } if (!settings_->login_user.empty() && !settings_->login_password.empty()) send("logout \x0D"); @@ -329,7 +343,7 @@ namespace io { } // Configuring the corrections connection - for (auto ntrip : settings_->rtk_settings.ntrip) + for (auto ntrip : settings_->rtk.ntrip) { if (!ntrip.id.empty()) { @@ -359,7 +373,7 @@ namespace io { } } - for (auto ip_server : settings_->rtk_settings.ip_server) + for (auto ip_server : settings_->rtk.ip_server) { if (!ip_server.id.empty()) // Since the Rx does not have internet (and you will not @@ -394,7 +408,7 @@ namespace io { } } - for (auto serial : settings_->rtk_settings.serial) + for (auto serial : settings_->rtk.serial) { if (!serial.port.empty()) { @@ -626,6 +640,27 @@ namespace io { } } + // OSNMA + if (settings_->osnma.mode == "loose" || settings_->osnma.mode == "strict") + { + std::stringstream ss; + ss << "sou, " << settings_->osnma.mode << " \x0D"; + send(ss.str()); + + if (!settings_->osnma.ntp_server.empty()) + { + std::stringstream ss; + ss << "snc, on, " << settings_->osnma.ntp_server << " \x0D"; + send(ss.str()); + } else + { + if (settings_->osnma.mode == "strict") + node_->log( + log_level::ERROR, + "OSNMA mode set to strict but no NTP server provided. In Strict mode an NTP server is mandatory!"); + } + } + // TODO UDP // send("siss, IPS1, 28785, UDP, 10.255.255.200\x0D"); // Setting up SBF blocks with rx_period_rest @@ -646,6 +681,11 @@ namespace io { { blocks << " +ReceiverStatus +QualityInd"; } + if (settings_->osnma.mode == "loose" || + settings_->osnma.mode == "strict") + { + blocks << " +GALAuthStatus"; + } blocks << " +ReceiverSetup"; diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 66326644..0ad978a3 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2091,6 +2091,21 @@ namespace io { assembleGpsFix(); break; } + case GAL_AUTH_STATUS: + { + GalAuthStatusMsg msg; + + if (!GalAuthStatusParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in GalAuthStatus"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("/galauthstatus", msg); + // TODO assemble diagnostics + break; + } case INS_NAV_CART: // Position, velocity and orientation in cartesian // coordinate frame (ENU frame) { @@ -2338,11 +2353,11 @@ namespace io { "receiver setup firmware: " + last_receiversetup_.rx_version); static const int32_t ins_major = 1; - static const int32_t ins_minor = 3; - static const int32_t ins_patch = 2; + static const int32_t ins_minor = 4; + static const int32_t ins_patch = 0; static const int32_t gnss_major = 4; - static const int32_t gnss_minor = 10; - static const int32_t gnss_patch = 0; + static const int32_t gnss_minor = 12; + static const int32_t gnss_patch = 1; boost::tokenizer<> tok(last_receiversetup_.rx_version); boost::tokenizer<>::iterator it = tok.begin(); std::vector major_minor_patch; diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 4961f783..0357f35f 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -132,6 +132,10 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) return false; } + // OSNMA parameters + param("osnma/mode", settings_.osnma.mode, std::string("off")); + param("osnma/ntp_server", settings_.osnma.ntp_server, std::string("")); + // multi_antenna param param("multi_antenna", settings_.multi_antenna, false); @@ -422,7 +426,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) ntripSettings.send_gga = "off"; param("rtk_settings/" + ntrip + "/keep_open", ntripSettings.keep_open, true); - settings_.rtk_settings.ntrip.push_back(ntripSettings); + settings_.rtk.ntrip.push_back(ntripSettings); } // IP server for (uint8_t i = 1; i < 6; ++i) @@ -444,7 +448,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) ipSettings.send_gga = "off"; param("rtk_settings/" + ips + "/keep_open", ipSettings.keep_open, true); - settings_.rtk_settings.ip_server.push_back(ipSettings); + settings_.rtk.ip_server.push_back(ipSettings); } // Serial for (uint8_t i = 1; i < 6; ++i) @@ -468,7 +472,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("rtk_settings/" + serial + "/keep_open", serialSettings.keep_open, true); - settings_.rtk_settings.serial.push_back(serialSettings); + settings_.rtk.serial.push_back(serialSettings); } { From cd3f2476d45946416aad4c4ea3e2eff0fc2893c9 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 4 Feb 2023 01:04:08 +0100 Subject: [PATCH 085/170] Add keep_open option to OSNMA --- README.md | 2 ++ config/gnss.yaml | 1 + config/ins.yaml | 1 + config/rover.yaml | 5 +++++ include/septentrio_gnss_driver/communication/settings.hpp | 2 ++ .../communication/communication_core.cpp | 4 ++-- src/septentrio_gnss_driver/node/rosaic_node.cpp | 1 + 7 files changed, 14 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 00b452a1..2db975aa 100644 --- a/README.md +++ b/README.md @@ -355,6 +355,8 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + default: off + `ntp_server`: In `strict` mode, OSNMA authentication requires the availability of external time information. In `loose` mode, this is optional but recommended for enhanced security. The receiver can connect to an NTP time server for this purpose. Options are `default` to let the receiver choose an NTP server or specify one like `pool.ntp.org` for example. + default: "" + + `keep_open`: Wether OSNMA shall be kept active on driver shutdown. + + default: true
diff --git a/config/gnss.yaml b/config/gnss.yaml index 097b7074..aee76a2c 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -17,6 +17,7 @@ login: osnma: mode: off ntp_server: "" + keep_open: true frame_id: gnss diff --git a/config/ins.yaml b/config/ins.yaml index 77a2d921..4a5822d2 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -17,6 +17,7 @@ login: osnma: mode: off ntp_server: "" + keep_open: true frame_id: gnss diff --git a/config/rover.yaml b/config/rover.yaml index 562727db..5adeb466 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -14,6 +14,11 @@ login: user: "" password: "" +osnma: + mode: off + ntp_server: "" + keep_open: true + frame_id: gnss imu_frame_id: imu diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 8fe67a61..d74312b8 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -40,6 +40,8 @@ struct Osnma std::string mode; //! Server for NTP synchronization std::string ntp_server; + //! Wether OSNMA shall be kept open on shutdown + bool keep_open; }; struct RtkNtrip diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index b3cb7918..e713726e 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -132,8 +132,8 @@ namespace io { ", auto, none\x0D"); } } - if (settings_->osnma.mode == "loose" || - settings_->osnma.mode == "strict") + if (!settings_->osnma.keep_open && (settings_->osnma.mode == "loose" || + settings_->osnma.mode == "strict")) { std::stringstream ss; ss << "sou, off \x0D"; diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 0357f35f..02c0d3e9 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -135,6 +135,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) // OSNMA parameters param("osnma/mode", settings_.osnma.mode, std::string("off")); param("osnma/ntp_server", settings_.osnma.ntp_server, std::string("")); + param("osnma/keep_open", settings_.osnma.keep_open, true); // multi_antenna param param("multi_antenna", settings_.multi_antenna, false); From d6ef0c0c30fd89e20f86fb4ce072d9b83079eb97 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 5 Feb 2023 00:39:22 +0100 Subject: [PATCH 086/170] Add OSNMA diagnostics output --- .../communication/message_handler.hpp | 7 ++ .../communication/message_handler.cpp | 100 +++++++++++++++++- 2 files changed, 102 insertions(+), 5 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index de79e45a..28568e04 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -319,6 +319,13 @@ namespace io { */ void assembleDiagnosticArray(const std::shared_ptr& telegram); + /** + * @brief "Callback" function when constructing + * OSNMA DiagnosticArrayMsg messages + * @param[in] status GalAuthStatusMsg from which the msg was assembled + */ + void assembleOsnmaDiagnosticArray(const GalAuthStatusMsg& status); + /** * @brief "Callback" function when constructing * ImuMsg messages diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 0ad978a3..5567c49e 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -222,7 +222,7 @@ namespace io { if (validValue(last_receiversetup_.block_header.tow)) serialnumber = last_receiversetup_.rx_serial_number; else - serialnumber = "invalid"; + serialnumber = "unknown"; DiagnosticStatusMsg gnss_status; // Constructing the "level of operation" field uint16_t indicators_type_mask = static_cast(255); @@ -320,10 +320,10 @@ namespace io { (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); } } - gnss_status.hardware_id = serialnumber; - gnss_status.name = "gnss"; + gnss_status.hardware_id = "Septentrio"; + gnss_status.name = "serial number " + serialnumber; gnss_status.message = - "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; + "GNSS quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; msg.status.push_back(gnss_status); std::string frame_id; if (settings_->septentrio_receiver_type == "gnss") @@ -344,6 +344,96 @@ namespace io { publish("/diagnostics", msg); }; + void MessageHandler::assembleOsnmaDiagnosticArray(const GalAuthStatusMsg& status) + { + DiagnosticArrayMsg msg; + DiagnosticStatusMsg diag; + + diag.hardware_id = "Septentrio"; + diag.name = "OSNMA"; + diag.message = "Current status of the OSNMA authentication"; + + diag.values.resize(6); + diag.values[0].key = "status"; + switch (status.osnma_status & 7) + { + case 0: + { + diag.values[0].value = "Disabled"; + break; + } + case 1: + { + uint16_t percent = (status.osnma_status >> 3) & 127; + diag.values[0].value = "Initializing " + std::to_string(percent) + " %"; + break; + } + case 2: + { + diag.values[0].value = "Waiting on NTP"; + break; + } + case 3: + { + diag.values[0].value = "Init failed - inconsistent time"; + break; + } + case 4: + { + diag.values[0].value = "Init failed - KROOT signature invalid"; + break; + } + case 5: + { + diag.values[0].value = "Init failed - invalid param received"; + break; + } + case 6: + { + diag.values[0].value = "Authenticating"; + break; + } + + default: + break; + } + + diag.values[1].key = "trusted_time_delta"; + if (validValue(status.trusted_time_delta)) + diag.values[1].value = std::to_string(status.trusted_time_delta); + else + diag.values[1].value = "N/A"; + + std::bitset<64> gal_active = status.gal_active_mask; + std::bitset<64> gal_auth = status.gal_authentic_mask; + uint8_t gal_authentic = (gal_auth & gal_active).count(); + uint8_t gal_spoofed = (~gal_auth & gal_active).count(); + diag.values[2].key = "Galileo authentic"; + diag.values[2].value = std::to_string(gal_authentic); + diag.values[3].key = "Galileo spoofed"; + diag.values[3].value = std::to_string(gal_spoofed); + + std::bitset<64> gps_active = status.gps_active_mask; + std::bitset<64> gps_auth = status.gps_authentic_mask; + uint8_t gps_authentic = (gps_auth & gps_active).count(); + uint8_t gps_spoofed = (~gps_auth & gps_active).count(); + diag.values[4].key = "GPS authentic"; + diag.values[4].value = std::to_string(gps_authentic); + diag.values[5].key = "GPS spoofed"; + diag.values[5].value = std::to_string(gps_spoofed); + + if ((gal_spoofed + gps_spoofed) == 0) + diag.level = DiagnosticStatusMsg::OK; + else if ((gal_authentic + gps_authentic) > 0) + diag.level = DiagnosticStatusMsg::WARN; + else + diag.level = DiagnosticStatusMsg::ERROR; + + msg.status.push_back(diag); + msg.header = status.header; + publish("/diagnostics", msg); + } + void MessageHandler::assembleImu() { ImuMsg msg; @@ -2103,7 +2193,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, msg); publish("/galauthstatus", msg); - // TODO assemble diagnostics + assembleOsnmaDiagnosticArray(msg); break; } case INS_NAV_CART: // Position, velocity and orientation in cartesian From d17a5e9554b07b4910ec3bc4991439ed7f4b62a3 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 5 Feb 2023 22:49:07 +0100 Subject: [PATCH 087/170] Add missing param to example in readme --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 2db975aa..e4ca4e1f 100644 --- a/README.md +++ b/README.md @@ -97,6 +97,7 @@ configure_rx: true osnma: mode: off ntp_server: "" + keep_open: true frame_id: gnss From 59e4f58543e0e8ac86e7dcb7e90aba30ecf989f3 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 5 Feb 2023 23:04:27 +0100 Subject: [PATCH 088/170] Fix param type misinterpretation --- README.md | 2 +- config/gnss.yaml | 2 +- config/ins.yaml | 2 +- config/rover.yaml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index e4ca4e1f..5a0fc111 100644 --- a/README.md +++ b/README.md @@ -95,7 +95,7 @@ configure_rx: true password: "" osnma: - mode: off + mode: "off" ntp_server: "" keep_open: true diff --git a/config/gnss.yaml b/config/gnss.yaml index aee76a2c..b80fc574 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -15,7 +15,7 @@ login: password: "" osnma: - mode: off + mode: "off" ntp_server: "" keep_open: true diff --git a/config/ins.yaml b/config/ins.yaml index 4a5822d2..7fe85780 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -15,7 +15,7 @@ login: password: "" osnma: - mode: off + mode: "off" ntp_server: "" keep_open: true diff --git a/config/rover.yaml b/config/rover.yaml index 5adeb466..3fae7352 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -15,7 +15,7 @@ login: password: "" osnma: - mode: off + mode: "off" ntp_server: "" keep_open: true From fc56ba6ff7bece8847aa3130d22e29fbbe0ff31f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 8 Feb 2023 00:03:27 +0100 Subject: [PATCH 089/170] Add option for latency compensation --- CHANGELOG.rst | 1 + README.md | 4 ++++ config/gnss.yaml | 2 ++ config/ins.yaml | 2 ++ config/rover.yaml | 2 ++ .../communication/message_handler.hpp | 3 +++ .../communication/settings.hpp | 2 ++ .../communication/message_handler.cpp | 20 ++++++++++++++----- .../node/rosaic_node.cpp | 1 + 9 files changed, 32 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 22c02db5..78d191de 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -8,6 +8,7 @@ Changelog for package septentrio_gnss_driver * Recovery from connection interruption * Add option to bypass configuration of Rx * OSNMA + * Latency compensation for ROS timestamps * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 5a0fc111..e4cb7078 100644 --- a/README.md +++ b/README.md @@ -147,6 +147,8 @@ configure_rx: true use_gnss_time: false + latency_compensation: false + rtk_settings: ntrip_1: id: "NTR1" @@ -473,6 +475,8 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `use_gnss_time`: `true` if the ROS message headers' unix epoch time field shall be constructed from the TOW/WNC (in the SBF case) and UTC (in the NMEA case) data, `false` if those times shall be taken by the driver from ROS time. If `use_gnss_time` is set to `true`, make sure the ROS system is synchronized to an NTP time server either via internet or ideally via the Septentrio receiver since the latter serves as a Stratum 1 time server not dependent on an internet connection. The NTP server of the receiver is automatically activated on the Septentrio receiver (for INS/GNSS a firmware >= 1.3.3 is needed). + default: `true` + + `latency_compensation`: Rx reports processing latency in PVT and INS blocks. If set to `true`this latency is subtracted from ROS timestamps in related blocks (i.e., `use_gnss_time` set to `false`). Related blocks are INS, PVT, Covariances, and BaseVectors. In case of `use_gnss_time` set to `true`, the latency is already compensated within the RX and included in the reported timestamps. + + default: `false`
diff --git a/config/gnss.yaml b/config/gnss.yaml index b80fc574..932a3eda 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -48,6 +48,8 @@ polling_period: use_gnss_time: false +latency_compensation: false + rtk_settings: ntrip_1: id: "" diff --git a/config/ins.yaml b/config/ins.yaml index 7fe85780..ef473efa 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -62,6 +62,8 @@ polling_period: use_gnss_time: false +latency_compensation: false + rtk_settings: keep_open: true ntrip_1: diff --git a/config/rover.yaml b/config/rover.yaml index 3fae7352..7191118e 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -69,6 +69,8 @@ polling_period: use_gnss_time: false +latency_compensation: false + rtk_settings: ntrip_1: id: "" diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 28568e04..7b865731 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -293,6 +293,9 @@ namespace io { //! by the time stamps found in the SBF blocks therein. Timestamp unix_time_; + //! Last reported PVT processing latency + mutable uint64_t last_pvt_latency_ = 0; + //! Current leap seconds as received, do not use value is -128 int32_t current_leap_seconds_ = -128; diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index d74312b8..2731c3f7 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -294,6 +294,8 @@ struct Settings //! (in the SBF case) and UTC (in the NMEA case) data. If false, times are //! constructed within the driver via time(NULL) of the \ library. bool use_gnss_time; + //! Wether processing latency shall be compensated for in ROS timestamp + bool latency_compensation; //! The frame ID used in the header of every published ROS message std::string frame_id; //! The frame ID used in the header of published ROS Imu message diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 5567c49e..a57c3377 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1929,14 +1929,24 @@ namespace io { : telegram->stamp; msg.header.frame_id = frameId; - if constexpr (std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value) + if (!settings_->use_gnss_time && settings_->latency_compensation) { - if (!settings_->use_gnss_time) + if constexpr (std::is_same::value || + std::is_same::value) { time_obj -= msg.latency * 100000ul; // from 0.0001 s to ns + } else if constexpr (std::is_same::value || + std::is_same::value) + { + last_pvt_latency_ = msg.latency * 100000ul; // from 0.0001 s to ns + time_obj -= last_pvt_latency_; + } else if constexpr (std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value) + { + time_obj -= last_pvt_latency_; } } diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 02c0d3e9..baa5966a 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -67,6 +67,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) [[nodiscard]] bool rosaic_node::ROSaicNode::getROSParams() { param("use_gnss_time", settings_.use_gnss_time, true); + param("latency_compensation", settings_.latency_compensation, false); param("frame_id", settings_.frame_id, static_cast("gnss")); param("imu_frame_id", settings_.imu_frame_id, static_cast("imu")); param("poi_frame_id", settings_.poi_frame_id, From a7e52b70d11d69b850a877a38e4e9122bf8ffeb4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 8 Feb 2023 10:34:56 +0100 Subject: [PATCH 090/170] Ensure latency reporting block is activated --- README.md | 2 +- .../communication/communication_core.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e4cb7078..1025fcc4 100644 --- a/README.md +++ b/README.md @@ -365,7 +365,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Receiver Configuration - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index e713726e..cde54525 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -747,7 +747,8 @@ namespace io { (settings_->publish_gpsfix && (settings_->septentrio_receiver_type == "gnss")) || (settings_->publish_pose && - (settings_->septentrio_receiver_type == "gnss"))) + (settings_->septentrio_receiver_type == "gnss")) || + settings_->latency_compensation) { blocks << " +PVTGeodetic"; } From 0077ac13782c462ecc417f4945cdda6dbb53da67 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 8 Feb 2023 23:06:54 +0100 Subject: [PATCH 091/170] Refine Rx type check --- .../communication/telegram_handler.hpp | 16 ++++++++++++- .../communication/communication_core.cpp | 23 ++++++++++++++++--- .../communication/message_handler.cpp | 17 +++++--------- .../communication/telegram_handler.cpp | 22 ++++++++++++++---- 4 files changed, 59 insertions(+), 19 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 3a84cd7a..70079ecb 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -112,7 +112,7 @@ namespace io { public: TelegramHandler(ROSaicNodeBase* node) : node_(node), messageHandler_(node) {} - ~TelegramHandler() + ~TelegramHandler() { cdSemaphore_.notify(); responseSemaphore_.notify(); @@ -142,6 +142,15 @@ namespace io { //! Waits for response void waitForResponse() { responseSemaphore_.wait(); } + //! Waits for capabilities + void waitForCapabilities() { capabilitiesSemaphore_.wait(); } + + //! Check if capability contains INS + bool isIns() { return isIns_; } + + //! Check if capability contains Heading + bool hasHeading() { return hasHeading_; } + private: void handleSbf(const std::shared_ptr& telegram); void handleNmea(const std::shared_ptr& telegram); @@ -154,8 +163,13 @@ namespace io { //! MessageHandler parser MessageHandler messageHandler_; + //! Rx capabilities + mutable bool isIns_ = false; + mutable bool hasHeading_ = false; + Semaphore cdSemaphore_; Semaphore responseSemaphore_; + Semaphore capabilitiesSemaphore_; std::string mainConnectionDescriptor_ = std::string(); }; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index cde54525..c1d95065 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -284,6 +284,20 @@ namespace io { send("sso, all, none, none, off \x0D"); send("sno, all, none, none, off \x0D"); + // Get Rx capabilities + send("grc \x0D"); + telegramHandler_.waitForCapabilities(); + + bool ins_in_gnss_mode = settings_->ins_in_gnss_mode; + if (telegramHandler_.isIns() && + (settings_->septentrio_receiver_type == "gnss") && !ins_in_gnss_mode) + { + node_->log( + log_level::WARN, + "INS receiver seems to be used as GNSS. If this is intended, please consider setting receiver type to 'ins_in_gnss_mode'."); + ins_in_gnss_mode = true; + } + // Activate NTP server if (settings_->use_gnss_time) send("sntp, on \x0D"); @@ -297,8 +311,7 @@ namespace io { send(ss.str()); } - if ((settings_->septentrio_receiver_type == "ins") || - settings_->ins_in_gnss_mode) + if ((settings_->septentrio_receiver_type == "ins") || ins_in_gnss_mode) { { std::stringstream ss; @@ -438,7 +451,11 @@ namespace io { // Setting multi antenna if (settings_->multi_antenna) { - send("sga, MultiAntenna \x0D"); + if (telegramHandler_.hasHeading()) + send("sga, MultiAntenna \x0D"); + else + node_->log(log_level::WARN, + "Multi antenna requested but Rx does not support it."); } else { send("sga, none \x0D"); diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index a57c3377..6d2f25da 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2494,17 +2494,12 @@ namespace io { } } else if (settings_->septentrio_receiver_type == "gnss") { - if (major_minor_patch[0] < 3) - { - node_->log( - log_level::WARN, - "INS receiver seems to be used as GNSS. Some settings may trigger warnings or errors. Consider using 'ins_in_gnss_mode' as receiver type."); - } else if ((major_minor_patch[0] < gnss_major) || - ((major_minor_patch[0] == gnss_major) && - (major_minor_patch[1] < gnss_minor)) || - ((major_minor_patch[0] == gnss_major) && - (major_minor_patch[1] == gnss_minor) && - (major_minor_patch[2] < gnss_patch))) + if ((major_minor_patch[0] < gnss_major) || + ((major_minor_patch[0] == gnss_major) && + (major_minor_patch[1] < gnss_minor)) || + ((major_minor_patch[0] == gnss_major) && + (major_minor_patch[1] == gnss_minor) && + (major_minor_patch[2] < gnss_patch))) { node_->log( log_level::WARN, diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index b1a2c8ad..5d9062ed 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -74,9 +74,23 @@ namespace io { } case telegram_type::UNKNOWN: { - node_->log(log_level::DEBUG, "Unhandeled message received: " + - std::string(telegram->message.begin(), - telegram->message.end())); + std::string block_in_string(telegram->message.begin(), + telegram->message.end()); + + node_->log(log_level::DEBUG, "A message received: " + block_in_string); + if (block_in_string.find("ReceiverCapabilities") != std::string::npos) + { + if (block_in_string.find("INS") != std::string::npos) + { + isIns_ = true; + } + + if (block_in_string.find("Heading") != std::string::npos) + { + hasHeading_ = true; + } + capabilitiesSemaphore_.notify(); + } break; } default: @@ -123,7 +137,7 @@ namespace io { { node_->log( log_level::WARN, - "Rx does understand sao command, maybe an INS is used as GNSS. In this case set parameter receiver_type to ins_in_gnss_mode."); + "Rx does not understand sao command, maybe an INS is used as GNSS. In this case set parameter receiver_type to ins_in_gnss_mode."); } } else { From 06beaccb637933a2b360a2d14d2232518ae782be Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 9 Feb 2023 12:22:22 +0100 Subject: [PATCH 092/170] Add VelCovCartesian output --- CHANGELOG.rst | 1 + README.md | 3 +++ config/gnss.yaml | 1 + config/ins.yaml | 1 + config/rover.yaml | 1 + .../communication/message_handler.hpp | 1 + .../communication/settings.hpp | 5 ++++- .../communication/communication_core.cpp | 4 ++++ .../communication/message_handler.cpp | 14 +++++++++++++ .../node/rosaic_node.cpp | 21 +++++++------------ 10 files changed, 37 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 78d191de..63958c74 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -9,6 +9,7 @@ Changelog for package septentrio_gnss_driver * Add option to bypass configuration of Rx * OSNMA * Latency compensation for ROS timestamps + * Output of SBf block VelCovCartesian * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 1025fcc4..85c0ef4e 100644 --- a/README.md +++ b/README.md @@ -215,6 +215,7 @@ configure_rx: true basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: false atteuler: true attcoveuler: true @@ -611,6 +612,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/basevectorgeod`: `true` to publish `septentrio_gnss_driver/BaseVectorGeod.msg` messages into the topic `/basevectorgeod` + `publish/poscovcartesian`: `true` to publish `septentrio_gnss_driver/PosCovCartesian.msg` messages into the topic `/poscovcartesian` + `publish/poscovgeodetic`: `true` to publish `septentrio_gnss_driver/PosCovGeodetic.msg` messages into the topic `/poscovgeodetic` + + `publish/velcovcartesian`: `true` to publish `septentrio_gnss_driver/VelCovCartesian.msg` messages into the topic `/velcovcartesian` + `publish/velcovgeodetic`: `true` to publish `septentrio_gnss_driver/VelCovGeodetic.msg` messages into the topic `/velcovgeodetic` + `publish/atteuler`: `true` to publish `septentrio_gnss_driver/AttEuler.msg` messages into the topic `/atteuler` + `publish/attcoveuler`: `true` to publish `septentrio_gnss_driver/AttCovEuler.msg` messages into the topic `/attcoveuler` @@ -651,6 +653,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/basevectorgeod`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorGeod.msg`, corresponding to the SBF block `BaseVectorGeod`. + `/poscovcartesian`: publishes custom ROS message `septentrio_gnss_driver/PosCovCartesian.msg`, corresponding to SBF block `PosCovCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/poscovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PosCovGeodetic.msg`, corresponding to SBF block `PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + + `/velcovcartesian`: publishes custom ROS message `septentrio_gnss_driver/VelCovCartesian.msg`, corresponding to SBF block `VelCovCartesian` (GNSS case). + `/velcovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/VelCovGeodetic.msg`, corresponding to SBF block `VelCovGeodetic` (GNSS case). + `/atteuler`: publishes custom ROS message `septentrio_gnss_driver/AttEuler.msg`, corresponding to SBF block `AttEuler`. + `/attcoveuler`: publishes custom ROS message `septentrio_gnss_driver/AttCovEuler.msg`, corresponding to the SBF block `AttCovEuler`. diff --git a/config/gnss.yaml b/config/gnss.yaml index 932a3eda..e107c89d 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -91,6 +91,7 @@ publish: basevectorgeod: false poscovcartesian: true poscovgeodetic: true + velcovcartesian: false velcovgeodetic: true atteuler: true attcoveuler: true diff --git a/config/ins.yaml b/config/ins.yaml index ef473efa..b7ac2415 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -103,6 +103,7 @@ publish: basevectorgeod: false poscovcartesian: false poscovgeodetic: false + velcovcartesian: false velcovgeodetic: false atteuler: false attcoveuler: false diff --git a/config/rover.yaml b/config/rover.yaml index 7191118e..983535d7 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -112,6 +112,7 @@ publish: basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: true atteuler: true attcoveuler: true diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 7b865731..b3b6c2df 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -112,6 +112,7 @@ enum SbfId CHANNEL_STATUS = 4013, MEAS_EPOCH = 4027, DOP = 4001, + VEL_COV_CARTESIAN = 5907, VEL_COV_GEODETIC = 5908, RECEIVER_STATUS = 4014, QUALITY_IND = 4082, diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 2731c3f7..add89cdd 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -239,6 +239,9 @@ struct Settings //! Whether or not to publish the PosCovGeodeticMsg //! message bool publish_poscovgeodetic; + //! Whether or not to publish the VelCovCartesianMsg + //! message + bool publish_velcovcartesian; //! Whether or not to publish the VelCovGeodeticMsg //! message bool publish_velcovgeodetic; @@ -294,7 +297,7 @@ struct Settings //! (in the SBF case) and UTC (in the NMEA case) data. If false, times are //! constructed within the driver via time(NULL) of the \ library. bool use_gnss_time; - //! Wether processing latency shall be compensated for in ROS timestamp + //! Wether processing latency shall be compensated for in ROS timestamp bool latency_compensation; //! The frame ID used in the header of every published ROS message std::string frame_id; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index c1d95065..b29ee27e 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -791,6 +791,10 @@ namespace io { { blocks << " +PosCovGeodetic"; } + if (settings_->publish_velcovcartesian) + { + blocks << " +VelCovCartesian"; + } if (settings_->publish_velcovgeodetic || settings_->publish_twist || (settings_->publish_gpsfix && (settings_->septentrio_receiver_type == "gnss"))) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 6d2f25da..75c64fb5 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2402,6 +2402,20 @@ namespace io { assembleGpsFix(); break; } + case VEL_COV_CARTESIAN: + { + VelCovCartesianMsg msg; + if (!VelCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in VelCovCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + if (settings_->publish_velcovcartesian) + publish("/velcovcartesian", msg); + break; + } case VEL_COV_GEODETIC: { diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index baa5966a..5eb502cb 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -153,24 +153,17 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("publish/gpgsv", settings_.publish_gpgsv, false); param("publish/measepoch", settings_.publish_measepoch, false); param("publish/pvtcartesian", settings_.publish_pvtcartesian, false); - param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, - (settings_.septentrio_receiver_type == "gnss")); + param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, false); param("publish/basevectorcart", settings_.publish_basevectorcart, false); param("publish/basevectorgeod", settings_.publish_basevectorgeod, false); param("publish/poscovcartesian", settings_.publish_poscovcartesian, false); - param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, - (settings_.septentrio_receiver_type == "gnss")); - param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, - (settings_.septentrio_receiver_type == "gnss")); - param( - "publish/atteuler", settings_.publish_atteuler, - ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)); - param( - "publish/attcoveuler", settings_.publish_attcoveuler, - ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)); + param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, false); + param("publish/velcovcartesian", settings_.publish_velcovcartesian, false); + param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, false); + param("publish/atteuler", settings_.publish_atteuler, false); + param("publish/attcoveuler", settings_.publish_attcoveuler, false); param("publish/insnavcart", settings_.publish_insnavcart, false); - param("publish/insnavgeod", settings_.publish_insnavgeod, - (settings_.septentrio_receiver_type == "ins")); + param("publish/insnavgeod", settings_.publish_insnavgeod, false); param("publish/imusetup", settings_.publish_imusetup, false); param("publish/velsensorsetup", settings_.publish_velsensorsetup, false); param("publish/exteventinsnavgeod", settings_.publish_exteventinsnavgeod, false); From 0048123d70c1283bf672ca1b249b07cb7004f16e Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 9 Feb 2023 12:22:22 +0100 Subject: [PATCH 093/170] Add VelCovCartesian output --- CHANGELOG.rst | 1 + README.md | 3 +++ config/gnss.yaml | 1 + config/ins.yaml | 1 + config/rover.yaml | 1 + .../communication/message_handler.hpp | 1 + .../communication/settings.hpp | 5 ++++- .../communication/communication_core.cpp | 4 ++++ .../communication/message_handler.cpp | 15 +++++++++++++ .../node/rosaic_node.cpp | 21 +++++++------------ 10 files changed, 38 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 78d191de..63958c74 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -9,6 +9,7 @@ Changelog for package septentrio_gnss_driver * Add option to bypass configuration of Rx * OSNMA * Latency compensation for ROS timestamps + * Output of SBf block VelCovCartesian * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 1025fcc4..85c0ef4e 100644 --- a/README.md +++ b/README.md @@ -215,6 +215,7 @@ configure_rx: true basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: false atteuler: true attcoveuler: true @@ -611,6 +612,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/basevectorgeod`: `true` to publish `septentrio_gnss_driver/BaseVectorGeod.msg` messages into the topic `/basevectorgeod` + `publish/poscovcartesian`: `true` to publish `septentrio_gnss_driver/PosCovCartesian.msg` messages into the topic `/poscovcartesian` + `publish/poscovgeodetic`: `true` to publish `septentrio_gnss_driver/PosCovGeodetic.msg` messages into the topic `/poscovgeodetic` + + `publish/velcovcartesian`: `true` to publish `septentrio_gnss_driver/VelCovCartesian.msg` messages into the topic `/velcovcartesian` + `publish/velcovgeodetic`: `true` to publish `septentrio_gnss_driver/VelCovGeodetic.msg` messages into the topic `/velcovgeodetic` + `publish/atteuler`: `true` to publish `septentrio_gnss_driver/AttEuler.msg` messages into the topic `/atteuler` + `publish/attcoveuler`: `true` to publish `septentrio_gnss_driver/AttCovEuler.msg` messages into the topic `/attcoveuler` @@ -651,6 +653,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/basevectorgeod`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorGeod.msg`, corresponding to the SBF block `BaseVectorGeod`. + `/poscovcartesian`: publishes custom ROS message `septentrio_gnss_driver/PosCovCartesian.msg`, corresponding to SBF block `PosCovCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/poscovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PosCovGeodetic.msg`, corresponding to SBF block `PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + + `/velcovcartesian`: publishes custom ROS message `septentrio_gnss_driver/VelCovCartesian.msg`, corresponding to SBF block `VelCovCartesian` (GNSS case). + `/velcovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/VelCovGeodetic.msg`, corresponding to SBF block `VelCovGeodetic` (GNSS case). + `/atteuler`: publishes custom ROS message `septentrio_gnss_driver/AttEuler.msg`, corresponding to SBF block `AttEuler`. + `/attcoveuler`: publishes custom ROS message `septentrio_gnss_driver/AttCovEuler.msg`, corresponding to the SBF block `AttCovEuler`. diff --git a/config/gnss.yaml b/config/gnss.yaml index 932a3eda..e107c89d 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -91,6 +91,7 @@ publish: basevectorgeod: false poscovcartesian: true poscovgeodetic: true + velcovcartesian: false velcovgeodetic: true atteuler: true attcoveuler: true diff --git a/config/ins.yaml b/config/ins.yaml index ef473efa..b7ac2415 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -103,6 +103,7 @@ publish: basevectorgeod: false poscovcartesian: false poscovgeodetic: false + velcovcartesian: false velcovgeodetic: false atteuler: false attcoveuler: false diff --git a/config/rover.yaml b/config/rover.yaml index 7191118e..983535d7 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -112,6 +112,7 @@ publish: basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: true atteuler: true attcoveuler: true diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 7b865731..b3b6c2df 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -112,6 +112,7 @@ enum SbfId CHANNEL_STATUS = 4013, MEAS_EPOCH = 4027, DOP = 4001, + VEL_COV_CARTESIAN = 5907, VEL_COV_GEODETIC = 5908, RECEIVER_STATUS = 4014, QUALITY_IND = 4082, diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 2731c3f7..add89cdd 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -239,6 +239,9 @@ struct Settings //! Whether or not to publish the PosCovGeodeticMsg //! message bool publish_poscovgeodetic; + //! Whether or not to publish the VelCovCartesianMsg + //! message + bool publish_velcovcartesian; //! Whether or not to publish the VelCovGeodeticMsg //! message bool publish_velcovgeodetic; @@ -294,7 +297,7 @@ struct Settings //! (in the SBF case) and UTC (in the NMEA case) data. If false, times are //! constructed within the driver via time(NULL) of the \ library. bool use_gnss_time; - //! Wether processing latency shall be compensated for in ROS timestamp + //! Wether processing latency shall be compensated for in ROS timestamp bool latency_compensation; //! The frame ID used in the header of every published ROS message std::string frame_id; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index c1d95065..b29ee27e 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -791,6 +791,10 @@ namespace io { { blocks << " +PosCovGeodetic"; } + if (settings_->publish_velcovcartesian) + { + blocks << " +VelCovCartesian"; + } if (settings_->publish_velcovgeodetic || settings_->publish_twist || (settings_->publish_gpsfix && (settings_->septentrio_receiver_type == "gnss"))) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 6d2f25da..b9b0519a 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1942,6 +1942,7 @@ namespace io { time_obj -= last_pvt_latency_; } else if constexpr (std::is_same::value || std::is_same::value || + std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) @@ -2402,6 +2403,20 @@ namespace io { assembleGpsFix(); break; } + case VEL_COV_CARTESIAN: + { + VelCovCartesianMsg msg; + if (!VelCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in VelCovCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + if (settings_->publish_velcovcartesian) + publish("/velcovcartesian", msg); + break; + } case VEL_COV_GEODETIC: { diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index baa5966a..5eb502cb 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -153,24 +153,17 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("publish/gpgsv", settings_.publish_gpgsv, false); param("publish/measepoch", settings_.publish_measepoch, false); param("publish/pvtcartesian", settings_.publish_pvtcartesian, false); - param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, - (settings_.septentrio_receiver_type == "gnss")); + param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, false); param("publish/basevectorcart", settings_.publish_basevectorcart, false); param("publish/basevectorgeod", settings_.publish_basevectorgeod, false); param("publish/poscovcartesian", settings_.publish_poscovcartesian, false); - param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, - (settings_.septentrio_receiver_type == "gnss")); - param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, - (settings_.septentrio_receiver_type == "gnss")); - param( - "publish/atteuler", settings_.publish_atteuler, - ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)); - param( - "publish/attcoveuler", settings_.publish_attcoveuler, - ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)); + param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, false); + param("publish/velcovcartesian", settings_.publish_velcovcartesian, false); + param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, false); + param("publish/atteuler", settings_.publish_atteuler, false); + param("publish/attcoveuler", settings_.publish_attcoveuler, false); param("publish/insnavcart", settings_.publish_insnavcart, false); - param("publish/insnavgeod", settings_.publish_insnavgeod, - (settings_.septentrio_receiver_type == "ins")); + param("publish/insnavgeod", settings_.publish_insnavgeod, false); param("publish/imusetup", settings_.publish_imusetup, false); param("publish/velsensorsetup", settings_.publish_velsensorsetup, false); param("publish/exteventinsnavgeod", settings_.publish_exteventinsnavgeod, false); From ff01619cbdbbc1551c75b30afb615f900b08cb54 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 9 Feb 2023 21:21:44 +0100 Subject: [PATCH 094/170] Add RFStatus diagnostics --- CMakeLists.txt | 2 + README.md | 1 + .../abstraction/typedefs.hpp | 4 + .../communication/message_handler.hpp | 16 +- .../parsers/sbf_blocks.hpp | 56 ++++++- msg/RFBand.msg | 5 + msg/RFStatus.msg | 12 ++ .../communication/communication_core.cpp | 2 +- .../communication/message_handler.cpp | 158 +++++++++++++----- 9 files changed, 204 insertions(+), 52 deletions(-) create mode 100644 msg/RFBand.msg create mode 100644 msg/RFStatus.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 02ccfd45..8548c271 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,6 +85,8 @@ add_message_files( BaseVectorGeod.msg BlockHeader.msg GALAuthStatus.msg + RFBand.msg + RFStatus.msg MeasEpoch.msg MeasEpochChannelType1.msg MeasEpochChannelType2.msg diff --git a/README.md b/README.md index 85c0ef4e..80568d0a 100644 --- a/README.md +++ b/README.md @@ -647,6 +647,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/gpgsv`: publishes [`nmea_msgs/Gpgsv.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsv.html) - converted from the NMEA sentence GSV. + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. + + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, corresponding to the SBF block `RFStatus`. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 63c3aff2..6f6768d3 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -65,6 +65,8 @@ #include #include #include +#include +#include #include #include #include @@ -110,6 +112,8 @@ typedef septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg; typedef septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg; typedef septentrio_gnss_driver::BlockHeader BlockHeaderMsg; typedef septentrio_gnss_driver::GALAuthStatus GalAuthStatusMsg; +typedef septentrio_gnss_driver::RFStatus RfStatusMsg; +typedef septentrio_gnss_driver::RFBand RfBandMsg; typedef septentrio_gnss_driver::MeasEpoch MeasEpochMsg; typedef septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg; typedef septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg; diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index b3b6c2df..a90c0708 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -125,7 +125,8 @@ enum SbfId VEL_SENSOR_SETUP = 4244, EXT_SENSOR_MEAS = 4050, RECEIVER_TIME = 5914, - GAL_AUTH_STATUS = 4245 + GAL_AUTH_STATUS = 4245, + RF_STATUS = 4092 }; namespace io { @@ -290,6 +291,16 @@ namespace io { */ ReceiverSetup last_receiversetup_; + /** + * @brief Stores incoming GALAuthStatus block + */ + GalAuthStatusMsg last_gal_auth_status_; + + /** + * @brief Stores incoming RFStatus block + */ + RfStatusMsg last_rf_status_; + //! When reading from an SBF file, the ROS publishing frequency is governed //! by the time stamps found in the SBF blocks therein. Timestamp unix_time_; @@ -326,9 +337,8 @@ namespace io { /** * @brief "Callback" function when constructing * OSNMA DiagnosticArrayMsg messages - * @param[in] status GalAuthStatusMsg from which the msg was assembled */ - void assembleOsnmaDiagnosticArray(const GalAuthStatusMsg& status); + void assembleOsnmaDiagnosticArray(); /** * @brief "Callback" function when constructing diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 2cacd85c..4616c9c8 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -550,7 +550,7 @@ void MeasEpochChannelType2Parser(It& it, MeasEpochChannelType2Msg& msg, }; /** - * @class MeasEpochChannelType1Parser + * MeasEpochChannelType1Parser * @brief Qi based parser for the SBF sub-block "MeasEpochChannelType1" */ template @@ -587,7 +587,7 @@ template }; /** - * @class MeasEpoch + * MeasEpochParser * @brief Qi based parser for the SBF block "MeasEpoch" */ template @@ -631,7 +631,7 @@ template }; /** - * @class GALAuthStatus + * GALAuthStatus * @brief Qi based parser for the SBF block "GALAuthStatus" */ template @@ -660,6 +660,52 @@ template return true; }; +/** + * RFBandParser + * @brief Qi based parser for the SBF sub-block "RFBand" + */ +template +void RfBandParser(It& it, RfBandMsg& msg, uint8_t sb_length) +{ + qiLittleEndianParser(it, msg.frequency); + qiLittleEndianParser(it, msg.bandwidth); + qiLittleEndianParser(it, msg.info); + std::advance(it, sb_length - 7); // skip padding +}; + +/** + * RFStatusParser + * @brief Qi based parser for the SBF block "RFStatus" + */ +template +[[nodiscard]] bool RfStatusParser(ROSaicNodeBase* node, It it, It itEnd, + RfStatusMsg& msg) +{ + if (!BlockHeaderParser(node, it, msg.block_header)) + return false; + if (msg.block_header.id != 4092) + { + node->log(log_level::ERROR, "Parse error: Wrong header ID " + + std::to_string(msg.block_header.id)); + return false; + } + qiLittleEndianParser(it, msg.n); + qiLittleEndianParser(it, msg.sb_length); + qiLittleEndianParser(it, msg.flags); + std::advance(it, 3); // reserved + msg.rfband.resize(msg.n); + for (auto& rfband : msg.rfband) + { + RfBandParser(it, rfband, msg.sb_length); + } + if (it > itEnd) + { + node->log(log_level::ERROR, "Parse error: iterator past end."); + return false; + } + return true; +}; + /** * ReceiverSetupParser * @brief Qi based parser for the SBF block "ReceiverSetup" @@ -973,7 +1019,7 @@ void VectorInfoCartParser(It& it, VectorInfoCartMsg& msg, uint8_t sb_length) }; /** - * @class BaseVectorCart + * BaseVectorCartParser * @brief Qi based parser for the SBF block "BaseVectorCart" */ template @@ -1035,7 +1081,7 @@ void VectorInfoGeodParser(It& it, VectorInfoGeodMsg& msg, uint8_t sb_length) }; /** - * @class BaseVectorGeod + * BaseVectorGeodParser * @brief Qi based parser for the SBF block "BaseVectorGeod" */ template diff --git a/msg/RFBand.msg b/msg/RFBand.msg new file mode 100644 index 00000000..69c977e6 --- /dev/null +++ b/msg/RFBand.msg @@ -0,0 +1,5 @@ +# RFband block + +uint32 frequency # Hz +uint16 bandwidth # kHz +uint8 info \ No newline at end of file diff --git a/msg/RFStatus.msg b/msg/RFStatus.msg new file mode 100644 index 00000000..ff7853fc --- /dev/null +++ b/msg/RFStatus.msg @@ -0,0 +1,12 @@ +# RFStatus block +# ROS message header +std_msgs/Header header + +# SBF block header including time header +BlockHeader block_header + +uint8 n +uint8 sb_length +uint8 flags +RFBand[] rfband + diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index b29ee27e..d66d383a 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -701,7 +701,7 @@ namespace io { if (settings_->osnma.mode == "loose" || settings_->osnma.mode == "strict") { - blocks << " +GALAuthStatus"; + blocks << " +GALAuthStatus +RFStatus"; } blocks << " +ReceiverSetup"; diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index b9b0519a..9b63e5e3 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -344,53 +344,112 @@ namespace io { publish("/diagnostics", msg); }; - void MessageHandler::assembleOsnmaDiagnosticArray(const GalAuthStatusMsg& status) + void MessageHandler::assembleOsnmaDiagnosticArray() { + if (!validValue(last_gal_auth_status_.block_header.tow) || + (last_gal_auth_status_.block_header.tow != + last_rf_status_.block_header.tow)) + return; + DiagnosticArrayMsg msg; - DiagnosticStatusMsg diag; + DiagnosticStatusMsg diagRf; + diagRf.hardware_id = "Septentrio"; + diagRf.name = "RF"; + diagRf.message = "Current status of the radio-frequency (RF) signal"; + + diagRf.values.resize(2); + diagRf.values[0].key = "interference"; + diagRf.values[0].value = "spectrum clean"; + bool mitigated = false; + bool detected = false; + for (auto rfband : last_rf_status_.rfband) + { + std::bitset<8> info = rfband.info; + if (info.test(1)) + mitigated = true; + if (info.test(3)) + { + detected = true; + break; + } + } + if (detected) + diagRf.values[0].value = "detected"; + else if (mitigated) + diagRf.values[0].value = "mitigated"; + + diagRf.values[1].key = "spoofing"; + bool spoofed = false; + std::bitset<8> flags = last_rf_status_.flags; + if (flags.test(0) && flags.test(1)) + { + diagRf.values[1].value = "detected by OSNMA and authenticity test"; + spoofed = true; + } else if (flags.test(0)) + { + diagRf.values[1].value = "detected by authenticity test"; + spoofed = true; + } else if (flags.test(1)) + { + diagRf.values[1].value = "detected by OSNMA"; + spoofed = true; + } else + diagRf.values[1].value = "none detected"; + + if (spoofed || detected) + diagRf.level = DiagnosticStatusMsg::ERROR; + else if (mitigated) + diagRf.level = DiagnosticStatusMsg::WARN; + else + diagRf.level = DiagnosticStatusMsg::OK; - diag.hardware_id = "Septentrio"; - diag.name = "OSNMA"; - diag.message = "Current status of the OSNMA authentication"; + msg.status.push_back(diagRf); - diag.values.resize(6); - diag.values[0].key = "status"; - switch (status.osnma_status & 7) + DiagnosticStatusMsg diagOsnma; + + diagOsnma.hardware_id = "Septentrio"; + diagOsnma.name = "OSNMA"; + diagOsnma.message = "Current status of the OSNMA authentication"; + + diagOsnma.values.resize(6); + diagOsnma.values[0].key = "status"; + switch (last_gal_auth_status_.osnma_status & 7) { case 0: { - diag.values[0].value = "Disabled"; + diagOsnma.values[0].value = "Disabled"; break; } case 1: { - uint16_t percent = (status.osnma_status >> 3) & 127; - diag.values[0].value = "Initializing " + std::to_string(percent) + " %"; + uint16_t percent = (last_gal_auth_status_.osnma_status >> 3) & 127; + diagOsnma.values[0].value = + "Initializing " + std::to_string(percent) + " %"; break; } case 2: { - diag.values[0].value = "Waiting on NTP"; + diagOsnma.values[0].value = "Waiting on NTP"; break; } case 3: { - diag.values[0].value = "Init failed - inconsistent time"; + diagOsnma.values[0].value = "Init failed - inconsistent time"; break; } case 4: { - diag.values[0].value = "Init failed - KROOT signature invalid"; + diagOsnma.values[0].value = "Init failed - KROOT signature invalid"; break; } case 5: { - diag.values[0].value = "Init failed - invalid param received"; + diagOsnma.values[0].value = "Init failed - invalid param received"; break; } case 6: { - diag.values[0].value = "Authenticating"; + diagOsnma.values[0].value = "Authenticating"; break; } @@ -398,39 +457,41 @@ namespace io { break; } - diag.values[1].key = "trusted_time_delta"; - if (validValue(status.trusted_time_delta)) - diag.values[1].value = std::to_string(status.trusted_time_delta); + diagOsnma.values[1].key = "trusted_time_delta"; + if (validValue(last_gal_auth_status_.trusted_time_delta)) + diagOsnma.values[1].value = + std::to_string(last_gal_auth_status_.trusted_time_delta); else - diag.values[1].value = "N/A"; + diagOsnma.values[1].value = "N/A"; - std::bitset<64> gal_active = status.gal_active_mask; - std::bitset<64> gal_auth = status.gal_authentic_mask; + std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask; + std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask; uint8_t gal_authentic = (gal_auth & gal_active).count(); uint8_t gal_spoofed = (~gal_auth & gal_active).count(); - diag.values[2].key = "Galileo authentic"; - diag.values[2].value = std::to_string(gal_authentic); - diag.values[3].key = "Galileo spoofed"; - diag.values[3].value = std::to_string(gal_spoofed); + diagOsnma.values[2].key = "Galileo authentic"; + diagOsnma.values[2].value = std::to_string(gal_authentic); + diagOsnma.values[3].key = "Galileo spoofed"; + diagOsnma.values[3].value = std::to_string(gal_spoofed); - std::bitset<64> gps_active = status.gps_active_mask; - std::bitset<64> gps_auth = status.gps_authentic_mask; + std::bitset<64> gps_active = last_gal_auth_status_.gps_active_mask; + std::bitset<64> gps_auth = last_gal_auth_status_.gps_authentic_mask; uint8_t gps_authentic = (gps_auth & gps_active).count(); uint8_t gps_spoofed = (~gps_auth & gps_active).count(); - diag.values[4].key = "GPS authentic"; - diag.values[4].value = std::to_string(gps_authentic); - diag.values[5].key = "GPS spoofed"; - diag.values[5].value = std::to_string(gps_spoofed); + diagOsnma.values[4].key = "GPS authentic"; + diagOsnma.values[4].value = std::to_string(gps_authentic); + diagOsnma.values[5].key = "GPS spoofed"; + diagOsnma.values[5].value = std::to_string(gps_spoofed); if ((gal_spoofed + gps_spoofed) == 0) - diag.level = DiagnosticStatusMsg::OK; + diagOsnma.level = DiagnosticStatusMsg::OK; else if ((gal_authentic + gps_authentic) > 0) - diag.level = DiagnosticStatusMsg::WARN; + diagOsnma.level = DiagnosticStatusMsg::WARN; else - diag.level = DiagnosticStatusMsg::ERROR; + diagOsnma.level = DiagnosticStatusMsg::ERROR; + + msg.status.push_back(diagOsnma); + msg.header = last_gal_auth_status_.header; - msg.status.push_back(diag); - msg.header = status.header; publish("/diagnostics", msg); } @@ -2194,17 +2255,28 @@ namespace io { } case GAL_AUTH_STATUS: { - GalAuthStatusMsg msg; - if (!GalAuthStatusParser(node_, telegram->message.begin(), - telegram->message.end(), msg)) + telegram->message.end(), last_gal_auth_status_)) { node_->log(log_level::ERROR, "parse error in GalAuthStatus"); break; } - assembleHeader(settings_->frame_id, telegram, msg); - publish("/galauthstatus", msg); - assembleOsnmaDiagnosticArray(msg); + assembleHeader(settings_->frame_id, telegram, last_gal_auth_status_); + publish("/galauthstatus", last_gal_auth_status_); + assembleOsnmaDiagnosticArray(); + break; + } + case RF_STATUS: + { + if (!RfStatusParser(node_, telegram->message.begin(), + telegram->message.end(), last_rf_status_)) + { + node_->log(log_level::ERROR, "parse error inRfStatus"); + break; + } + assembleHeader(settings_->frame_id, telegram, last_rf_status_); + publish("/rfstatus", last_rf_status_); + assembleOsnmaDiagnosticArray(); break; } case INS_NAV_CART: // Position, velocity and orientation in cartesian From 616d1c29ef3555d461153b3fef7e60d0cda7ad09 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 10 Feb 2023 13:55:38 +0100 Subject: [PATCH 095/170] Robustify command reset --- .../communication/communication_core.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index d66d383a..94d0906c 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -258,8 +258,8 @@ namespace io { mainConnectionPort_ = resetMainConnection(); node_->log(log_level::INFO, "The connection descriptor is " + mainConnectionPort_); - // streamPort_ = "IPS1"; // TODO UDP streamPort_ = mainConnectionPort_; + // streamPort_ = "IPS1"; // TODO UDP node_->log(log_level::INFO, "Setting up Rx."); @@ -903,10 +903,13 @@ namespace io { std::string CommunicationCore::resetMainConnection() { // Escape sequence (escape from correction mode), ensuring that we - // can send our real commands afterwards... + // can send our real commands afterwards... has to be sent twice. std::string cmd("\x0DSSSSSSSSSS\x0D\x0D"); telegramHandler_.resetWaitforMainCd(); manager_.get()->send(cmd); + std::ignore = telegramHandler_.getMainCd(); + telegramHandler_.resetWaitforMainCd(); + manager_.get()->send(cmd); return telegramHandler_.getMainCd(); } From ce7410b3abd8dc28305d219b8331e7ca03680305 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 10 Feb 2023 15:51:00 +0100 Subject: [PATCH 096/170] Catch invalid UTM conversion --- .../communication/message_handler.cpp | 43 +++++++++++++------ 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 9b63e5e3..272ce271 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -850,22 +850,39 @@ namespace io { double meridian_convergence = 0.0; if (fixedUtmZone_) { - double k; - GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, - northernHemisphere); - GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), - rad2deg(last_insnavgeod_.longitude), zone, - northernHemisphere, easting, northing, - meridian_convergence, k, zone); + try + { + GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, + northernHemisphere); + double k; + GeographicLib::UTMUPS::Forward( + rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), zone, northernHemisphere, + easting, northing, meridian_convergence, k, zone); + } catch (const std::exception& e) + { + node_->log(log_level::DEBUG, + "UTMUPS conversion exception: " + std::string(e.what())); + return; + } zonestring = *fixedUtmZone_; } else { - double k; - GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), - rad2deg(last_insnavgeod_.longitude), zone, - northernHemisphere, easting, northing, - meridian_convergence, k); - zonestring = GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); + try + { + double k; + GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude), + rad2deg(last_insnavgeod_.longitude), + zone, northernHemisphere, easting, + northing, meridian_convergence, k); + zonestring = + GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere); + } catch (const std::exception& e) + { + node_->log(log_level::DEBUG, + "UTMUPS conversion exception: " + std::string(e.what())); + return; + } } if (settings_->lock_utm_zone && !fixedUtmZone_) fixedUtmZone_ = std::make_shared(zonestring); From 531558ffdd55e87cb1cd57f102b90194016564d8 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 11 Feb 2023 00:38:07 +0100 Subject: [PATCH 097/170] Add custom message to report AIM+ status --- CMakeLists.txt | 1 + README.md | 8 +- config/gnss.yaml | 2 + config/ins.yaml | 2 + config/rover.yaml | 4 +- .../abstraction/typedefs.hpp | 2 + .../communication/message_handler.hpp | 11 ++ .../communication/settings.hpp | 5 + msg/AIMPlusStatus.msg | 27 +++ .../communication/communication_core.cpp | 11 +- .../communication/message_handler.cpp | 178 +++++++++++------- .../node/rosaic_node.cpp | 2 + 12 files changed, 181 insertions(+), 72 deletions(-) create mode 100644 msg/AIMPlusStatus.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 8548c271..20b40e67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,7 @@ endif () ## Generate messages in the 'msg' folder add_message_files( FILES + AIMPlusStatus.msg BaseVectorCart.msg BaseVectorGeod.msg BlockHeader.msg diff --git a/README.md b/README.md index 80568d0a..7aba2a88 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,7 @@ Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers - Supports serial, TCP/IP and USB connections, the latter being compatible with both serial (RNDIS) and TCP/IP protocols - Supports several ASCII (including key NMEA ones) messages and SBF (Septentrio Binary Format) blocks +- Reports status of AIM+ (Advanced Interference Mitigation including OSNMA) anti-jamming and anti-spoofing. - Can publish `nav_msgs/Odometry` message for INS receivers - Can blend SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic` and `DOP` in order to publish `gps_common/GPSFix` and `sensor_msgs/NavSatFix` messages - Supports axis convention conversion as Septentrio follows the NED convention, whereas ROS is ENU. @@ -222,6 +223,8 @@ configure_rx: true pose: false twist: false diagnostics: false + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false @@ -606,6 +609,8 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/gpgsa`: `true` to publish `nmea_msgs/GPGSA.msg` messages into the topic `/gpgsa` + `publish/gpgsv`: `true` to publish `nmea_msgs/GPGSV.msg` messages into the topic `/gpgsv` + `publish/measepoch`: `true` to publish `septentrio_gnss_driver/MeasEpoch.msg` messages into the topic `/measepoch` + + `publish/galauthstatus`: `true` to publish `septentrio_gnss_driver/GALAuthStatus.msg` messages into the topic `/galauthstatus` and corresponding `/diganostics` + + `publish/aimplusstatus`: `true` to publish `septentrio_gnss_driver/RFStatus.msg` messages into the topic `/rfstatus`, `septentrio_gnss_driver/AIMPlusStatus.msg` messages into `/aimplusstatus` and corresponding `/diganostics` + `publish/pvtcartesian`: `true` to publish `septentrio_gnss_driver/PVTCartesian.msg` messages into the topic `/pvtcartesian` + `publish/pvtgeodetic`: `true` to publish `septentrio_gnss_driver/PVTGeodetic.msg` messages into the topic `/pvtgeodetic` + `publish/basevectorcart`: `true` to publish `septentrio_gnss_driver/BaseVectorCart.msg` messages into the topic `/basevectorcart` @@ -647,7 +652,8 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/gpgsv`: publishes [`nmea_msgs/Gpgsv.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsv.html) - converted from the NMEA sentence GSV. + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. - + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, corresponding to the SBF block `RFStatus`. + + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, compiled from the SBF block `RFStatus`. + + `/aimplusstatus`: publishes custom ROS message `septentrio_gnss_driver/AIMPlusStatus.msg`, reporting status of AIM+. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. diff --git a/config/gnss.yaml b/config/gnss.yaml index e107c89d..61c33926 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -98,6 +98,8 @@ publish: pose: true twist: false diagnostics: true + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false diff --git a/config/ins.yaml b/config/ins.yaml index b7ac2415..748c26ec 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -110,6 +110,8 @@ publish: pose: false twist: true diagnostics: true + aimplusstatus: true + galauthstatus: false # For INS Rx only insnavcart: true insnavgeod: true diff --git a/config/rover.yaml b/config/rover.yaml index 983535d7..3285bd2e 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -15,7 +15,7 @@ login: password: "" osnma: - mode: "off" + mode: "loose" ntp_server: "" keep_open: true @@ -119,6 +119,8 @@ publish: pose: false twist: false diagnostics: false + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 6f6768d3..998c3e71 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -52,6 +52,7 @@ #include #include // GNSS msg includes +#include #include #include #include @@ -108,6 +109,7 @@ typedef sensor_msgs::Imu ImuMsg; typedef nav_msgs::Odometry LocalizationMsg; // Septentrio GNSS SBF messages +typedef septentrio_gnss_driver::AIMPlusStatus AimPlusStatusMsg; typedef septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg; typedef septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg; typedef septentrio_gnss_driver::BlockHeader BlockHeaderMsg; diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index a90c0708..4dd46f66 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -296,6 +296,11 @@ namespace io { */ GalAuthStatusMsg last_gal_auth_status_; + /** + * @brief Wether OSNMA info has been received + */ + bool osnma_info_available_ = false; + /** * @brief Stores incoming RFStatus block */ @@ -340,6 +345,12 @@ namespace io { */ void assembleOsnmaDiagnosticArray(); + /** + * @brief "Callback" function when constructing + * RFStatus DiagnosticArrayMsg messages + */ + void assembleAimAndDiagnosticArray(); + /** * @brief "Callback" function when constructing * ImuMsg messages diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index add89cdd..0d2eae13 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -223,6 +223,11 @@ struct Settings bool publish_gpgsv; //! Whether or not to publish the MeasEpoch message bool publish_measepoch; + //! Whether or not to publish the RFStatus and AIMPlusStatus message and + //! diagnostics + bool publish_aimplusstatus; + //! Whether or not to publish the GALAuthStatus message and diagnostics + bool publish_galauthstatus; //! Whether or not to publish the PVTCartesianMsg //! message bool publish_pvtcartesian; diff --git a/msg/AIMPlusStatus.msg b/msg/AIMPlusStatus.msg new file mode 100644 index 00000000..82ac3e6b --- /dev/null +++ b/msg/AIMPlusStatus.msg @@ -0,0 +1,27 @@ +# AIMPlusStatus message +# ROS message header +std_msgs/Header header + +uint32 tow # ms since week start +uint16 wnc # weeks since Jan 06, 1980 at 00:00:00 UTC + +uint8 interference +#------------------------------- +uint8 SPECTRUM_CLEAN = 0 +uint8 INTERFERENCE_MITIGATED = 1 +uint8 INTERFERENCE_PRESENT = 2 +#------------------------------- + +uint8 spoofing +#-------------------------------------------------------- +uint8 NONE_DETECTED = 0 +uint8 SPOOFING_DETECTED_BY_OSNMA = 1 +uint8 SPOOFING_DETECTED_BY_AUTHENTCITY_TEST = 2 +uint8 SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST = 3 +#-------------------------------------------------------- + +bool osnma_info_available +uint8 galileo_authentic +uint8 galileo_spoofed +uint8 gps_authentic +uint8 gps_spoofed \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 94d0906c..68c433fb 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -698,10 +698,15 @@ namespace io { { blocks << " +ReceiverStatus +QualityInd"; } - if (settings_->osnma.mode == "loose" || - settings_->osnma.mode == "strict") + if (settings_->publish_aimplusstatus) { - blocks << " +GALAuthStatus +RFStatus"; + blocks << " +RFStatus"; + } + if (settings_->publish_galauthstatus || + (settings_->osnma.mode == "loose") || + (settings_->osnma.mode == "strict")) + { + blocks << " +GALAuthStatus"; } blocks << " +ReceiverSetup"; diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 272ce271..79a1b0ac 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -346,65 +346,7 @@ namespace io { void MessageHandler::assembleOsnmaDiagnosticArray() { - if (!validValue(last_gal_auth_status_.block_header.tow) || - (last_gal_auth_status_.block_header.tow != - last_rf_status_.block_header.tow)) - return; - DiagnosticArrayMsg msg; - DiagnosticStatusMsg diagRf; - diagRf.hardware_id = "Septentrio"; - diagRf.name = "RF"; - diagRf.message = "Current status of the radio-frequency (RF) signal"; - - diagRf.values.resize(2); - diagRf.values[0].key = "interference"; - diagRf.values[0].value = "spectrum clean"; - bool mitigated = false; - bool detected = false; - for (auto rfband : last_rf_status_.rfband) - { - std::bitset<8> info = rfband.info; - if (info.test(1)) - mitigated = true; - if (info.test(3)) - { - detected = true; - break; - } - } - if (detected) - diagRf.values[0].value = "detected"; - else if (mitigated) - diagRf.values[0].value = "mitigated"; - - diagRf.values[1].key = "spoofing"; - bool spoofed = false; - std::bitset<8> flags = last_rf_status_.flags; - if (flags.test(0) && flags.test(1)) - { - diagRf.values[1].value = "detected by OSNMA and authenticity test"; - spoofed = true; - } else if (flags.test(0)) - { - diagRf.values[1].value = "detected by authenticity test"; - spoofed = true; - } else if (flags.test(1)) - { - diagRf.values[1].value = "detected by OSNMA"; - spoofed = true; - } else - diagRf.values[1].value = "none detected"; - - if (spoofed || detected) - diagRf.level = DiagnosticStatusMsg::ERROR; - else if (mitigated) - diagRf.level = DiagnosticStatusMsg::WARN; - else - diagRf.level = DiagnosticStatusMsg::OK; - - msg.status.push_back(diagRf); - DiagnosticStatusMsg diagOsnma; diagOsnma.hardware_id = "Septentrio"; @@ -495,6 +437,106 @@ namespace io { publish("/diagnostics", msg); } + void MessageHandler::assembleAimAndDiagnosticArray() + { + AimPlusStatusMsg aimMsg; + DiagnosticArrayMsg msg; + DiagnosticStatusMsg diagRf; + diagRf.hardware_id = "Septentrio"; + diagRf.name = "RF"; + diagRf.message = "Current status of the radio-frequency (RF) signal"; + + diagRf.values.resize(2); + diagRf.values[0].key = "interference"; + bool mitigated = false; + bool detected = false; + for (auto rfband : last_rf_status_.rfband) + { + std::bitset<8> info = rfband.info; + if (info.test(1)) + mitigated = true; + if (info.test(3)) + { + detected = true; + break; + } + } + if (detected) + { + diagRf.values[0].value = "present"; + aimMsg.interference = AimPlusStatusMsg::INTERFERENCE_PRESENT; + } else if (mitigated) + { + diagRf.values[0].value = "mitigated"; + aimMsg.interference = AimPlusStatusMsg::INTERFERENCE_MITIGATED; + } else + { + diagRf.values[0].value = "spectrum clean"; + aimMsg.interference = AimPlusStatusMsg::SPECTRUM_CLEAN; + } + + diagRf.values[1].key = "spoofing"; + bool spoofed = false; + std::bitset<8> flags = last_rf_status_.flags; + if (flags.test(0) && flags.test(1)) + { + diagRf.values[1].value = "detected by OSNMA and authenticity test"; + aimMsg.spoofing = + AimPlusStatusMsg::SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST; + spoofed = true; + } else if (flags.test(0)) + { + diagRf.values[1].value = "detected by authenticity test"; + aimMsg.spoofing = + AimPlusStatusMsg::SPOOFING_DETECTED_BY_AUTHENTCITY_TEST; + spoofed = true; + } else if (flags.test(1)) + { + diagRf.values[1].value = "detected by OSNMA"; + aimMsg.spoofing = AimPlusStatusMsg::SPOOFING_DETECTED_BY_OSNMA; + spoofed = true; + } else + { + diagRf.values[1].value = "none detected"; + aimMsg.spoofing = AimPlusStatusMsg::NONE_DETECTED; + } + if (osnma_info_available_) + { + aimMsg.osnma_info_available = true; + std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask; + std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask; + aimMsg.galileo_authentic = (gal_auth & gal_active).count(); + aimMsg.galileo_spoofed = (~gal_auth & gal_active).count(); + std::bitset<64> gps_active = last_gal_auth_status_.gps_active_mask; + std::bitset<64> gps_auth = last_gal_auth_status_.gps_authentic_mask; + aimMsg.gps_authentic = (gps_auth & gps_active).count(); + aimMsg.gps_spoofed = (~gps_auth & gps_active).count(); + } else + { + aimMsg.osnma_info_available = false; + aimMsg.galileo_authentic = 0; + aimMsg.galileo_spoofed = 0; + aimMsg.gps_authentic = 0; + aimMsg.gps_spoofed = 0; + } + aimMsg.header = last_rf_status_.header; + aimMsg.tow = last_rf_status_.block_header.tow; + aimMsg.wnc = last_rf_status_.block_header.wnc; + publish("/aimplusstatus", aimMsg); + + if (spoofed || detected) + diagRf.level = DiagnosticStatusMsg::ERROR; + else if (mitigated) + diagRf.level = DiagnosticStatusMsg::WARN; + else + diagRf.level = DiagnosticStatusMsg::OK; + + msg.status.push_back(diagRf); + msg.header = last_rf_status_.header; + + publish("/diagnostics", msg); + } + void MessageHandler::assembleImu() { ImuMsg msg; @@ -2221,7 +2263,6 @@ namespace io { } case POS_COV_GEODETIC: { - if (!PosCovGeodeticParser(node_, telegram->message.begin(), telegram->message.end(), last_poscovgeodetic_)) { @@ -2238,7 +2279,6 @@ namespace io { } case ATT_EULER: { - if (!AttEulerParser(node_, telegram->message.begin(), telegram->message.end(), last_atteuler_, settings_->use_ros_axis_orientation)) @@ -2255,7 +2295,6 @@ namespace io { } case ATT_COV_EULER: { - if (!AttCovEulerParser(node_, telegram->message.begin(), telegram->message.end(), last_attcoveuler_, settings_->use_ros_axis_orientation)) @@ -2278,9 +2317,13 @@ namespace io { node_->log(log_level::ERROR, "parse error in GalAuthStatus"); break; } + osnma_info_available_ = true; assembleHeader(settings_->frame_id, telegram, last_gal_auth_status_); - publish("/galauthstatus", last_gal_auth_status_); - assembleOsnmaDiagnosticArray(); + if (settings_->publish_galauthstatus) + { + publish("/galauthstatus", last_gal_auth_status_); + assembleOsnmaDiagnosticArray(); + } break; } case RF_STATUS: @@ -2292,14 +2335,16 @@ namespace io { break; } assembleHeader(settings_->frame_id, telegram, last_rf_status_); - publish("/rfstatus", last_rf_status_); - assembleOsnmaDiagnosticArray(); + if (settings_->publish_aimplusstatus) + { + publish("/rfstatus", last_rf_status_); + assembleAimAndDiagnosticArray(); + } break; } case INS_NAV_CART: // Position, velocity and orientation in cartesian // coordinate frame (ENU frame) { - if (!INSNavCartParser(node_, telegram->message.begin(), telegram->message.end(), last_insnavcart_, settings_->use_ros_axis_orientation)) @@ -2323,7 +2368,6 @@ namespace io { case INS_NAV_GEOD: // Position, velocity and orientation in geodetic // coordinate frame (ENU frame) { - if (!INSNavGeodParser(node_, telegram->message.begin(), telegram->message.end(), last_insnavgeod_, settings_->use_ros_axis_orientation)) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 5eb502cb..96c426a3 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -147,6 +147,8 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("publish/gpsfix", settings_.publish_gpsfix, false); param("publish/pose", settings_.publish_pose, false); param("publish/diagnostics", settings_.publish_diagnostics, false); + param("publish/aimplusstatus", settings_.publish_aimplusstatus, false); + param("publish/galauthstatus", settings_.publish_galauthstatus, false); param("publish/gpgga", settings_.publish_gpgga, false); param("publish/gprmc", settings_.publish_gprmc, false); param("publish/gpgsa", settings_.publish_gpgsa, false); From bf8c801d0019cc5cd42e9653e3717aa30cc1e4bd Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 11 Feb 2023 22:15:48 +0100 Subject: [PATCH 098/170] Rename msg var --- msg/AIMPlusStatus.msg | 2 +- src/septentrio_gnss_driver/communication/message_handler.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/msg/AIMPlusStatus.msg b/msg/AIMPlusStatus.msg index 82ac3e6b..f6b3ae70 100644 --- a/msg/AIMPlusStatus.msg +++ b/msg/AIMPlusStatus.msg @@ -20,7 +20,7 @@ uint8 SPOOFING_DETECTED_BY_AUTHENTCITY_TEST = 2 uint8 SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST = 3 #-------------------------------------------------------- -bool osnma_info_available +bool osnma_authenticating uint8 galileo_authentic uint8 galileo_spoofed uint8 gps_authentic diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 79a1b0ac..3b8d678c 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -502,7 +502,8 @@ namespace io { } if (osnma_info_available_) { - aimMsg.osnma_info_available = true; + aimMsg.osnma_authenticating = + ((last_gal_auth_status_.osnma_status & 7) == 6); std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask; std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask; aimMsg.galileo_authentic = (gal_auth & gal_active).count(); @@ -513,7 +514,7 @@ namespace io { aimMsg.gps_spoofed = (~gps_auth & gps_active).count(); } else { - aimMsg.osnma_info_available = false; + aimMsg.osnma_authenticating = false; aimMsg.galileo_authentic = 0; aimMsg.galileo_spoofed = 0; aimMsg.gps_authentic = 0; From 06aa4d3b98bdb655c2042f79148fd7db0cce0d91 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 11 Feb 2023 22:31:21 +0100 Subject: [PATCH 099/170] Reformulate readme about ROS and ROS2 --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7aba2a88..1808b228 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ## Overview -This repository hosts a ROS Melodic and Noetic driver (i.e. for Linux only) - written in C++ - that works with mosaic and AsteRx - two of Septentrio's cutting-edge GNSS/INS receiver families - and beyond. Since Noetic will only be supported until 2025, a ROS2 version is available in the branch `ros2`. +This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with mosaic and AsteRx - two of Septentrio's cutting-edge GNSS/INS receiver families - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers From 60c834afa8a7e3259487d237857bd7742e94cc29 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 11 Feb 2023 23:51:21 +0100 Subject: [PATCH 100/170] Expand readme on AIM+ --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1808b228..a2d9a978 100644 --- a/README.md +++ b/README.md @@ -610,7 +610,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/gpgsv`: `true` to publish `nmea_msgs/GPGSV.msg` messages into the topic `/gpgsv` + `publish/measepoch`: `true` to publish `septentrio_gnss_driver/MeasEpoch.msg` messages into the topic `/measepoch` + `publish/galauthstatus`: `true` to publish `septentrio_gnss_driver/GALAuthStatus.msg` messages into the topic `/galauthstatus` and corresponding `/diganostics` - + `publish/aimplusstatus`: `true` to publish `septentrio_gnss_driver/RFStatus.msg` messages into the topic `/rfstatus`, `septentrio_gnss_driver/AIMPlusStatus.msg` messages into `/aimplusstatus` and corresponding `/diganostics` + + `publish/aimplusstatus`: `true` to publish `septentrio_gnss_driver/RFStatus.msg` messages into the topic `/rfstatus`, `septentrio_gnss_driver/AIMPlusStatus.msg` messages into `/aimplusstatus` and corresponding `/diganostics`. Some information is only available with active OSNMA. + `publish/pvtcartesian`: `true` to publish `septentrio_gnss_driver/PVTCartesian.msg` messages into the topic `/pvtcartesian` + `publish/pvtgeodetic`: `true` to publish `septentrio_gnss_driver/PVTGeodetic.msg` messages into the topic `/pvtgeodetic` + `publish/basevectorcart`: `true` to publish `septentrio_gnss_driver/BaseVectorCart.msg` messages into the topic `/basevectorcart` @@ -653,7 +653,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, compiled from the SBF block `RFStatus`. - + `/aimplusstatus`: publishes custom ROS message `septentrio_gnss_driver/AIMPlusStatus.msg`, reporting status of AIM+. + + `/aimplusstatus`: publishes custom ROS message `septentrio_gnss_driver/AIMPlusStatus.msg`, reporting status of AIM+. Converted from SBF blocks `RFStatus` and optionally `GALAuthStatus`. For the latter OSNMA has to be activated. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. From 1ebf9211ab46d04b6d9bc46d23d586fa738e48d1 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 16 Feb 2023 15:40:55 +0100 Subject: [PATCH 101/170] Change diagnostics naming scheme --- .../communication/message_handler.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 3b8d678c..8ca0d6e9 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -320,8 +320,8 @@ namespace io { (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); } } - gnss_status.hardware_id = "Septentrio"; - gnss_status.name = "serial number " + serialnumber; + gnss_status.hardware_id = "serial number " + serialnumber; + gnss_status.name = "Septentrio"; gnss_status.message = "GNSS quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; msg.status.push_back(gnss_status); @@ -349,8 +349,8 @@ namespace io { DiagnosticArrayMsg msg; DiagnosticStatusMsg diagOsnma; - diagOsnma.hardware_id = "Septentrio"; - diagOsnma.name = "OSNMA"; + diagOsnma.hardware_id = ""; + diagOsnma.name = "Septentrio"; diagOsnma.message = "Current status of the OSNMA authentication"; diagOsnma.values.resize(6); @@ -442,8 +442,8 @@ namespace io { AimPlusStatusMsg aimMsg; DiagnosticArrayMsg msg; DiagnosticStatusMsg diagRf; - diagRf.hardware_id = "Septentrio"; - diagRf.name = "RF"; + diagRf.hardware_id = ""; + diagRf.name = "Septentrio"; diagRf.message = "Current status of the radio-frequency (RF) signal"; diagRf.values.resize(2); From 05d91291713b5c1f3c031b6bfea9640896c8f0d4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 16 Feb 2023 23:10:21 +0100 Subject: [PATCH 102/170] Refine diagnostics naming scheme and add trigger to ensure emission of ReceiverSetup --- .../communication/communication_core.cpp | 3 +++ .../communication/message_handler.cpp | 15 ++++++++------- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 68c433fb..7fe43006 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -718,6 +718,9 @@ namespace io { ++stream; } + // send command to trigger emission of receiver setup + send("sop, TestObserver, TestAgency\x0D"); + // Setting up NMEA streams { if (settings_->septentrio_receiver_type == "ins") diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 8ca0d6e9..480d40aa 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -320,8 +320,8 @@ namespace io { (last_qualityind_.indicators[i] & indicators_value_mask) >> 8); } } - gnss_status.hardware_id = "serial number " + serialnumber; - gnss_status.name = "Septentrio"; + gnss_status.hardware_id = serialnumber; + gnss_status.name = "septentrio_driver: Quality indicators"; gnss_status.message = "GNSS quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)"; msg.status.push_back(gnss_status); @@ -349,8 +349,8 @@ namespace io { DiagnosticArrayMsg msg; DiagnosticStatusMsg diagOsnma; - diagOsnma.hardware_id = ""; - diagOsnma.name = "Septentrio"; + diagOsnma.hardware_id = last_receiversetup_.rx_serial_number; + diagOsnma.name = "septentrio_driver: OSNMA"; diagOsnma.message = "Current status of the OSNMA authentication"; diagOsnma.values.resize(6); @@ -442,9 +442,10 @@ namespace io { AimPlusStatusMsg aimMsg; DiagnosticArrayMsg msg; DiagnosticStatusMsg diagRf; - diagRf.hardware_id = ""; - diagRf.name = "Septentrio"; - diagRf.message = "Current status of the radio-frequency (RF) signal"; + diagRf.hardware_id = last_receiversetup_.rx_serial_number; + diagRf.name = "septentrio_driver: AIM+ status"; + diagRf.message = + "Current status of the AIM+ interference and spoofing mitigation"; diagRf.values.resize(2); diagRf.values[0].key = "interference"; From 6400256c086ad3fc46b33ffd2c2f01c8c9471c13 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 16 Feb 2023 23:22:12 +0100 Subject: [PATCH 103/170] Change to empty fields --- src/septentrio_gnss_driver/communication/communication_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 7fe43006..f04a530b 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -719,7 +719,7 @@ namespace io { } // send command to trigger emission of receiver setup - send("sop, TestObserver, TestAgency\x0D"); + send("sop, \"\", \"\" \x0D"); // Setting up NMEA streams { From 41ba2b4ee8457a8f7ab2186f0beb109de4a16f84 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 14 Mar 2023 21:41:28 +0100 Subject: [PATCH 104/170] Always publish raw IMU data as indicated --- .../septentrio_gnss_driver/parsers/sbf_blocks.hpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp index 4616c9c8..29d4693d 100644 --- a/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp +++ b/include/septentrio_gnss_driver/parsers/sbf_blocks.hpp @@ -1807,13 +1807,6 @@ ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg qiLittleEndianParser(it, msg.acceleration_x); qiLittleEndianParser(it, msg.acceleration_y); qiLittleEndianParser(it, msg.acceleration_z); - if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi - { - if (validValue(msg.acceleration_y)) - msg.acceleration_y = -msg.acceleration_y; - if (validValue(msg.acceleration_z)) - msg.acceleration_z = -msg.acceleration_z; - } hasAcc = true; break; } @@ -1822,13 +1815,6 @@ ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg qiLittleEndianParser(it, msg.angular_rate_x); qiLittleEndianParser(it, msg.angular_rate_y); qiLittleEndianParser(it, msg.angular_rate_z); - if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi - { - if (validValue(msg.angular_rate_y)) - msg.angular_rate_y = -msg.angular_rate_y; - if (validValue(msg.angular_rate_z)) - msg.angular_rate_z = -msg.angular_rate_z; - } hasOmega = true; break; } From 6f60d1c5b3a22fa4997cfb101e744a9fe2791d33 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 15 Mar 2023 19:35:26 +0100 Subject: [PATCH 105/170] Change VSM to be averaged and published with 2 Hz --- .../abstraction/typedefs.hpp | 180 ++++++++++-------- 1 file changed, 103 insertions(+), 77 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 998c3e71..5e711f66 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -388,93 +388,119 @@ class ROSaicNodeBase void processTwist(Timestamp stamp, const geometry_msgs::TwistWithCovariance& twist) { - time_t epochSeconds = stamp / 1000000000; - struct tm* tm_temp = std::gmtime(&epochSeconds); - std::stringstream timeUtc; - timeUtc << std::setfill('0') << std::setw(2) - << std::to_string(tm_temp->tm_hour) << std::setw(2) - << std::to_string(tm_temp->tm_min) << std::setw(2) - << std::to_string(tm_temp->tm_sec) << "." << std::setw(3) - << std::to_string((stamp - (stamp / 1000000000) * 1000000000) / - 1000000); - - std::string v_x; - std::string v_y; - std::string v_z; - std::string std_x; - std::string std_y; - std::string std_z; - if (settings_.ins_vsm_ros_config[0]) + // in case stamp was not set + if (stamp == 0) + stamp = getTime(); + + static Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + static Eigen::Vector3d var = Eigen::Vector3d::Zero(); + static uint64_t ctr = 0; + static Timestamp lastStamp = 0; + + ++ctr; + vel[0] += twist.twist.linear.x; + vel[1] += twist.twist.linear.y; + vel[2] += twist.twist.linear.z; + var[0] += twist.covariance[0]; + var[1] += twist.covariance[7]; + var[2] += twist.covariance[14]; + + // Rx expects averaged velocity at a rate of 2 Hz + if ((stamp - lastStamp) >= 495000000) // allow for 5 ms jitter { - v_x = string_utilities::trimDecimalPlaces(twist.twist.linear.x); - if (settings_.ins_vsm_ros_variances_by_parameter) - std_x = string_utilities::trimDecimalPlaces( - settings_.ins_vsm_ros_variances[0]); - else if (twist.covariance[0] > 0.0) - std_x = string_utilities::trimDecimalPlaces( - std::sqrt(twist.covariance[0])); - else + vel /= ctr; + var /= ctr; + time_t epochSeconds = stamp / 1000000000; + struct tm* tm_temp = std::gmtime(&epochSeconds); + std::stringstream timeUtc; + timeUtc << std::setfill('0') << std::setw(2) + << std::to_string(tm_temp->tm_hour) << std::setw(2) + << std::to_string(tm_temp->tm_min) << std::setw(2) + << std::to_string(tm_temp->tm_sec) << "." << std::setw(3) + << std::to_string((stamp - (stamp / 1000000000) * 1000000000) / + 1000000); + + std::string v_x; + std::string v_y; + std::string v_z; + std::string std_x; + std::string std_y; + std::string std_z; + if (settings_.ins_vsm_ros_config[0]) { - ROS_ERROR_STREAM("Invalid covariance value for v_x: " - << std::to_string(twist.covariance[0]) - << ". Ignoring measurement."); - } - } else - std_x = std::to_string(1000000.0); - if (settings_.ins_vsm_ros_config[1]) - { - if (settings_.use_ros_axis_orientation) - v_y = "-"; - v_y += string_utilities::trimDecimalPlaces(twist.twist.linear.y); - if (settings_.ins_vsm_ros_variances_by_parameter) - std_y = string_utilities::trimDecimalPlaces( - settings_.ins_vsm_ros_variances[1]); - else if (twist.covariance[7] > 0.0) - std_y = string_utilities::trimDecimalPlaces( - std::sqrt(twist.covariance[7])); - else + v_x = string_utilities::trimDecimalPlaces(vel[0]); + if (settings_.ins_vsm_ros_variances_by_parameter) + std_x = string_utilities::trimDecimalPlaces( + settings_.ins_vsm_ros_variances[0]); + else if (var[0] > 0.0) + std_x = string_utilities::trimDecimalPlaces(std::sqrt(var[0])); + else + { + log(log_level::ERROR, "Invalid covariance value for v_x: " + + std::to_string(var[0]) + + ". Ignoring measurement."); + } + } else + std_x = std::to_string(1000000.0); + if (settings_.ins_vsm_ros_config[1]) { - ROS_ERROR_STREAM("Invalid covariance value for v_y: " - << std::to_string(twist.covariance[1]) - << ". Ignoring measurement."); - v_y = ""; + if (settings_.use_ros_axis_orientation) + v_y = "-"; + v_y += string_utilities::trimDecimalPlaces(vel[1]); + if (settings_.ins_vsm_ros_variances_by_parameter) + std_y = string_utilities::trimDecimalPlaces( + settings_.ins_vsm_ros_variances[1]); + else if (var[1] > 0.0) + std_y = string_utilities::trimDecimalPlaces(std::sqrt(var[1])); + else + { + log(log_level::ERROR, "Invalid covariance value for v_y: " + + std::to_string(var[1]) + + ". Ignoring measurement."); + v_y = ""; + std_y = string_utilities::trimDecimalPlaces(1000000.0); + } + } else std_y = string_utilities::trimDecimalPlaces(1000000.0); - } - } else - std_y = string_utilities::trimDecimalPlaces(1000000.0); - if (settings_.ins_vsm_ros_config[2]) - { - if (settings_.use_ros_axis_orientation) - v_z = "-"; - v_z += string_utilities::trimDecimalPlaces(twist.twist.linear.z); - if (settings_.ins_vsm_ros_variances_by_parameter) - std_z = string_utilities::trimDecimalPlaces( - settings_.ins_vsm_ros_variances[2]); - else if (twist.covariance[14] > 0.0) - std_z = string_utilities::trimDecimalPlaces( - std::sqrt(twist.covariance[14])); - else + if (settings_.ins_vsm_ros_config[2]) { - ROS_ERROR_STREAM("Invalid covariance value for v_z: " - << std::to_string(twist.covariance[2]) - << ". Ignoring measurement."); - v_z = ""; + if (settings_.use_ros_axis_orientation) + v_z = "-"; + v_z += string_utilities::trimDecimalPlaces(vel[2]); + if (settings_.ins_vsm_ros_variances_by_parameter) + std_z = string_utilities::trimDecimalPlaces( + settings_.ins_vsm_ros_variances[2]); + else if (var[2] > 0.0) + std_z = string_utilities::trimDecimalPlaces(std::sqrt(var[2])); + else + { + log(log_level::ERROR, "Invalid covariance value for v_z: " + + std::to_string(var[2]) + + ". Ignoring measurement."); + v_z = ""; + std_z = string_utilities::trimDecimalPlaces(1000000.0); + } + } else std_z = string_utilities::trimDecimalPlaces(1000000.0); - } - } else - std_z = string_utilities::trimDecimalPlaces(1000000.0); - std::string velNmea = "$PSSN,VSM," + timeUtc.str() + "," + v_x + "," + v_y + - "," + std_x + "," + std_y + "," + v_z + "," + std_z; + std::string velNmea = "$PSSN,VSM," + timeUtc.str() + "," + v_x + "," + + v_y + "," + std_x + "," + std_y + "," + v_z + "," + + std_z; - char crc = std::accumulate(velNmea.begin() + 1, velNmea.end(), 0, - [](char sum, char ch) { return sum ^ ch; }); + char crc = std::accumulate(velNmea.begin() + 1, velNmea.end(), 0, + [](char sum, char ch) { return sum ^ ch; }); - std::stringstream crcss; - crcss << std::hex << static_cast(crc); + std::stringstream crcss; + crcss << std::hex << static_cast(crc); - velNmea += "*" + crcss.str() + "\r\n"; - sendVelocity(velNmea); + velNmea += "*" + crcss.str() + "\r\n"; + sendVelocity(velNmea); + + vel = Eigen::Vector3d::Zero(); + var = Eigen::Vector3d::Zero(); + ctr = 0; + lastStamp = stamp; + } } protected: From d7a052431f48df596ccb1c2e67a201e9408dfddf Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 30 Mar 2023 12:44:15 +0200 Subject: [PATCH 106/170] Fix localization stamp and tf publishing --- .../abstraction/typedefs.hpp | 5 ++++- .../communication/message_handler.cpp | 16 ++++++++++++---- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 5e711f66..abb8c191 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -184,7 +184,10 @@ namespace log_level { class ROSaicNodeBase { public: - ROSaicNodeBase() : pNh_(new ros::NodeHandle("~")), tfListener_(tfBuffer_) {} + ROSaicNodeBase() : + pNh_(new ros::NodeHandle("~")), tfListener_(tfBuffer_), lastTfStamp_(0) + { + } virtual ~ROSaicNodeBase() {} diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 480d40aa..c52cfbf7 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -881,7 +881,7 @@ namespace io { */ void MessageHandler::assembleLocalizationUtm() { - if (!settings_->publish_localization) + if (!settings_->publish_localization && !settings_->publish_tf) return; LocalizationMsg msg; @@ -945,6 +945,7 @@ namespace io { } msg.header.frame_id = "utm_" + zonestring; + msg.header.stamp = last_insnavgeod_.header.stamp; if (settings_->ins_use_poi) msg.child_frame_id = settings_->poi_frame_id; // TODO param else @@ -1071,7 +1072,10 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); - publish("/localization", msg); + if (settings_->publish_localization) + publish("/localization", msg); + if (settings_->publish_tf) + publishTf(msg); }; /** @@ -1081,7 +1085,7 @@ namespace io { */ void MessageHandler::assembleLocalizationEcef() { - if (!settings_->publish_localization_ecef) + if (!settings_->publish_localization_ecef && !settings_->publish_tf_ecef) return; if ((!validValue(last_insnavcart_.block_header.tow)) || @@ -1091,6 +1095,7 @@ namespace io { LocalizationMsg msg; msg.header.frame_id = "ecef"; + msg.header.stamp = last_insnavcart_.header.stamp; if (settings_->ins_use_poi) msg.child_frame_id = settings_->poi_frame_id; // TODO param else @@ -1236,7 +1241,10 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); - publish("/localization_ecef", msg); + if (settings_->publish_localization_ecef) + publish("/localization_ecef", msg); + if (settings_->publish_tf_ecef) + publishTf(msg); }; void MessageHandler::assembleLocalizationMsgTwist(double roll, double pitch, From 397ddab25935fea43a681504a9d49607d5c1631f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 31 Mar 2023 18:50:34 +0200 Subject: [PATCH 107/170] Fix multi msg per packet --- .../communication/io.hpp | 96 ++++++++++++------- .../communication/settings.hpp | 2 + .../communication/communication_core.cpp | 19 ++-- .../node/rosaic_node.cpp | 1 + 4 files changed, 75 insertions(+), 43 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 4c9753f5..644bd5f9 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -93,63 +93,74 @@ namespace io { } void asyncReceive() - { - std::shared_ptr telegram(new Telegram(MAX_SBF_SIZE)); + { socket_->async_receive_from( - boost::asio::buffer(telegram->message, MAX_SBF_SIZE), eP_, + boost::asio::buffer(buffer_, MAX_SBF_SIZE), eP_, boost::bind(&UdpClient::handleReceive, this, boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred, telegram)); + boost::asio::placeholders::bytes_transferred)); } void handleReceive(const boost::system::error_code& error, - size_t bytes_recvd, std::shared_ptr telegram) + size_t bytes_recvd) { - telegram->stamp = node_->getTime(); + Timestamp stamp = node_->getTime(); + size_t idx = 0; if (!error && (bytes_recvd > 0)) { - if (bytes_recvd > 2) + while ((bytes_recvd - idx) > 2) { - telegram->message.resize(bytes_recvd); + std::shared_ptr telegram(new Telegram); + telegram->stamp = stamp; /*node_->log(log_level::DEBUG, "Buffer: " + std::string(telegram->message.begin(), telegram->message.end()));*/ - if (telegram->message[0] == SYNC_BYTE_1) + if (buffer_[idx] == SYNC_BYTE_1) { - if (telegram->message[1] == SBF_SYNC_BYTE_2) + if (buffer_[idx + 1] == SBF_SYNC_BYTE_2) { - if (crc::isValid(telegram->message)) + if ((bytes_recvd - idx) > SBF_HEADER_SIZE) { - telegram->type = telegram_type::SBF; - telegramQueue_->push(telegram); - } else - node_->log( - log_level::DEBUG, - "AsyncManager crc failed for SBF " + - std::to_string(parsing_utilities::getId( - telegram->message)) + - "."); - } else if ((telegram->message[1] == NMEA_SYNC_BYTE_2) && - (telegram->message[2] == NMEA_SYNC_BYTE_3) && - (telegram->message[telegram->message.size() - - 2] == CR) && - (telegram->message[telegram->message.size() - - 1] == LF)) + uint16_t length = parsing_utilities::parseUInt16( + &buffer_[idx + 6]); + telegram->message.assign(&buffer_[idx], + &buffer_[idx + length]); + if (crc::isValid(telegram->message)) + { + telegram->type = telegram_type::SBF; + telegramQueue_->push(telegram); + } else + node_->log( + log_level::DEBUG, + "AsyncManager crc failed for SBF " + + std::to_string(parsing_utilities::getId( + telegram->message)) + + "."); + + idx += length; + } + + } else if ((buffer_[idx + 1] == NMEA_SYNC_BYTE_2) && + (buffer_[idx + 2] == NMEA_SYNC_BYTE_3)) { + size_t idx_end = findNmeaEnd(idx, bytes_recvd); + telegram->message.assign(&buffer_[idx], + &buffer_[idx_end + 1]); telegram->type = telegram_type::NMEA; telegramQueue_->push(telegram); + idx = idx_end + 1; - } else if ((telegram->message[1] == NMEA_INS_SYNC_BYTE_2) && - (telegram->message[2] == NMEA_INS_SYNC_BYTE_3) && - (telegram->message[telegram->message.size() - - 2] == CR) && - (telegram->message[telegram->message.size() - - 1] == LF)) + } else if ((buffer_[idx + 1] == NMEA_INS_SYNC_BYTE_2) && + (buffer_[idx + 2] == NMEA_INS_SYNC_BYTE_3)) { + size_t idx_end = findNmeaEnd(idx, bytes_recvd); + telegram->message.assign(&buffer_[idx], + &buffer_[idx_end + 1]); telegram->type = telegram_type::NMEA_INS; telegramQueue_->push(telegram); + idx = idx_end + 1; } else { node_->log(log_level::DEBUG, @@ -160,13 +171,10 @@ namespace io { } } else { - node_->log(log_level::DEBUG, - "head: " + std::string(std::string( - telegram->message.begin(), - telegram->message.begin() + 2))); + node_->log(log_level::DEBUG, "UDP msg resync."); + ++idx; } } - } else { node_->log(log_level::ERROR, @@ -200,6 +208,19 @@ namespace io { } private: + size_t findNmeaEnd(size_t idx, size_t bytes_recvd) + { + size_t idx_end = idx + 2; + + while (idx_end < bytes_recvd) + { + if ((buffer_[idx_end] == LF) && (buffer_[idx_end - 1] == CR)) + break; + + ++idx_end; + } + return idx_end; + } //! Pointer to the node ROSaicNodeBase* node_; std::atomic running_; @@ -209,6 +230,7 @@ namespace io { std::thread watchdogThread_; boost::asio::ip::udp::endpoint eP_; std::unique_ptr socket_; + std::array buffer_; TelegramQueue* telegramQueue_; }; diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 0d2eae13..2d41f911 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -132,6 +132,8 @@ struct Settings std::string tcp_ip; //! TCP port std::string tcp_port; + //! UDP port + uint32_t udp_port; //! Filename std::string file_name; //! Username for login diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index f04a530b..38d15b75 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -86,7 +86,10 @@ namespace io { !settings_->read_from_pcap) { resetMainConnection(); - send("sdio, " + streamPort_ + ", auto, none\x0D"); + send("sdio, " + mainConnectionPort_ + ", auto, none\x0D"); + // Turning off all current SBF/NMEA output + send("sso, all, none, none, off \x0D"); + send("sno, all, none, none, off \x0D"); for (auto ntrip : settings_->rtk.ntrip) { if (!ntrip.id.empty() && !ntrip.keep_open) @@ -102,6 +105,7 @@ namespace io { send("siss, " + ip_server.id + ", 0\x0D"); } } + send("siss, IPS1, 0\x0D"); // TODO UDP for (auto serial : settings_->rtk.serial) { if (!serial.port.empty() && !serial.keep_open) @@ -202,7 +206,7 @@ namespace io { case device_type::TCP: { manager_.reset(new AsyncManager(node_, &telegramQueue_)); - // udpClient_.reset(new UdpClient(node_, 28785, &telegramQueue_)); //TODO + udpClient_.reset(new UdpClient(node_, 28785, &telegramQueue_)); // TODO // UDP break; } @@ -259,7 +263,8 @@ namespace io { node_->log(log_level::INFO, "The connection descriptor is " + mainConnectionPort_); streamPort_ = mainConnectionPort_; - // streamPort_ = "IPS1"; // TODO UDP + streamPort_ = "IPS1"; // TODO UDP + send("siss, " + streamPort_ + ", 28785, UDP, 192.168.7.1\x0D"); // TODO UDP node_->log(log_level::INFO, "Setting up Rx."); @@ -678,8 +683,6 @@ namespace io { } } - // TODO UDP - // send("siss, IPS1, 28785, UDP, 10.255.255.200\x0D"); // Setting up SBF blocks with rx_period_rest { std::stringstream blocks; @@ -911,13 +914,16 @@ namespace io { std::string CommunicationCore::resetMainConnection() { // Escape sequence (escape from correction mode), ensuring that we - // can send our real commands afterwards... has to be sent twice. + // can send our real commands afterwards... has to be sent multiple times. std::string cmd("\x0DSSSSSSSSSS\x0D\x0D"); telegramHandler_.resetWaitforMainCd(); manager_.get()->send(cmd); std::ignore = telegramHandler_.getMainCd(); telegramHandler_.resetWaitforMainCd(); manager_.get()->send(cmd); + std::ignore = telegramHandler_.getMainCd(); + telegramHandler_.resetWaitforMainCd(); + manager_.get()->send(cmd); return telegramHandler_.getMainCd(); } @@ -927,6 +933,7 @@ namespace io { { std::shared_ptr telegram; telegramQueue_.pop(telegram); + if (telegram->type != telegram_type::EMPTY) telegramHandler_.handleTelegram(telegram); } diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 96c426a3..6e375fe9 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -110,6 +110,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) settings_.septentrio_receiver_type = "gnss"; settings_.ins_in_gnss_mode = true; } + settings_.udp_port = 28785; // Polling period parameters getUint32Param("polling_period/pvt", settings_.polling_period_pvt, From 022d1065609696084e6a19f99d76ec8a130a6f8f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 31 Mar 2023 23:09:05 +0200 Subject: [PATCH 108/170] Add UDP params and setup logic --- .../communication/settings.hpp | 4 +++ .../communication/communication_core.cpp | 34 +++++++++++++++---- .../node/rosaic_node.cpp | 4 ++- 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 2d41f911..38ea7731 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -134,6 +134,10 @@ struct Settings std::string tcp_port; //! UDP port uint32_t udp_port; + //! UDP unicast destination ip + std::string udp_unicast_ip; + //! UDP IP server id + std::string udp_ip_server; //! Filename std::string file_name; //! Username for login diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 38d15b75..e29a47d5 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -90,6 +90,10 @@ namespace io { // Turning off all current SBF/NMEA output send("sso, all, none, none, off \x0D"); send("sno, all, none, none, off \x0D"); + if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) + { + send("siss, " + settings_->udp_ip_server + ", 0\x0D"); + } for (auto ntrip : settings_->rtk.ntrip) { if (!ntrip.id.empty() && !ntrip.keep_open) @@ -105,7 +109,6 @@ namespace io { send("siss, " + ip_server.id + ", 0\x0D"); } } - send("siss, IPS1, 0\x0D"); // TODO UDP for (auto serial : settings_->rtk.serial) { if (!serial.port.empty() && !serial.keep_open) @@ -201,13 +204,17 @@ namespace io { [[nodiscard]] bool CommunicationCore::initializeIo() { node_->log(log_level::DEBUG, "Called initializeIo() method"); + if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) + { + udpClient_.reset( + new UdpClient(node_, settings_->udp_port, &telegramQueue_)); + } + switch (settings_->device_type) { case device_type::TCP: { manager_.reset(new AsyncManager(node_, &telegramQueue_)); - udpClient_.reset(new UdpClient(node_, 28785, &telegramQueue_)); // TODO - // UDP break; } case device_type::SERIAL: @@ -227,8 +234,11 @@ namespace io { } default: { - node_->log(log_level::DEBUG, "Unsupported device."); - return false; + if (settings_->configure_rx) + { + node_->log(log_level::DEBUG, "Unsupported device."); + return false; + } break; } } @@ -263,8 +273,18 @@ namespace io { node_->log(log_level::INFO, "The connection descriptor is " + mainConnectionPort_); streamPort_ = mainConnectionPort_; - streamPort_ = "IPS1"; // TODO UDP - send("siss, " + streamPort_ + ", 28785, UDP, 192.168.7.1\x0D"); // TODO UDP + if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) + { + streamPort_ = settings_->udp_ip_server; + std::string destination; + if (!settings_->udp_unicast_ip.empty()) + destination = settings_->udp_unicast_ip; + else + destination = "255.255.255.255"; + send("siss, " + streamPort_ + ", " + + std::to_string(settings_->udp_port) + ", UDP, " + destination + + "\x0D"); + } node_->log(log_level::INFO, "Setting up Rx."); diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 6e375fe9..75da5e42 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -90,6 +90,9 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) static_cast(921600)); param("serial/hw_flow_control", settings_.hw_flow_control, static_cast("off")); + getUint32Param("udp/port", settings_.udp_port, static_cast(0)); + param("udp/unicast_ip", settings_.udp_unicast_ip, static_cast("")); + param("udp/ip_server", settings_.udp_ip_server, static_cast("")); param("login/user", settings_.login_user, static_cast("")); param("login/password", settings_.login_password, static_cast("")); settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list. @@ -110,7 +113,6 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) settings_.septentrio_receiver_type = "gnss"; settings_.ins_in_gnss_mode = true; } - settings_.udp_port = 28785; // Polling period parameters getUint32Param("polling_period/pvt", settings_.polling_period_pvt, From 839591266c03e1086cbd0500096ee306a6487382 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 15 Apr 2023 23:01:36 +0200 Subject: [PATCH 109/170] Add device check logic --- .../communication/communication_core.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index e29a47d5..acdb4588 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -203,11 +203,13 @@ namespace io { [[nodiscard]] bool CommunicationCore::initializeIo() { + bool udp = false; node_->log(log_level::DEBUG, "Called initializeIo() method"); if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { udpClient_.reset( new UdpClient(node_, settings_->udp_port, &telegramQueue_)); + udp = true; } switch (settings_->device_type) @@ -234,7 +236,9 @@ namespace io { } default: { - if (settings_->configure_rx) + if (!udp || settings_->configure_rx || + (settings_->ins_vsm_ros_source == "odometry") || + (settings_->ins_vsm_ros_source == "twist")) { node_->log(log_level::DEBUG, "Unsupported device."); return false; From eb86b2895c9802059ed06fb36031e96f612517dd Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 15 Apr 2023 23:05:16 +0200 Subject: [PATCH 110/170] Add latency compensation to att msgs --- src/septentrio_gnss_driver/communication/message_handler.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index c52cfbf7..eeb29c48 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2074,6 +2074,8 @@ namespace io { std::is_same::value || std::is_same::value || std::is_same::value || + std::is_same::value || + std::is_same::value || std::is_same::value || std::is_same::value) { From fc39bb96224e9be31d337f2eebc2ead4dadb571d Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 15 Apr 2023 23:22:51 +0200 Subject: [PATCH 111/170] Add checks for IP server duplicates --- .../node/rosaic_node.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 75da5e42..383ef1e6 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -627,6 +627,34 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) " will be used."); registerSubscriber(); } + + if (settings_.udp_ip_server == settings_.ins_vsm_ip_server_id) + this->log( + log_level::ERROR, + "udp/ip_server and ins_vsm/ip_server/id cannot use the same IP server"); + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.udp_ip_server == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "udp/ip_server and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.ins_vsm_ip_server_id == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "ins_vsm/ip_server/id and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } + if (settings_.rtk.ip_server.size() == 2) + { + if (settings_.rtk.ip_server[0].id == settings_.rtk.ip_server[1].id) + this->log( + log_level::ERROR, + "rtk_settings/ip_server_1/id and rtk_settings/ip_server_2/id cannot use the same IP server"); + } } boost::smatch match; From 67fb0b7093206846876c8c40d5cc56f286ecd5c4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 15 Apr 2023 23:32:24 +0200 Subject: [PATCH 112/170] Add device check to node --- src/septentrio_gnss_driver/node/rosaic_node.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 383ef1e6..589fa61d 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -687,10 +687,15 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) settings_.device = proto; } else { - std::stringstream ss; - ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; - this->log(log_level::ERROR, ss.str()); - return false; + if (settings_.udp_ip_server.empty() || settings_.configure_rx || + (settings_.ins_vsm_ros_source == "odometry") || + (settings_.ins_vsm_ros_source == "twist")) + { + std::stringstream ss; + ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?"; + this->log(log_level::ERROR, ss.str()); + return false; + } } // To be implemented: RTCM, raw data settings, PPP, SBAS ... From bb653eb12ba99e80193e495914cb2c1060263a4b Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 15 Apr 2023 23:44:01 +0200 Subject: [PATCH 113/170] Update readme and changelog --- CHANGELOG.rst | 1 + README.md | 11 ++++++++++- config/gnss.yaml | 7 ++++++- config/ins.yaml | 7 ++++++- config/rover.yaml | 5 +++++ 5 files changed, 28 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 63958c74..f4371a17 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -10,6 +10,7 @@ Changelog for package septentrio_gnss_driver * OSNMA * Latency compensation for ROS timestamps * Output of SBf block VelCovCartesian + * Support for UDP * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index a2d9a978..5b0d2684 100644 --- a/README.md +++ b/README.md @@ -89,7 +89,12 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo baudrate: 921600 hw_flow_control: off -configure_rx: true + udp: + ip_server: "" + port: 0 + unicast_ip: "" + + configure_rx: true login: user: "" @@ -349,6 +354,10 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` + + `udp`: specifications for low latecy UDP reception of SBF blocks and NMEA sentences + + `ip_server`: IP server of Rx to be used. + + `port`: UDP destination port. + + `unicast_ip`: Set to computer's IP to use unicast. If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password diff --git a/config/gnss.yaml b/config/gnss.yaml index 61c33926..f2be3f49 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -8,6 +8,11 @@ serial: baudrate: 921600 hw_flow_control: "off" +udp: + ip_server: "" + port: 0 + unicast_ip: "" + configure_rx: true login: @@ -15,7 +20,7 @@ login: password: "" osnma: - mode: "off" + mode: "loose" ntp_server: "" keep_open: true diff --git a/config/ins.yaml b/config/ins.yaml index 748c26ec..91329252 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -8,6 +8,11 @@ serial: baudrate: 921600 hw_flow_control: "off" +udp: + ip_server: "" + port: 0 + unicast_ip: "" + configure_rx: true login: @@ -15,7 +20,7 @@ login: password: "" osnma: - mode: "off" + mode: "loose" ntp_server: "" keep_open: true diff --git a/config/rover.yaml b/config/rover.yaml index 3285bd2e..a73d70e4 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -8,6 +8,11 @@ serial: baudrate: 921600 hw_flow_control: off +udp: + ip_server: "" + port: 0 + unicast_ip: "" + configure_rx: true login: From 0142d043e8a5d65a150e0527565991691b126aa5 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 15 Apr 2023 23:53:45 +0200 Subject: [PATCH 114/170] Add const for max udp packet size --- README.md | 2 +- include/septentrio_gnss_driver/communication/io.hpp | 4 ++-- include/septentrio_gnss_driver/communication/telegram.hpp | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 5b0d2684..28721107 100644 --- a/README.md +++ b/README.md @@ -354,7 +354,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` - + `udp`: specifications for low latecy UDP reception of SBF blocks and NMEA sentences + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences + `ip_server`: IP server of Rx to be used. + `port`: UDP destination port. + `unicast_ip`: Set to computer's IP to use unicast. If not set multicast will be used. diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 644bd5f9..b107ae7c 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -96,7 +96,7 @@ namespace io { { socket_->async_receive_from( - boost::asio::buffer(buffer_, MAX_SBF_SIZE), eP_, + boost::asio::buffer(buffer_, MAX_UDP_PACKET_SIZE), eP_, boost::bind(&UdpClient::handleReceive, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred)); @@ -230,7 +230,7 @@ namespace io { std::thread watchdogThread_; boost::asio::ip::udp::endpoint eP_; std::unique_ptr socket_; - std::array buffer_; + std::array buffer_; TelegramQueue* telegramQueue_; }; diff --git a/include/septentrio_gnss_driver/communication/telegram.hpp b/include/septentrio_gnss_driver/communication/telegram.hpp index 8e6f3b09..56987e2d 100644 --- a/include/septentrio_gnss_driver/communication/telegram.hpp +++ b/include/septentrio_gnss_driver/communication/telegram.hpp @@ -85,6 +85,7 @@ static const uint8_t CONNECTION_DESCRIPTOR_FOOTER = 0x3E; static const uint16_t SBF_HEADER_SIZE = 8; static const uint16_t MAX_SBF_SIZE = 65535; +static const uint16_t MAX_UDP_PACKET_SIZE = 65535; namespace telegram_type { enum TelegramType From 032d8a538f1bc85107716609ea811b2c7366d1a8 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 16 Apr 2023 01:10:48 +0200 Subject: [PATCH 115/170] Add more publishing checks for configured Rx --- .../communication/message_handler.cpp | 241 ++++++++++-------- 1 file changed, 134 insertions(+), 107 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index eeb29c48..50b73d2d 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -56,6 +56,9 @@ using parsing_utilities::square; namespace io { void MessageHandler::assemblePoseWithCovarianceStamped() { + if (!settings_->publish_pose) + return; + static auto last_ins_tow = last_insnavgeod_.block_header.tow; PoseWithCovarianceStampedMsg msg; @@ -213,6 +216,9 @@ namespace io { void MessageHandler::assembleDiagnosticArray( const std::shared_ptr& telegram) { + if (!settings_->publish_diagnostics) + return; + DiagnosticArrayMsg msg; if (!validValue(last_receiverstatus_.block_header.tow) || (last_receiverstatus_.block_header.tow != @@ -655,6 +661,7 @@ namespace io { if (!settings_->publish_twist) return; TwistWithCovarianceStampedMsg msg; + msg.header.frame_id = "navigation"; if (fromIns) { @@ -767,6 +774,7 @@ namespace io { msg.twist.covariance[28] = -1.0; msg.twist.covariance[35] = -1.0; + publish("/twist_ins", msg); } else { if ((!validValue(last_pvtgeodetic_.block_header.tow)) || @@ -866,11 +874,9 @@ namespace io { msg.twist.covariance[21] = -1.0; msg.twist.covariance[28] = -1.0; msg.twist.covariance[35] = -1.0; - } - - msg.header.frame_id = "navigation"; - publish("/twist_ins", msg); + publish("/twist_gnss", msg); + } }; /** @@ -1372,11 +1378,11 @@ namespace io { */ void MessageHandler::assembleNavSatFix() { - static auto last_ins_tow = last_insnavgeod_.block_header.tow; - if (!settings_->publish_navsatfix) return; + static auto last_ins_tow = last_insnavgeod_.block_header.tow; + NavSatFixMsg msg; uint16_t mask = 15; // We extract the first four bits using this mask. if (settings_->septentrio_receiver_type == "gnss") @@ -2198,16 +2204,19 @@ namespace io { { case PVT_CARTESIAN: // Position and velocity in XYZ { - PVTCartesianMsg msg; - - if (!PVTCartesianParser(node_, telegram->message.begin(), - telegram->message.end(), msg)) + if (settings_->publish_pvtcartesian) { - node_->log(log_level::ERROR, "parse error in PVTCartesian"); - break; + PVTCartesianMsg msg; + + if (!PVTCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in PVTCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("/pvtcartesian", msg); } - assembleHeader(settings_->frame_id, telegram, msg); - publish("/pvtcartesian", msg); break; } case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame @@ -2233,44 +2242,53 @@ namespace io { } case BASE_VECTOR_CART: { - BaseVectorCartMsg msg; - - if (!BaseVectorCartParser(node_, telegram->message.begin(), - telegram->message.end(), msg)) + if (settings_->publish_basevectorcart) { - node_->log(log_level::ERROR, "parse error in BaseVectorCart"); - break; + BaseVectorCartMsg msg; + + if (!BaseVectorCartParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in BaseVectorCart"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("/basevectorcart", msg); } - assembleHeader(settings_->frame_id, telegram, msg); - publish("/basevectorcart", msg); break; } case BASE_VECTOR_GEOD: { - BaseVectorGeodMsg msg; - - if (!BaseVectorGeodParser(node_, telegram->message.begin(), - telegram->message.end(), msg)) + if (settings_->publish_basevectorgeod) { - node_->log(log_level::ERROR, "parse error in BaseVectorGeod"); - break; + BaseVectorGeodMsg msg; + + if (!BaseVectorGeodParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in BaseVectorGeod"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("/basevectorgeod", msg); } - assembleHeader(settings_->frame_id, telegram, msg); - publish("/basevectorgeod", msg); break; } case POS_COV_CARTESIAN: { - PosCovCartesianMsg msg; - - if (!PosCovCartesianParser(node_, telegram->message.begin(), - telegram->message.end(), msg)) + if (settings_->publish_poscovcartesian) { - node_->log(log_level::ERROR, "parse error in PosCovCartesian"); - break; + PosCovCartesianMsg msg; + + if (!PosCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in PosCovCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); + publish("/poscovcartesian", msg); } - assembleHeader(settings_->frame_id, telegram, msg); - publish("/poscovcartesian", msg); break; } case POS_COV_GEODETIC: @@ -2373,7 +2391,8 @@ namespace io { frame_id = settings_->frame_id; } assembleHeader(frame_id, telegram, last_insnavcart_); - publish("/insnavcart", last_insnavcart_); + if (settings_->publish_insnavcart) + publish("/insnavcart", last_insnavcart_); assembleLocalizationEcef(); break; } @@ -2408,88 +2427,97 @@ namespace io { assembleTimeReference(telegram); break; } - case IMU_SETUP: // IMU orientation and lever arm { - IMUSetupMsg msg; - - if (!IMUSetupParser(node_, telegram->message.begin(), - telegram->message.end(), msg, - settings_->use_ros_axis_orientation)) + if (settings_->publish_imu) { - node_->log(log_level::ERROR, "parse error in IMUSetup"); - break; + IMUSetupMsg msg; + + if (!IMUSetupParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in IMUSetup"); + break; + } + assembleHeader(settings_->vehicle_frame_id, telegram, msg); + publish("/imusetup", msg); } - assembleHeader(settings_->vehicle_frame_id, telegram, msg); - publish("/imusetup", msg); break; } - case VEL_SENSOR_SETUP: // Velocity sensor lever arm { - VelSensorSetupMsg msg; - - if (!VelSensorSetupParser(node_, telegram->message.begin(), - telegram->message.end(), msg, - settings_->use_ros_axis_orientation)) + if (settings_->publish_velcovgeodetic) { - node_->log(log_level::ERROR, "parse error in VelSensorSetup"); - break; + VelSensorSetupMsg msg; + + if (!VelSensorSetupParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, "parse error in VelSensorSetup"); + break; + } + assembleHeader(settings_->vehicle_frame_id, telegram, msg); + publish("/velsensorsetup", msg); } - assembleHeader(settings_->vehicle_frame_id, telegram, msg); - publish("/velsensorsetup", msg); break; } - case EXT_EVENT_INS_NAV_CART: // Position, velocity and orientation in // cartesian coordinate frame (ENU frame) { - INSNavCartMsg msg; - - if (!INSNavCartParser(node_, telegram->message.begin(), - telegram->message.end(), msg, - settings_->use_ros_axis_orientation)) - { - node_->log(log_level::ERROR, "parse error in ExtEventINSNavCart"); - break; - } - std::string frame_id; - if (settings_->ins_use_poi) + if (settings_->publish_exteventinsnavcart) { - frame_id = settings_->poi_frame_id; - } else - { - frame_id = settings_->frame_id; + INSNavCartMsg msg; + + if (!INSNavCartParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, + "parse error in ExtEventINSNavCart"); + break; + } + std::string frame_id; + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + assembleHeader(frame_id, telegram, msg); + publish("/exteventinsnavcart", msg); } - assembleHeader(frame_id, telegram, msg); - publish("/exteventinsnavcart", msg); break; } - case EXT_EVENT_INS_NAV_GEOD: { - INSNavGeodMsg msg; - - if (!INSNavGeodParser(node_, telegram->message.begin(), - telegram->message.end(), msg, - settings_->use_ros_axis_orientation)) + if (settings_->publish_exteventinsnavgeod) { - node_->log(log_level::ERROR, "parse error in ExtEventINSNavGeod"); - break; - } - std::string frame_id; - if (settings_->ins_use_poi) - { - frame_id = settings_->poi_frame_id; - } else - { - frame_id = settings_->frame_id; + INSNavGeodMsg msg; + + if (!INSNavGeodParser(node_, telegram->message.begin(), + telegram->message.end(), msg, + settings_->use_ros_axis_orientation)) + { + node_->log(log_level::ERROR, + "parse error in ExtEventINSNavGeod"); + break; + } + std::string frame_id; + if (settings_->ins_use_poi) + { + frame_id = settings_->poi_frame_id; + } else + { + frame_id = settings_->frame_id; + } + assembleHeader(frame_id, telegram, msg); + publish("/exteventinsnavgeod", msg); } - assembleHeader(frame_id, telegram, msg); - publish("/exteventinsnavgeod", msg); break; } - case EXT_SENSOR_MEAS: { bool hasImuMeas = false; @@ -2523,7 +2551,6 @@ namespace io { } case MEAS_EPOCH: { - if (!MeasEpochParser(node_, telegram->message.begin(), telegram->message.end(), last_measepoch_)) { @@ -2538,7 +2565,6 @@ namespace io { } case DOP: { - if (!DOPParser(node_, telegram->message.begin(), telegram->message.end(), last_dop_)) { @@ -2550,16 +2576,18 @@ namespace io { } case VEL_COV_CARTESIAN: { - VelCovCartesianMsg msg; - if (!VelCovCartesianParser(node_, telegram->message.begin(), - telegram->message.end(), msg)) - { - node_->log(log_level::ERROR, "parse error in VelCovCartesian"); - break; - } - assembleHeader(settings_->frame_id, telegram, msg); if (settings_->publish_velcovcartesian) + { + VelCovCartesianMsg msg; + if (!VelCovCartesianParser(node_, telegram->message.begin(), + telegram->message.end(), msg)) + { + node_->log(log_level::ERROR, "parse error in VelCovCartesian"); + break; + } + assembleHeader(settings_->frame_id, telegram, msg); publish("/velcovcartesian", msg); + } break; } case VEL_COV_GEODETIC: @@ -2591,7 +2619,6 @@ namespace io { } case QUALITY_IND: { - if (!QualityIndParser(node_, telegram->message.begin(), telegram->message.end(), last_qualityind_)) { From fc83f9402d2dcdcdec976f2cbf0a6059a73c5950 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 16 Apr 2023 01:34:14 +0200 Subject: [PATCH 116/170] Fix publish check --- src/septentrio_gnss_driver/communication/message_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 50b73d2d..f94d7ba7 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2429,7 +2429,7 @@ namespace io { } case IMU_SETUP: // IMU orientation and lever arm { - if (settings_->publish_imu) + if (settings_->publish_imusetup) { IMUSetupMsg msg; From db55f1666325688d64975599a1f9c7db364919c4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 16 Apr 2023 21:24:21 +0200 Subject: [PATCH 117/170] Add more info un UDP configuration --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 28721107..b1c157de 100644 --- a/README.md +++ b/README.md @@ -340,7 +340,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Connectivity Specs - + `device`: location of device connection + + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. SBF blocks and NMEA sentences are recevied either via this interface or UDP. The former will be utilized if section `udp` is not configured. + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. @@ -354,7 +354,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` - + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, intreface specified by `device` will be utilized. + `ip_server`: IP server of Rx to be used. + `port`: UDP destination port. + `unicast_ip`: Set to computer's IP to use unicast. If not set multicast will be used. From 44030435a0f4d752e6100236ef1781e136d6c18f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 16 Apr 2023 21:41:40 +0200 Subject: [PATCH 118/170] Improve server duplicate check --- .../node/rosaic_node.cpp | 41 +++++++++++-------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 589fa61d..85bf7063 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -628,29 +628,36 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) registerSubscriber(); } - if (settings_.udp_ip_server == settings_.ins_vsm_ip_server_id) - this->log( - log_level::ERROR, - "udp/ip_server and ins_vsm/ip_server/id cannot use the same IP server"); - for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + if (!settings_.udp_ip_server.empty()) { - if (settings_.udp_ip_server == settings_.rtk.ip_server[i].id) - this->log(log_level::ERROR, - "udp/ip_server and rtk_settings/ip_server_" + - std::to_string(i + 1) + - "/id cannot use the same IP server"); + if (settings_.udp_ip_server == settings_.ins_vsm_ip_server_id) + this->log( + log_level::ERROR, + "udp/ip_server and ins_vsm/ip_server/id cannot use the same IP server"); + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.udp_ip_server == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "udp/ip_server and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } } - for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + if (!settings_.ins_vsm_ip_server_id.empty()) { - if (settings_.ins_vsm_ip_server_id == settings_.rtk.ip_server[i].id) - this->log(log_level::ERROR, - "ins_vsm/ip_server/id and rtk_settings/ip_server_" + - std::to_string(i + 1) + - "/id cannot use the same IP server"); + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.ins_vsm_ip_server_id == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "ins_vsm/ip_server/id and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } } if (settings_.rtk.ip_server.size() == 2) { - if (settings_.rtk.ip_server[0].id == settings_.rtk.ip_server[1].id) + if (!settings_.rtk.ip_server[0].id.empty() && + (settings_.rtk.ip_server[0].id == settings_.rtk.ip_server[1].id)) this->log( log_level::ERROR, "rtk_settings/ip_server_1/id and rtk_settings/ip_server_2/id cannot use the same IP server"); From 335ef2034fd1b3f3f9adae5eab66b85e634b09ed Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 16 Apr 2023 21:54:54 +0200 Subject: [PATCH 119/170] Refine readme on UDP --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b1c157de..7c482152 100644 --- a/README.md +++ b/README.md @@ -355,9 +355,9 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, intreface specified by `device` will be utilized. - + `ip_server`: IP server of Rx to be used. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + `port`: UDP destination port. - + `unicast_ip`: Set to computer's IP to use unicast. If not set multicast will be used. + + `unicast_ip`: Set to computer's IP to use unicast (optinal). If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password From 549f340e42ade384e7024935260a559aa7ac481a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 17 Apr 2023 15:16:05 +0200 Subject: [PATCH 120/170] Fix invald v_x var case --- include/septentrio_gnss_driver/abstraction/typedefs.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index abb8c191..7826f793 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -442,6 +442,8 @@ class ROSaicNodeBase log(log_level::ERROR, "Invalid covariance value for v_x: " + std::to_string(var[0]) + ". Ignoring measurement."); + v_x = ""; + std_x = string_utilities::trimDecimalPlaces(1000000.0); } } else std_x = std::to_string(1000000.0); From 414361b43a9cde5cb83751e8e192d0438b1e867f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 17 Apr 2023 23:45:42 +0200 Subject: [PATCH 121/170] Change INS in GNSS node detection to auto --- README.md | 1 - .../communication/message_handler.hpp | 10 ++++++++++ .../communication/settings.hpp | 2 -- .../communication/communication_core.cpp | 13 ++----------- .../communication/message_handler.cpp | 3 +-- .../communication/telegram_handler.cpp | 7 +------ src/septentrio_gnss_driver/node/rosaic_node.cpp | 9 +-------- 7 files changed, 15 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index 7c482152..6b5b27b9 100644 --- a/README.md +++ b/README.md @@ -388,7 +388,6 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `receiver_type`: This parameter is to select the type of the Septentrio receiver + `gnss` for GNSS receivers. + `ins` for INS receivers. - + `ins_in_gnss_mode` INS receivers in GNSS mode. + default: `gnss` + `multi_antenna`: Whether or not the Rx has multiple antennas. + default: `false` diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 4dd46f66..512f1416 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -166,6 +166,11 @@ namespace io { */ void parseNmea(const std::shared_ptr& telegram); + /** + * @brief Set INS to true + */ + void setIsIns() { isIns_ = true; } + private: /** * @brief Header assembling @@ -426,5 +431,10 @@ namespace io { * epoch */ Timestamp timestampSBF(uint32_t tow, uint16_t wnc) const; + + /** + * Bool to signal if Rx is INS + */ + bool isIns_ = false; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 38ea7731..793548de 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -302,8 +302,6 @@ struct Settings std::string local_frame_id; //! Septentrio receiver type, either "gnss" or "ins" std::string septentrio_receiver_type; - //! Handle the case when an INS is used in GNSS mode - bool ins_in_gnss_mode = false; //! If true, the ROS message headers' unix time field is constructed from the TOW //! (in the SBF case) and UTC (in the NMEA case) data. If false, times are //! constructed within the driver via time(NULL) of the \ library. diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index acdb4588..c42c4417 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -317,16 +317,6 @@ namespace io { send("grc \x0D"); telegramHandler_.waitForCapabilities(); - bool ins_in_gnss_mode = settings_->ins_in_gnss_mode; - if (telegramHandler_.isIns() && - (settings_->septentrio_receiver_type == "gnss") && !ins_in_gnss_mode) - { - node_->log( - log_level::WARN, - "INS receiver seems to be used as GNSS. If this is intended, please consider setting receiver type to 'ins_in_gnss_mode'."); - ins_in_gnss_mode = true; - } - // Activate NTP server if (settings_->use_gnss_time) send("sntp, on \x0D"); @@ -340,7 +330,8 @@ namespace io { send(ss.str()); } - if ((settings_->septentrio_receiver_type == "ins") || ins_in_gnss_mode) + if ((settings_->septentrio_receiver_type == "ins") || + telegramHandler_.isIns()) { { std::stringstream ss; diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index f94d7ba7..6a164dac 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2660,8 +2660,7 @@ namespace io { node_->log(log_level::ERROR, "parse error of firmware version."); } else { - if ((settings_->septentrio_receiver_type == "ins") || - settings_->ins_in_gnss_mode) + if ((settings_->septentrio_receiver_type == "ins") || isIns_) { if ((major_minor_patch[0] < ins_major) || ((major_minor_patch[0] == ins_major) && diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 5d9062ed..0737e761 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -83,6 +83,7 @@ namespace io { if (block_in_string.find("INS") != std::string::npos) { isIns_ = true; + messageHandler_.setIsIns(); } if (block_in_string.find("Heading") != std::string::npos) @@ -133,12 +134,6 @@ namespace io { log_level::WARN, "Rx does not support dual antenna mode, set parameter multi_antenna to false and/or disable publishing of atteuler."); } - if (block_in_string.substr(0, 8) == "$R? sao,") - { - node_->log( - log_level::WARN, - "Rx does not understand sao command, maybe an INS is used as GNSS. In this case set parameter receiver_type to ins_in_gnss_mode."); - } } else { node_->log(log_level::DEBUG, "The Rx's response contains " + diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 85bf7063..3b9e74e0 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -99,8 +99,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) param("receiver_type", settings_.septentrio_receiver_type, static_cast("gnss")); if (!((settings_.septentrio_receiver_type == "gnss") || - (settings_.septentrio_receiver_type == "ins") || - (settings_.septentrio_receiver_type == "ins_in_gnss_mode"))) + (settings_.septentrio_receiver_type == "ins"))) { this->log(log_level::FATAL, "Unkown septentrio_receiver_type " + settings_.septentrio_receiver_type + @@ -108,12 +107,6 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) return false; } - if (settings_.septentrio_receiver_type == "ins_in_gnss_mode") - { - settings_.septentrio_receiver_type = "gnss"; - settings_.ins_in_gnss_mode = true; - } - // Polling period parameters getUint32Param("polling_period/pvt", settings_.polling_period_pvt, static_cast(1000)); From ec2fec7ea50727b19fd7c5e4c74dad79f6fdb5ab Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 18 Apr 2023 11:53:29 +0200 Subject: [PATCH 122/170] Add improved VSM handling --- CHANGELOG.rst | 1 + README.md | 1 + .../abstraction/typedefs.hpp | 38 +++++++++++++++++-- .../communication/message_handler.hpp | 10 ----- .../communication/settings.hpp | 11 ++++++ .../communication/telegram_handler.hpp | 10 ----- .../communication/communication_core.cpp | 5 +-- .../communication/message_handler.cpp | 7 ++-- .../communication/telegram_handler.cpp | 5 +-- 9 files changed, 56 insertions(+), 32 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f4371a17..4e5ce5e7 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +11,7 @@ Changelog for package septentrio_gnss_driver * Latency compensation for ROS timestamps * Output of SBf block VelCovCartesian * Support for UDP + * New VSM handling allows for unknown variances (INS firmware >= 1.4.1) * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 6b5b27b9..65cfc0c8 100644 --- a/README.md +++ b/README.md @@ -73,6 +73,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo * GNSS with firmware < 4.12.1 does not support OSNMA. * INS with firmware < 1.3.2 does not support NTP. * INS with firmware < 1.4 does not support OSNMA. + * INS with firmware < 1.4.1 does not support improved VSM handling allowing for unknown variances. * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 7826f793..875e8430 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -373,6 +373,36 @@ class ROSaicNodeBase tf2Publisher_.sendTransform(transformStamped); } + /** + * @brief Set INS to true + */ + void setIsIns() { capabilities_.is_ins = true; } + + /** + * @brief Set has heading to true + */ + void setHasHeading() { capabilities_.has_heading = true; } + + /** + * @brief Set improved VSM handling to true + */ + void setImprovedVsmHandling() { capabilities_.has_improved_vsm_handling = true; } + + /** + * @brief Check if Rx is INS + */ + bool isIns() { return capabilities_.is_ins; } + + /** + * @brief Check if Rx has heading + */ + bool hasHeading() { return capabilities_.has_heading; } + + /** + * @brief Check if Rx has improved VSM handling + */ + bool hasImprovedVsmHandling() { return capabilities_.has_improved_vsm_handling; } + private: void callbackOdometry(const nav_msgs::Odometry::ConstPtr& odo) { @@ -437,7 +467,7 @@ class ROSaicNodeBase settings_.ins_vsm_ros_variances[0]); else if (var[0] > 0.0) std_x = string_utilities::trimDecimalPlaces(std::sqrt(var[0])); - else + else if (!capabilities_.has_improved_vsm_handling) { log(log_level::ERROR, "Invalid covariance value for v_x: " + std::to_string(var[0]) + @@ -457,7 +487,7 @@ class ROSaicNodeBase settings_.ins_vsm_ros_variances[1]); else if (var[1] > 0.0) std_y = string_utilities::trimDecimalPlaces(std::sqrt(var[1])); - else + else if (!capabilities_.has_improved_vsm_handling) { log(log_level::ERROR, "Invalid covariance value for v_y: " + std::to_string(var[1]) + @@ -477,7 +507,7 @@ class ROSaicNodeBase settings_.ins_vsm_ros_variances[2]); else if (var[2] > 0.0) std_z = string_utilities::trimDecimalPlaces(std::sqrt(var[2])); - else + else if (!capabilities_.has_improved_vsm_handling) { log(log_level::ERROR, "Invalid covariance value for v_z: " + std::to_string(var[2]) + @@ -533,4 +563,6 @@ class ROSaicNodeBase tf2_ros::Buffer tfBuffer_; // tf listener tf2_ros::TransformListener tfListener_; + // Capabilities of Rx + Capabilities capabilities_; }; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/message_handler.hpp b/include/septentrio_gnss_driver/communication/message_handler.hpp index 512f1416..4dd46f66 100644 --- a/include/septentrio_gnss_driver/communication/message_handler.hpp +++ b/include/septentrio_gnss_driver/communication/message_handler.hpp @@ -166,11 +166,6 @@ namespace io { */ void parseNmea(const std::shared_ptr& telegram); - /** - * @brief Set INS to true - */ - void setIsIns() { isIns_ = true; } - private: /** * @brief Header assembling @@ -431,10 +426,5 @@ namespace io { * epoch */ Timestamp timestampSBF(uint32_t tow, uint16_t wnc) const; - - /** - * Bool to signal if Rx is INS - */ - bool isIns_ = false; }; } // namespace io \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index 793548de..f09a1971 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -349,4 +349,15 @@ struct Settings uint32_t ins_vsm_serial_baud_rate; //! Wether VSM shall be kept open om shutdown bool ins_vsm_serial_keep_open; +}; + +//! Capabilities struct +struct Capabilities +{ + //! Wether Rx is INS + bool is_ins = false; + //! Wether Rx has heading + bool has_heading = false; + //! Wether Rx has improved VSM handling + bool has_improved_vsm_handling = false; }; \ No newline at end of file diff --git a/include/septentrio_gnss_driver/communication/telegram_handler.hpp b/include/septentrio_gnss_driver/communication/telegram_handler.hpp index 70079ecb..6e4ceecd 100644 --- a/include/septentrio_gnss_driver/communication/telegram_handler.hpp +++ b/include/septentrio_gnss_driver/communication/telegram_handler.hpp @@ -145,12 +145,6 @@ namespace io { //! Waits for capabilities void waitForCapabilities() { capabilitiesSemaphore_.wait(); } - //! Check if capability contains INS - bool isIns() { return isIns_; } - - //! Check if capability contains Heading - bool hasHeading() { return hasHeading_; } - private: void handleSbf(const std::shared_ptr& telegram); void handleNmea(const std::shared_ptr& telegram); @@ -163,10 +157,6 @@ namespace io { //! MessageHandler parser MessageHandler messageHandler_; - //! Rx capabilities - mutable bool isIns_ = false; - mutable bool hasHeading_ = false; - Semaphore cdSemaphore_; Semaphore responseSemaphore_; Semaphore capabilitiesSemaphore_; diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index c42c4417..14ec5088 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -330,8 +330,7 @@ namespace io { send(ss.str()); } - if ((settings_->septentrio_receiver_type == "ins") || - telegramHandler_.isIns()) + if ((settings_->septentrio_receiver_type == "ins") || node_->isIns()) { { std::stringstream ss; @@ -471,7 +470,7 @@ namespace io { // Setting multi antenna if (settings_->multi_antenna) { - if (telegramHandler_.hasHeading()) + if (node_->hasHeading()) send("sga, MultiAntenna \x0D"); else node_->log(log_level::WARN, diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 6a164dac..316e5dec 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2641,7 +2641,7 @@ namespace io { static const int32_t ins_major = 1; static const int32_t ins_minor = 4; - static const int32_t ins_patch = 0; + static const int32_t ins_patch = 1; static const int32_t gnss_major = 4; static const int32_t gnss_minor = 12; static const int32_t gnss_patch = 1; @@ -2660,7 +2660,7 @@ namespace io { node_->log(log_level::ERROR, "parse error of firmware version."); } else { - if ((settings_->septentrio_receiver_type == "ins") || isIns_) + if ((settings_->septentrio_receiver_type == "ins") || node_->isIns()) { if ((major_minor_patch[0] < ins_major) || ((major_minor_patch[0] == ins_major) && @@ -2677,7 +2677,8 @@ namespace io { std::to_string(ins_major) + "." + std::to_string(ins_minor) + "." + std::to_string(ins_patch) + " or consult README."); - } + } else + node_->setImprovedVsmHandling(); } else if (settings_->septentrio_receiver_type == "gnss") { if ((major_minor_patch[0] < gnss_major) || diff --git a/src/septentrio_gnss_driver/communication/telegram_handler.cpp b/src/septentrio_gnss_driver/communication/telegram_handler.cpp index 0737e761..d7c8170e 100644 --- a/src/septentrio_gnss_driver/communication/telegram_handler.cpp +++ b/src/septentrio_gnss_driver/communication/telegram_handler.cpp @@ -82,13 +82,12 @@ namespace io { { if (block_in_string.find("INS") != std::string::npos) { - isIns_ = true; - messageHandler_.setIsIns(); + node_->setIsIns(); } if (block_in_string.find("Heading") != std::string::npos) { - hasHeading_ = true; + node_->setHasHeading(); } capabilitiesSemaphore_.notify(); } From 0072498b422b08ea4a083a2b011ce61ec9b7aef2 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 18 Apr 2023 12:28:16 +0200 Subject: [PATCH 123/170] Change log level of firmware check --- src/septentrio_gnss_driver/communication/message_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 316e5dec..e6341a7c 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2670,7 +2670,7 @@ namespace io { (major_minor_patch[2] < ins_patch))) { node_->log( - log_level::WARN, + log_level::INFO, "INS receiver has firmware version: " + last_receiversetup_.rx_version + ", which does not support all features. Please update to at least " + @@ -2689,7 +2689,7 @@ namespace io { (major_minor_patch[2] < gnss_patch))) { node_->log( - log_level::WARN, + log_level::INFO, "GNSS receiver has firmware version: " + last_receiversetup_.rx_version + ", which may not support all features. Please update to at least " + From 02f74b45f778df40fe9497a130baf98097cce0d0 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sun, 23 Apr 2023 00:26:28 +0200 Subject: [PATCH 124/170] Move constant --- src/septentrio_gnss_driver/parsers/parsing_utilities.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index 88743e1f..ec443ac8 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -44,6 +44,8 @@ namespace parsing_utilities { + const double pihalf = boost::math::constants::pi() / 2.0; + namespace qi = boost::spirit::qi; [[nodiscard]] double wrapAngle180to180(double angle) @@ -341,7 +343,6 @@ namespace parsing_utilities { [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon) { - static double pihalf = boost::math::constants::pi() / 2.0; double sr = sin((pihalf - lat) / 2.0); double cr = cos((pihalf - lat) / 2.0); double sy = sin((lon + pihalf) / 2.0); @@ -352,7 +353,6 @@ namespace parsing_utilities { [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon) { - static double pihalf = boost::math::constants::pi() / 2.0; double sp = sin((-lat - pihalf) / 2.0); double cp = cos((-lat - pihalf) / 2.0); double sy = sin(lon / 2.0); From 8a7b64a673b9f9618aba865e063d79935a4b9b38 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 27 Apr 2023 14:22:44 +0200 Subject: [PATCH 125/170] Add heading to GPSFix msg --- CHANGELOG.rst | 1 + README.md | 2 +- .../communication/message_handler.cpp | 41 +++++++++++-------- 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4e5ce5e7..68b24995 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -12,6 +12,7 @@ Changelog for package septentrio_gnss_driver * Output of SBf block VelCovCartesian * Support for UDP * New VSM handling allows for unknown variances (INS firmware >= 1.4.1) + * Add heading angle to GPSFix msg (by diverting dip field, cf. readme) * Improvements * Rework IO core and message handling * Unified stream processing diff --git a/README.md b/README.md index 65cfc0c8..9a29df0d 100644 --- a/README.md +++ b/README.md @@ -676,7 +676,7 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its block header. + `/navsatfix`: publishes generic ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`,`PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + The ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) can be fed directly into the [`navsat_transform_node`](https://docs.ros.org/melodic/api/robot_localization/html/navsat_transform_node.html) of the ROS navigation stack. - + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). + + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). In order to publish heading information, the field *dip* is diverted from its intended meaning an populated with the heading angle and *err_dip* with its uncertainty. + `/pose`: publishes generic ROS message [`geometry_msgs/PoseWithCovarianceStamped.msg`](https://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `AttEuler`, `AttCovEuler` (GNSS case) or `INSNavGeod` (INS case). + Note that GNSS provides absolute positioning, while robots are often localized within a local level cartesian frame. The pose field of this ROS message contains position with respect to the absolute ENU frame (longitude, latitude, height), i.e. not a cartesian frame, while the orientation is with respect to a vehicle-fixed (e.g. for mosaic-x5 in moving base mode via the command `setAttitudeOffset`, ...) !local! NED frame or ENU frame if `use_ros_axis_directions` is set `true`. Thus the orientation is !not! given with respect to the same frame as the position is given in. The cross-covariances are hence set to 0. + `/twist`: publishes generic ROS message [`geometry_msgs/TwistWithCovarianceStamped.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/TwistWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic` and `VelCovGeodetic`. diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index e6341a7c..d9c18ff8 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1799,8 +1799,11 @@ namespace io { msg.speed = std::sqrt(square(last_pvtgeodetic_.vn) + square(last_pvtgeodetic_.ve)); msg.climb = last_pvtgeodetic_.vu; - msg.pitch = last_atteuler_.pitch; + msg.roll = last_atteuler_.roll; + msg.pitch = last_atteuler_.pitch; + msg.dip = last_atteuler_.heading; + if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) { msg.gdop = -1.0; @@ -1839,8 +1842,8 @@ namespace io { } msg.time = static_cast(last_pvtgeodetic_.block_header.tow) / 1000 + - static_cast(last_pvtgeodetic_.block_header.wnc * 7 * 24 * - 60 * 60); + static_cast(last_pvtgeodetic_.block_header.wnc * 604800); + // position msg.err = 2 * (std::sqrt(static_cast(last_poscovgeodetic_.cov_latlat) + @@ -1852,6 +1855,7 @@ namespace io { static_cast(last_poscovgeodetic_.cov_lonlon))); msg.err_vert = 2 * std::sqrt(static_cast(last_poscovgeodetic_.cov_hgthgt)); + // motion msg.err_track = 2 * (std::sqrt(square(1.0 / (last_pvtgeodetic_.vn + square(last_pvtgeodetic_.ve) / @@ -1866,10 +1870,14 @@ namespace io { static_cast(last_velcovgeodetic_.cov_veve))); msg.err_climb = 2 * std::sqrt(static_cast(last_velcovgeodetic_.cov_vuvu)); - msg.err_pitch = - 2 * std::sqrt(static_cast(last_attcoveuler_.cov_pitchpitch)); + // attitude msg.err_roll = 2 * std::sqrt(static_cast(last_attcoveuler_.cov_rollroll)); + msg.err_pitch = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_pitchpitch)); + msg.err_dip = + 2 * std::sqrt(static_cast(last_attcoveuler_.cov_headhead)); + msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon; msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon; msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt; @@ -1932,9 +1940,9 @@ namespace io { // Note that cog is of type float32 while track is of type float64. if ((last_insnavgeod_.sb_list & 2) != 0) { - msg.track = last_insnavgeod_.heading; msg.pitch = last_insnavgeod_.pitch; msg.roll = last_insnavgeod_.roll; + msg.dip = last_insnavgeod_.heading; } if ((last_insnavgeod_.sb_list & 8) != 0) { @@ -1942,6 +1950,7 @@ namespace io { square(last_insnavgeod_.ve)); msg.climb = last_insnavgeod_.vu; + msg.track = std::atan2(last_insnavgeod_.vn, last_insnavgeod_.ve); } if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0) { @@ -1981,8 +1990,7 @@ namespace io { } msg.time = static_cast(last_insnavgeod_.block_header.tow) / 1000 + - static_cast(last_insnavgeod_.block_header.wnc * 7 * 24 * 60 * - 60); + static_cast(last_insnavgeod_.block_header.wnc * 604800); if ((last_insnavgeod_.sb_list & 1) != 0) { msg.err = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + @@ -1991,8 +1999,7 @@ namespace io { msg.err_horz = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) + square(last_insnavgeod_.longitude_std_dev))); - msg.err_vert = - 2 * (std::sqrt(square(last_insnavgeod_.height_std_dev))); + msg.err_vert = 2 * last_insnavgeod_.height_std_dev; } if (((last_insnavgeod_.sb_list & 8) != 0) || ((last_insnavgeod_.sb_list & 1) != 0)) @@ -2009,17 +2016,15 @@ namespace io { } if ((last_insnavgeod_.sb_list & 8) != 0) { - msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.vn) + - square(last_insnavgeod_.ve))); - msg.err_climb = 2 * std::sqrt(square(last_insnavgeod_.vn)); - } - if ((last_insnavgeod_.sb_list & 2) != 0) - { - msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.pitch)); + msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.ve_std_dev) + + square(last_insnavgeod_.vn_std_dev))); + msg.err_climb = 2 * last_insnavgeod_.vu_std_dev; } if ((last_insnavgeod_.sb_list & 2) != 0) { - msg.err_pitch = 2 * std::sqrt(square(last_insnavgeod_.roll)); + msg.err_pitch = 2 * last_insnavgeod_.pitch_std_dev; + msg.err_roll = 2 * last_insnavgeod_.roll_std_dev; + msg.err_dip = 2 * last_insnavgeod_.heading_std_dev; } if ((last_insnavgeod_.sb_list & 1) != 0) { From 5328a63ab3d612d6bf5263e2b1eca01ba12202a2 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 17 May 2023 17:25:35 +0200 Subject: [PATCH 126/170] Fix spelling --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9a29df0d..34a8ce1c 100644 --- a/README.md +++ b/README.md @@ -355,10 +355,10 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` - + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, intreface specified by `device` will be utilized. + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, interface specified by `device` will be utilized. + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + `port`: UDP destination port. - + `unicast_ip`: Set to computer's IP to use unicast (optinal). If not set multicast will be used. + + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password From 6b2e40d3d79d17719a4e0da3859597d782e558b3 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 18 May 2023 21:43:43 +0200 Subject: [PATCH 127/170] Add units to msgs --- msg/MeasEpochChannelType2.msg | 6 +++--- msg/VelCovGeodetic.msg | 20 ++++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/msg/MeasEpochChannelType2.msg b/msg/MeasEpochChannelType2.msg index f3b41939..83cb96e4 100644 --- a/msg/MeasEpochChannelType2.msg +++ b/msg/MeasEpochChannelType2.msg @@ -6,6 +6,6 @@ uint8 cn0 # 0.25 dB-Hz uint8 offsets_msb # 65.536 m or 65.536 Hz int8 carrier_msb # 65.536 cycles uint8 obs_info -uint16 code_offset_lsb -uint16 carrier_lsb -uint16 doppler_offset_lsb \ No newline at end of file +uint16 code_offset_lsb # 0.001 m +uint16 carrier_lsb # 0.001 cycles +uint16 doppler_offset_lsb # 0.0001 Hz \ No newline at end of file diff --git a/msg/VelCovGeodetic.msg b/msg/VelCovGeodetic.msg index 0c65d8e4..396586fe 100644 --- a/msg/VelCovGeodetic.msg +++ b/msg/VelCovGeodetic.msg @@ -7,13 +7,13 @@ BlockHeader block_header uint8 mode uint8 error -float32 cov_vnvn -float32 cov_veve -float32 cov_vuvu -float32 cov_dtdt -float32 cov_vnve -float32 cov_vnvu -float32 cov_vndt -float32 cov_vevu -float32 cov_vedt -float32 cov_vudt \ No newline at end of file +float32 cov_vnvn # m^2/s^2 +float32 cov_veve # m^2/s^2 +float32 cov_vuvu # m^2/s^2 +float32 cov_dtdt # m^2/s^2 +float32 cov_vnve # m^2/s^2 +float32 cov_vnvu # m^2/s^2 +float32 cov_vndt # m^2/s^2 +float32 cov_vevu # m^2/s^2 +float32 cov_vedt # m^2/s^2 +float32 cov_vudt # m^2/s^2 \ No newline at end of file From 95e2ee58f74db507bee35b769831bf84b13ed5e2 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 18 May 2023 23:05:53 +0200 Subject: [PATCH 128/170] Add TCP communication via static IP server --- CHANGELOG.rst | 2 +- README.md | 19 +++++++---- config/gnss.yaml | 4 +++ config/ins.yaml | 4 +++ config/rover.yaml | 4 +++ .../communication/async_manager.hpp | 8 +++++ .../communication/communication_core.hpp | 1 + .../communication/io.hpp | 16 +++++---- .../communication/settings.hpp | 8 +++-- .../communication/communication_core.cpp | 22 ++++++++++--- .../node/rosaic_node.cpp | 33 +++++++++++++++++-- 11 files changed, 100 insertions(+), 21 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 68b24995..b7fd39f4 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -10,7 +10,7 @@ Changelog for package septentrio_gnss_driver * OSNMA * Latency compensation for ROS timestamps * Output of SBf block VelCovCartesian - * Support for UDP + * Support for UDP and TCP via IP server * New VSM handling allows for unknown variances (INS firmware >= 1.4.1) * Add heading angle to GPSFix msg (by diverting dip field, cf. readme) * Improvements diff --git a/README.md b/README.md index 34a8ce1c..a44aad42 100644 --- a/README.md +++ b/README.md @@ -90,10 +90,14 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo baudrate: 921600 hw_flow_control: off + tcp: + ip_server: "" + port: 0 + udp: - ip_server: "" - port: 0 - unicast_ip: "" + ip_server: "" + port: 0 + unicast_ip: "" configure_rx: true @@ -341,7 +345,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Connectivity Specs - + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. SBF blocks and NMEA sentences are recevied either via this interface or UDP. The former will be utilized if section `udp` is not configured. + + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. SBF blocks and NMEA sentences are recevied either via this interface, a static IP server for TCP or UDP. The former will be utilized if section `tcp` and `udp` are not configured. + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. @@ -355,7 +359,10 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` - + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, interface specified by `device` will be utilized. + + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences. If left unconfigured, interface specified by `device` or `udp` will be utilized. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, interface specified by `device` or `tcp` will be utilized. + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + `port`: UDP destination port. + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. @@ -379,7 +386,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Receiver Configuration - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. ON the driver side communication has to set accorsingly to serial, TCP or UDP. For TCP communication it is recommended to use a static TCP server (`tcp/ip_server` and `tcp/port`) as dynamic connections is not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
diff --git a/config/gnss.yaml b/config/gnss.yaml index f2be3f49..1211c7e3 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -8,6 +8,10 @@ serial: baudrate: 921600 hw_flow_control: "off" +tcp: + ip_server: "" + port: 0 + udp: ip_server: "" port: 0 diff --git a/config/ins.yaml b/config/ins.yaml index 91329252..603daae1 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -8,6 +8,10 @@ serial: baudrate: 921600 hw_flow_control: "off" +tcp: + ip_server: "" + port: 0 + udp: ip_server: "" port: 0 diff --git a/config/rover.yaml b/config/rover.yaml index a73d70e4..c02cdb2a 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -8,6 +8,10 @@ serial: baudrate: 921600 hw_flow_control: off +tcp: + ip_server: "" + port: 0 + udp: ip_server: "" port: 0 diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 148829cb..5ed37192 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -119,6 +119,8 @@ namespace io { [[nodiscard]] bool connect(); + void setPort(const std::string& port); + void send(const std::string& cmd); private: @@ -189,6 +191,12 @@ namespace io { return true; } + template + void AsyncManager::setPort(const std::string& port) + { + ioInterface_.setPort(port); + } + template void AsyncManager::send(const std::string& cmd) { diff --git a/include/septentrio_gnss_driver/communication/communication_core.hpp b/include/septentrio_gnss_driver/communication/communication_core.hpp index 87851851..320f9a5b 100644 --- a/include/septentrio_gnss_driver/communication/communication_core.hpp +++ b/include/septentrio_gnss_driver/communication/communication_core.hpp @@ -166,6 +166,7 @@ namespace io { //! This declaration is deliberately stream-independent (Serial or TCP). std::unique_ptr manager_; + std::unique_ptr> tcpClient_; std::unique_ptr udpClient_; bool nmeaActivated_ = false; diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index b107ae7c..d3125e9a 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -242,12 +242,15 @@ namespace io { node_(node), ioService_(ioService) { + port_ = node_->settings()->device_tcp_port; } ~TcpIo() { stream_->close(); } void close() { stream_->close(); } + void setPort(const std::string& port) { port_ = port; } + [[nodiscard]] bool connect() { boost::asio::ip::tcp::resolver::iterator endpointIterator; @@ -256,22 +259,21 @@ namespace io { { boost::asio::ip::tcp::resolver resolver(*ioService_); boost::asio::ip::tcp::resolver::query query( - node_->settings()->tcp_ip, node_->settings()->tcp_port); + node_->settings()->device_tcp_ip, port_); endpointIterator = resolver.resolve(query); } catch (std::runtime_error& e) { node_->log(log_level::ERROR, - "Could not resolve " + node_->settings()->tcp_ip + - " on port " + node_->settings()->tcp_port + ": " + - e.what()); + "Could not resolve " + node_->settings()->device_tcp_ip + + " on port " + port_ + ": " + e.what()); return false; } stream_.reset(new boost::asio::ip::tcp::socket(*ioService_)); node_->log(log_level::INFO, "Connecting to tcp://" + - node_->settings()->tcp_ip + ":" + - node_->settings()->tcp_port + "..."); + node_->settings()->device_tcp_ip + ":" + + port_ + "..."); try { @@ -297,6 +299,8 @@ namespace io { ROSaicNodeBase* node_; std::shared_ptr ioService_; + std::string port_; + public: std::unique_ptr stream_; }; diff --git a/include/septentrio_gnss_driver/communication/settings.hpp b/include/septentrio_gnss_driver/communication/settings.hpp index f09a1971..47ef6f6f 100644 --- a/include/septentrio_gnss_driver/communication/settings.hpp +++ b/include/septentrio_gnss_driver/communication/settings.hpp @@ -129,15 +129,19 @@ struct Settings //! Device type device_type::DeviceType device_type; //! TCP IP - std::string tcp_ip; + std::string device_tcp_ip; //! TCP port - std::string tcp_port; + std::string device_tcp_port; //! UDP port uint32_t udp_port; //! UDP unicast destination ip std::string udp_unicast_ip; //! UDP IP server id std::string udp_ip_server; + //! TCP port + uint32_t tcp_port; + //! TCP IP server id + std::string tcp_ip_server; //! Filename std::string file_name; //! Username for login diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 14ec5088..cb88b36e 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -203,13 +203,21 @@ namespace io { [[nodiscard]] bool CommunicationCore::initializeIo() { - bool udp = false; + bool client = false; node_->log(log_level::DEBUG, "Called initializeIo() method"); + if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty())) + { + tcpClient_.reset(new AsyncManager(node_, &telegramQueue_)); + tcpClient_->setPort(std::to_string(settings_->tcp_port)); + if (!settings_->configure_rx) + tcpClient_->connect(); + client = true; + } if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { udpClient_.reset( new UdpClient(node_, settings_->udp_port, &telegramQueue_)); - udp = true; + client = true; } switch (settings_->device_type) @@ -236,7 +244,7 @@ namespace io { } default: { - if (!udp || settings_->configure_rx || + if (!client || settings_->configure_rx || (settings_->ins_vsm_ros_source == "odometry") || (settings_->ins_vsm_ros_source == "twist")) { @@ -277,7 +285,13 @@ namespace io { node_->log(log_level::INFO, "The connection descriptor is " + mainConnectionPort_); streamPort_ = mainConnectionPort_; - if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) + if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty())) + { + streamPort_ = settings_->tcp_ip_server; + send("siss, " + streamPort_ + ", " + + std::to_string(settings_->tcp_port) + ", TCP, " + "\x0D"); + tcpClient_->connect(); + } else if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { streamPort_ = settings_->udp_ip_server; std::string destination; diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 3b9e74e0..190d8c9f 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -90,6 +90,8 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) static_cast(921600)); param("serial/hw_flow_control", settings_.hw_flow_control, static_cast("off")); + getUint32Param("tcp/port", settings_.tcp_port, static_cast(0)); + param("tcp/ip_server", settings_.tcp_ip_server, static_cast("")); getUint32Param("udp/port", settings_.udp_port, static_cast(0)); param("udp/unicast_ip", settings_.udp_unicast_ip, static_cast("")); param("udp/ip_server", settings_.udp_ip_server, static_cast("")); @@ -107,6 +109,14 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) return false; } + if (settings_.configure_rx && !settings_.tcp_ip_server.empty() && + !settings_.udp_ip_server.empty()) + { + this->log( + log_level::WARN, + "Both UDP and TCP have been defined to receive data, only TCP will be used. If UDP is intended, leave tcp/ip_server empty."); + } + // Polling period parameters getUint32Param("polling_period/pvt", settings_.polling_period_pvt, static_cast(1000)); @@ -621,6 +631,25 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) registerSubscriber(); } + if (!settings_.tcp_ip_server.empty()) + { + if (settings_.tcp_ip_server == settings_.udp_ip_server) + this->log( + log_level::ERROR, + "tcp/ip_server and udp/ip_server cannot use the same IP server"); + if (settings_.tcp_ip_server == settings_.ins_vsm_ip_server_id) + this->log( + log_level::ERROR, + "tcp/ip_server and ins_vsm/ip_server/id cannot use the same IP server"); + for (size_t i = 0; i < settings_.rtk.ip_server.size(); ++i) + { + if (settings_.tcp_ip_server == settings_.rtk.ip_server[i].id) + this->log(log_level::ERROR, + "tcp/ip_server and rtk_settings/ip_server_" + + std::to_string(i + 1) + + "/id cannot use the same IP server"); + } + } if (!settings_.udp_ip_server.empty()) { if (settings_.udp_ip_server == settings_.ins_vsm_ip_server_id) @@ -661,8 +690,8 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) if (boost::regex_match(settings_.device, match, boost::regex("(tcp)://(.+):(\\d+)"))) { - settings_.tcp_ip = match[2]; - settings_.tcp_port = match[3]; + settings_.device_tcp_ip = match[2]; + settings_.device_tcp_port = match[3]; settings_.device_type = device_type::TCP; } else if (boost::regex_match(settings_.device, match, boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)"))) From 2954c1d64facef84d1b4d81e50ca035e881a940d Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 19 May 2023 01:13:57 +0200 Subject: [PATCH 129/170] Change angle wrapping --- .../parsers/parsing_utilities.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index ec443ac8..6879b7ed 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -50,15 +50,7 @@ namespace parsing_utilities { [[nodiscard]] double wrapAngle180to180(double angle) { - while (angle > 180.0) - { - angle -= 360.0; - } - while (angle < -180.0) - { - angle += 360.0; - } - return angle; + return std::remainder(angle, 360.0); } [[nodiscard]] double parseDouble(const uint8_t* buffer) From a797d69b4e6436877d4d0b64e4aed6f734768f21 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 19 May 2023 01:22:51 +0200 Subject: [PATCH 130/170] Fix spelling --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a44aad42..8fd9abaf 100644 --- a/README.md +++ b/README.md @@ -386,7 +386,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Receiver Configuration - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. ON the driver side communication has to set accorsingly to serial, TCP or UDP. For TCP communication it is recommended to use a static TCP server (`tcp/ip_server` and `tcp/port`) as dynamic connections is not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`tcp/ip_server` and `tcp/port`) as dynamic connections is not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
From 814c5108f63918bc2bcdf9f0b121cab1c1a6293a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 19 May 2023 19:12:11 +0200 Subject: [PATCH 131/170] Add keep alive check for TCP --- .../septentrio_gnss_driver/communication/async_manager.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index 5ed37192..fad018b8 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -259,6 +259,13 @@ namespace io { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); receive(); } + } else if (running_ && std::is_same::value) + { + // Send to check if TCP connection still alive + std::string empty = " "; + boost::asio::async_write( + *(ioInterface_.stream_), boost::asio::buffer(empty.data(), 1), + [](boost::system::error_code ec, std::size_t /*length*/) {}); } } } From b62a35aaf922a4762cc51f222faea34aa82d94fd Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 20 May 2023 16:28:03 +0200 Subject: [PATCH 132/170] Categorize stream params --- README.md | 33 ++++++++++--------- config/gnss.yaml | 16 ++++----- config/ins.yaml | 16 ++++----- config/rover.yaml | 16 ++++----- .../node/rosaic_node.cpp | 15 ++++++--- 5 files changed, 51 insertions(+), 45 deletions(-) diff --git a/README.md b/README.md index 8fd9abaf..6ad074c6 100644 --- a/README.md +++ b/README.md @@ -90,14 +90,14 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo baudrate: 921600 hw_flow_control: off - tcp: - ip_server: "" - port: 0 - - udp: - ip_server: "" - port: 0 - unicast_ip: "" + stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" configure_rx: true @@ -345,7 +345,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Connectivity Specs - + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. SBF blocks and NMEA sentences are recevied either via this interface, a static IP server for TCP or UDP. The former will be utilized if section `tcp` and `udp` are not configured. + + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. Incoming data streams of SBF blocks and NMEA sentences are recevied either via this interface or a static IP server for TCP and/or UDP. The former will be utilized if section `stream_device/tcp` and `stream_device/udp` are not configured. + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. @@ -359,13 +359,14 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` - + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences. If left unconfigured, interface specified by `device` or `udp` will be utilized. - + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. - + `port`: UDP destination port. - + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, interface specified by `device` or `tcp` will be utilized. - + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. - + `port`: UDP destination port. - + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. + + `stream_device`: If left unconfigured, by default `device` is utilized for the data streams. Within `stream_device` static IP servers may be defined instead. In config mode (`configure_rx` set to `true`), TCP will be prioritized over UDP. If Rx is pre-configured, both may be set simultaneously. + + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password diff --git a/config/gnss.yaml b/config/gnss.yaml index 1211c7e3..f422930a 100644 --- a/config/gnss.yaml +++ b/config/gnss.yaml @@ -8,14 +8,14 @@ serial: baudrate: 921600 hw_flow_control: "off" -tcp: - ip_server: "" - port: 0 - -udp: - ip_server: "" - port: 0 - unicast_ip: "" +stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" configure_rx: true diff --git a/config/ins.yaml b/config/ins.yaml index 603daae1..8d9076e0 100644 --- a/config/ins.yaml +++ b/config/ins.yaml @@ -8,14 +8,14 @@ serial: baudrate: 921600 hw_flow_control: "off" -tcp: - ip_server: "" - port: 0 - -udp: - ip_server: "" - port: 0 - unicast_ip: "" +stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" configure_rx: true diff --git a/config/rover.yaml b/config/rover.yaml index c02cdb2a..ee81113c 100644 --- a/config/rover.yaml +++ b/config/rover.yaml @@ -8,14 +8,14 @@ serial: baudrate: 921600 hw_flow_control: off -tcp: - ip_server: "" - port: 0 - -udp: - ip_server: "" - port: 0 - unicast_ip: "" +stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" configure_rx: true diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 190d8c9f..6d5e6ecd 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -90,11 +90,16 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) static_cast(921600)); param("serial/hw_flow_control", settings_.hw_flow_control, static_cast("off")); - getUint32Param("tcp/port", settings_.tcp_port, static_cast(0)); - param("tcp/ip_server", settings_.tcp_ip_server, static_cast("")); - getUint32Param("udp/port", settings_.udp_port, static_cast(0)); - param("udp/unicast_ip", settings_.udp_unicast_ip, static_cast("")); - param("udp/ip_server", settings_.udp_ip_server, static_cast("")); + getUint32Param("stream_device/tcp/port", settings_.tcp_port, + static_cast(0)); + param("stream_device/tcp/ip_server", settings_.tcp_ip_server, + static_cast("")); + getUint32Param("stream_device/udp/port", settings_.udp_port, + static_cast(0)); + param("stream_device/udp/unicast_ip", settings_.udp_unicast_ip, + static_cast("")); + param("stream_device/udp/ip_server", settings_.udp_ip_server, + static_cast("")); param("login/user", settings_.login_user, static_cast("")); param("login/password", settings_.login_password, static_cast("")); settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list. From d14b88da4eeaf8042f383f69dfaf6090d8cfadaa Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Sat, 20 May 2023 16:35:52 +0200 Subject: [PATCH 133/170] Improve explanations in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6ad074c6..a01c09cb 100644 --- a/README.md +++ b/README.md @@ -387,7 +387,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Receiver Configuration - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`tcp/ip_server` and `tcp/port`) as dynamic connections is not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`stream_device/tcp/ip_server` and `stream_device/tcp/port`), since dynamic connections (`device` is tcp) are not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + default: true
From ae3270c7afcce3706fc6dbe95f4ff45e9e1deb7c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 12 Jun 2023 16:00:05 +0200 Subject: [PATCH 134/170] Update readme --- README.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/README.md b/README.md index a01c09cb..888a36d6 100644 --- a/README.md +++ b/README.md @@ -68,6 +68,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo + Once the catkin build or binary installation is finished, adapt the `config/rover.yaml` file according to your needs. The `launch/rover.launch` need not be modified. Specify the communication parameters, the ROS messages to be published, the frequency at which the latter should happen etc.:
+ Note for setting `ant_serial_nr` and `ant_aux1_serial_nr`: This is a string parameter, numeric-only serial numbers should be put in quotes. If this is not done a warning will be issued and the driver tries to parse it as integer. + Besides the aforementioned config file `rover.yaml` containing all parameters, specialized launch files for GNSS `config/gnss.yaml` and INS `config/ins.yaml` respectively contain only the relevant parameters in each case. + - NOTE: Unless `configure_rx` is set to `false`, this driver will overwrite the previous values of the parameters, even if the value is left to zero in the "yaml" file. + The driver was developed and tested with firmware versions >= 4.10.0 for GNSS and >= 1.3.2 for INS. Receivers with older firmware versions are supported but some features may not be available. Known limitations are: * GNSS with firmware < 4.10.0 does not support IP over USB. * GNSS with firmware < 4.12.1 does not support OSNMA. @@ -77,8 +78,6 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. - + An INS can be used in GNSS mode but some features may not be supported. Known limitations are: - * Antenna types cannot be set, leading to an error messages. The receiver still works, but precision may be degraded by a few mm. :
``` # Example configuration Settings for the Rover Rx @@ -336,7 +335,6 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo - Alternatively the lever arms may be specified via tf. Set `get_spatial_config_from_tf`to `true` in this case. - If the point of interest is neither the IMU nor the ARP of the main GNSS antenna, the vector between the IMU and the point of interest can be provided with the `ins_solution/poi_lever_arm` parameter. - - NOTE: This driver will always overwrite the previous value of the above mentioned parameters, also if the value is left to zero in the "yaml" file. - For further more information about Septentrio receivers, visit Septentrio [support resources](https://www.septentrio.com/en/supportresources) or check out the [user manual](https://www.septentrio.com/system/files/support/asterx_sbi3_user_manual_v1.0_0.pdf) and [reference guide](https://www.septentrio.com/system/files/support/asterx_sbi3_pro_firmware_v1.3.0_reference_guide.pdf) of the AsteRx SBi3 receiver. # ROSaic Parameters From adbb0155b784816a421ce5b1b0a25b5b7cb4b1ea Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 12 Jun 2023 16:31:16 +0200 Subject: [PATCH 135/170] Update version --- CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b7fd39f4..4dc84c4f 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.2.4 (upcoming) +1.3.0 (upcoming) ------------------ * New Features * Recovery from connection interruption From 222cf93efc492a1760fcdee008cea05989e5a2d4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 12 Jun 2023 18:09:41 +0200 Subject: [PATCH 136/170] Add known issues to readme --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 888a36d6..296db868 100644 --- a/README.md +++ b/README.md @@ -77,6 +77,8 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo * INS with firmware < 1.4.1 does not support improved VSM handling allowing for unknown variances. * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + + Known issues: + * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4.1. + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. :
``` From 926cbfbc244e4d764eebc1567f6f9f8e4dc93310 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 13 Jun 2023 14:08:04 +0200 Subject: [PATCH 137/170] Add expected release dates --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 296db868..ecd4998b 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + Known issues: - * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4.1. + * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4.1. For GNSS it will be fixed in the soon-to-be-released 4.14 (expected in July 2023), for INS it will be fixed in 1.4.1 (expected in November 2023). + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. :
``` From 9fbace029837a6dcdfde21d08345a952003aa212 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 15 Jun 2023 12:29:31 +0100 Subject: [PATCH 138/170] v1.3.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 3f8601b4..aa673511 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.2.3 + 1.3.0 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From b29058332b681ae566c98384b9a1c9b9fc44ce4a Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 15 Jun 2023 12:31:32 +0100 Subject: [PATCH 139/170] Updated package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index aa673511..3f8601b4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.0 + 1.2.3 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 4f4b0e75fe80c684c5bd87006ea73dbd398ac4c4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 15 Jun 2023 13:35:00 +0200 Subject: [PATCH 140/170] Update firmware info --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ecd4998b..52035db1 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + Known issues: - * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4.1. For GNSS it will be fixed in the soon-to-be-released 4.14 (expected in July 2023), for INS it will be fixed in 1.4.1 (expected in November 2023). + * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4. For GNSS it will be fixed in version 4.14 (released on June 15th 2023), for INS it will be fixed in to-be-released 1.4.1 (expected in November 2023). + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. :
``` From b039fe7c95ab629ee2d84c1203be996b30eac630 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 15 Jun 2023 12:36:31 +0100 Subject: [PATCH 141/170] v1.3.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 3f8601b4..aa673511 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.2.3 + 1.3.0 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From d2ba1c9036abb6b737ffda563e2241a850e3339e Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 15 Jun 2023 12:37:27 +0100 Subject: [PATCH 142/170] Updated package.xml --- CHANGELOG.rst | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4dc84c4f..dce1fda0 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.3.0 (upcoming) +1.3.0 (2022-06-15) ------------------ * New Features * Recovery from connection interruption diff --git a/package.xml b/package.xml index aa673511..3f8601b4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.0 + 1.2.3 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 256ae95700261519eed6a320dc6c7f499f65e3e1 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 15 Jun 2023 12:37:44 +0100 Subject: [PATCH 143/170] v1.3.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 3f8601b4..aa673511 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.2.3 + 1.3.0 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 6dd4e95c8b430c50e335ed0fc14d41ccbcd828e4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 15 Jun 2023 13:39:10 +0200 Subject: [PATCH 144/170] Update firmware info --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 52035db1..38d6b19e 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + Known issues: - * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4. For GNSS it will be fixed in version 4.14 (released on June 15th 2023), for INS it will be fixed in to-be-released 1.4.1 (expected in November 2023). + * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4. For GNSS it is fixed in version 4.14 (released on June 15th 2023), for INS it will be fixed in to-be-released 1.4.1 (expected in November 2023). + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. :
``` From ffb8b5c45c27fe014db0129afb8a3c0d57a4ee3f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 15 Jun 2023 13:46:16 +0200 Subject: [PATCH 145/170] Reduce INS firmware version info to released version --- src/septentrio_gnss_driver/communication/message_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index d9c18ff8..4b8d567c 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -2646,7 +2646,7 @@ namespace io { static const int32_t ins_major = 1; static const int32_t ins_minor = 4; - static const int32_t ins_patch = 1; + static const int32_t ins_patch = 0; static const int32_t gnss_major = 4; static const int32_t gnss_minor = 12; static const int32_t gnss_patch = 1; From 6c5de9c8331d7c8da868acaaa198b08443f44344 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 23 Jun 2023 13:48:24 +0200 Subject: [PATCH 146/170] Fix navsatfix containig only zeros for INS --- CHANGELOG.rst | 2 ++ src/septentrio_gnss_driver/communication/message_handler.cpp | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4dc84c4f..11ec849a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -20,6 +20,8 @@ Changelog for package septentrio_gnss_driver * Prevent message loss in file reading * Add some explanatory warnings for parameter mismatches * Add units to message definitions +* Fixes + * navsatfix for INS * Preliminary Features * Output of localization and tf in ECEF frame, testing and feedback welcome diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 4b8d567c..e2ec1096 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -1475,7 +1475,6 @@ namespace io { } last_ins_tow = last_insnavgeod_.block_header.tow; - NavSatFixMsg msg; uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; switch (type_of_pvt) { From 886c0b1bccfa03d07f93adf78cddc88833022bc6 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Fri, 23 Jun 2023 16:04:22 +0200 Subject: [PATCH 147/170] Align indent --- CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 11ec849a..bcf9ead7 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -21,7 +21,7 @@ Changelog for package septentrio_gnss_driver * Add some explanatory warnings for parameter mismatches * Add units to message definitions * Fixes - * navsatfix for INS + * navsatfix for INS * Preliminary Features * Output of localization and tf in ECEF frame, testing and feedback welcome From cadad3c729c9a3a93b6397f857124e5562e2f863 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 26 Jun 2023 11:11:32 +0200 Subject: [PATCH 148/170] Fix empty headers --- CHANGELOG.rst | 3 ++- .../communication/message_handler.cpp | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index bcf9ead7..cb2e4206 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -21,7 +21,8 @@ Changelog for package septentrio_gnss_driver * Add some explanatory warnings for parameter mismatches * Add units to message definitions * Fixes - * navsatfix for INS + * navsatfix for INS + * Empty headers * Preliminary Features * Output of localization and tf in ECEF frame, testing and feedback welcome diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index e2ec1096..c6f100d0 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -661,7 +661,6 @@ namespace io { if (!settings_->publish_twist) return; TwistWithCovarianceStampedMsg msg; - msg.header.frame_id = "navigation"; if (fromIns) { @@ -1392,6 +1391,8 @@ namespace io { last_poscovgeodetic_.block_header.tow)) return; + msg.header = last_pvtgeodetic_.header; + uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask; switch (type_of_pvt) { @@ -1475,6 +1476,8 @@ namespace io { } last_ins_tow = last_insnavgeod_.block_header.tow; + msg.header = last_insnavgeod_.header; + uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask; switch (type_of_pvt) { @@ -1560,6 +1563,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } + msg.header.frame_id = "wgs84"; publish("/navsatfix", msg); }; @@ -1732,6 +1736,7 @@ namespace io { if (settings_->septentrio_receiver_type == "gnss") { + msg.header = last_pvtgeodetic_.header; // PVT Status Analysis uint16_t status_mask = @@ -1889,6 +1894,8 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN; } else if (settings_->septentrio_receiver_type == "ins") { + msg.header = last_insnavgeod_.header; + // PVT Status Analysis uint16_t status_mask = 15; // We extract the first four bits using this mask. @@ -2045,6 +2052,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } + msg.header.frame_id = "wgs84"; publish("/gpsfix", msg); } From 2e103efd2e60ed22fc8f77a28badd61bb2d02d05 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 26 Jun 2023 11:14:54 +0200 Subject: [PATCH 149/170] Bump version --- CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index cb2e4206..7e259d4e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.3.0 (upcoming) +1.3.1 (upcoming) ------------------ * New Features * Recovery from connection interruption From fee109490161bb059bda094a4bf156c25efa1d66 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 29 Jun 2023 09:32:14 +0200 Subject: [PATCH 150/170] Fix single antenna receiver setup --- src/septentrio_gnss_driver/communication/communication_core.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index cb88b36e..2f5c593f 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -354,6 +354,7 @@ namespace io { } // Configure Aux1 antenna + if (settings_->multi_antenna) { std::stringstream ss; ss << "sat, Aux1, \"" << settings_->ant_type << "\"" @@ -377,6 +378,7 @@ namespace io { } // Configure Aux1 antenna + if (settings_->multi_antenna) { std::stringstream ss; ss << "sao, Aux1, " << string_utilities::trimDecimalPlaces(0.0) From b1a502888f3a85098635f865f6cdd215d8450cd1 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 29 Jun 2023 09:38:39 +0200 Subject: [PATCH 151/170] Update changelog --- CHANGELOG.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 7e259d4e..088b0798 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -23,6 +23,7 @@ Changelog for package septentrio_gnss_driver * Fixes * navsatfix for INS * Empty headers + * Single antenna receiver setup * Preliminary Features * Output of localization and tf in ECEF frame, testing and feedback welcome From bbcbe02b4c119e2cb9763a19da0320e3913cb047 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 3 Jul 2023 15:57:52 +0200 Subject: [PATCH 152/170] Fix navsatfix and gpsfix frame ids --- .../communication/message_handler.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index c6f100d0..fc4e7b17 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -952,7 +952,7 @@ namespace io { msg.header.frame_id = "utm_" + zonestring; msg.header.stamp = last_insnavgeod_.header.stamp; if (settings_->ins_use_poi) - msg.child_frame_id = settings_->poi_frame_id; // TODO param + msg.child_frame_id = settings_->poi_frame_id; else msg.child_frame_id = settings_->frame_id; @@ -1102,7 +1102,7 @@ namespace io { msg.header.frame_id = "ecef"; msg.header.stamp = last_insnavcart_.header.stamp; if (settings_->ins_use_poi) - msg.child_frame_id = settings_->poi_frame_id; // TODO param + msg.child_frame_id = settings_->poi_frame_id; else msg.child_frame_id = settings_->frame_id; @@ -1563,7 +1563,6 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - msg.header.frame_id = "wgs84"; publish("/navsatfix", msg); }; @@ -2052,7 +2051,6 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - msg.header.frame_id = "wgs84"; publish("/gpsfix", msg); } From e7bf6c9bc084c14dc2ec99f46bfd9a233156dc6e Mon Sep 17 00:00:00 2001 From: septentrio-users <71842935+septentrio-users@users.noreply.github.com> Date: Thu, 9 Mar 2023 13:19:41 +0100 Subject: [PATCH 153/170] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 38d6b19e..a0752e65 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ## Overview -This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with mosaic and AsteRx - two of Septentrio's cutting-edge GNSS/INS receiver families - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. +This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers From b606270681746dddd19e3b7c9eccf9bf0b3c5f83 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:22:58 +0100 Subject: [PATCH 154/170] Updated changelog --- CHANGELOG.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 088b0798..6c6b0ddd 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,14 +2,14 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.3.1 (upcoming) +1.3.1 (2023-07-06) ------------------ * New Features * Recovery from connection interruption * Add option to bypass configuration of Rx * OSNMA * Latency compensation for ROS timestamps - * Output of SBf block VelCovCartesian + * Output of SBF block VelCovCartesian * Support for UDP and TCP via IP server * New VSM handling allows for unknown variances (INS firmware >= 1.4.1) * Add heading angle to GPSFix msg (by diverting dip field, cf. readme) From ef544d9df906d124e7eb2b8d66de983ed6e4b8f7 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:24:17 +0100 Subject: [PATCH 155/170] v1.3.1 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index aa673511..92171be4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.0 + 1.3.1 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From c19252a145f1d9d0857ad303fd53e6e8e05fbf66 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:33:41 +0100 Subject: [PATCH 156/170] Updated package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 92171be4..aa673511 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.1 + 1.3.0 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 391cd7bd1ecae8ed7435eab4d54567a21a6b92d4 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:33:59 +0100 Subject: [PATCH 157/170] v1.3.1 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index aa673511..92171be4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.0 + 1.3.1 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 9fed982ae45cacce2ed9e3fe57d76ffe56ab5296 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:34:40 +0100 Subject: [PATCH 158/170] Updated package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 92171be4..aa673511 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.1 + 1.3.0 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From bdad2367ee75bd9dc2f8ccdabfb440cc2224fb82 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:34:52 +0100 Subject: [PATCH 159/170] v1.3.1 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index aa673511..92171be4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.0 + 1.3.1 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 4d61f60f1c906d1fa3f80003c7638934e06f6676 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:35:32 +0100 Subject: [PATCH 160/170] Updated package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 92171be4..aa673511 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.1 + 1.3.0 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 3641013d54f4812ed43d880a412d39ff4c6060e5 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Thu, 6 Jul 2023 23:35:46 +0100 Subject: [PATCH 161/170] v1.3.1 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index aa673511..92171be4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.0 + 1.3.1 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond From 056cc9a1f5406bdcd9784c43e7c94909d6274008 Mon Sep 17 00:00:00 2001 From: septentrio-users <71842935+septentrio-users@users.noreply.github.com> Date: Thu, 9 Mar 2023 13:19:41 +0100 Subject: [PATCH 162/170] Update README.md From dc7747b808af03d0c0daeac7ff3d474d756b6373 Mon Sep 17 00:00:00 2001 From: septentrio-users <71842935+septentrio-users@users.noreply.github.com> Date: Thu, 9 Mar 2023 13:19:41 +0100 Subject: [PATCH 163/170] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a0752e65..75b438b9 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ## Overview -This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. +This repository hosts a ROS Melodic and Noetic driver (i.e. for Linux only) - written in C++ - that works with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. Since Noetic will only be supported until 2025, a ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers From fab5159c529661de168c0ebefa77f73a5e175884 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 18 Jul 2023 15:14:45 +0200 Subject: [PATCH 164/170] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 75b438b9..a0752e65 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ## Overview -This repository hosts a ROS Melodic and Noetic driver (i.e. for Linux only) - written in C++ - that works with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. Since Noetic will only be supported until 2025, a ROS2 version is available in the branch `ros2`. +This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers From 3f659e8dde116d4453b196fe4f05d9fa772c1a34 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 18 Jul 2023 15:23:39 +0200 Subject: [PATCH 165/170] Merge upstream README --- README.md | 127 ++++++++++++++---------------------------------------- 1 file changed, 32 insertions(+), 95 deletions(-) diff --git a/README.md b/README.md index a0752e65..b772ee09 100644 --- a/README.md +++ b/README.md @@ -3,13 +3,12 @@ ## Overview -This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. +This repository hosts a ROS Melodic and Noetic driver (i.e. for Linux only) - written in C++ - that works with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. Since Noetic will only be supported until 2025, a ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers - Supports serial, TCP/IP and USB connections, the latter being compatible with both serial (RNDIS) and TCP/IP protocols - Supports several ASCII (including key NMEA ones) messages and SBF (Septentrio Binary Format) blocks -- Reports status of AIM+ (Advanced Interference Mitigation including OSNMA) anti-jamming and anti-spoofing. - Can publish `nav_msgs/Odometry` message for INS receivers - Can blend SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic` and `DOP` in order to publish `gps_common/GPSFix` and `sensor_msgs/NavSatFix` messages - Supports axis convention conversion as Septentrio follows the NED convention, whereas ROS is ENU. @@ -68,18 +67,14 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo + Once the catkin build or binary installation is finished, adapt the `config/rover.yaml` file according to your needs. The `launch/rover.launch` need not be modified. Specify the communication parameters, the ROS messages to be published, the frequency at which the latter should happen etc.:
+ Note for setting `ant_serial_nr` and `ant_aux1_serial_nr`: This is a string parameter, numeric-only serial numbers should be put in quotes. If this is not done a warning will be issued and the driver tries to parse it as integer. + Besides the aforementioned config file `rover.yaml` containing all parameters, specialized launch files for GNSS `config/gnss.yaml` and INS `config/ins.yaml` respectively contain only the relevant parameters in each case. - - NOTE: Unless `configure_rx` is set to `false`, this driver will overwrite the previous values of the parameters, even if the value is left to zero in the "yaml" file. + The driver was developed and tested with firmware versions >= 4.10.0 for GNSS and >= 1.3.2 for INS. Receivers with older firmware versions are supported but some features may not be available. Known limitations are: * GNSS with firmware < 4.10.0 does not support IP over USB. - * GNSS with firmware < 4.12.1 does not support OSNMA. * INS with firmware < 1.3.2 does not support NTP. - * INS with firmware < 1.4 does not support OSNMA. - * INS with firmware < 1.4.1 does not support improved VSM handling allowing for unknown variances. * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. - + Known issues: - * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4. For GNSS it is fixed in version 4.14 (released on June 15th 2023), for INS it will be fixed in to-be-released 1.4.1 (expected in November 2023). + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. + + An INS can be used in GNSS mode but some features may not be supported. Known limitations are: + * Antenna types cannot be set, leading to an error messages. The receiver still works, but precision may be degraded by a few mm. :
``` # Example configuration Settings for the Rover Rx @@ -89,28 +84,13 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo serial: baudrate: 921600 + rx_serial_port: USB1 hw_flow_control: off - - stream_device: - tcp: - ip_server: "" - port: 0 - udp: - ip_server: "" - port: 0 - unicast_ip: "" - - configure_rx: true login: user: "" password: "" - osnma: - mode: "off" - ntp_server: "" - keep_open: true - frame_id: gnss imu_frame_id: imu @@ -159,8 +139,6 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo use_gnss_time: false - latency_compensation: false - rtk_settings: ntrip_1: id: "NTR1" @@ -227,15 +205,12 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo basevectorgeod: false poscovcartesian: false poscovgeodetic: true - velcovcartesian: false velcovgeodetic: false atteuler: true attcoveuler: true pose: false twist: false diagnostics: false - aimplusstatus: true - galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false @@ -249,9 +224,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo exteventinsnavgeod: false imu: false localization: false - tf: false - localization_ecef: false - tf_ecef: false + tf: false # INS-Specific Parameters @@ -337,6 +310,7 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo - Alternatively the lever arms may be specified via tf. Set `get_spatial_config_from_tf`to `true` in this case. - If the point of interest is neither the IMU nor the ARP of the main GNSS antenna, the vector between the IMU and the point of interest can be provided with the `ins_solution/poi_lever_arm` parameter. + - NOTE: This driver will always overwrite the previous value of the above mentioned parameters, also if the value is left to zero in the "yaml" file. - For further more information about Septentrio receivers, visit Septentrio [support resources](https://www.septentrio.com/en/supportresources) or check out the [user manual](https://www.septentrio.com/system/files/support/asterx_sbi3_user_manual_v1.0_0.pdf) and [reference guide](https://www.septentrio.com/system/files/support/asterx_sbi3_pro_firmware_v1.3.0_reference_guide.pdf) of the AsteRx SBi3 receiver. # ROSaic Parameters @@ -345,51 +319,25 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Connectivity Specs - + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. Incoming data streams of SBF blocks and NMEA sentences are recevied either via this interface or a static IP server for TCP and/or UDP. The former will be utilized if section `stream_device/tcp` and `stream_device/udp` are not configured. - + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + + `device`: location of device connection + + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyUSB0` + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. + Regarding the file path, ROS_HOME=\`pwd\` in front of `roslaunch septentrio...` might be useful to specify that the node should be started using the executable's directory as its working-directory. + `tcp://host:port` format for TCP/IP connections + `28784` should be used as the default (command) port for TCP/IP connections. If another port is specified, the receiver needs to be (re-)configured via the Web Interface before ROSaic can be used. + An RNDIS IP interface is provided via USB, assigning the address `192.168.3.1` to the receiver. This should work on most modern Linux distributions. To verify successful connection, open a web browser to access the web interface of the receiver using the IP address `192.168.3.1`. - + default: `tcp://192.168.3.1:28784` + + default: `tcp://192.168.3.1:28784 ` + `serial`: specifications for serial communication + `baudrate`: serial baud rate to be used in a serial connection. Ensure the provided rate is sufficient for the chosen SBF blocks. For example, activating MeasEpoch (also necessary for /gpsfix) may require up to almost 400 kBit/s. - + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not - + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + + `rx_serial_port`: determines to which (virtual) serial port of the Rx we want to get connected to, e.g. USB1 or COM1 + + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART HW flow control enabled or not + + `off` to disable UART HW flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` - + `stream_device`: If left unconfigured, by default `device` is utilized for the data streams. Within `stream_device` static IP servers may be defined instead. In config mode (`configure_rx` set to `true`), TCP will be prioritized over UDP. If Rx is pre-configured, both may be set simultaneously. - + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences. - + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. - + `port`: UDP destination port. - + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. - + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. - + `port`: UDP destination port. - + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password
- -
- OSNMA - - + `osnma`: Configuration of the Open Service Navigation Message Authentication (OSNMA) feature. - + `mode`: Three operating modes are supported: `off` where OSNMA authentication is disabled, `loose` where satellites are included in the PVT if they are successfully authenticated or if their authentication status is unknown, and `strict` where only successfully-authenticated satellites are included in the PVT. In case of `strict` synchronization via NTP is mandatory. - + default: off - + `ntp_server`: In `strict` mode, OSNMA authentication requires the availability of external time information. In `loose` mode, this is optional but recommended for enhanced security. The receiver can connect to an NTP time server for this purpose. Options are `default` to let the receiver choose an NTP server or specify one like `pool.ntp.org` for example. - + default: "" - + `keep_open`: Wether OSNMA shall be kept active on driver shutdown. - + default: true -
- -
- Receiver Configuration - - + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`stream_device/tcp/ip_server` and `stream_device/tcp/port`), since dynamic connections (`device` is tcp) are not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). - + default: true -
Receiver Type @@ -397,13 +345,14 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `receiver_type`: This parameter is to select the type of the Septentrio receiver + `gnss` for GNSS receivers. + `ins` for INS receivers. + + `ins_in_gnss_mode` INS receivers in GNSS mode. + default: `gnss` + `multi_antenna`: Whether or not the Rx has multiple antennas. + default: `false`
- Frame IDs + Frame ID + `frame_id`: name of the ROS tf frame for the Rx, placed in the header of published GNSS messages. It corresponds to the frame of the main antenna. + In ROS, the [tf package](https://wiki.ros.org/tf) lets you keep track of multiple coordinate frames over time. The frame ID will be resolved by [`tf_prefix`](http://wiki.ros.org/geometry/CoordinateFrameConventions) if defined. If a ROS message has a header (all of those we publish do), the frame ID can be found via `rostopic echo /topic`, where `/topic` is the topic into which the message is being published. @@ -496,8 +445,6 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `use_gnss_time`: `true` if the ROS message headers' unix epoch time field shall be constructed from the TOW/WNC (in the SBF case) and UTC (in the NMEA case) data, `false` if those times shall be taken by the driver from ROS time. If `use_gnss_time` is set to `true`, make sure the ROS system is synchronized to an NTP time server either via internet or ideally via the Septentrio receiver since the latter serves as a Stratum 1 time server not dependent on an internet connection. The NTP server of the receiver is automatically activated on the Septentrio receiver (for INS/GNSS a firmware >= 1.3.3 is needed). + default: `true` - + `latency_compensation`: Rx reports processing latency in PVT and INS blocks. If set to `true`this latency is subtracted from ROS timestamps in related blocks (i.e., `use_gnss_time` set to `false`). Related blocks are INS, PVT, Covariances, and BaseVectors. In case of `use_gnss_time` set to `true`, the latency is already compensated within the RX and included in the reported timestamps. - + default: `false`
@@ -591,7 +538,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + default: "" + `config`: Defines which measurements belonging to the respective axes are forwarded to the INS. In addition, non-holonomic constraints may be introduced for directions known to be restricted in movement. For example, a vehicle with Ackermann steering is limited in its sidewards and upwards movement. So, even if only motion in x-direction may be measured, zero-velocities for y and z may be sent. + default: [] - + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values from inside the ROS messages are used. + + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values inside the messaged are used. + default: false + `variances`: Variances of the respective axes. Only have to be set if `ins_vsm/ros/variances_by_parameter` is set to `true`. Values must be > 0.0, else measurements cannot not be used. + default: [] @@ -626,15 +573,12 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/gpgsa`: `true` to publish `nmea_msgs/GPGSA.msg` messages into the topic `/gpgsa` + `publish/gpgsv`: `true` to publish `nmea_msgs/GPGSV.msg` messages into the topic `/gpgsv` + `publish/measepoch`: `true` to publish `septentrio_gnss_driver/MeasEpoch.msg` messages into the topic `/measepoch` - + `publish/galauthstatus`: `true` to publish `septentrio_gnss_driver/GALAuthStatus.msg` messages into the topic `/galauthstatus` and corresponding `/diganostics` - + `publish/aimplusstatus`: `true` to publish `septentrio_gnss_driver/RFStatus.msg` messages into the topic `/rfstatus`, `septentrio_gnss_driver/AIMPlusStatus.msg` messages into `/aimplusstatus` and corresponding `/diganostics`. Some information is only available with active OSNMA. + `publish/pvtcartesian`: `true` to publish `septentrio_gnss_driver/PVTCartesian.msg` messages into the topic `/pvtcartesian` + `publish/pvtgeodetic`: `true` to publish `septentrio_gnss_driver/PVTGeodetic.msg` messages into the topic `/pvtgeodetic` + `publish/basevectorcart`: `true` to publish `septentrio_gnss_driver/BaseVectorCart.msg` messages into the topic `/basevectorcart` + `publish/basevectorgeod`: `true` to publish `septentrio_gnss_driver/BaseVectorGeod.msg` messages into the topic `/basevectorgeod` + `publish/poscovcartesian`: `true` to publish `septentrio_gnss_driver/PosCovCartesian.msg` messages into the topic `/poscovcartesian` + `publish/poscovgeodetic`: `true` to publish `septentrio_gnss_driver/PosCovGeodetic.msg` messages into the topic `/poscovgeodetic` - + `publish/velcovcartesian`: `true` to publish `septentrio_gnss_driver/VelCovCartesian.msg` messages into the topic `/velcovcartesian` + `publish/velcovgeodetic`: `true` to publish `septentrio_gnss_driver/VelCovGeodetic.msg` messages into the topic `/velcovgeodetic` + `publish/atteuler`: `true` to publish `septentrio_gnss_driver/AttEuler.msg` messages into the topic `/atteuler` + `publish/attcoveuler`: `true` to publish `septentrio_gnss_driver/AttCovEuler.msg` messages into the topic `/attcoveuler` @@ -653,38 +597,32 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/exteventinsnavgeod`: `true` to publish `septentrio_gnss_driver/ExtEventINSNavGeod.msgs` message into the topic`/exteventinsnavgeod` + `publish/imu`: `true` to publish `sensor_msgs/Imu.msg` message into the topic`/imu` + `publish/localization`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` - + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`. - + `publish/localization_ecef`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` related to ECEF frame. - + `publish/tf_ecef`: `true` to broadcast tf of localization related to ECEF frame. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`. + + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf.
## ROS Topic Publications A selection of NMEA sentences, the majority being standardized sentences, and proprietary SBF blocks is translated into ROS messages, partly generic and partly custom, and can be published at the discretion of the user into the following ROS topics. All published ROS messages, even custom ones, start with a ROS generic header [`std_msgs/Header.msg`](https://docs.ros.org/melodic/api/std_msgs/html/msg/Header.html), which includes the receiver time stamp as well as the frame ID, the latter being specified in the ROS parameter `frame_id`.
Available ROS Topics - + + `/gpgga`: publishes [`nmea_msgs/Gpgga.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgga.html) - converted from the NMEA sentence GGA. + `/gprmc`: publishes [`nmea_msgs/Gprmc.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gprmc.html) - converted from the NMEA sentence RMC. + `/gpgsa`: publishes [`nmea_msgs/Gpgsa.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsa.html) - converted from the NMEA sentence GSA. + `/gpgsv`: publishes [`nmea_msgs/Gpgsv.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsv.html) - converted from the NMEA sentence GSV. + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. - + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. - + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, compiled from the SBF block `RFStatus`. - + `/aimplusstatus`: publishes custom ROS message `septentrio_gnss_driver/AIMPlusStatus.msg`, reporting status of AIM+. Converted from SBF blocks `RFStatus` and optionally `GALAuthStatus`. For the latter OSNMA has to be activated. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. + `/basevectorgeod`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorGeod.msg`, corresponding to the SBF block `BaseVectorGeod`. + `/poscovcartesian`: publishes custom ROS message `septentrio_gnss_driver/PosCovCartesian.msg`, corresponding to SBF block `PosCovCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/poscovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PosCovGeodetic.msg`, corresponding to SBF block `PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). - + `/velcovcartesian`: publishes custom ROS message `septentrio_gnss_driver/VelCovCartesian.msg`, corresponding to SBF block `VelCovCartesian` (GNSS case). + `/velcovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/VelCovGeodetic.msg`, corresponding to SBF block `VelCovGeodetic` (GNSS case). + `/atteuler`: publishes custom ROS message `septentrio_gnss_driver/AttEuler.msg`, corresponding to SBF block `AttEuler`. + `/attcoveuler`: publishes custom ROS message `septentrio_gnss_driver/AttCovEuler.msg`, corresponding to the SBF block `AttCovEuler`. - + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its block header. + + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its header, or - if `use_gnss_time` is set to `false` - from the systems's wall-clock time. + `/navsatfix`: publishes generic ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`,`PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + The ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) can be fed directly into the [`navsat_transform_node`](https://docs.ros.org/melodic/api/robot_localization/html/navsat_transform_node.html) of the ROS navigation stack. - + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). In order to publish heading information, the field *dip* is diverted from its intended meaning an populated with the heading angle and *err_dip* with its uncertainty. + + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). + `/pose`: publishes generic ROS message [`geometry_msgs/PoseWithCovarianceStamped.msg`](https://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `AttEuler`, `AttCovEuler` (GNSS case) or `INSNavGeod` (INS case). + Note that GNSS provides absolute positioning, while robots are often localized within a local level cartesian frame. The pose field of this ROS message contains position with respect to the absolute ENU frame (longitude, latitude, height), i.e. not a cartesian frame, while the orientation is with respect to a vehicle-fixed (e.g. for mosaic-x5 in moving base mode via the command `setAttitudeOffset`, ...) !local! NED frame or ENU frame if `use_ros_axis_directions` is set `true`. Thus the orientation is !not! given with respect to the same frame as the position is given in. The cross-covariances are hence set to 0. + `/twist`: publishes generic ROS message [`geometry_msgs/TwistWithCovarianceStamped.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/TwistWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic` and `VelCovGeodetic`. @@ -701,14 +639,13 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + The ROS message [`sensor_msgs/Imu.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + `/localization`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. - + `/localization_ecef`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF blocks `INSNavCart` and `INSNavGeod`. - + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention.
## Suggestions for Improvements
Some Ideas + + Automatic Search: If the host address of the receiver is omitted in the `host:port` specification, the driver could automatically search and establish a connection on the specified port. + Equip ROSaic with an NTRIP client such that it can forward corrections to the receiver independently of `Data Link`.
@@ -718,16 +655,16 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr Is there an SBF or NMEA message that is not being addressed while being important to your application? If yes, follow these steps: 1. Find the log reference of interest in the publicly accessible, official documentation. Hence select the reference guide file, e.g. for mosaic-x5 in the [product support section for mosaic-X5](https://www.septentrio.com/en/support/mosaic/mosaic-x5), Chapter 4, of Septentrio's homepage. - 2. SBF: Add a new `.msg` file to the `../msg` folder. And modify the `../CMakeLists.txt` file by adding a new entry to the `add_message_files` section. - 3. Add msg header and typedef to `typedefs.hpp`. - 4. Parsers: - - SBF: Add a parser to the `sbf_blocks.hpp` file. - - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `../src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `../include/septentrio_gnss_driver/parsers/nmea_parsers` folder. - 5. Processing the message/block: - - SBF: Extend the `SbfId` enumeration in the `message_handler.hpp` file with a new entry. - - SBF: Extend the SBF switch-case in `message_handler.cpp` file with a new case. - - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. - - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. - 6. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. - 7. Add SBF block or NMEA to data stream setup in `communication_core.cpp` (function `configureRx()`). + 2. Add a new `.msg` file to the `septentrio_gnss_driver/msg` folder. + 3. SBF: Add the new struct definition to the `sbf_structs.hpp` file. + 4. Parsing/Processing the message/block: + - Both: Add a new include guard to let the compiler know about the existence of the header file (such as `septentrio_gnss_driver/PVTGeodetic.h`) that gets compiler-generated from the `.msg` file constructed in step 3. + - SBF: Extend the `NMEA_ID_Enum` enumeration in the `rx_message.hpp` file with a new entry. + - SBF: Extend the initialization of the `RxIDMap` map in the `rx_message.cpp` file with a new pair. + - SBF: Add a new callback function declaration, a new method, to the `io_comm_rx::RxMessage class` in the `rx_message.hpp` file. + - SBF: Add the latter's definition to the `rx_message.cpp` file. + - SBF: Add a new C++ "case" (part of the C++ switch-case structure) in the `rx_message.hpp` file. It should be modeled on the existing `evPVTGeodetic` case, e.g. one needs a static counter variable declaration. + - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `septentrio_gnss_driver/src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `septentrio_gnss_driver/include/septentrio_gnss_driver/parsers/nmea_parsers` folder. + 5. Create a new `publish/..` ROSaic parameter in the `septentrio_gnss_driver/config/rover.yaml` file, create a global boolean variable `publish_...` in the `septentrio_gnss_driver/src/septentrio_gnss_driver/node/rosaic_node.cpp` file, insert the publishing callback function to the C++ "multimap" `IO.handlers_.callbackmap_` - which is already storing all the others - in the `rosaic_node::ROSaicNode::defineMessages()` method in the same file and add an `extern bool publish_...;` line to the `septentrio_gnss_driver/include/septentrio_gnss_driver/node/rosaic_node.hpp` file. + 6. Modify the `septentrio_gnss_driver/CMakeLists.txt` file by adding a new entry to the `add_message_files` section.
From 1bce6ccad2febbacdedd482b651b68206119c25f Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Tue, 18 Jul 2023 15:24:31 +0200 Subject: [PATCH 166/170] Merge upstream README pt2 --- README.md | 127 ++++++++++++++++++++++++++++++++++++++++-------------- 1 file changed, 95 insertions(+), 32 deletions(-) diff --git a/README.md b/README.md index b772ee09..a0752e65 100644 --- a/README.md +++ b/README.md @@ -3,12 +3,13 @@ ## Overview -This repository hosts a ROS Melodic and Noetic driver (i.e. for Linux only) - written in C++ - that works with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. Since Noetic will only be supported until 2025, a ROS2 version is available in the branch `ros2`. +This repository hosts drivers for ROS (Melodic and Noetic) and ROS 2 (Foxy, Galactic, Humble, Rolling, and beyond) - written in C++ - that work with [mosaic](https://web.septentrio.com/GH-SSN-modules) and [AsteRx](https://web.septentrio.com/INS-SSN-Rx) - two of Septentrio's cutting-edge GNSS and GNSS/INS [receiver families](https://web.septentrio.com/GH-SSN-RX) - and beyond. The ROs version is available in this branch, whereas the ROS2 version is available in the branch `ros2`. Main Features: - Supports Septentrio's single antenna GNSS, dual antenna GNSS and INS receivers - Supports serial, TCP/IP and USB connections, the latter being compatible with both serial (RNDIS) and TCP/IP protocols - Supports several ASCII (including key NMEA ones) messages and SBF (Septentrio Binary Format) blocks +- Reports status of AIM+ (Advanced Interference Mitigation including OSNMA) anti-jamming and anti-spoofing. - Can publish `nav_msgs/Odometry` message for INS receivers - Can blend SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic` and `DOP` in order to publish `gps_common/GPSFix` and `sensor_msgs/NavSatFix` messages - Supports axis convention conversion as Septentrio follows the NED convention, whereas ROS is ENU. @@ -67,14 +68,18 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo + Once the catkin build or binary installation is finished, adapt the `config/rover.yaml` file according to your needs. The `launch/rover.launch` need not be modified. Specify the communication parameters, the ROS messages to be published, the frequency at which the latter should happen etc.:
+ Note for setting `ant_serial_nr` and `ant_aux1_serial_nr`: This is a string parameter, numeric-only serial numbers should be put in quotes. If this is not done a warning will be issued and the driver tries to parse it as integer. + Besides the aforementioned config file `rover.yaml` containing all parameters, specialized launch files for GNSS `config/gnss.yaml` and INS `config/ins.yaml` respectively contain only the relevant parameters in each case. + - NOTE: Unless `configure_rx` is set to `false`, this driver will overwrite the previous values of the parameters, even if the value is left to zero in the "yaml" file. + The driver was developed and tested with firmware versions >= 4.10.0 for GNSS and >= 1.3.2 for INS. Receivers with older firmware versions are supported but some features may not be available. Known limitations are: * GNSS with firmware < 4.10.0 does not support IP over USB. + * GNSS with firmware < 4.12.1 does not support OSNMA. * INS with firmware < 1.3.2 does not support NTP. + * INS with firmware < 1.4 does not support OSNMA. + * INS with firmware < 1.4.1 does not support improved VSM handling allowing for unknown variances. * INS with firmware 1.2.0 does not support velocity aiding. * INS with firmware 1.2.0 does not support setting of initial heading. + + Known issues: + * UDP over USB: Blocks are sent twice on GNSS with firmware <= 4.12.1 and INS with firmware <= 1.4. For GNSS it is fixed in version 4.14 (released on June 15th 2023), for INS it will be fixed in to-be-released 1.4.1 (expected in November 2023). + If `use_ros_axis_orientation` to `true` axis orientations are converted by the driver between NED (Septentrio: yaw = 0 is north, positive clockwise) and ENU (ROS: yaw = 0 is east, positive counterclockwise). There is no conversion when setting this parameter to `false` and the angles will be consistent with the web GUI. - + An INS can be used in GNSS mode but some features may not be supported. Known limitations are: - * Antenna types cannot be set, leading to an error messages. The receiver still works, but precision may be degraded by a few mm. :
``` # Example configuration Settings for the Rover Rx @@ -84,13 +89,28 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo serial: baudrate: 921600 - rx_serial_port: USB1 hw_flow_control: off + + stream_device: + tcp: + ip_server: "" + port: 0 + udp: + ip_server: "" + port: 0 + unicast_ip: "" + + configure_rx: true login: user: "" password: "" + osnma: + mode: "off" + ntp_server: "" + keep_open: true + frame_id: gnss imu_frame_id: imu @@ -139,6 +159,8 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo use_gnss_time: false + latency_compensation: false + rtk_settings: ntrip_1: id: "NTR1" @@ -205,12 +227,15 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo basevectorgeod: false poscovcartesian: false poscovgeodetic: true + velcovcartesian: false velcovgeodetic: false atteuler: true attcoveuler: true pose: false twist: false diagnostics: false + aimplusstatus: true + galauthstatus: false # For GNSS Rx only gpgsa: false gpgsv: false @@ -224,7 +249,9 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo exteventinsnavgeod: false imu: false localization: false - tf: false + tf: false + localization_ecef: false + tf_ecef: false # INS-Specific Parameters @@ -310,7 +337,6 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo - Alternatively the lever arms may be specified via tf. Set `get_spatial_config_from_tf`to `true` in this case. - If the point of interest is neither the IMU nor the ARP of the main GNSS antenna, the vector between the IMU and the point of interest can be provided with the `ins_solution/poi_lever_arm` parameter. - - NOTE: This driver will always overwrite the previous value of the above mentioned parameters, also if the value is left to zero in the "yaml" file. - For further more information about Septentrio receivers, visit Septentrio [support resources](https://www.septentrio.com/en/supportresources) or check out the [user manual](https://www.septentrio.com/system/files/support/asterx_sbi3_user_manual_v1.0_0.pdf) and [reference guide](https://www.septentrio.com/system/files/support/asterx_sbi3_pro_firmware_v1.3.0_reference_guide.pdf) of the AsteRx SBi3 receiver. # ROSaic Parameters @@ -319,25 +345,51 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
Connectivity Specs - + `device`: location of device connection - + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyUSB0` + + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. Incoming data streams of SBF blocks and NMEA sentences are recevied either via this interface or a static IP server for TCP and/or UDP. The former will be utilized if section `stream_device/tcp` and `stream_device/udp` are not configured. + + `serial:xxx` format for serial connections, where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`. + `file_name:path/to/file.sbf` format for publishing from an SBF log + `file_name:path/to/file.pcap` format for publishing from PCAP capture. + Regarding the file path, ROS_HOME=\`pwd\` in front of `roslaunch septentrio...` might be useful to specify that the node should be started using the executable's directory as its working-directory. + `tcp://host:port` format for TCP/IP connections + `28784` should be used as the default (command) port for TCP/IP connections. If another port is specified, the receiver needs to be (re-)configured via the Web Interface before ROSaic can be used. + An RNDIS IP interface is provided via USB, assigning the address `192.168.3.1` to the receiver. This should work on most modern Linux distributions. To verify successful connection, open a web browser to access the web interface of the receiver using the IP address `192.168.3.1`. - + default: `tcp://192.168.3.1:28784 ` + + default: `tcp://192.168.3.1:28784` + `serial`: specifications for serial communication + `baudrate`: serial baud rate to be used in a serial connection. Ensure the provided rate is sufficient for the chosen SBF blocks. For example, activating MeasEpoch (also necessary for /gpsfix) may require up to almost 400 kBit/s. - + `rx_serial_port`: determines to which (virtual) serial port of the Rx we want to get connected to, e.g. USB1 or COM1 - + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART HW flow control enabled or not - + `off` to disable UART HW flow control, `RTS|CTS` to enable it + + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not + + `off` to disable UART hardware flow control, `RTS|CTS` to enable it + default: `921600`, `USB1`, `off` + + `stream_device`: If left unconfigured, by default `device` is utilized for the data streams. Within `stream_device` static IP servers may be defined instead. In config mode (`configure_rx` set to `true`), TCP will be prioritized over UDP. If Rx is pre-configured, both may be set simultaneously. + + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. + + `ip_server`: IP server of Rx to be used, e.g. “IPS1”. + + `port`: UDP destination port. + + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used. + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access. + `user`: user name + `password`: password
+ +
+ OSNMA + + + `osnma`: Configuration of the Open Service Navigation Message Authentication (OSNMA) feature. + + `mode`: Three operating modes are supported: `off` where OSNMA authentication is disabled, `loose` where satellites are included in the PVT if they are successfully authenticated or if their authentication status is unknown, and `strict` where only successfully-authenticated satellites are included in the PVT. In case of `strict` synchronization via NTP is mandatory. + + default: off + + `ntp_server`: In `strict` mode, OSNMA authentication requires the availability of external time information. In `loose` mode, this is optional but recommended for enhanced security. The receiver can connect to an NTP time server for this purpose. Options are `default` to let the receiver choose an NTP server or specify one like `pool.ntp.org` for example. + + default: "" + + `keep_open`: Wether OSNMA shall be kept active on driver shutdown. + + default: true +
+ +
+ Receiver Configuration + + + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`stream_device/tcp/ip_server` and `stream_device/tcp/port`), since dynamic connections (`device` is tcp) are not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*). + + default: true +
Receiver Type @@ -345,14 +397,13 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `receiver_type`: This parameter is to select the type of the Septentrio receiver + `gnss` for GNSS receivers. + `ins` for INS receivers. - + `ins_in_gnss_mode` INS receivers in GNSS mode. + default: `gnss` + `multi_antenna`: Whether or not the Rx has multiple antennas. + default: `false`
- Frame ID + Frame IDs + `frame_id`: name of the ROS tf frame for the Rx, placed in the header of published GNSS messages. It corresponds to the frame of the main antenna. + In ROS, the [tf package](https://wiki.ros.org/tf) lets you keep track of multiple coordinate frames over time. The frame ID will be resolved by [`tf_prefix`](http://wiki.ros.org/geometry/CoordinateFrameConventions) if defined. If a ROS message has a header (all of those we publish do), the frame ID can be found via `rostopic echo /topic`, where `/topic` is the topic into which the message is being published. @@ -445,6 +496,8 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `use_gnss_time`: `true` if the ROS message headers' unix epoch time field shall be constructed from the TOW/WNC (in the SBF case) and UTC (in the NMEA case) data, `false` if those times shall be taken by the driver from ROS time. If `use_gnss_time` is set to `true`, make sure the ROS system is synchronized to an NTP time server either via internet or ideally via the Septentrio receiver since the latter serves as a Stratum 1 time server not dependent on an internet connection. The NTP server of the receiver is automatically activated on the Septentrio receiver (for INS/GNSS a firmware >= 1.3.3 is needed). + default: `true` + + `latency_compensation`: Rx reports processing latency in PVT and INS blocks. If set to `true`this latency is subtracted from ROS timestamps in related blocks (i.e., `use_gnss_time` set to `false`). Related blocks are INS, PVT, Covariances, and BaseVectors. In case of `use_gnss_time` set to `true`, the latency is already compensated within the RX and included in the reported timestamps. + + default: `false`
@@ -538,7 +591,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + default: "" + `config`: Defines which measurements belonging to the respective axes are forwarded to the INS. In addition, non-holonomic constraints may be introduced for directions known to be restricted in movement. For example, a vehicle with Ackermann steering is limited in its sidewards and upwards movement. So, even if only motion in x-direction may be measured, zero-velocities for y and z may be sent. + default: [] - + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values inside the messaged are used. + + `variances_by_parameter`: Wether variances shall be entered by parameter `ins_vsm/ros/variances` or the values from inside the ROS messages are used. + default: false + `variances`: Variances of the respective axes. Only have to be set if `ins_vsm/ros/variances_by_parameter` is set to `true`. Values must be > 0.0, else measurements cannot not be used. + default: [] @@ -573,12 +626,15 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/gpgsa`: `true` to publish `nmea_msgs/GPGSA.msg` messages into the topic `/gpgsa` + `publish/gpgsv`: `true` to publish `nmea_msgs/GPGSV.msg` messages into the topic `/gpgsv` + `publish/measepoch`: `true` to publish `septentrio_gnss_driver/MeasEpoch.msg` messages into the topic `/measepoch` + + `publish/galauthstatus`: `true` to publish `septentrio_gnss_driver/GALAuthStatus.msg` messages into the topic `/galauthstatus` and corresponding `/diganostics` + + `publish/aimplusstatus`: `true` to publish `septentrio_gnss_driver/RFStatus.msg` messages into the topic `/rfstatus`, `septentrio_gnss_driver/AIMPlusStatus.msg` messages into `/aimplusstatus` and corresponding `/diganostics`. Some information is only available with active OSNMA. + `publish/pvtcartesian`: `true` to publish `septentrio_gnss_driver/PVTCartesian.msg` messages into the topic `/pvtcartesian` + `publish/pvtgeodetic`: `true` to publish `septentrio_gnss_driver/PVTGeodetic.msg` messages into the topic `/pvtgeodetic` + `publish/basevectorcart`: `true` to publish `septentrio_gnss_driver/BaseVectorCart.msg` messages into the topic `/basevectorcart` + `publish/basevectorgeod`: `true` to publish `septentrio_gnss_driver/BaseVectorGeod.msg` messages into the topic `/basevectorgeod` + `publish/poscovcartesian`: `true` to publish `septentrio_gnss_driver/PosCovCartesian.msg` messages into the topic `/poscovcartesian` + `publish/poscovgeodetic`: `true` to publish `septentrio_gnss_driver/PosCovGeodetic.msg` messages into the topic `/poscovgeodetic` + + `publish/velcovcartesian`: `true` to publish `septentrio_gnss_driver/VelCovCartesian.msg` messages into the topic `/velcovcartesian` + `publish/velcovgeodetic`: `true` to publish `septentrio_gnss_driver/VelCovGeodetic.msg` messages into the topic `/velcovgeodetic` + `publish/atteuler`: `true` to publish `septentrio_gnss_driver/AttEuler.msg` messages into the topic `/atteuler` + `publish/attcoveuler`: `true` to publish `septentrio_gnss_driver/AttCovEuler.msg` messages into the topic `/attcoveuler` @@ -597,32 +653,38 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi + `publish/exteventinsnavgeod`: `true` to publish `septentrio_gnss_driver/ExtEventINSNavGeod.msgs` message into the topic`/exteventinsnavgeod` + `publish/imu`: `true` to publish `sensor_msgs/Imu.msg` message into the topic`/imu` + `publish/localization`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` - + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. + + `publish/tf`: `true` to broadcast tf of localization. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`. + + `publish/localization_ecef`: `true` to publish `nav_msgs/Odometry.msg` message into the topic`/localization` related to ECEF frame. + + `publish/tf_ecef`: `true` to broadcast tf of localization related to ECEF frame. `ins_use_poi` must also be set to true to publish tf. Note that only one of `publish/tf` or `publish/tf_ecef` may be `true`.
## ROS Topic Publications A selection of NMEA sentences, the majority being standardized sentences, and proprietary SBF blocks is translated into ROS messages, partly generic and partly custom, and can be published at the discretion of the user into the following ROS topics. All published ROS messages, even custom ones, start with a ROS generic header [`std_msgs/Header.msg`](https://docs.ros.org/melodic/api/std_msgs/html/msg/Header.html), which includes the receiver time stamp as well as the frame ID, the latter being specified in the ROS parameter `frame_id`.
Available ROS Topics - + + `/gpgga`: publishes [`nmea_msgs/Gpgga.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgga.html) - converted from the NMEA sentence GGA. + `/gprmc`: publishes [`nmea_msgs/Gprmc.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gprmc.html) - converted from the NMEA sentence RMC. + `/gpgsa`: publishes [`nmea_msgs/Gpgsa.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsa.html) - converted from the NMEA sentence GSA. + `/gpgsv`: publishes [`nmea_msgs/Gpgsv.msg`](https://docs.ros.org/api/nmea_msgs/html/msg/Gpgsv.html) - converted from the NMEA sentence GSV. + `/measepoch`: publishes custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch`. + + `/galauthstatus`: publishes custom ROS message `septentrio_gnss_driver/GALAuthStatus.msg`, corresponding to the SBF block `GALAuthStatus`. + + `/rfstatus`: publishes custom ROS message `septentrio_gnss_driver/RFStatus.msg`, compiled from the SBF block `RFStatus`. + + `/aimplusstatus`: publishes custom ROS message `septentrio_gnss_driver/AIMPlusStatus.msg`, reporting status of AIM+. Converted from SBF blocks `RFStatus` and optionally `GALAuthStatus`. For the latter OSNMA has to be activated. + `/pvtcartesian`: publishes custom ROS message `septentrio_gnss_driver/PVTCartesian.msg`, corresponding to the SBF block `PVTCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/pvtgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PVTGeodetic.msg`, corresponding to the SBF block `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case). + `/basevectorcart`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorCart.msg`, corresponding to the SBF block `BaseVectorCart`. + `/basevectorgeod`: publishes custom ROS message `septentrio_gnss_driver/BaseVectorGeod.msg`, corresponding to the SBF block `BaseVectorGeod`. + `/poscovcartesian`: publishes custom ROS message `septentrio_gnss_driver/PosCovCartesian.msg`, corresponding to SBF block `PosCovCartesian` (GNSS case) or `INSNavGeod` (INS case). + `/poscovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/PosCovGeodetic.msg`, corresponding to SBF block `PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + + `/velcovcartesian`: publishes custom ROS message `septentrio_gnss_driver/VelCovCartesian.msg`, corresponding to SBF block `VelCovCartesian` (GNSS case). + `/velcovgeodetic`: publishes custom ROS message `septentrio_gnss_driver/VelCovGeodetic.msg`, corresponding to SBF block `VelCovGeodetic` (GNSS case). + `/atteuler`: publishes custom ROS message `septentrio_gnss_driver/AttEuler.msg`, corresponding to SBF block `AttEuler`. + `/attcoveuler`: publishes custom ROS message `septentrio_gnss_driver/AttCovEuler.msg`, corresponding to the SBF block `AttCovEuler`. - + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its header, or - if `use_gnss_time` is set to `false` - from the systems's wall-clock time. + + `/gpst` (for GPS Time): publishes generic ROS message [`sensor_msgs/TimeReference.msg`](https://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html), converted from the `PVTGeodetic` (GNSS case) or `INSNavGeod` (INS case) block's GPS time information, stored in its block header. + `/navsatfix`: publishes generic ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`,`PosCovGeodetic` (GNSS case) or `INSNavGeod` (INS case). + The ROS message [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) can be fed directly into the [`navsat_transform_node`](https://docs.ros.org/melodic/api/robot_localization/html/navsat_transform_node.html) of the ROS navigation stack. - + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). + + `/gpsfix`: publishes generic ROS message [`gps_msgs/GPSFix.msg`](https://github.com/swri-robotics/gps_umd/tree/dashing-devel), which is much more detailed than [`sensor_msgs/NavSatFix.msg`](https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/NavSatFix.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `ChannelStatus`, `MeasEpoch`, `AttEuler`, `AttCovEuler`, `VelCovGeodetic`, `DOP` (GNSS case) or `INSNavGeod`, `DOP` (INS case). In order to publish heading information, the field *dip* is diverted from its intended meaning an populated with the heading angle and *err_dip* with its uncertainty. + `/pose`: publishes generic ROS message [`geometry_msgs/PoseWithCovarianceStamped.msg`](https://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic`, `AttEuler`, `AttCovEuler` (GNSS case) or `INSNavGeod` (INS case). + Note that GNSS provides absolute positioning, while robots are often localized within a local level cartesian frame. The pose field of this ROS message contains position with respect to the absolute ENU frame (longitude, latitude, height), i.e. not a cartesian frame, while the orientation is with respect to a vehicle-fixed (e.g. for mosaic-x5 in moving base mode via the command `setAttitudeOffset`, ...) !local! NED frame or ENU frame if `use_ros_axis_directions` is set `true`. Thus the orientation is !not! given with respect to the same frame as the position is given in. The cross-covariances are hence set to 0. + `/twist`: publishes generic ROS message [`geometry_msgs/TwistWithCovarianceStamped.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/TwistWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic` and `VelCovGeodetic`. @@ -639,13 +701,14 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr + The ROS message [`sensor_msgs/Imu.msg`](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + `/localization`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF block `INSNavGeod` and transformed to UTM. + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention. + + `/localization_ecef`: accepts generic ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html), converted from the SBF blocks `INSNavCart` and `INSNavGeod`. + + The ROS message [`nav_msgs/Odometry.msg`](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) can be fed directly into the [`robot_localization`](https://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) of the ROS navigation stack. Note that `use_ros_axis_orientation` should be set to `true` to adhere to the ENU convention.
## Suggestions for Improvements
Some Ideas - + Automatic Search: If the host address of the receiver is omitted in the `host:port` specification, the driver could automatically search and establish a connection on the specified port. + Equip ROSaic with an NTRIP client such that it can forward corrections to the receiver independently of `Data Link`.
@@ -655,16 +718,16 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr Is there an SBF or NMEA message that is not being addressed while being important to your application? If yes, follow these steps: 1. Find the log reference of interest in the publicly accessible, official documentation. Hence select the reference guide file, e.g. for mosaic-x5 in the [product support section for mosaic-X5](https://www.septentrio.com/en/support/mosaic/mosaic-x5), Chapter 4, of Septentrio's homepage. - 2. Add a new `.msg` file to the `septentrio_gnss_driver/msg` folder. - 3. SBF: Add the new struct definition to the `sbf_structs.hpp` file. - 4. Parsing/Processing the message/block: - - Both: Add a new include guard to let the compiler know about the existence of the header file (such as `septentrio_gnss_driver/PVTGeodetic.h`) that gets compiler-generated from the `.msg` file constructed in step 3. - - SBF: Extend the `NMEA_ID_Enum` enumeration in the `rx_message.hpp` file with a new entry. - - SBF: Extend the initialization of the `RxIDMap` map in the `rx_message.cpp` file with a new pair. - - SBF: Add a new callback function declaration, a new method, to the `io_comm_rx::RxMessage class` in the `rx_message.hpp` file. - - SBF: Add the latter's definition to the `rx_message.cpp` file. - - SBF: Add a new C++ "case" (part of the C++ switch-case structure) in the `rx_message.hpp` file. It should be modeled on the existing `evPVTGeodetic` case, e.g. one needs a static counter variable declaration. - - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `septentrio_gnss_driver/src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `septentrio_gnss_driver/include/septentrio_gnss_driver/parsers/nmea_parsers` folder. - 5. Create a new `publish/..` ROSaic parameter in the `septentrio_gnss_driver/config/rover.yaml` file, create a global boolean variable `publish_...` in the `septentrio_gnss_driver/src/septentrio_gnss_driver/node/rosaic_node.cpp` file, insert the publishing callback function to the C++ "multimap" `IO.handlers_.callbackmap_` - which is already storing all the others - in the `rosaic_node::ROSaicNode::defineMessages()` method in the same file and add an `extern bool publish_...;` line to the `septentrio_gnss_driver/include/septentrio_gnss_driver/node/rosaic_node.hpp` file. - 6. Modify the `septentrio_gnss_driver/CMakeLists.txt` file by adding a new entry to the `add_message_files` section. + 2. SBF: Add a new `.msg` file to the `../msg` folder. And modify the `../CMakeLists.txt` file by adding a new entry to the `add_message_files` section. + 3. Add msg header and typedef to `typedefs.hpp`. + 4. Parsers: + - SBF: Add a parser to the `sbf_blocks.hpp` file. + - NMEA: Construct two new parsing files such as `gpgga.cpp` to the `../src/septentrio_gnss_driver/parsers/nmea_parsers` folder and one such as `gpgga.hpp` to the `../include/septentrio_gnss_driver/parsers/nmea_parsers` folder. + 5. Processing the message/block: + - SBF: Extend the `SbfId` enumeration in the `message_handler.hpp` file with a new entry. + - SBF: Extend the SBF switch-case in `message_handler.cpp` file with a new case. + - NMEA: Extend the `nmeaMap_` in the `message_handler.hpp` file with a new pair. + - NMEA: Extend the NMEA switch-case in `message_handler.cpp` file with a new case. + 6. Create a new `publish/..` ROSaic parameter in the `../config/rover.yaml` file and create a boolean variable `publish_xxx` in the struct in the `settings.h` file. Parse the parameter in the `rosaic_node.cpp` file. + 7. Add SBF block or NMEA to data stream setup in `communication_core.cpp` (function `configureRx()`).
From 05cfc0a56791488de0a27f35e0b8a224cdd177e7 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Wed, 18 Oct 2023 11:04:53 +0200 Subject: [PATCH 167/170] Fix units of imu angular rates --- .../communication/message_handler.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index fc4e7b17..d78daf44 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -555,9 +555,9 @@ namespace io { msg.linear_acceleration.y = last_extsensmeas_.acceleration_y; msg.linear_acceleration.z = last_extsensmeas_.acceleration_z; - msg.angular_velocity.x = last_extsensmeas_.angular_rate_x; - msg.angular_velocity.y = last_extsensmeas_.angular_rate_y; - msg.angular_velocity.z = last_extsensmeas_.angular_rate_z; + msg.angular_velocity.x = deg2rad(last_extsensmeas_.angular_rate_x); + msg.angular_velocity.y = deg2rad(last_extsensmeas_.angular_rate_y); + msg.angular_velocity.z = deg2rad(last_extsensmeas_.angular_rate_z); bool valid_orientation = false; if (settings_->septentrio_receiver_type == "ins") From d6354103b4b14fc736ae023067a8befa1f1ce7a3 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Thu, 19 Oct 2023 14:21:50 +0200 Subject: [PATCH 168/170] Fix topics namespace --- .../communication/message_handler.cpp | 68 +++++++++---------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index d78daf44..ab5136b1 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -210,7 +210,7 @@ namespace io { msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch); msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead); } - publish("/pose", msg); + publish("pose", msg); }; void MessageHandler::assembleDiagnosticArray( @@ -530,7 +530,7 @@ namespace io { aimMsg.header = last_rf_status_.header; aimMsg.tow = last_rf_status_.block_header.tow; aimMsg.wnc = last_rf_status_.block_header.wnc; - publish("/aimplusstatus", aimMsg); + publish("aimplusstatus", aimMsg); if (spoofed || detected) diagRf.level = DiagnosticStatusMsg::ERROR; @@ -653,7 +653,7 @@ namespace io { msg.orientation_covariance[8] = -1.0; } - publish("/imu", msg); + publish("imu", msg); }; void MessageHandler::assembleTwist(bool fromIns /* = false*/) @@ -773,7 +773,7 @@ namespace io { msg.twist.covariance[28] = -1.0; msg.twist.covariance[35] = -1.0; - publish("/twist_ins", msg); + publish("twist_ins", msg); } else { if ((!validValue(last_pvtgeodetic_.block_header.tow)) || @@ -874,7 +874,7 @@ namespace io { msg.twist.covariance[28] = -1.0; msg.twist.covariance[35] = -1.0; - publish("/twist_gnss", msg); + publish("twist_gnss", msg); } }; @@ -1078,7 +1078,7 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); if (settings_->publish_localization) - publish("/localization", msg); + publish("localization", msg); if (settings_->publish_tf) publishTf(msg); }; @@ -1247,7 +1247,7 @@ namespace io { assembleLocalizationMsgTwist(roll, pitch, yaw, msg); if (settings_->publish_localization_ecef) - publish("/localization_ecef", msg); + publish("localization_ecef", msg); if (settings_->publish_tf_ecef) publishTf(msg); }; @@ -1563,7 +1563,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - publish("/navsatfix", msg); + publish("navsatfix", msg); }; /** @@ -2051,7 +2051,7 @@ namespace io { msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; } - publish("/gpsfix", msg); + publish("gpsfix", msg); } void @@ -2062,7 +2062,7 @@ namespace io { msg.time_ref = timestampToRos(time_obj); msg.source = "GPST"; assembleHeader(settings_->frame_id, telegram, msg); - publish("/gpst", msg); + publish("gpst", msg); } template @@ -2225,7 +2225,7 @@ namespace io { break; } assembleHeader(settings_->frame_id, telegram, msg); - publish("/pvtcartesian", msg); + publish("pvtcartesian", msg); } break; } @@ -2240,7 +2240,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, last_pvtgeodetic_); if (settings_->publish_pvtgeodetic) - publish("/pvtgeodetic", last_pvtgeodetic_); + publish("pvtgeodetic", last_pvtgeodetic_); assembleTwist(); assembleNavSatFix(); assemblePoseWithCovarianceStamped(); @@ -2263,7 +2263,7 @@ namespace io { break; } assembleHeader(settings_->frame_id, telegram, msg); - publish("/basevectorcart", msg); + publish("basevectorcart", msg); } break; } @@ -2280,7 +2280,7 @@ namespace io { break; } assembleHeader(settings_->frame_id, telegram, msg); - publish("/basevectorgeod", msg); + publish("basevectorgeod", msg); } break; } @@ -2297,7 +2297,7 @@ namespace io { break; } assembleHeader(settings_->frame_id, telegram, msg); - publish("/poscovcartesian", msg); + publish("poscovcartesian", msg); } break; } @@ -2311,7 +2311,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, last_poscovgeodetic_); if (settings_->publish_poscovgeodetic) - publish("/poscovgeodetic", last_poscovgeodetic_); + publish("poscovgeodetic", last_poscovgeodetic_); assembleNavSatFix(); assemblePoseWithCovarianceStamped(); assembleGpsFix(); @@ -2328,7 +2328,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, last_atteuler_); if (settings_->publish_atteuler) - publish("/atteuler", last_atteuler_); + publish("atteuler", last_atteuler_); assemblePoseWithCovarianceStamped(); assembleGpsFix(); break; @@ -2344,7 +2344,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, last_attcoveuler_); if (settings_->publish_attcoveuler) - publish("/attcoveuler", last_attcoveuler_); + publish("attcoveuler", last_attcoveuler_); assemblePoseWithCovarianceStamped(); assembleGpsFix(); break; @@ -2361,7 +2361,7 @@ namespace io { assembleHeader(settings_->frame_id, telegram, last_gal_auth_status_); if (settings_->publish_galauthstatus) { - publish("/galauthstatus", last_gal_auth_status_); + publish("galauthstatus", last_gal_auth_status_); assembleOsnmaDiagnosticArray(); } break; @@ -2377,7 +2377,7 @@ namespace io { assembleHeader(settings_->frame_id, telegram, last_rf_status_); if (settings_->publish_aimplusstatus) { - publish("/rfstatus", last_rf_status_); + publish("rfstatus", last_rf_status_); assembleAimAndDiagnosticArray(); } break; @@ -2402,7 +2402,7 @@ namespace io { } assembleHeader(frame_id, telegram, last_insnavcart_); if (settings_->publish_insnavcart) - publish("/insnavcart", last_insnavcart_); + publish("insnavcart", last_insnavcart_); assembleLocalizationEcef(); break; } @@ -2426,7 +2426,7 @@ namespace io { } assembleHeader(frame_id, telegram, last_insnavgeod_); if (settings_->publish_insnavgeod) - publish("/insnavgeod", last_insnavgeod_); + publish("insnavgeod", last_insnavgeod_); assembleLocalizationUtm(); assembleLocalizationEcef(); assembleTwist(true); @@ -2451,7 +2451,7 @@ namespace io { break; } assembleHeader(settings_->vehicle_frame_id, telegram, msg); - publish("/imusetup", msg); + publish("imusetup", msg); } break; } @@ -2469,7 +2469,7 @@ namespace io { break; } assembleHeader(settings_->vehicle_frame_id, telegram, msg); - publish("/velsensorsetup", msg); + publish("velsensorsetup", msg); } break; } @@ -2497,7 +2497,7 @@ namespace io { frame_id = settings_->frame_id; } assembleHeader(frame_id, telegram, msg); - publish("/exteventinsnavcart", msg); + publish("exteventinsnavcart", msg); } break; } @@ -2524,7 +2524,7 @@ namespace io { frame_id = settings_->frame_id; } assembleHeader(frame_id, telegram, msg); - publish("/exteventinsnavgeod", msg); + publish("exteventinsnavgeod", msg); } break; } @@ -2541,7 +2541,7 @@ namespace io { } assembleHeader(settings_->imu_frame_id, telegram, last_extsensmeas_); if (settings_->publish_extsensormeas) - publish("/extsensormeas", last_extsensmeas_); + publish("extsensormeas", last_extsensmeas_); if (settings_->publish_imu && hasImuMeas) { assembleImu(); @@ -2569,7 +2569,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, last_measepoch_); if (settings_->publish_measepoch) - publish("/measepoch", last_measepoch_); + publish("measepoch", last_measepoch_); assembleGpsFix(); break; } @@ -2596,7 +2596,7 @@ namespace io { break; } assembleHeader(settings_->frame_id, telegram, msg); - publish("/velcovcartesian", msg); + publish("velcovcartesian", msg); } break; } @@ -2611,7 +2611,7 @@ namespace io { } assembleHeader(settings_->frame_id, telegram, last_velcovgeodetic_); if (settings_->publish_velcovgeodetic) - publish("/velcovgeodetic", last_velcovgeodetic_); + publish("velcovgeodetic", last_velcovgeodetic_); assembleTwist(); assembleGpsFix(); break; @@ -2793,7 +2793,7 @@ namespace io { "GpggaMsg: " + std::string(e.what())); break; } - publish("/gpgga", msg); + publish("gpgga", msg); break; } case 1: @@ -2813,7 +2813,7 @@ namespace io { "GprmcMsg: " + std::string(e.what())); break; } - publish("/gprmc", msg); + publish("gprmc", msg); break; } case 2: @@ -2851,7 +2851,7 @@ namespace io { } } else msg.header.stamp = timestampToRos(telegram->stamp); - publish("/gpgsa", msg); + publish("gpgsa", msg); break; } case 4: @@ -2890,7 +2890,7 @@ namespace io { } } else msg.header.stamp = timestampToRos(telegram->stamp); - publish("/gpgsv", msg); + publish("gpgsv", msg); break; } } From 66bd41c33d13f91d965ea0a16fd50235631e9b6c Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Sun, 19 Nov 2023 12:26:44 +0000 Subject: [PATCH 169/170] Updated CHANGELOG.rst --- CHANGELOG.rst | 277 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 277 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6c6b0ddd..851d9906 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,283 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.2 (2023-11-19) +------------------ +* Commits + * Merge pull request `#97 `_ from thomasemter/dev/next + Integrate README changes from master + * Fix topics namespace + * Fix units of imu angular rates + * Merge upstream README pt2 + * Merge upstream README + * Update README.md + * Update README.md + * Update README.md + * v1.3.1 + * Updated package.xml + * v1.3.1 + * Updated package.xml + * v1.3.1 + * Updated package.xml + * v1.3.1 + * Updated changelog + * Merge pull request `#95 `_ from thomasemter/dev/next + Fix navsatfix and gpsfix frame ids + * Update README.md + * Fix navsatfix and gpsfix frame ids + * Merge pull request `#92 `_ from thomasemter/dev/next + Fix single antenna receiver setup + * Update changelog + * Merge + * Fix single antenna receiver setup + * Merge pull request `#90 `_ from thomasemter/dev/next + Fix empty headers + * Merge branch 'dev' into dev/next + * Bump version + * Fix empty headers + * Merge pull request `#88 `_ from thomasemter/dev/next + Fix navsatfix containing only zeros for INS + * Align indent + * Fix navsatfix containig only zeros for INS + * Merge pull request `#87 `_ from thomasemter/dev/next + Update firmware info + * Reduce INS firmware version info to released version + * Update firmware info + * v1.3.0 + * Updated package.xml + * v1.3.0 + * Update firmware info + * Updated package.xml + * v1.3.0 + * Merge pull request `#84 `_ from thomasemter/dev/next + Update readme + * Add expected release dates + * Add known issues to readme + * Update version + * Update readme + * Merge pull request `#81 `_ from thomasemter/dev/next + Fix spelling + * Improve explanations in readme + * Categorize stream params + * Add keep alive check for TCP + * Fix spelling + * Change angle wrapping + * Add TCP communication via static IP server + * Add units to msgs + * Fix spelling + * Merge pull request `#75 `_ from thomasemter/dev/next + upcoming release + * Add heading to GPSFix msg + * Move constant + * Change log level of firmware check + * Add improved VSM handling + * Change INS in GNSS node detection to auto + * Fix invald v_x var case + * Refine readme on UDP + * Improve server duplicate check + * Add more info un UDP configuration + * Fix publish check + * Add more publishing checks for configured Rx + * Add const for max udp packet size + * Update readme and changelog + * Add device check to node + * Add checks for IP server duplicates + * Add latency compensation to att msgs + * Add device check logic + * Add UDP params and setup logic + * Fix multi msg per packet + * Fix localization stamp and tf publishing + * Change VSM to be averaged and published with 2 Hz + * Always publish raw IMU data as indicated + * Change to empty fields + * Refine diagnostics naming scheme and add trigger to ensure emission of ReceiverSetup + * Change diagnostics naming scheme + * Expand readme on AIM+ + * Reformulate readme about ROS and ROS2 + * Rename msg var + * Add custom message to report AIM+ status + * Catch invalid UTM conversion + * Robustify command reset + * Add RFStatus diagnostics + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Add VelCovCartesian output + * Add VelCovCartesian output + * Refine Rx type check + * Ensure latency reporting block is activated + * Add option for latency compensation + * Fix param type misinterpretation + * Add missing param to example in readme + * Add OSNMA diagnostics output + * Add keep_open option to OSNMA + * Add OSNMA msg + * Update changelog + * Refine README and fix compiled message logic + * Update changelog + * Add warning for configuring INS as GNSS + * Add warn log for misconfiguration + * Fix pose publishing rate + * Fix navsatfix publishing + * Make vars const + * Replace log functions + * Add small fixes and cleanup + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Add option to bypass configuration of Rx + * Add option to bypass confugration of Rx + * Add diagnostics and time ref msgs again + * Update README on how to add new messages + * Add automiatic detection of serial port ID + * Refine changelog + * Change invalid timestamp handling for reading from file + * Add USB serial by ID info to README + * Fix leap seconds for timestamp if reading from file + * Fix reconnection logic + * Replace flow control setup + * Refactor ioservice + * notify semaphores in destructor + * Send port reset only once + * Fix serial connection + * Fix talker ID setting for INS + * Add NMEA talker ID setting to ensure reception + * Prepare communication for UDP option (still inactive) + * Fix UDP message identification logic + * Add test code for UDP, WIP + * Add UDP client, WIP + * Set do-not-use for temp to NaN + * Add processing latency correction for ROS timestamps + * Fix extsensor parser + * Add units to remaining msgs + * Add nodiscard attribute to functions + * Add nodiscard attribute to functions + * Add nodiscard attribute to functions + * Add const specifiers to functions + * Make settings access const + * Move SBF ID handling + * Refactor header assembly + * Rename message handler again + * Change parsing utilities and crc to get message + * Add namespace to enum + * Change timestamp code + * Update changelog + * Change class privacy + * Add assembled messages, to be tested + * Add units to come msg definitions + * Add custom BlockHeader constructor + * Move wait + * Remove copy paste vars + * Add file readers + * Fix reset main connection on exit hang + * Fix handling of INS NMEA talker ID + * Fix error response detection + * Add packing of generic string messages + * Exchange concurrent queue + * Remove obsolete includes + * Add NMEA handling + * Change syncronization to semaphore + * Add message parser, WIP + * Rearrange io handling, WIP + * Refactor and cleanup + * Improve io handling, WIP + * Refactor message parser, WIP + * Add message handler + * Add io modules + * Add new low level interface, WIP + * Change connection thread + * Fix attitude cov flipped twice + * Add cov alignment from true north to grid north + * Rename meridian convergence and fix sense + * Remove obsolete define + * Fix spelling errors + * Merge branch 'master' into dev/next + * v1.2.3 + * Update package.xml + * v1.2.3 + * Update package.xml + * v1.2.3 + * Update package.xml + * v1.2.3 + * Update package.xml + * Merge pull request `#68 `_ from thomasemter/master + dev + * Fix lat/long in rad + * Reorder localization msg filling + * Update readme + * Fix NED to ECEF rotation matrix + * Add localization ECEF publishing + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Merge branch 'dev/next' of https://github.com/thomasemter/septentrio_gnss_driver into dev/next + * Add ecef localization msg + * Add local to ecef transforms + * Change default datum to Default + * Clean up block data size defines + * Change default datum to WGS84 + * Set correct value for max number of base vector info + * Add check on shutdown to only close ports on hardware + * Refine readme + * Merge branch 'master' of https://github.com/thomasemter/septentrio_gnss_driver + * Add missing param + * Add possibility to use multiple RTK corrections of the same type + * Only set baud rates on real serial ports + * Fix decimal places trimming + * Update changelog + * Merge branch 'dev/next' + * Add base vecotr info to README + * Change param to empty vector + * Change param to empty vector + * Fix template argument + * Add quotation marks to pw if it contains spaces + * Add quotation marks to pw if it contains spaces + * Add option to keep aiding connections open on shutdown + * Merge branch 'master' into dev/next + * Add option to keep aiding connections open on shutdown + * Disable ntrip on shutdown + * Disable ntrip on shutdown + * Add base vector callbacks and publishing, WIP + * Add base vector msgs and parsers, WIP + * Fix comment swap + * Add send_gga option to serial RTK and fix IP server id + * Add possibility to specify IP server connection + * Increase version number in package.xml and harmonize it with ROS2 + * Reset main port to auto input + * Add reset all used ports on shutdown + * Improve readme + * Change vsm options to allow simultaneous input + * Change corrections settings to receiver simultaneously + * Change correction options to be used simultenously + * Change param name for future extensibility + * Change param name for future extensibility + * Rework corretion parameters and add more flexible options + * Fix some spelling in readme + * Add receiver type INS as GNSS + * Add option to use external VSM input + * Add more log output for vsm + * Add VSM from odometry or twist ROS messages + * Fix GPGGA and GPRMC timestamp from GNSS + * Use only one stream for NMEA messages + * Fix merge error + * Fix merge error + * Add all possible periods and rework validity check + * Update changelog + * Add 5 ms period option + * Fix changelog + * Add twist output + * Add missing files to clang-formatting + * Merge branch 'dev/next' + * Merge branch 'master' of https://github.com/thomasemter/septentrio_gnss_driver + * Format according to clang-format + * Change log level of local frame tf insertion + * Always register ReceiverSetup + * Add firmware checks + * Add log and info to README + * Add insertion of local frame + * Update README and CHANGELOG + * Use leap seconds from receiver + * Update changelog + * Add config files for GNSS and INS + * Add ReceiverTime, WIP + * Add configs for GNSS and INS + * Contributors: Thomas Emter, Tibor Dome, septentrio-users, tibordome + 1.3.1 (2023-07-06) ------------------ * New Features From 4b78de3a74380863eb7fcc04dc27dd00db626c37 Mon Sep 17 00:00:00 2001 From: septentrio-users Date: Sun, 19 Nov 2023 12:27:03 +0000 Subject: [PATCH 170/170] v1.3.2 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 92171be4..75ce0cc4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ septentrio_gnss_driver - 1.3.1 + 1.3.2 ROSaic: C++ driver for Septentrio's mosaic receivers and beyond