From 64463c3180b498d930bdd10457052a8d5a4570cd Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 28 Jun 2024 16:07:03 +0200 Subject: [PATCH] Improve NMEA message with all NMEA topics --- fixposition_driver_lib/CMakeLists.txt | 1 + .../messages/nmea_type.hpp | 131 +++++++- .../src/messages/nmea/nmea.cpp | 135 +++++++++ fixposition_driver_ros1/CMakeLists.txt | 1 - .../fixposition_driver_ros1/data_to_ros1.hpp | 58 +--- .../fixposition_driver_node.hpp | 13 +- .../fixposition_driver_ros1/ros_msgs.hpp | 1 - fixposition_driver_ros1/msg/NMEA.msg | 98 +++--- fixposition_driver_ros1/msg/VRTK.msg | 28 -- fixposition_driver_ros1/src/data_to_ros1.cpp | 282 ++++++++++-------- .../src/fixposition_driver_node.cpp | 202 +++++-------- 11 files changed, 560 insertions(+), 390 deletions(-) create mode 100644 fixposition_driver_lib/src/messages/nmea/nmea.cpp delete mode 100644 fixposition_driver_ros1/msg/VRTK.msg diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index cd62ab8..5acdadc 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -42,6 +42,7 @@ add_library( src/messages/nmea/gprmc.cpp src/messages/nmea/gpvtg.cpp src/messages/nmea/gpzda.cpp + src/messages/nmea/nmea.cpp src/fixposition_driver.cpp src/helper.cpp src/parser.cpp diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp index 07ba69e..0cf8261 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp @@ -386,8 +386,8 @@ struct GP_ZDA { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Message fields - std::string date_str; - std::string time_str; + std::string date_str; // Format: dd/mm/yyyy + std::string time_str; // Format: hhmmss.ss(ss) times::GpsTime stamp; uint8_t local_hr; uint8_t local_min; @@ -400,8 +400,8 @@ struct GP_ZDA { GP_ZDA() { stamp = fixposition::times::GpsTime(); - time_str = "hhmmss.ss(ss)"; - date_str = "dd/mm/yyyy"; + time_str = ""; + date_str = ""; local_hr = 0; local_min = 0; } @@ -410,8 +410,8 @@ struct GP_ZDA { void ResetData() { stamp = fixposition::times::GpsTime(); - time_str = "hhmmss.ss(ss)"; - date_str = "dd/mm/yyyy"; + time_str = ""; + date_str = ""; local_hr = 0; local_min = 0; } @@ -419,20 +419,131 @@ struct GP_ZDA { struct NmeaMessage { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - GP_GGA gpgga; - GP_ZDA gpzda; - GP_RMC gprmc; + + // Message fields + std::string gpgga_time_str; // GP_GGA + std::string gpzda_time_str; // GP_ZDA + std::string time_str; // GP_ZDA (alt. GP_GGA, GP_GST, GP_RMC) + std::string date_str; // GP_ZDA (alt. GP_RMC) + times::GpsTime stamp; // GP_ZDA + Eigen::Vector3d llh; // GP_GGA (alt. LL only for GP_GLL, GP_RMC) + uint8_t quality; // GP_GGA (alt. GP_RMC, GP_VTG, or limited GP_GLL, GP_GSA) + uint8_t num_sv; // GP_GGA + std::vector ids; // GN_GSA + float hdop_receiver; // GP_GGA + float pdop; // GN_GSA + float hdop; // GN_GSA (alt. GP_GGA) + float vdop; // GN_GSA + float rms_range; // GP_GST + float std_major; // GP_GST + float std_minor; // GP_GST + float angle_major; // GP_GST + float std_lat; // GP_GST (alt. GP_GGA) + float std_lon; // GP_GST (alt. GP_GGA) + float std_alt; // GP_GST (alt. GP_GGA) + Eigen::Matrix cov; // GP_GST (alt. GP_GGA) + uint8_t cov_type; // GP_GST (alt. GP_GGA) + uint8_t num_sats; // GX_GSV + std::vector sat_id; // GX_GSV + std::vector elev; // GX_GSV + std::vector azim; // GX_GSV + std::vector cno; // GX_GSV + float heading; // GP_HDT + float speed; // GP_RMC (alt. GP_VTG) + float course; // GP_RMC (alt. GP_VTG) + float diff_age; // GP_GGA + std::string diff_sta; // GP_GGA /** * @brief Check if GNSS epoch is complete */ bool checkEpoch() { - if ((gpgga.time_str.compare(gpzda.time_str) == 0) && (gpgga.time_str.compare(gprmc.time_str) == 0)) { + if ((gpgga_time_str.compare(gpzda_time_str) == 0)) { return true; } else { return false; } } + + NmeaMessage() { + gpgga_time_str = ""; + gpzda_time_str = ""; + time_str = ""; + date_str = ""; + stamp = fixposition::times::GpsTime(); + llh.setZero(); + quality = -1; + diff_age = 0.0; + diff_sta = ""; + ids.clear(); + hdop_receiver = 0.0; + pdop = 0.0; + hdop = 0.0; + vdop = 0.0; + rms_range = 0.0; + std_major = 0.0; + std_minor = 0.0; + angle_major = 0.0; + std_lat = 0.0; + std_lon = 0.0; + std_alt = 0.0; + cov.setZero(); + cov_type = 0; + num_sv = 0; + num_sats = 0; + sat_id.clear(); + elev.clear(); + azim.clear(); + cno.clear(); + heading = 0.0; + speed = 0.0; + course = 0.0; + } + + void ResetData() { + gpgga_time_str = ""; + gpzda_time_str = ""; + time_str = ""; + date_str = ""; + stamp = fixposition::times::GpsTime(); + llh.setZero(); + quality = -1; + diff_age = 0.0; + diff_sta = ""; + ids.clear(); + hdop_receiver = 0.0; + pdop = 0.0; + hdop = 0.0; + vdop = 0.0; + rms_range = 0.0; + std_major = 0.0; + std_minor = 0.0; + angle_major = 0.0; + std_lat = 0.0; + std_lon = 0.0; + std_alt = 0.0; + cov.setZero(); + cov_type = 0; + num_sv = 0; + num_sats = 0; + sat_id.clear(); + elev.clear(); + azim.clear(); + cno.clear(); + heading = 0.0; + speed = 0.0; + course = 0.0; + } + + void AddNmeaEpoch(const GP_GGA& msg); + void AddNmeaEpoch(const GP_GLL& msg); + void AddNmeaEpoch(const GN_GSA& msg); + void AddNmeaEpoch(const GP_GST& msg); + void AddNmeaEpoch(const GX_GSV& msg); + void AddNmeaEpoch(const GP_HDT& msg); + void AddNmeaEpoch(const GP_RMC& msg); + void AddNmeaEpoch(const GP_VTG& msg); + void AddNmeaEpoch(const GP_ZDA& msg); }; } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/nmea.cpp b/fixposition_driver_lib/src/messages/nmea/nmea.cpp new file mode 100644 index 0000000..a48593c --- /dev/null +++ b/fixposition_driver_lib/src/messages/nmea/nmea.cpp @@ -0,0 +1,135 @@ +/** + * @file + * @brief Implementation of NMEA-GX-GSV parser + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* PACKAGE */ +#include +#include + +namespace fixposition { + +void NmeaMessage::AddNmeaEpoch(const GP_GGA& msg) { + // Populate time if empty + if (time_str == "") { + time_str = msg.time_str; + } + gpgga_time_str = msg.time_str; + + // Populate LLH position + llh = msg.llh; + + // Populate quality + quality = msg.quality; + + // Populate total number of satellites + num_sv = msg.num_sv; + + // Populate GNSS receiver HDOP + hdop_receiver = msg.hdop; + + // Populate differential data information + diff_age = msg.diff_age; + diff_sta = msg.diff_sta; + + // Populate position covariance [m^2] + cov(0, 0) = msg.hdop * msg.hdop; + cov(1, 1) = msg.hdop * msg.hdop; + cov(2, 2) = 4 * msg.hdop * msg.hdop; + cov(0, 1) = cov(1, 0) = 0.0; + cov(0, 2) = cov(2, 0) = 0.0; + cov(1, 2) = cov(2, 1) = 0.0; +} + +void NmeaMessage::AddNmeaEpoch(const GP_GLL& msg) { + // Populate time if empty + if (time_str == "") { + time_str = msg.time_str; + } + + // Populate latitude and longitude if the vector is empty + if (llh.isZero()) { + llh(0) = msg.latlon(0); + llh(1) = msg.latlon(1); + } +} + +void NmeaMessage::AddNmeaEpoch(const GN_GSA& msg) { + // Populate DOP values (priority) + pdop = msg.pdop; + hdop = msg.hdop; + vdop = msg.vdop; + + // Populate satellite information (priority) + ids = msg.ids; +} + +void NmeaMessage::AddNmeaEpoch(const GP_GST& msg) { + // Populate time if empty + if (time_str == "") { + time_str = msg.time_str; + } + + // Populate GNSS pseudorange error statistics (priority) + rms_range = msg.rms_range; + std_major = msg.std_major; + std_minor = msg.std_minor; + angle_major = msg.angle_major; + std_lat = msg.std_lat; + std_lon = msg.std_lon; + std_alt = msg.std_alt; +} + +void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) { + // Populate GNSS satellites in view (priority) + num_sats = msg.num_sats; + sat_id = msg.sat_id; + elev = msg.elev; + azim = msg.azim; + cno = msg.cno; +} + +void NmeaMessage::AddNmeaEpoch(const GP_HDT& msg) { + // Populate true heading (priority) + heading = msg.heading; +} + +void NmeaMessage::AddNmeaEpoch(const GP_RMC& msg) { + // Populate time if empty + if (time_str == "") { + time_str = msg.time_str; + } + + // Populate time if empty + if (date_str == "") { + date_str = msg.date_str; + } + + // Populate SOG and COG (priority) + speed = msg.speed_ms; + course = msg.course; + + // Populate latitude and longitude if the vector is empty + if (llh.isZero()) { + llh(0) = msg.latlon(0); + llh(1) = msg.latlon(1); + } +} + +void NmeaMessage::AddNmeaEpoch(const GP_ZDA& msg) { + // Populate time and date + time_str = msg.time_str; + date_str = msg.date_str; + gpzda_time_str = msg.time_str; +} + +} // namespace fixposition diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 22478d5..6695b22 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -25,7 +25,6 @@ find_package(catkin REQUIRED COMPONENTS ) add_message_files( FILES - VRTK.msg Speed.msg WheelSensor.msg NMEA.msg diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 657acdc..847644f 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -30,13 +30,15 @@ namespace fixposition { * @param[in] data * @param[out] msg */ -void FpToRosMsg( const FP_GNSSANT& data, ros::Publisher& pub); -void FpToRosMsg(const FP_GNSSCORR& data, ros::Publisher& pub); -void FpToRosMsg( const FP_LLH& data, ros::Publisher& pub); -void FpToRosMsg( const FP_ODOMENU& data, ros::Publisher& pub); -void FpToRosMsg(const FP_ODOMETRY& data, ros::Publisher& pub); -void FpToRosMsg( const FP_ODOMSH& data, ros::Publisher& pub); -void FpToRosMsg( const FP_TEXT& data, ros::Publisher& pub); +void FpToRosMsg(const OdometryData& data, ros::Publisher& pub); +void FpToRosMsg( const ImuData& data, ros::Publisher& pub); +void FpToRosMsg( const FP_GNSSANT& data, ros::Publisher& pub); +void FpToRosMsg( const FP_GNSSCORR& data, ros::Publisher& pub); +void FpToRosMsg( const FP_LLH& data, ros::Publisher& pub); +void FpToRosMsg( const FP_ODOMENU& data, ros::Publisher& pub); +void FpToRosMsg( const FP_ODOMETRY& data, ros::Publisher& pub); +void FpToRosMsg( const FP_ODOMSH& data, ros::Publisher& pub); +void FpToRosMsg( const FP_TEXT& data, ros::Publisher& pub); void FpToRosMsg(const GP_GGA& data, ros::Publisher& pub); void FpToRosMsg(const GP_GLL& data, ros::Publisher& pub); @@ -54,15 +56,7 @@ void FpToRosMsg(const GP_ZDA& data, ros::Publisher& pub); * @param[in] data * @param[out] msg */ -void ImuDataToMsg(const ImuData& data, sensor_msgs::Imu& msg); - -/** - * @brief - * - * @param[in] data - * @param[in] msg - */ -void NavSatStatusDataToMsg(const NavSatStatusData& data, sensor_msgs::NavSatStatus& msg); +void TfDataToMsg(const TfData& data, geometry_msgs::TransformStamped& msg); /** * @brief @@ -94,15 +88,7 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWit * @param[in] data * @param[out] msg */ -void OdometryDataToMsg(const OdometryData& data, nav_msgs::Odometry& msg); - -/** - * @brief - * - * @param[in] data - * @param[out] msg - */ -void OdometryDataToTf(const OdometryData& data, geometry_msgs::TransformStamped& msg); +void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub); /** * @brief @@ -110,7 +96,7 @@ void OdometryDataToTf(const OdometryData& data, geometry_msgs::TransformStamped& * @param[in] data * @param[out] msg */ -void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, sensor_msgs::NavSatFix& msg); +void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub); /** * @brief @@ -118,7 +104,7 @@ void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, sensor_msgs::NavSatFi * @param[in] data * @param[out] msg */ -void OdomToVrtkMsg(const fixposition::FP_ODOMETRY& data, fixposition_driver_ros1::VRTK& msg); +void OdomToImuMsg(const FP_ODOMETRY& data, ros::Publisher& pub); /** * @brief @@ -126,23 +112,7 @@ void OdomToVrtkMsg(const fixposition::FP_ODOMETRY& data, fixposition_driver_ros1 * @param[in] data * @param[out] msg */ -void OdomToImuMsg(const fixposition::FP_ODOMETRY& data, sensor_msgs::Imu& msg); - -/** - * @brief - * - * @param[in] data - * @param[out] msg - */ -void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros1::VRTK& msg); - -/** - * @brief - * - * @param[in] data - * @param[out] msg - */ -void TfDataToMsg(const TfData& data, geometry_msgs::TransformStamped& msg); +void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub); } // namespace fixposition diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 1a07fc7..be0b2ba 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -72,13 +72,6 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Subscriber rtcm_sub_; //!< RTCM3 message subscriber // ROS publishers - // ODOMETRY - ros::Publisher odometry_ecef_pub_; //!< ECEF Odometry - ros::Publisher odometry_llh_pub_; //!< LLH Odometry - ros::Publisher odometry_enu_pub_; //!< ENU Odometry - ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF) - ros::Publisher vrtk_pub_; //!< FP_A-ODOMETRY message - // FP_A messages ros::Publisher fpa_gnssant_pub_; //!< FP_A-GNSSANT message ros::Publisher fpa_gnsscorr_pub_; //!< FP_A-GNSSCORR message @@ -98,6 +91,12 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher nmea_gprmc_pub_; //!< NMEA-GP-RMC message ros::Publisher nmea_gpvtg_pub_; //!< NMEA-GP-VTG message ros::Publisher nmea_gpzda_pub_; //!< NMEA-GP-ZDA message + + // ODOMETRY + ros::Publisher odometry_ecef_pub_; //!< ECEF Odometry + ros::Publisher odometry_llh_pub_; //!< LLH Odometry + ros::Publisher odometry_enu_pub_; //!< ENU Odometry + ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF) // Orientation ros::Publisher eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp index 6c00a9c..16247fd 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp @@ -24,7 +24,6 @@ #include #include -#include #include #include diff --git a/fixposition_driver_ros1/msg/NMEA.msg b/fixposition_driver_ros1/msg/NMEA.msg index 98d2649..85d27db 100644 --- a/fixposition_driver_ros1/msg/NMEA.msg +++ b/fixposition_driver_ros1/msg/NMEA.msg @@ -8,59 +8,63 @@ # #################################################################################################### # -# Fixposition NMEA Message +# Fixposition NMEA Message. Specified using the WGS 84 reference ellipsoid. # # #################################################################################################### -# Navigation Satellite fix for any Global Navigation Satellite System -# -# Specified using the WGS 84 reference ellipsoid +# Format | Field | Unit | Description +# -------|-------------|-------------------|----------------------------------------------------------------------| +Header header # [ns] | Specifies the ROS time and Euclidian reference frame. +string time # [hhmmss.ss(ss)] | UTC time (hours, minutes and seconds). +string date # [ddmmyy] | UTC date (day, month and year). +float64 latitude # [ddmm.mmmmm(mm)] | Latitude. Positive is north of equator; negative is south. +float64 longitude # [dddmm.mmmmm(mm)] | Longitude. Positive is east of prime meridian; negative is west. +float64 altitude # [m] | Altitude. Positive is above the WGS 84 ellipsoid. +int8 quality # [-] | Quality indicator (see below). +int8 num_sv # [-] | Number of satellites. Strict NMEA: 00-12. High-precision NMEA: 00-99. +int8[] ids # [-] | ID numbers of satellites used in solution. See the NMEA 0183 version 4.11 standard document. +float64 hdop_rec # [0.10-99.99] | Horizontal dilution of precision. +float64 pdop # [-] | Position dillution of precision. +float64 hdop # [-] | Horizontal dillution of precision. +float64 vdop # [-] | Vertical dillution of precision. +float64 rms_range # [-] | RMS value of the standard deviation of the range inputs to the navigation process. +float64 std_major # [m] | Standard deviation of semi-major axis of error ellipse. +float64 std_minor # [m] | Standard deviation of semi-minor axis of error ellipse. +float64 angle_major # [deg] | Angle of semi-major axis of error ellipse from true North. +float64 std_lat # [m] | Standard deviation of latitude error. +float64 std_lon # [m] | Standard deviation of longitude error. +float64 std_alt # [m] | Standard deviation of altitude error. +float64[9] covariance # [m2] | Position covariance defined relative to a tangential plane (ENU frame). +int8 cov_type # [-] | Method employed to estimate covariance (see below). +int16 num_sats # [-] | Total number of satellites in view. +int16[] sat_id # [-] | Satellite ID number. +int16[] elev # [deg] | Satellite elevation. +int16[] azim # [deg] | Satellite azimuth from true North. +int16[] cno # [db-Hz] | Satellite SNR (C/No). +float64 heading # [deg] | True heading. +float64 speed # [m/s] | Speed over ground. +float64 course # [deg] | Course over ground (w.r.t. True North). +float64 diff_age # [-] | Approximate age of differential data (last GPS MSM message received). +string diff_sta # [-] | DGPS station ID (0000-1023). +string sentence # [-] | ASCII string to be used by NTRIP clients. -# header.stamp specifies the ROS time for this measurement (the -# corresponding satellite time may be reported using the -# sensor_msgs/TimeReference message). +# Quality indicator table # -# header.frame_id is the frame of reference reported by the satellite -# receiver, usually the location of the antenna. This is a -# Euclidean frame relative to the vehicle, not a reference -# ellipsoid. -Header header - -# Latitude [degrees]. Positive is north of equator; negative is south. -float64 latitude - -# Longitude [degrees]. Positive is east of prime meridian; negative is west. -float64 longitude - -# Altitude [m]. Positive is above the WGS 84 ellipsoid. -float64 altitude - -# Speed over ground [m/s] -float64 speed +# | ID | Signal | +# |----|----------------| +# | 0 | Invalid | +# | 1 | Non-RTK fix | +# | 4 | RTK fixed | +# | 5 | RTK float | +# | 6 | Dead-reckoning | -# Course over ground [deg] -float64 course -# Position covariance [m^2] defined relative to a tangential plane -# through the reported position. The components are East, North, and -# Up (ENU), in row-major order. +# Covariance type table # -# Beware: this coordinate system exhibits singularities at the poles. - -float64[9] position_covariance - -# If the covariance of the fix is known, fill it in completely. If the -# GPS receiver provides the variance of each measurement, put them -# along the diagonal. If only Dilution of Precision is available, -# estimate an approximate covariance from that. - -uint8 COVARIANCE_TYPE_UNKNOWN = 0 -uint8 COVARIANCE_TYPE_APPROXIMATED = 1 -uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 -uint8 COVARIANCE_TYPE_KNOWN = 3 - -uint8 position_covariance_type - -# Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N -string mode \ No newline at end of file +# | ID | Signal | +# |----|----------------| +# | 0 | Unknown | +# | 1 | Approximated | +# | 2 | Diagonal known | +# | 3 | Known | diff --git a/fixposition_driver_ros1/msg/VRTK.msg b/fixposition_driver_ros1/msg/VRTK.msg deleted file mode 100644 index 9b7034f..0000000 --- a/fixposition_driver_ros1/msg/VRTK.msg +++ /dev/null @@ -1,28 +0,0 @@ -#################################################################################################### -# -# Copyright (c) 2023 ___ ___ -# \\ \\ / / -# \\ \\/ / -# / /\\ \\ -# /__/ \\__\\ Fixposition AG -# -#################################################################################################### -# -# Fixposition VRTK Message -# -# -#################################################################################################### - -Header header -string pose_frame # frame of the pose values [pose, quaternion] -string kin_frame # frame of the kinematic values [linear/angular velocity, acceleration] -geometry_msgs/PoseWithCovariance pose # position, orientation -geometry_msgs/TwistWithCovariance velocity # linear, angular -geometry_msgs/Vector3 acceleration # linear acceleration - -int16 fusion_status # field for the fusion status -int16 imu_bias_status # field for the IMU bias status -int16 gnss1_status # field for the gnss1 status -int16 gnss2_status # field for the gnss2 status -int16 wheelspeed_status # field for the wheelspeed status -string version # Fixposition software version diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index f30341d..21cfdc5 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -17,6 +17,50 @@ namespace fixposition { +void FpToRosMsg(const OdometryData& data, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + // Create message + nav_msgs::Odometry msg; + + // Populate message + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); + } + + msg.header.frame_id = data.frame_id; + msg.child_frame_id = data.child_frame_id; + + PoseWithCovDataToMsg(data.pose, msg.pose); + TwistWithCovDataToMsg(data.twist, msg.twist); + + // Publish message + pub.publish(msg); + } +} + +void FpToRosMsg(const ImuData& data, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + // Create message + sensor_msgs::Imu msg; + + // Populate message + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); + } + + msg.header.frame_id = data.frame_id; + tf::vectorEigenToMsg(data.linear_acceleration, msg.linear_acceleration); + tf::vectorEigenToMsg(data.angular_velocity, msg.angular_velocity); + + // Publish message + pub.publish(msg); + } +} + void FpToRosMsg(const FP_GNSSANT& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message @@ -386,21 +430,18 @@ void FpToRosMsg(const GP_ZDA& data, ros::Publisher& pub) { } } -void ImuDataToMsg(const ImuData& data, sensor_msgs::Imu& msg) { +void TfDataToMsg(const TfData& data, geometry_msgs::TransformStamped& msg) { + msg.header.frame_id = data.frame_id; + msg.child_frame_id = data.child_frame_id; + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = ros::Time::now(); } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); } - - msg.header.frame_id = data.frame_id; - tf::vectorEigenToMsg(data.linear_acceleration, msg.linear_acceleration); - tf::vectorEigenToMsg(data.angular_velocity, msg.angular_velocity); -} -void NavSatStatusDataToMsg(const NavSatStatusData& data, sensor_msgs::NavSatStatus& msg) { - msg.status = data.status; - msg.service = data.service; + tf::quaternionEigenToMsg(data.rotation, msg.transform.rotation); + tf::vectorEigenToMsg(data.translation, msg.transform.translation); } void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::NavSatFix& msg) { @@ -411,7 +452,8 @@ void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::NavSatFix& msg) } msg.header.frame_id = data.frame_id; - NavSatStatusDataToMsg(data.status, msg.status); + msg.status.status = data.status.status; + msg.status.service = data.status.service; msg.latitude = data.latitude; msg.longitude = data.longitude; msg.altitude = data.altitude; @@ -431,7 +473,7 @@ void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::PoseWithCo cov_map = data.cov; } -void TwistWithCovDataToMsg(const fixposition::TwistWithCovData& data, geometry_msgs::TwistWithCovariance& msg) { +void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWithCovariance& msg) { tf::vectorEigenToMsg(data.linear, msg.twist.linear); tf::vectorEigenToMsg(data.angular, msg.twist.angular); @@ -439,148 +481,124 @@ void TwistWithCovDataToMsg(const fixposition::TwistWithCovData& data, geometry_m cov_map = data.cov; } -void OdometryDataToMsg(const fixposition::OdometryData& data, nav_msgs::Odometry& msg) { - if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); - } - - msg.header.frame_id = data.frame_id; - msg.child_frame_id = data.child_frame_id; +void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub) { + if (data.fusion_status > 0) { + // Create message + geometry_msgs::TransformStamped msg; + + // Populate message + msg.header.frame_id = data.odom.frame_id; + msg.child_frame_id = data.odom.child_frame_id; - PoseWithCovDataToMsg(data.pose, msg.pose); - TwistWithCovDataToMsg(data.twist, msg.twist); -} + if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); + } -void OdometryDataToTf(const OdometryData& data, geometry_msgs::TransformStamped& msg) { - msg.header.frame_id = data.frame_id; - msg.child_frame_id = data.child_frame_id; + tf::quaternionEigenToMsg(data.odom.pose.orientation, msg.transform.rotation); + tf::vectorEigenToMsg(data.odom.pose.position, msg.transform.translation); - if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); + // Publish message + pub.sendTransform(msg); } - - tf::quaternionEigenToMsg(data.pose.orientation, msg.transform.rotation); - tf::vectorEigenToMsg(data.pose.position, msg.transform.translation); } -void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, sensor_msgs::NavSatFix& msg) { - // Populate message header - if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); - } - msg.header.frame_id = data.odom.frame_id; - - // Populate LLH position - const Eigen::Vector3d llh_pos = gnss_tf::TfWgs84LlhEcef(data.odom.pose.position); - msg.latitude = RadToDeg(llh_pos(0)); - msg.longitude = RadToDeg(llh_pos(1)); - msg.altitude = llh_pos(2); - - // Populate LLH covariance - const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = gnss_tf::RotEnuEcef(data.odom.pose.position); - const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); - - Eigen::Map> cov_map = - Eigen::Map>(msg.position_covariance.data()); - cov_map = p_cov_l; - msg.position_covariance_type = 3; +void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + // Create message + sensor_msgs::NavSatFix msg; + + // Populate message header + if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); + } + msg.header.frame_id = data.odom.frame_id; + + // Populate LLH position + const Eigen::Vector3d llh_pos = gnss_tf::TfWgs84LlhEcef(data.odom.pose.position); + msg.latitude = RadToDeg(llh_pos(0)); + msg.longitude = RadToDeg(llh_pos(1)); + msg.altitude = llh_pos(2); + + // Populate LLH covariance + const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); + const Eigen::Matrix3d C_l_e = gnss_tf::RotEnuEcef(data.odom.pose.position); + const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); + + Eigen::Map> cov_map = + Eigen::Map>(msg.position_covariance.data()); + cov_map = p_cov_l; + msg.position_covariance_type = 3; - // Populate LLH status - int status_flag = std::max(data.gnss1_status, data.gnss2_status); + // Populate LLH status + int status_flag = std::max(data.gnss1_status, data.gnss2_status); - if (status_flag < static_cast(GnssStatus::FIX_TYPE_S2D)) { - msg.status.status = static_cast(NavSatStatusData::Status::STATUS_NO_FIX); - msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_NONE); + if (status_flag < static_cast(GnssStatus::FIX_TYPE_S2D)) { + msg.status.status = static_cast(NavSatStatusData::Status::STATUS_NO_FIX); + msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_NONE); - } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_S2D) || status_flag < static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { - msg.status.status = static_cast(NavSatStatusData::Status::STATUS_FIX); - msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_ALL); + } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_S2D) || status_flag < static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { + msg.status.status = static_cast(NavSatStatusData::Status::STATUS_FIX); + msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_ALL); - } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { - msg.status.status = static_cast(NavSatStatusData::Status::STATUS_GBAS_FIX); - msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_ALL); + } else if (status_flag >= static_cast(GnssStatus::FIX_TYPE_RTK_FLOAT)) { + msg.status.status = static_cast(NavSatStatusData::Status::STATUS_GBAS_FIX); + msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_ALL); - } else { - msg.status.status = static_cast(NavSatStatusData::Status::STATUS_NO_FIX); - msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_NONE); - } -} + } else { + msg.status.status = static_cast(NavSatStatusData::Status::STATUS_NO_FIX); + msg.status.service = static_cast(NavSatStatusData::Service::SERVICE_NONE); + } -void OdomToVrtkMsg(const fixposition::FP_ODOMETRY& data, fixposition_driver_ros1::VRTK& msg) { - if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); + // Publish message + pub.publish(msg); } - - msg.header.frame_id = data.odom.frame_id; - msg.pose_frame = data.odom.child_frame_id; - msg.kin_frame = data.odom.child_frame_id; - - PoseWithCovDataToMsg(data.odom.pose, msg.pose); - TwistWithCovDataToMsg(data.odom.twist, msg.velocity); - tf::vectorEigenToMsg(data.acceleration, msg.acceleration); - msg.fusion_status = data.fusion_status; - msg.imu_bias_status = data.imu_bias_status; - msg.gnss1_status = data.gnss1_status; - msg.gnss2_status = data.gnss2_status; - msg.wheelspeed_status = data.wheelspeed_status; - msg.version = data.version; } -void OdomToImuMsg(const fixposition::FP_ODOMETRY& data, sensor_msgs::Imu& msg) { - if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); - } - - msg.header.frame_id = data.odom.frame_id; - tf::vectorEigenToMsg(data.acceleration, msg.linear_acceleration); - tf::vectorEigenToMsg(data.odom.twist.angular, msg.angular_velocity); -} +void OdomToImuMsg(const FP_ODOMETRY& data, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + // Create message + sensor_msgs::Imu msg; -void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros1::VRTK& msg) { - if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); + // Populate message + if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); + } + + msg.header.frame_id = data.odom.frame_id; + tf::vectorEigenToMsg(data.acceleration, msg.linear_acceleration); + tf::vectorEigenToMsg(data.odom.twist.angular, msg.angular_velocity); + + // Publish message + pub.publish(msg); } - - msg.header.frame_id = data.frame_id; - msg.pose_frame = data.pose_frame; - msg.kin_frame = data.kin_frame; - - PoseWithCovDataToMsg(data.pose, msg.pose); - TwistWithCovDataToMsg(data.velocity, msg.velocity); - tf::vectorEigenToMsg(data.acceleration, msg.acceleration); - msg.fusion_status = data.fusion_status; - msg.imu_bias_status = data.imu_bias_status; - msg.gnss1_status = data.gnss1_status; - msg.gnss2_status = data.gnss2_status; - msg.wheelspeed_status = data.wheelspeed_status; - msg.version = data.version; } -void TfDataToMsg(const TfData& data, geometry_msgs::TransformStamped& msg) { - msg.header.frame_id = data.frame_id; - msg.child_frame_id = data.child_frame_id; +void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + // Create message + geometry_msgs::Vector3Stamped msg; - if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); - } + // Populate message + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { + msg.header.stamp = ros::Time::now(); + } else { + msg.header.stamp = ros::Time::fromBoost(times::GpsTimeToPtime(data.stamp)); + } + msg.header.frame_id = "FP_ENU"; - tf::quaternionEigenToMsg(data.rotation, msg.transform.rotation); - tf::vectorEigenToMsg(data.translation, msg.transform.translation); + // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll + Eigen::Vector3d enu_euler = gnss_tf::RotToEul(data.pose.orientation.toRotationMatrix()); + tf::vectorEigenToMsg(enu_euler, msg.vector); + + // Publish message + pub.publish(msg); + } } } // namespace fixposition diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 4083ddd..079a24f 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -19,13 +19,6 @@ namespace fixposition { FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& params) : FixpositionDriver(params), nh_("~"), - // ODOMETRY - odometry_ecef_pub_(nh_.advertise("/fixposition/odometry_ecef", 10)), - odometry_llh_pub_(nh_.advertise("/fixposition/odometry_llh", 10)), - odometry_enu_pub_(nh_.advertise("/fixposition/odometry_enu", 10)), - odometry_smooth_pub_(nh_.advertise("/fixposition/odometry_smooth", 10)), - vrtk_pub_(nh_.advertise("/fixposition/vrtk", 10)), - // FP_A messages fpa_gnssant_pub_(nh_.advertise("/fixposition/fpa/gnssant", 10)), fpa_gnsscorr_pub_(nh_.advertise("/fixposition/fpa/gnsscorr", 10)), @@ -46,6 +39,12 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para nmea_gpvtg_pub_(nh_.advertise("/fixposition/nmea/gpvtg", 10)), nmea_gpzda_pub_(nh_.advertise("/fixposition/nmea/gpzda", 10)), + // ODOMETRY + odometry_ecef_pub_(nh_.advertise("/fixposition/odometry_ecef", 10)), + odometry_llh_pub_(nh_.advertise("/fixposition/odometry_llh", 10)), + odometry_enu_pub_(nh_.advertise("/fixposition/odometry_enu", 10)), + odometry_smooth_pub_(nh_.advertise("/fixposition/odometry_smooth", 10)), + // Orientation eul_pub_(nh_.advertise("/fixposition/ypr", 10)), eul_imu_pub_(nh_.advertise("/fixposition/imu_ypr", 10)), @@ -100,74 +99,23 @@ void FixpositionDriverNode::RegisterObservers() { dynamic_cast*>(a_converters_["ODOMETRY"].get()) ->AddObserver([this](const FP_ODOMETRY& data) { FpToRosMsg(data, fpa_odometry_pub_); - - if (odometry_ecef_pub_.getNumSubscribers() > 0) { - nav_msgs::Odometry odometry; - OdometryDataToMsg(data.odom, odometry); - odometry_ecef_pub_.publish(odometry); - } - - if (vrtk_pub_.getNumSubscribers() > 0) { - fixposition_driver_ros1::VRTK vrtk; - OdomToVrtkMsg(data, vrtk); - vrtk_pub_.publish(vrtk); - } - - if (poiimu_pub_.getNumSubscribers() > 0) { - sensor_msgs::Imu poiimu; - OdomToImuMsg(data, poiimu); - poiimu_pub_.publish(poiimu); - } - - if (odometry_llh_pub_.getNumSubscribers() > 0) { - sensor_msgs::NavSatFix msg; - OdomToNavSatFix(data, msg); - odometry_llh_pub_.publish(msg); - } - - // TFs - if (data.fusion_status > 0) { - geometry_msgs::TransformStamped tf_ecef_poi; - OdometryDataToTf(data.odom, tf_ecef_poi); - br_.sendTransform(tf_ecef_poi); - } + FpToRosMsg(data.odom, odometry_ecef_pub_); + OdomToImuMsg(data, poiimu_pub_); + OdomToNavSatFix(data, odometry_llh_pub_); + OdometryDataToTf(data, br_); }); } else if (format == "ODOMENU") { dynamic_cast*>(a_converters_["ODOMENU"].get()) ->AddObserver([this](const FP_ODOMENU& data) { FpToRosMsg(data, fpa_odomenu_pub_); - - if (odometry_enu_pub_.getNumSubscribers() > 0) { - nav_msgs::Odometry odometry_enu0; - OdometryDataToMsg(data.odom, odometry_enu0); - odometry_enu_pub_.publish(odometry_enu0); - } - - if (eul_pub_.getNumSubscribers() > 0) { - geometry_msgs::Vector3Stamped ypr; - if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { - ypr.header.stamp = ros::Time::now(); - } else { - ypr.header.stamp = ros::Time::fromBoost(fixposition::times::GpsTimeToPtime(data.odom.stamp)); - } - ypr.header.frame_id = "FP_ENU"; - - // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll - Eigen::Vector3d enu_euler = gnss_tf::RotToEul(data.odom.pose.orientation.toRotationMatrix()); - tf::vectorEigenToMsg(enu_euler, ypr.vector); - eul_pub_.publish(ypr); - } + FpToRosMsg(data.odom, odometry_enu_pub_); + OdomToYprMsg(data.odom, eul_pub_); }); } else if (format == "ODOMSH") { dynamic_cast*>(a_converters_["ODOMSH"].get()) ->AddObserver([this](const FP_ODOMSH& data) { FpToRosMsg(data, fpa_odomsh_pub_); - - if (odometry_smooth_pub_.getNumSubscribers() > 0) { - nav_msgs::Odometry odometry; - OdometryDataToMsg(data.odom, odometry); - odometry_smooth_pub_.publish(odometry); - } + FpToRosMsg(data.odom, odometry_smooth_pub_); }); } else if (format == "LLH") { dynamic_cast*>(a_converters_["LLH"].get()) @@ -182,19 +130,11 @@ void FixpositionDriverNode::RegisterObservers() { dynamic_cast*>(a_converters_["TEXT"].get()) ->AddObserver([this](const FP_TEXT& data) { FpToRosMsg(data, fpa_text_pub_); }); } else if (format == "RAWIMU") { - dynamic_cast*>(a_converters_["RAWIMU"].get())->AddObserver([this](const FP_RAWIMU& data) { - // RAWIMU Observer Lambda - sensor_msgs::Imu msg; - ImuDataToMsg(data.imu, msg); - rawimu_pub_.publish(msg); - }); + dynamic_cast*>(a_converters_["RAWIMU"].get()) + ->AddObserver([this](const FP_RAWIMU& data) { FpToRosMsg(data.imu, rawimu_pub_); }); } else if (format == "CORRIMU") { - dynamic_cast*>(a_converters_["CORRIMU"].get())->AddObserver([this](const FP_CORRIMU& data) { - // CORRIMU Observer Lambda - sensor_msgs::Imu msg; - ImuDataToMsg(data.imu, msg); - corrimu_pub_.publish(msg); - }); + dynamic_cast*>(a_converters_["CORRIMU"].get()) + ->AddObserver([this](const FP_CORRIMU& data) { FpToRosMsg(data.imu, corrimu_pub_); }); } else if (format == "TF") { dynamic_cast*>(a_converters_["TF"].get())->AddObserver([this](const FP_TF& data) { // TF Observer Lambda @@ -221,10 +161,9 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "GPGGA") { dynamic_cast*>(a_converters_["GPGGA"].get())->AddObserver([this](const GP_GGA& data) { FpToRosMsg(data, nmea_gpgga_pub_); - - if (nmea_pub_.getNumSubscribers() > 0) { - nmea_message_.gpgga = data; - PublishNmea(nmea_message_); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.AddNmeaEpoch(data); + PublishNmea(nmea_message_); // GPGGA controls the NMEA output } }); } else if (format == "GPGLL") { @@ -245,11 +184,7 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "GPRMC") { dynamic_cast*>(a_converters_["GPRMC"].get())->AddObserver([this](const GP_RMC& data) { FpToRosMsg(data, nmea_gprmc_pub_); - - if (nmea_pub_.getNumSubscribers() > 0) { - nmea_message_.gprmc = data; - PublishNmea(nmea_message_); - } + if (nmea_pub_.getNumSubscribers() > 0) { nmea_message_.AddNmeaEpoch(data); } }); } else if (format == "GPVTG") { dynamic_cast*>(a_converters_["GPVTG"].get()) @@ -257,11 +192,7 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "GPZDA") { dynamic_cast*>(a_converters_["GPZDA"].get())->AddObserver([this](const GP_ZDA& data) { FpToRosMsg(data, nmea_gpzda_pub_); - - if (nmea_pub_.getNumSubscribers() > 0) { - nmea_message_.gpzda = data; - PublishNmea(nmea_message_); - } + if (nmea_pub_.getNumSubscribers() > 0) { nmea_message_.AddNmeaEpoch(data); } }); } } @@ -274,50 +205,81 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) { fixposition_driver_ros1::NMEA msg; // ROS Header - if (data.gpzda.stamp.tow == 0.0 && data.gpzda.stamp.wno == 0) { + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { msg.header.stamp = ros::Time::now(); } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp)); + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); } msg.header.frame_id = "FP_POI"; + // Time and date fields + msg.time = data.time_str; + msg.date = data.date_str; + // Latitude [degrees]. Positive is north of equator; negative is south - msg.latitude = data.gpgga.llh(0); + msg.latitude = data.llh(0); // Longitude [degrees]. Positive is east of prime meridian; negative is west - msg.longitude = data.gpgga.llh(1); + msg.longitude = data.llh(1); - // Altitude [m]. Positive is above the WGS 84 ellipsoid - msg.altitude = data.gpgga.llh(2); + // Altitude [m]. Positive is above the WGS-84 ellipsoid + msg.altitude = data.llh(2); - // Speed over ground [m/s] - msg.speed = data.gprmc.speed; + // Quality indicator + msg.quality = data.quality; - // Course over ground [deg] - msg.course = data.gprmc.course; + // Number of satellites + msg.num_sv = data.num_sv; + + // ID numbers of satellites used in solution + for (unsigned int i = 0; i < data.ids.size(); i++) { + msg.ids.push_back(data.ids.at(i)); + } + + // Dilution of precision + msg.hdop_rec = data.hdop_receiver; + msg.pdop = data.pdop; + msg.hdop = data.hdop; + msg.vdop = data.vdop; + + // Populate GNSS pseudorange error statistics + msg.rms_range = data.rms_range; + msg.std_major = data.std_major; + msg.std_minor = data.std_minor; + msg.angle_major = data.angle_major; + msg.std_lat = data.std_lat; + msg.std_lon = data.std_lon; + msg.std_alt = data.std_alt; - // TODO: Get better position covariance from NMEA-GP-GST // Position covariance [m^2] Eigen::Map> cov_map = - Eigen::Map>(msg.position_covariance.data()); - - // Covariance diagonals - Eigen::Matrix gpgga_cov; - const double hdop = data.gpgga.hdop; - gpgga_cov(0, 0) = hdop * hdop; - gpgga_cov(1, 1) = hdop * hdop; - gpgga_cov(2, 2) = 4 * hdop * hdop; - - // Rest of covariance fields - gpgga_cov(0, 1) = gpgga_cov(1, 0) = 0.0; - gpgga_cov(0, 2) = gpgga_cov(2, 0) = 0.0; - gpgga_cov(1, 2) = gpgga_cov(2, 1) = 0.0; - - cov_map = gpgga_cov; - msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED + Eigen::Map>(msg.covariance.data()); + cov_map = data.cov; + + // Method employed to estimate covariance + msg.cov_type = data.cov_type; + + // Populate GNSS satellites in view + msg.num_sats = data.num_sats; + for (unsigned int i = 0; i < data.sat_id.size(); i++) { + msg.sat_id.push_back(data.sat_id.at(i)); + msg.elev.push_back(data.elev.at(i)); + msg.azim.push_back(data.azim.at(i)); + msg.cno.push_back(data.cno.at(i)); + } + + // True heading + msg.heading = data.heading; + + // Speed over ground [m/s] + msg.speed = data.speed; + + // Course over ground [deg] + msg.course = data.course; - // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N - msg.mode = data.gprmc.mode; + // Populate differential data information + msg.diff_age = data.diff_age; + msg.diff_sta = data.diff_sta; // Publish message nmea_pub_.publish(msg);