diff --git a/include/mrs_uav_hw_api/publishers.h b/include/mrs_uav_hw_api/publishers.h index 512fe24..897d30d 100644 --- a/include/mrs_uav_hw_api/publishers.h +++ b/include/mrs_uav_hw_api/publishers.h @@ -3,7 +3,6 @@ #include #include -#include #include #include #include @@ -14,6 +13,7 @@ #include #include #include +#include #include @@ -29,7 +29,7 @@ namespace mrs_uav_hw_api //} typedef std::function publishGNSS_t; -typedef std::function publishGNSSStatus_t; +typedef std::function publishGNSSStatus_t; typedef std::function publishRTK_t; typedef std::function publishAltitude_t; typedef std::function publishMagnetometerHeading_t; diff --git a/src/hw_api_manager.cpp b/src/hw_api_manager.cpp index 68bedaf..5b7e84d 100644 --- a/src/hw_api_manager.cpp +++ b/src/hw_api_manager.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #include @@ -105,7 +104,7 @@ class HwApiManager : public nodelet::Nodelet { mrs_lib::PublisherHandler ph_capabilities_; mrs_lib::PublisherHandler ph_gnss_; - mrs_lib::PublisherHandler ph_gnss_status_; + mrs_lib::PublisherHandler ph_gnss_status_; mrs_lib::PublisherHandler ph_rtk_; mrs_lib::PublisherHandler ph_imu_; mrs_lib::PublisherHandler ph_distance_sensor_; @@ -127,7 +126,7 @@ class HwApiManager : public nodelet::Nodelet { std::string getWorldFrameName(void); void publishGNSS(const sensor_msgs::NavSatFix& msg); - void publishGNSSStatus(const sensor_msgs::NavSatStatus& msg); + void publishGNSSStatus(const mrs_msgs::GpsInfo& msg); void publishRTK(const mrs_msgs::RtkGps& msg); void publishOdometry(const nav_msgs::Odometry& msg); void publishGroundTruth(const nav_msgs::Odometry& msg); @@ -246,7 +245,7 @@ void HwApiManager::onInit() { ph_connected_ = mrs_lib::PublisherHandler(nh_, "connected", 1); ph_gnss_ = mrs_lib::PublisherHandler(nh_, "gnss", 1, false, 50); - ph_gnss_status_ = mrs_lib::PublisherHandler(nh_, "gnss_status", 1, false, 10); + ph_gnss_status_ = mrs_lib::PublisherHandler(nh_, "gnss_status", 1, false, 10); ph_rtk_ = mrs_lib::PublisherHandler(nh_, "rtk", 1, false, 50); ph_distance_sensor_ = mrs_lib::PublisherHandler(nh_, "distance_sensor", 1, false, 250); ph_mag_heading_ = mrs_lib::PublisherHandler(nh_, "mag_heading", 1, false, 100); @@ -630,7 +629,7 @@ void HwApiManager::publishGNSS(const sensor_msgs::NavSatFix& msg) { /* publishGNSSStatus() //{ */ -void HwApiManager::publishGNSSStatus(const sensor_msgs::NavSatStatus& msg) { +void HwApiManager::publishGNSSStatus(const mrs_msgs::GpsInfo& msg) { if (!is_initialized_) { return;