diff --git a/include/subscriber_gnss.h b/include/subscriber_gnss.h index 5acba7671aa7b63d3acb1429d31884440cfeb24e..f51518f63bf4169f5062c550cfbca60eaaf40be5 100644 --- a/include/subscriber_gnss.h +++ b/include/subscriber_gnss.h @@ -14,7 +14,7 @@ #include <ros/ros.h> #include <iri_gnss_msgs/Observation.h> #include <iri_gnss_msgs/Navigation.h> -#include <iri_gnss_msgs/iri_gnss_msgs_to_gnssutils.h> +#include <iri_gnss_msgs/iri_gnss_msgs_to_gnss_utils.h> /************************** * STD includes * @@ -28,7 +28,8 @@ **************************/ #include "subscriber.h" #include "subscriber_factory.h" -#include "gnss_utils/gnss_utils.h" +#include "gnss_utils/navigation.h" +#include "gnss_utils/observations.h" using namespace wolf; @@ -37,10 +38,10 @@ class SubscriberGnss : public Subscriber protected: ros::Subscriber sub_nav_; - std::shared_ptr<GNSSUtils::Navigation> last_nav_ptr_; - //GNSSUtils::Navigation last_nav_; - //GNSSUtils::Observations obs_; - //GNSSUtils::ComputePosOutput get_pos_res_; + std::shared_ptr<GnssUtils::Navigation> last_nav_ptr_; + //GnssUtils::Navigation last_nav_; + //GnssUtils::Observations obs_; + //GnssUtils::ComputePosOutput get_pos_res_; //prcopt_t prcopt_; //bool new_obs_; diff --git a/include/subscriber_gnss_tdcp.h b/include/subscriber_gnss_tdcp.h index 91bf5c487f55477f5f8ba25288fc73b4ad8d2e6b..d5214c607670233c4ba1a2ab9611a37aadb01d4d 100644 --- a/include/subscriber_gnss_tdcp.h +++ b/include/subscriber_gnss_tdcp.h @@ -14,7 +14,7 @@ #include <ros/ros.h> #include <iri_gnss_msgs/Observation.h> #include <iri_gnss_msgs/Navigation.h> -#include <iri_gnss_msgs/iri_gnss_msgs_to_gnssutils.h> +#include <iri_gnss_msgs/iri_gnss_msgs_to_gnss_utils.h> /************************** * STD includes * @@ -37,7 +37,7 @@ class SubscriberGnssTdcp : public Subscriber protected: ros::Subscriber sub_nav_; - std::shared_ptr<GNSSUtils::Navigation> last_nav_ptr_; + std::shared_ptr<GnssUtils::Navigation> last_nav_ptr_; //prcopt_t prcopt_; //bool new_obs_; diff --git a/include/subscriber_gnss_ublox.h b/include/subscriber_gnss_ublox.h index c148ee0e169befc448592989b5111f5b22e2a699..decf5ce6c6c4261937072d8eb4cd9d5821e4d4de 100644 --- a/include/subscriber_gnss_ublox.h +++ b/include/subscriber_gnss_ublox.h @@ -19,7 +19,8 @@ **************************/ #include "subscriber.h" #include "subscriber_factory.h" -#include "gnss_utils/gnss_utils.h" +#include "gnss_utils/navigation.h" +#include "gnss_utils/observations.h" #include "gnss_utils/ublox_raw.h" using namespace wolf; @@ -38,6 +39,6 @@ class SubscriberGnssUblox : public Subscriber protected: - GNSSUtils::UBloxRaw ublox_raw_; + GnssUtils::UBloxRaw ublox_raw_; }; WOLF_REGISTER_SUBSCRIBER(SubscriberGnssUblox) diff --git a/src/subscriber_gnss.cpp b/src/subscriber_gnss.cpp index f80d76c016772b5da709269ecb60e49690e1b825..f6a81d35593c4aed77242a9dddb472e9a5089bf5 100644 --- a/src/subscriber_gnss.cpp +++ b/src/subscriber_gnss.cpp @@ -35,8 +35,8 @@ void SubscriberGnss::callbackObservation(const iri_gnss_msgs::Observation::Const if (last_nav_ptr_==nullptr) return; - GNSSUtils::ObservationsPtr obs_ptr = std::make_shared<GNSSUtils::Observations>(); - GNSSUtils::fillObservation(*obs_ptr, msg); + GnssUtils::ObservationsPtr obs_ptr = std::make_shared<GnssUtils::Observations>(); + GnssUtils::fillObservation(*obs_ptr, msg); CaptureGnssPtr new_capture = std::make_shared<CaptureGnss>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, obs_ptr, @@ -49,14 +49,14 @@ void SubscriberGnss::callbackNavigation(const iri_gnss_msgs::Navigation::ConstPt ROS_INFO("callbackNav!"); std::cout << "Ephemeris size: " << msg->eph.size() << "\n"; - last_nav_ptr_ = std::make_shared<GNSSUtils::Navigation>(); - GNSSUtils::fillNavigation(*last_nav_ptr_, msg); + last_nav_ptr_ = std::make_shared<GnssUtils::Navigation>(); + GnssUtils::fillNavigation(*last_nav_ptr_, msg); last_nav_ptr_->print(); // if (this->new_obs_) // { // std::cout << "Before computed!!! \n"; -// this->get_pos_res_ = GNSSUtils::computePos(this->obs_, *last_nav_ptr_, this->prcopt_); +// this->get_pos_res_ = GnssUtils::computePos(this->obs_, *last_nav_ptr_, this->prcopt_); // std::cout << "Computed!!! \n"; // } } diff --git a/src/subscriber_gnss_tdcp.cpp b/src/subscriber_gnss_tdcp.cpp index 375d3c5bfc4815949e0b5591d03b4053ef9961fc..ef09f47c9505bf3b227241cbd20d208c3aa55beb 100644 --- a/src/subscriber_gnss_tdcp.cpp +++ b/src/subscriber_gnss_tdcp.cpp @@ -23,8 +23,8 @@ void SubscriberGnssTdcp::callbackObservation(const iri_gnss_msgs::Observation::C // TODO copied from Tdcp_ros -// GNSSUtils::ObservationsPtr obs_ptr = std::make_shared<GNSSUtils::Observations>(); -// GNSSUtils::fillObservation(*obs_ptr, msg); +// GnssUtils::ObservationsPtr obs_ptr = std::make_shared<GnssUtils::Observations>(); +// GnssUtils::fillObservation(*obs_ptr, msg); // CaptureGnssPtr new_capture = std::make_shared<CaptureGnss>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), // sensor_ptr_, // obs_ptr, @@ -37,8 +37,8 @@ void SubscriberGnssTdcp::callbackNavigation(const iri_gnss_msgs::Navigation::Con { ROS_INFO("callbackNav!"); - last_nav_ptr_ = std::make_shared<GNSSUtils::Navigation>(); - GNSSUtils::fillNavigation(*last_nav_ptr_, msg); + last_nav_ptr_ = std::make_shared<GnssUtils::Navigation>(); + GnssUtils::fillNavigation(*last_nav_ptr_, msg); last_nav_ptr_->print(); // TODO copied from Tdcp_ros diff --git a/src/subscriber_gnss_ublox.cpp b/src/subscriber_gnss_ublox.cpp index f24b5ef4d9cee0e071bef8471ad89aa211c401ae..3e59af98542c1f9c649ba6d7b8093214f9e652f1 100644 --- a/src/subscriber_gnss_ublox.cpp +++ b/src/subscriber_gnss_ublox.cpp @@ -17,17 +17,17 @@ void SubscriberGnssUblox::initSubscriber(ros::NodeHandle& nh, const std::string& void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg) { - GNSSUtils::RawDataType res = ublox_raw_.addDataStream(msg.data); + GnssUtils::RawDataType res = ublox_raw_.addDataStream(msg.data); //std::cout << "res = " << (int) res << std::endl; // only create capture if observation is received - if (res == GNSSUtils::OBS) + if (res == GnssUtils::OBS) { //ROS_INFO("Observation received!"); - GNSSUtils::ObservationsPtr obs_ptr = std::make_shared<GNSSUtils::Observations>(ublox_raw_.getObservations()); + GnssUtils::ObservationsPtr obs_ptr = std::make_shared<GnssUtils::Observations>(ublox_raw_.getObservations()); //ROS_INFO("Observations copied!"); - GNSSUtils::NavigationPtr nav_ptr = std::make_shared<GNSSUtils::Navigation>(ublox_raw_.getNavigation()); + GnssUtils::NavigationPtr nav_ptr = std::make_shared<GnssUtils::Navigation>(ublox_raw_.getNavigation()); //ROS_INFO("Navigation copied!"); ros::Time now = ros::Time::now();