00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef UBLOX_GPS_NODE_H
00031 #define UBLOX_GPS_NODE_H
00032
00033
00034 #include <vector>
00035 #include <set>
00036
00037 #include <boost/algorithm/string.hpp>
00038 #include <boost/lexical_cast.hpp>
00039 #include <boost/regex.hpp>
00040
00041 #include <ros/ros.h>
00042 #include <ros/console.h>
00043 #include <ros/serialization.h>
00044 #include <diagnostic_updater/diagnostic_updater.h>
00045 #include <diagnostic_updater/publisher.h>
00046
00047 #include <geometry_msgs/TwistWithCovarianceStamped.h>
00048 #include <geometry_msgs/Vector3Stamped.h>
00049 #include <sensor_msgs/NavSatFix.h>
00050 #include <sensor_msgs/TimeReference.h>
00051 #include <sensor_msgs/Imu.h>
00052
00053 #include <ublox_msgs/ublox_msgs.h>
00054
00055 #include <ublox_gps/gps.h>
00056 #include <ublox_gps/utils.h>
00057 #include <ublox_gps/raw_data_pa.h>
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00084 namespace ublox_node {
00085
00087 constexpr static uint32_t kROSQueueSize = 1;
00089 constexpr static uint16_t kDefaultMeasPeriod = 250;
00091 constexpr static uint32_t kSubscribeRate = 1;
00093 constexpr static uint32_t kNavSvInfoSubscribeRate = 20;
00094
00095
00097 boost::shared_ptr<diagnostic_updater::Updater> updater;
00099 boost::shared_ptr<ros::NodeHandle> nh;
00100
00102 ublox_gps::Gps gps;
00104 std::set<std::string> supported;
00106
00110 std::map<std::string, bool> enabled;
00112 std::string frame_id;
00115 int fix_status_service;
00117 uint16_t meas_rate;
00119 uint16_t nav_rate;
00121 std::vector<uint8_t> rtcm_ids;
00123 std::vector<uint8_t> rtcm_rates;
00125 bool config_on_startup_flag_;
00126
00127
00129 struct UbloxTopicDiagnostic {
00130 UbloxTopicDiagnostic() {}
00131
00140 UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) {
00141 const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate);
00142 min_freq = target_freq;
00143 max_freq = target_freq;
00144 diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq,
00145 freq_tol, freq_window);
00146 diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic,
00147 *updater,
00148 freq_param);
00149 }
00150
00161 UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max,
00162 double freq_tol, int freq_window) {
00163 min_freq = freq_min;
00164 max_freq = freq_max;
00165 diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq,
00166 freq_tol, freq_window);
00167 diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic,
00168 *updater,
00169 freq_param);
00170 }
00171
00173 diagnostic_updater::HeaderlessTopicDiagnostic *diagnostic;
00175 double min_freq;
00177 double max_freq;
00178 };
00179
00181 struct FixDiagnostic {
00182 FixDiagnostic() {}
00183
00193 FixDiagnostic (std::string name, double freq_tol, int freq_window,
00194 double stamp_min) {
00195 const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate);
00196 min_freq = target_freq;
00197 max_freq = target_freq;
00198 diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq,
00199 freq_tol, freq_window);
00200 double stamp_max = meas_rate * 1e-3 * (1 + freq_tol);
00201 diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max);
00202 diagnostic = new diagnostic_updater::TopicDiagnostic(name,
00203 *updater,
00204 freq_param,
00205 time_param);
00206 }
00207
00209 diagnostic_updater::TopicDiagnostic *diagnostic;
00211 double min_freq;
00213 double max_freq;
00214 };
00215
00217 FixDiagnostic freq_diag;
00218
00234 uint8_t modelFromString(const std::string& model);
00235
00245 uint8_t fixModeFromString(const std::string& mode);
00246
00254 template <typename V, typename T>
00255 void checkMin(V val, T min, std::string name) {
00256 if(val < min) {
00257 std::stringstream oss;
00258 oss << "Invalid settings: " << name << " must be > " << min;
00259 throw std::runtime_error(oss.str());
00260 }
00261 }
00262
00271 template <typename V, typename T>
00272 void checkRange(V val, T min, T max, std::string name) {
00273 if(val < min || val > max) {
00274 std::stringstream oss;
00275 oss << "Invalid settings: " << name << " must be in range [" << min <<
00276 ", " << max << "].";
00277 throw std::runtime_error(oss.str());
00278 }
00279 }
00280
00289 template <typename V, typename T>
00290 void checkRange(std::vector<V> val, T min, T max, std::string name) {
00291 for(size_t i = 0; i < val.size(); i++) {
00292 std::stringstream oss;
00293 oss << name << "[" << i << "]";
00294 checkRange(val[i], min, max, oss.str());
00295 }
00296 }
00297
00305 template <typename U>
00306 bool getRosUint(const std::string& key, U &u) {
00307 int param;
00308 if (!nh->getParam(key, param)) return false;
00309
00310 U min = std::numeric_limits<U>::lowest();
00311 U max = std::numeric_limits<U>::max();
00312 checkRange(param, min, max, key);
00313
00314 u = (U) param;
00315 return true;
00316 }
00317
00326 template <typename U, typename V>
00327 void getRosUint(const std::string& key, U &u, V default_val) {
00328 if(!getRosUint(key, u))
00329 u = default_val;
00330 }
00331
00337 template <typename U>
00338 bool getRosUint(const std::string& key, std::vector<U> &u) {
00339 std::vector<int> param;
00340 if (!nh->getParam(key, param)) return false;
00341
00342
00343 U min = std::numeric_limits<U>::lowest();
00344 U max = std::numeric_limits<U>::max();
00345 checkRange(param, min, max, key);
00346
00347
00348 u.insert(u.begin(), param.begin(), param.end());
00349 return true;
00350 }
00351
00359 template <typename I>
00360 bool getRosInt(const std::string& key, I &u) {
00361 int param;
00362 if (!nh->getParam(key, param)) return false;
00363
00364 I min = std::numeric_limits<I>::lowest();
00365 I max = std::numeric_limits<I>::max();
00366 checkRange(param, min, max, key);
00367
00368 u = (I) param;
00369 return true;
00370 }
00371
00380 template <typename U, typename V>
00381 void getRosInt(const std::string& key, U &u, V default_val) {
00382 if(!getRosInt(key, u))
00383 u = default_val;
00384 }
00385
00391 template <typename I>
00392 bool getRosInt(const std::string& key, std::vector<I> &i) {
00393 std::vector<int> param;
00394 if (!nh->getParam(key, param)) return false;
00395
00396
00397 I min = std::numeric_limits<I>::lowest();
00398 I max = std::numeric_limits<I>::max();
00399 checkRange(param, min, max, key);
00400
00401
00402 i.insert(i.begin(), param.begin(), param.end());
00403 return true;
00404 }
00405
00414 template <typename MessageT>
00415 void publish(const MessageT& m, const std::string& topic) {
00416 static ros::Publisher publisher = nh->advertise<MessageT>(topic,
00417 kROSQueueSize);
00418 publisher.publish(m);
00419 }
00420
00426 bool supportsGnss(std::string gnss) {
00427 return supported.count(gnss) > 0;
00428 }
00429
00436 class ComponentInterface {
00437 public:
00443 virtual void getRosParams() = 0;
00444
00449 virtual bool configureUblox() = 0;
00450
00456 virtual void initializeRosDiagnostics() = 0;
00457
00461 virtual void subscribe() = 0;
00462 };
00463
00464 typedef boost::shared_ptr<ComponentInterface> ComponentPtr;
00465
00481 class UbloxNode : public virtual ComponentInterface {
00482 public:
00484 constexpr static int kResetWait = 10;
00486 constexpr static double kPollDuration = 1.0;
00487
00489 constexpr static float kDiagnosticPeriod = 0.2;
00491 constexpr static double kFixFreqTol = 0.15;
00493 constexpr static double kFixFreqWindow = 10;
00495 constexpr static double kTimeStampStatusMin = 0;
00496
00500 UbloxNode();
00501
00505 void getRosParams();
00506
00511 bool configureUblox();
00512
00516 void subscribe();
00517
00521 void initializeRosDiagnostics();
00522
00526 void printInf(const ublox_msgs::Inf &m, uint8_t id);
00527
00528 private:
00529
00533 void initializeIo();
00534
00539 void initialize();
00540
00544 void shutdown();
00545
00550 bool resetDevice();
00551
00557 void processMonVer();
00558
00563 void addFirmwareInterface();
00564
00572 void addProductInterface(std::string product_category,
00573 std::string ref_rov = "");
00574
00579 void pollMessages(const ros::TimerEvent& event);
00580
00584 void configureInf();
00585
00587
00591 std::vector<boost::shared_ptr<ComponentInterface> > components_;
00592
00594 float protocol_version_ = 0;
00595
00597 std::string device_;
00599 std::string dynamic_model_;
00601 std::string fix_mode_;
00603 uint8_t dmodel_;
00605 uint8_t fmode_;
00607 uint32_t baudrate_;
00609 uint16_t uart_in_;
00611 uint16_t uart_out_;
00613 uint16_t usb_tx_;
00615
00616 bool set_usb_;
00618 uint16_t usb_in_;
00620 uint16_t usb_out_ ;
00622 double rate_;
00624 bool set_dat_;
00626 ublox_msgs::CfgDAT cfg_dat_;
00628 bool enable_sbas_;
00630 bool enable_ppp_;
00632 uint8_t sbas_usage_;
00634 uint8_t max_sbas_;
00636 uint8_t dr_limit_;
00638 ublox_msgs::CfgCFG load_;
00640 ublox_msgs::CfgCFG save_;
00642 uint8_t tim_rate_;
00643
00645 RawDataStreamPa rawDataStreamPa_;
00646 };
00647
00653 class UbloxFirmware : public virtual ComponentInterface {
00654 public:
00658 void initializeRosDiagnostics();
00659
00660 protected:
00664 virtual void fixDiagnostic(
00665 diagnostic_updater::DiagnosticStatusWrapper& stat) = 0;
00666 };
00667
00671 class UbloxFirmware6 : public UbloxFirmware {
00672 public:
00673 UbloxFirmware6();
00674
00678 void getRosParams();
00679
00684 bool configureUblox();
00685
00689 void subscribe();
00690
00691 protected:
00696 void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
00697
00698 private:
00706 void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m);
00707
00714 void callbackNavVelNed(const ublox_msgs::NavVELNED& m);
00715
00722 void callbackNavSol(const ublox_msgs::NavSOL& m);
00723
00725 ublox_msgs::NavPOSLLH last_nav_pos_;
00727 ublox_msgs::NavVELNED last_nav_vel_;
00729 ublox_msgs::NavSOL last_nav_sol_;
00731 sensor_msgs::NavSatFix fix_;
00733 geometry_msgs::TwistWithCovarianceStamped velocity_;
00734
00736 ublox_msgs::CfgNMEA6 cfg_nmea_;
00738 bool set_nmea_;
00739 };
00740
00750 template<typename NavPVT>
00751 class UbloxFirmware7Plus : public UbloxFirmware {
00752 public:
00761 void callbackNavPvt(const NavPVT& m) {
00762 if(enabled["nav_pvt"]) {
00763
00764 static ros::Publisher publisher = nh->advertise<NavPVT>("navpvt",
00765 kROSQueueSize);
00766 publisher.publish(m);
00767 }
00768
00769
00770
00771
00772 static ros::Publisher fixPublisher =
00773 nh->advertise<sensor_msgs::NavSatFix>("fix", kROSQueueSize);
00774
00775 sensor_msgs::NavSatFix fix;
00776 fix.header.frame_id = frame_id;
00777
00778 uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED;
00779 if (((m.valid & valid_time) == valid_time) &&
00780 (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) {
00781
00782
00783
00784
00785 if (m.nano < 0) {
00786 fix.header.stamp.sec = toUtcSeconds(m) - 1;
00787 fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9);
00788 }
00789 else {
00790 fix.header.stamp.sec = toUtcSeconds(m);
00791 fix.header.stamp.nsec = (uint32_t)(m.nano);
00792 }
00793 } else {
00794
00795 fix.header.stamp = ros::Time::now();
00796 }
00797
00798 fix.latitude = m.lat * 1e-7;
00799 fix.longitude = m.lon * 1e-7;
00800 fix.altitude = m.height * 1e-3;
00801
00802 bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK;
00803 if (fixOk && m.fixType >= m.FIX_TYPE_2D) {
00804 fix.status.status = fix.status.STATUS_FIX;
00805 if(m.flags & m.CARRIER_PHASE_FIXED)
00806 fix.status.status = fix.status.STATUS_GBAS_FIX;
00807 }
00808 else {
00809 fix.status.status = fix.status.STATUS_NO_FIX;
00810 }
00811
00812 fix.status.service = fix_status_service;
00813
00814
00815 const double varH = pow(m.hAcc / 1000.0, 2);
00816 const double varV = pow(m.vAcc / 1000.0, 2);
00817 fix.position_covariance[0] = varH;
00818 fix.position_covariance[4] = varH;
00819 fix.position_covariance[8] = varV;
00820 fix.position_covariance_type =
00821 sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
00822
00823 fixPublisher.publish(fix);
00824
00825
00826
00827
00828 static ros::Publisher velocityPublisher =
00829 nh->advertise<geometry_msgs::TwistWithCovarianceStamped>("fix_velocity",
00830 kROSQueueSize);
00831 geometry_msgs::TwistWithCovarianceStamped velocity;
00832 velocity.header.stamp = fix.header.stamp;
00833 velocity.header.frame_id = frame_id;
00834
00835
00836 velocity.twist.twist.linear.x = m.velE * 1e-3;
00837 velocity.twist.twist.linear.y = m.velN * 1e-3;
00838 velocity.twist.twist.linear.z = -m.velD * 1e-3;
00839
00840 const double covSpeed = pow(m.sAcc * 1e-3, 2);
00841 const int cols = 6;
00842 velocity.twist.covariance[cols * 0 + 0] = covSpeed;
00843 velocity.twist.covariance[cols * 1 + 1] = covSpeed;
00844 velocity.twist.covariance[cols * 2 + 2] = covSpeed;
00845 velocity.twist.covariance[cols * 3 + 3] = -1;
00846
00847 velocityPublisher.publish(velocity);
00848
00849
00850
00851
00852 last_nav_pvt_ = m;
00853 freq_diag.diagnostic->tick(fix.header.stamp);
00854 updater->update();
00855 }
00856
00857 protected:
00858
00862 void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) {
00863
00864 if (last_nav_pvt_.fixType ==
00865 ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) {
00866 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
00867 stat.message = "Dead reckoning only";
00868 } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) {
00869 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
00870 stat.message = "2D fix";
00871 } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) {
00872 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00873 stat.message = "3D fix";
00874 } else if (last_nav_pvt_.fixType ==
00875 ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) {
00876 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00877 stat.message = "GPS and dead reckoning combined";
00878 } else if (last_nav_pvt_.fixType ==
00879 ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) {
00880 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00881 stat.message = "Time only fix";
00882 }
00883
00884
00885 if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) {
00886 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
00887 stat.message += ", fix not ok";
00888 }
00889
00890 if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) {
00891 stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00892 stat.message = "No fix";
00893 }
00894
00895
00896 stat.add("iTOW [ms]", last_nav_pvt_.iTOW);
00897 stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7);
00898 stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7);
00899 stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3);
00900 stat.add("Height above MSL [m]", last_nav_pvt_.hMSL * 1e-3);
00901 stat.add("Horizontal Accuracy [m]", last_nav_pvt_.hAcc * 1e-3);
00902 stat.add("Vertical Accuracy [m]", last_nav_pvt_.vAcc * 1e-3);
00903 stat.add("# SVs used", (int)last_nav_pvt_.numSV);
00904 }
00905
00907 NavPVT last_nav_pvt_;
00908
00910 bool enable_gps_;
00912 bool enable_glonass_;
00914 bool enable_qzss_;
00916 bool enable_sbas_;
00918 uint32_t qzss_sig_cfg_;
00919 };
00920
00924 class UbloxFirmware7 : public UbloxFirmware7Plus<ublox_msgs::NavPVT7> {
00925 public:
00926 UbloxFirmware7();
00927
00933 void getRosParams();
00934
00938 bool configureUblox();
00939
00945 void subscribe();
00946
00947 private:
00949
00952 ublox_msgs::CfgNMEA7 cfg_nmea_;
00954 bool set_nmea_;
00955 };
00956
00960 class UbloxFirmware8 : public UbloxFirmware7Plus<ublox_msgs::NavPVT> {
00961 public:
00962 UbloxFirmware8();
00963
00969 void getRosParams();
00970
00978 bool configureUblox();
00979
00987 void subscribe();
00988
00989 private:
00990
00992 bool enable_galileo_;
00994 bool enable_beidou_;
00996 bool enable_imes_;
00998 bool set_nmea_;
01000 ublox_msgs::CfgNMEA cfg_nmea_;
01002 bool clear_bbr_;
01003 };
01004
01008 class RawDataProduct: public virtual ComponentInterface {
01009 public:
01010 static constexpr double kRtcmFreqTol = 0.15;
01011 static constexpr int kRtcmFreqWindow = 25;
01012
01016 void getRosParams() {}
01017
01022 bool configureUblox() { return true; }
01023
01029 void subscribe();
01030
01034 void initializeRosDiagnostics();
01035
01036 private:
01038 std::vector<UbloxTopicDiagnostic> freq_diagnostics_;
01039 };
01040
01045 class AdrUdrProduct: public virtual ComponentInterface {
01046 public:
01052 void getRosParams();
01053
01059 bool configureUblox();
01060
01067 void subscribe();
01068
01073 void initializeRosDiagnostics() {
01074 ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s",
01075 "unimplemented. See AdrUdrProduct class in node.h & node.cpp.");
01076 }
01077
01078 protected:
01080 bool use_adr_;
01081
01082
01083 sensor_msgs::Imu imu_;
01084 sensor_msgs::TimeReference t_ref_;
01085 ublox_msgs::TimTM2 timtm2;
01086
01087 void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m);
01088 };
01089
01094 class FtsProduct: public virtual ComponentInterface {
01099 void getRosParams() {
01100 ROS_WARN("Functionality specific to u-blox FTS devices is %s",
01101 "unimplemented. See FtsProduct class in node.h & node.cpp.");
01102 }
01103
01108 bool configureUblox() { return false; }
01109
01114 void subscribe() {}
01115
01120 void initializeRosDiagnostics() {}
01121 };
01122
01127 class HpgRefProduct: public virtual ComponentInterface {
01128 public:
01138 void getRosParams();
01139
01148 bool configureUblox();
01149
01155 void subscribe();
01156
01160 void initializeRosDiagnostics();
01161
01170 void callbackNavSvIn(ublox_msgs::NavSVIN m);
01171
01172 protected:
01179 void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);
01180
01186 bool setTimeMode();
01187
01189 ublox_msgs::NavSVIN last_nav_svin_;
01190
01192 uint8_t tmode3_;
01193
01194
01196
01197 bool lla_flag_;
01199
01200 std::vector<float> arp_position_;
01202
01203 std::vector<int8_t> arp_position_hp_;
01205
01206 float fixed_pos_acc_;
01207
01208
01210
01215 bool svin_reset_;
01217
01218 uint32_t sv_in_min_dur_;
01220
01221 float sv_in_acc_lim_;
01222
01224 enum {
01225 INIT,
01226 FIXED,
01227 DISABLED,
01228 SURVEY_IN,
01229 TIME
01230 } mode_;
01231 };
01232
01236 class HpgRovProduct: public virtual ComponentInterface {
01237 public:
01238
01240 constexpr static double kRtcmFreqMin = 1;
01242 constexpr static double kRtcmFreqMax = 10;
01244 constexpr static double kRtcmFreqTol = 0.1;
01246 constexpr static int kRtcmFreqWindow = 25;
01252 void getRosParams();
01253
01260 bool configureUblox();
01261
01265 void subscribe();
01266
01271 void initializeRosDiagnostics();
01272
01273 protected:
01278 void carrierPhaseDiagnostics(
01279 diagnostic_updater::DiagnosticStatusWrapper& stat);
01280
01286 void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m);
01287
01288
01290 ublox_msgs::NavRELPOSNED last_rel_pos_;
01291
01293
01294 uint8_t dgnss_mode_;
01295
01297 UbloxTopicDiagnostic freq_rtcm_;
01298 };
01299
01304 class TimProduct: public virtual ComponentInterface {
01309 void getRosParams();
01310
01315 bool configureUblox();
01316
01322 void subscribe();
01323
01328 void initializeRosDiagnostics();
01329
01330 protected:
01335 void callbackTimTM2(const ublox_msgs::TimTM2 &m);
01336
01337 sensor_msgs::TimeReference t_ref_;
01338 };
01339
01340 }
01341
01342 #endif