diff --git a/README.md b/README.md index 070bb80..34c7daf 100644 --- a/README.md +++ b/README.md @@ -176,6 +176,12 @@ The output is published on the following: | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | +- The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (SOG) and course over ground (COG) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** + + | Topic | Message Type | Frequency | Description | + | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | + | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height | + #### Vision-RTK2 IMU data - From RAWIMU, at 200Hz diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 6df5040..b1b35af 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -29,6 +29,9 @@ add_library( src/tf.cpp src/helper.cpp src/parser.cpp + src/gpgga.cpp + src/gpzda.cpp + src/gprmc.cpp ) target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp new file mode 100644 index 0000000..98d339b --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp @@ -0,0 +1,64 @@ +/** + * @file + * @brief Declaration of GpggaConverter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include +#include +#include + +namespace fixposition { + +class GpggaConverter : public BaseAsciiConverter { + public: + using GpggaObserver = std::function; + /** + * @brief Construct a new GpggaConverter + * + */ + GpggaConverter() {} + + ~GpggaConverter() = default; + + /** + * @brief Take comma-delimited tokens of GPGGA message, convert to Data structs and if available, + * call observers + * Example: + * $GPGGA,151229.40,4723.54108,N,00826.88485,E,4,12,00.98,473.5,M,,,,*3A\r\n + * + * @param[in] tokens message split in tokens + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(GpggaObserver ob) { obs_.push_back(ob); } + + private: + GpggaData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kSize_ = 15; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp new file mode 100644 index 0000000..832de05 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp @@ -0,0 +1,64 @@ +/** + * @file + * @brief Declaration of GprmcConverter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include +#include +#include + +namespace fixposition { + +class GprmcConverter : public BaseAsciiConverter { + public: + using GprmcObserver = std::function; + /** + * @brief Construct a new GprmcConverter + * + */ + GprmcConverter() {} + + ~GprmcConverter() = default; + + /** + * @brief Take comma-delimited tokens of GPRMC message, convert to Data structs and if available, + * call observers + * Example: + * $GPRMC,151227.40,A,4723.54036,N,00826.88672,E,0.0,81.6,111022,,,R*7C\r\n + * + * @param[in] tokens message split in tokens + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(GprmcObserver ob) { obs_.push_back(ob); } + + private: + GprmcData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kSize_ = 13; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp new file mode 100644 index 0000000..a2447b2 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp @@ -0,0 +1,65 @@ +/** + * @file + * @brief Declaration of GpzdaConverter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include +#include +#include +#include + +namespace fixposition { + +class GpzdaConverter : public BaseAsciiConverter { + public: + using GpzdaObserver = std::function; + /** + * @brief Construct a new GpzdaConverter + * + */ + GpzdaConverter() {} + + ~GpzdaConverter() = default; + + /** + * @brief Take comma-delimited tokens of GPZDA message, convert to Data structs and if available, + * call observers + * Example: + * $GPZDA,090411.0001,10,10,2023,00,00*69\r\n + * + * @param[in] tokens message split in tokens + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(GpzdaObserver ob) { obs_.push_back(ob); } + + private: + GpzdaData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kSize_ = 7; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp index ecf9d57..ff38989 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp @@ -132,5 +132,35 @@ struct NavSatFixData { NavSatFixData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); } }; +struct GpggaData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string time; + double latitude; + double longitude; + double altitude; + Eigen::Matrix cov; + int position_covariance_type; + GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); } +}; + +struct GpzdaData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string time; + std::string date; + times::GpsTime stamp; + GpzdaData() : time(""), date("") {} +}; + +struct GprmcData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string time; + std::string mode; + double latitude; + double longitude; + double speed; + double course; + GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0) {} +}; + } // namespace fixposition #endif //__FIXPOSITION_DRIVER_LIB_MSG_DATA__ diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 3901539..1f2fb78 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -22,6 +22,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -137,6 +140,12 @@ bool FixpositionDriver::InitializeConverters() { a_converters_["RAWIMU"] = std::unique_ptr(new ImuConverter(false)); } else if (format == "CORRIMU") { a_converters_["CORRIMU"] = std::unique_ptr(new ImuConverter(true)); + } else if (format == "GPGGA") { + a_converters_["GPGGA"] = std::unique_ptr(new GpggaConverter()); + } else if (format == "GPZDA") { + a_converters_["GPZDA"] = std::unique_ptr(new GpzdaConverter()); + } else if (format == "GPRMC") { + a_converters_["GPRMC"] = std::unique_ptr(new GprmcConverter()); } else if (format == "TF") { if (a_converters_.find("TF") == a_converters_.end()) { a_converters_["TF"] = std::unique_ptr(new TfConverter()); @@ -230,12 +239,22 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); // if it doesn't start with FP then do nothing - if (tokens.at(0) != "FP") { + if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPRMC")) { return; } // Get the header of the sentence - const std::string header = tokens.at(1); + std::string _header; + if (tokens.at(0) == "GPGGA") { + _header = "GPGGA"; + } else if (tokens.at(0) == "GPZDA") { + _header = "GPZDA"; + } else if (tokens.at(0) == "GPRMC") { + _header = "GPRMC"; + } else { + _header = tokens.at(1); + } + const std::string header = _header; // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU" if (a_converters_[header] != nullptr) { diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp new file mode 100644 index 0000000..52e129a --- /dev/null +++ b/fixposition_driver_lib/src/gpgga.cpp @@ -0,0 +1,82 @@ +/** + * @file + * @brief Implementation of GpggaConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int time_idx = 1; +static constexpr const int lat_idx = 2; +static constexpr const int lat_ns_idx = 3; +static constexpr const int lon_idx = 4; +static constexpr const int lon_ew_idx = 5; +static constexpr const int quality_idx = 6; +static constexpr const int num_sv_idx = 7; +static constexpr const int hdop_idx = 8; +static constexpr const int alt_idx = 9; +static constexpr const int alt_unit_idx = 10; +static constexpr const int sep_idx = 11; +static constexpr const int sep_unit_idx = 12; +static constexpr const int diff_age_idx = 13; +static constexpr const int diff_sta_idx = 14; + +void GpggaConverter::ConvertTokens(const std::vector& tokens) { + // Check if message size is wrong + bool ok = tokens.size() == kSize_; + if (!ok) { + std::cout << "Error in parsing GPGGA string with " << tokens.size() << " fields! GPGGA message will be empty.\n"; + msg_ = GpggaData(); + return; + } + + // Header stamps + msg_.time = tokens.at(time_idx); + + // LLH coordinates + const std::string _latstr = tokens.at(lat_idx); + double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; + msg_.latitude = _lat; + + const std::string _lonstr = tokens.at(lon_idx); + double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; + msg_.longitude = _lon; + + msg_.altitude = StringToDouble(tokens.at(alt_idx)); + + // Covariance diagonals + const double hdop = StringToDouble(tokens.at(hdop_idx)); + msg_.cov(0, 0) = hdop * hdop; + msg_.cov(1, 1) = hdop * hdop; + msg_.cov(2, 2) = 4 * hdop * hdop; + + // Rest of covariance fields + msg_.cov(0, 1) = msg_.cov(1, 0) = 0.0; + msg_.cov(0, 2) = msg_.cov(2, 0) = 0.0; + msg_.cov(1, 2) = msg_.cov(2, 1) = 0.0; + msg_.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp new file mode 100644 index 0000000..4d6a6f4 --- /dev/null +++ b/fixposition_driver_lib/src/gprmc.cpp @@ -0,0 +1,76 @@ +/** + * @file + * @brief Implementation of GprmcConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +// unit conversion constant +static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s + +/// msg field indices +static constexpr const int time_idx = 1; +static constexpr const int status_idx = 2; +static constexpr const int lat_idx = 3; +static constexpr const int lat_ns_idx = 4; +static constexpr const int lon_idx = 5; +static constexpr const int lon_ew_idx = 6; +static constexpr const int speed_idx = 7; +static constexpr const int course_idx = 8; +static constexpr const int date_idx = 9; +static constexpr const int magvar_idx = 10; +static constexpr const int magvar_ew = 11; +static constexpr const int mode_idx = 12; + +void GprmcConverter::ConvertTokens(const std::vector& tokens) { + // Check if message size is wrong + bool ok = tokens.size() == kSize_; + if (!ok) { + std::cout << "Error in parsing GPRMC string with " << tokens.size() << " fields! GPRMC message will be empty.\n"; + msg_ = GprmcData(); + return; + } + + // Header stamps + msg_.time = tokens.at(time_idx); + + // LLH coordinates + const std::string _latstr = tokens.at(lat_idx); + double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; + msg_.latitude = _lat; + + const std::string _lonstr = tokens.at(lon_idx); + double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; + msg_.longitude = _lon; + + // Speed [m/s] and course [deg] over ground + msg_.speed = StringToDouble(tokens.at(speed_idx)) * knots_to_ms; + msg_.course = StringToDouble(tokens.at(course_idx)); + + // Get GPS status + msg_.mode = tokens.at(mode_idx); + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_lib/src/gpzda.cpp b/fixposition_driver_lib/src/gpzda.cpp new file mode 100644 index 0000000..e774de8 --- /dev/null +++ b/fixposition_driver_lib/src/gpzda.cpp @@ -0,0 +1,108 @@ +/** + * @file + * @brief Implementation of GpzdaConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int time_idx = 1; +static constexpr const int day_idx = 2; +static constexpr const int month_idx = 3; +static constexpr const int year_idx = 4; +static constexpr const int local_hr_idx = 5; +static constexpr const int local_min_idx = 6; + +#include +#include +#include + +// Function to convert UTC time with milliseconds to GPS time +void convertToGPSTime(const std::string& utcTimeString, std::string& gpsWeek, std::string& gpsTimeOfWeek) { + // Define constants + const double secondsInWeek = 604800.0; // 7 days in seconds + + // Parse the input string + std::tm tmTime = {}; + std::istringstream iss(utcTimeString); + iss >> std::get_time(&tmTime, "%d/%m/%Y %H:%M:%S"); + + // Read milliseconds from input + char dot; + std::string milliseconds; + iss >> dot >> milliseconds; + double ms = std::stod("0." + milliseconds); + + if (iss.fail()) { + std::cerr << "Error parsing input string.\n"; + return; + } + + // Convert UTC time to time since epoch + std::time_t utcTime = std::mktime(&tmTime); + + // GPS epoch time (January 6, 1980) + std::tm gpsEpoch = {}; + gpsEpoch.tm_year = 80; // years since 1900 + gpsEpoch.tm_mon = 0; // months since January + gpsEpoch.tm_mday = 6; // day of the month + std::time_t gpsEpochTime = std::mktime(&gpsEpoch); + + // Calculate GPS time of week and GPS week number + double timeDifference = std::difftime(utcTime, gpsEpochTime); + int gpsWeekNumber = static_cast(std::floor(timeDifference / secondsInWeek)); + double gpsTime = std::fmod(timeDifference, secondsInWeek); + + // Add milliseconds to GPS time + gpsTime += (18 + ms); + + // Convert results to strings + std::ostringstream ossWeek, ossTime; + ossWeek << gpsWeekNumber; + ossTime << std::fixed << std::setprecision(6) << gpsTime; + + gpsWeek = ossWeek.str(); + gpsTimeOfWeek = ossTime.str(); +} + +void GpzdaConverter::ConvertTokens(const std::vector& tokens) { + // Check if message size is wrong + bool ok = tokens.size() == kSize_; + if (!ok) { + std::cout << "Error in parsing GPZDA string with " << tokens.size() << " fields! GPZDA message will be empty.\n"; + msg_ = GpzdaData(); + return; + } + + // Populate time fields + msg_.time = tokens.at(time_idx); + msg_.date = tokens.at(day_idx) + '/' + tokens.at(month_idx) + '/' + tokens.at(year_idx); + + // Generate GPS timestamp + std::string utcTimeString = msg_.date + " " + msg_.time.substr(0,2) + ":" + msg_.time.substr(2,2) + ":" + msg_.time.substr(4); + std::string gps_tow, gps_week; + convertToGPSTime(utcTimeString, gps_week, gps_tow); + msg_.stamp = ConvertGpsTime(gps_week, gps_tow); + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 07e1898..74ce039 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -26,6 +26,7 @@ add_message_files( FILES VRTK.msg Speed.msg + NMEA.msg ) generate_messages( 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 0bb259f..2f5c093 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 @@ -27,6 +27,7 @@ /* PACKAGE */ #include +#include 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 064ea13..13dc590 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 @@ -52,6 +52,23 @@ class FixpositionDriverNode : public FixpositionDriver { void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg); + struct NmeaMessage { + GpggaData gpgga; + GpzdaData gpzda; + GprmcData gprmc; + + /** + * @brief Construct a new Fixposition Driver Node object + */ + bool checkEpoch() { + if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) { + return true; + } else { + return false; + } + } + }; + private: /** * @brief Observer Functions to publish NavSatFix from BestGnssPos @@ -61,6 +78,13 @@ class FixpositionDriverNode : public FixpositionDriver { */ void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + /** + * @brief Observer Function to publish NMEA message from GPGGA, GPRMC, and GPZDA once the GNSS epoch transmission is complete + * + * @param[in] data + */ + void PublishNmea(NmeaMessage data); + ros::NodeHandle nh_; ros::Subscriber ws_sub_; //!< wheelspeed message subscriber @@ -69,6 +93,7 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher navsatfix_pub_; ros::Publisher navsatfix_gnss1_pub_; ros::Publisher navsatfix_gnss2_pub_; + ros::Publisher nmea_pub_; ros::Publisher odometry_pub_; //!< ECEF Odometry ros::Publisher poiimu_pub_; //!< Bias corrected IMU ros::Publisher vrtk_pub_; //!< VRTK message @@ -76,6 +101,8 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU ros::Publisher eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal + NmeaMessage nmea_message_; + tf2_ros::TransformBroadcaster br_; tf2_ros::StaticTransformBroadcaster static_br_; }; diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index f9165b6..7bf49c4 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -1,5 +1,5 @@ fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"] + formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: tcp port: "21000" ip: "10.0.2.1" # change to VRTK2's IP address in the network diff --git a/fixposition_driver_ros1/msg/NMEA.msg b/fixposition_driver_ros1/msg/NMEA.msg new file mode 100644 index 0000000..98d2649 --- /dev/null +++ b/fixposition_driver_ros1/msg/NMEA.msg @@ -0,0 +1,66 @@ +#################################################################################################### +# +# Copyright (c) 2023 ___ ___ +# \\ \\ / / +# \\ \\/ / +# / /\\ \\ +# /__/ \\__\\ Fixposition AG +# +#################################################################################################### +# +# Fixposition NMEA Message +# +# +#################################################################################################### + +# Navigation Satellite fix for any Global Navigation Satellite System +# +# Specified using the WGS 84 reference ellipsoid + +# header.stamp specifies the ROS time for this measurement (the +# corresponding satellite time may be reported using the +# sensor_msgs/TimeReference message). +# +# 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 + +# 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. +# +# 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 diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 49a9225..161426c 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -22,6 +22,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -41,6 +44,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para navsatfix_pub_(nh_.advertise("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(nh_.advertise("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(nh_.advertise("/fixposition/gnss2", 100)), + nmea_pub_(nh_.advertise("/fixposition/nmea", 100)), // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), @@ -48,9 +52,13 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para odometry_enu0_pub_(nh_.advertise("/fixposition/odometry_enu", 100)), eul_pub_(nh_.advertise("/fixposition/ypr", 100)), eul_imu_pub_(nh_.advertise("/fixposition/imu_ypr", 100)) { + ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 100, &FixpositionDriverNode::WsCallback, this, ros::TransportHints().tcpNoDelay()); + + + Connect(); RegisterObservers(); } @@ -154,10 +162,74 @@ void FixpositionDriverNode::RegisterObservers() { static_br_.sendTransform(tf); } }); + } else if (format == "GPGGA") { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) { + // GPGGA Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gpgga = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPZDA") { + dynamic_cast(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) { + // GPZDA Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gpzda = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPRMC") { + dynamic_cast(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) { + // GPRMC Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gprmc = data; + PublishNmea(nmea_message_); + } + }); } } } +void FixpositionDriverNode::PublishNmea(NmeaMessage data) { + // If epoch message is complete, generate NMEA output + if (data.checkEpoch()) { + // Generate new message + fixposition_driver_ros1::NMEA msg; + + // ROS Header + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp)); + msg.header.frame_id = "LLH"; + + // Latitude [degrees]. Positive is north of equator; negative is south + msg.latitude = data.gpgga.latitude; + + // Longitude [degrees]. Positive is east of prime meridian; negative is west + msg.longitude = data.gpgga.longitude; + + // Altitude [m]. Positive is above the WGS 84 ellipsoid + msg.altitude = data.gpgga.altitude; + + // Speed over ground [m/s] + msg.speed = data.gprmc.speed; + + // Course over ground [deg] + msg.course = data.gprmc.course; + + // TODO: Get better position covariance from NMEA-GP-GST + // Position covariance [m^2] + Eigen::Map> cov_map = + Eigen::Map>(msg.position_covariance.data()); + cov_map = data.gpgga.cov; + msg.position_covariance_type = data.gpgga.position_covariance_type; + + // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N + msg.mode = data.gprmc.mode; + + // Publish message + nmea_pub_.publish(msg); + } +} + void FixpositionDriverNode::Run() { ros::Rate rate(params_.fp_output.rate); while (ros::ok()) { diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index ca2b66f..7353ac4 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(fixposition_driver_lib REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/VRTK.msg msg/Speed.msg + msg/NMEA.msg DEPENDENCIES std_msgs nav_msgs diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index 0bcdc16..8afe911 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -37,6 +37,7 @@ /* PACKAGE */ #include +#include namespace fixposition { diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 609ca39..6573259 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -55,6 +55,23 @@ class FixpositionDriverNode : public FixpositionDriver { void WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg); + struct NmeaMessage { + GpggaData gpgga; + GpzdaData gpzda; + GprmcData gprmc; + + /** + * @brief Check if GNSS epoch is complete + */ + bool checkEpoch() { + if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) { + return true; + } else { + return false; + } + } + }; + private: /** * @brief Observer Functions to publish NavSatFix from BestGnssPos @@ -63,6 +80,13 @@ class FixpositionDriverNode : public FixpositionDriver { * @param[in] payload */ void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + + /** + * @brief Observer Function to publish NMEA message from GPGGA, GPRMC, and GPZDA once the GNSS epoch transmission is complete + * + * @param[in] data + */ + void PublishNmea(NmeaMessage data); std::shared_ptr node_; rclcpp::Subscription::SharedPtr ws_sub_; //!< wheelspeed message subscriber @@ -72,6 +96,7 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr navsatfix_pub_; rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; + rclcpp::Publisher::SharedPtr nmea_pub_; rclcpp::Publisher::SharedPtr odometry_pub_; rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message @@ -83,6 +108,8 @@ class FixpositionDriverNode : public FixpositionDriver { std::shared_ptr br_; std::shared_ptr static_br_; + + NmeaMessage nmea_message_; }; } // namespace fixposition diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml index 9348466..09d7079 100644 --- a/fixposition_driver_ros2/launch/tcp.yaml +++ b/fixposition_driver_ros2/launch/tcp.yaml @@ -1,7 +1,7 @@ fixposition_driver_ros2: ros__parameters: fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"] + formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: "tcp" port: "21000" ip: "10.0.2.1" # change to VRTK2's IP address in the network diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg new file mode 100644 index 0000000..0c6064e --- /dev/null +++ b/fixposition_driver_ros2/msg/NMEA.msg @@ -0,0 +1,63 @@ +#################################################################################################### +# +# Copyright (c) 2023 +# Fixposition AG +# +#################################################################################################### +# +# Fixposition NMEA Message +# +# +#################################################################################################### + +# Navigation Satellite fix for any Global Navigation Satellite System +# +# Specified using the WGS 84 reference ellipsoid + +# header.stamp specifies the ROS time for this measurement (the +# corresponding satellite time may be reported using the +# sensor_msgs/TimeReference message). +# +# 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. +std_msgs/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 + +# 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. +# +# 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 diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 0046534..7c1d2b9 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -24,6 +24,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -43,6 +46,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, navsatfix_pub_(node_->create_publisher("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(node_->create_publisher("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(node_->create_publisher("/fixposition/gnss2", 100)), + nmea_pub_(node_->create_publisher("/fixposition/nmea", 100)), odometry_pub_(node_->create_publisher("/fixposition/odometry", 100)), poiimu_pub_(node_->create_publisher("/fixposition/poiimu", 100)), vrtk_pub_(node_->create_publisher("/fixposition/vrtk", 100)), @@ -183,10 +187,74 @@ void FixpositionDriverNode::RegisterObservers() { static_br_->sendTransform(tf); } }); + } else if (format == "GPGGA") { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) { + // GPGGA Observer Lambda + if (nmea_pub_->get_subscription_count() > 0) { + nmea_message_.gpgga = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPZDA") { + dynamic_cast(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) { + // GPZDA Observer Lambda + if (nmea_pub_->get_subscription_count() > 0) { + nmea_message_.gpzda = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPRMC") { + dynamic_cast(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) { + // GPRMC Observer Lambda + if (nmea_pub_->get_subscription_count() > 0) { + nmea_message_.gprmc = data; + PublishNmea(nmea_message_); + } + }); } } } +void FixpositionDriverNode::PublishNmea(NmeaMessage data) { + // If epoch message is complete, generate NMEA output + if (data.checkEpoch()) { + // Generate new message + fixposition_driver_ros2::msg::NMEA msg; + + // ROS Header + msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp); + msg.header.frame_id = "LLH"; + + // Latitude [degrees]. Positive is north of equator; negative is south + msg.latitude = data.gpgga.latitude; + + // Longitude [degrees]. Positive is east of prime meridian; negative is west + msg.longitude = data.gpgga.longitude; + + // Altitude [m]. Positive is above the WGS 84 ellipsoid + msg.altitude = data.gpgga.altitude; + + // Speed over ground [m/s] + msg.speed = data.gprmc.speed; + + // Course over ground [deg] + msg.course = data.gprmc.course; + + // TODO: Get better position covariance from NMEA-GP-GST + // Position covariance [m^2] + Eigen::Map> cov_map = + Eigen::Map>(msg.position_covariance.data()); + cov_map = data.gpgga.cov; + msg.position_covariance_type = data.gpgga.position_covariance_type; + + // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N + msg.mode = data.gprmc.mode; + + // Publish message + nmea_pub_->publish(msg); + } +} + void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) { FixpositionDriver::WsCallback(msg->speeds); }