diff --git a/include/wolf_ros_subscriber_gnss.h b/include/wolf_ros_subscriber_gnss.h index 689395638347bd06459f278844c28fd3739d881c..1bfd2168c0e350842c0a5fb0a6d6ea45837ec16a 100644 --- a/include/wolf_ros_subscriber_gnss.h +++ b/include/wolf_ros_subscriber_gnss.h @@ -38,12 +38,6 @@ class WolfSubscriberWrapperGnss : public WolfSubscriberWrapper ros::Subscriber sub_nav_; 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_; public: // Constructor diff --git a/src/wolf_ros_subscriber_gnss.cpp b/src/wolf_ros_subscriber_gnss.cpp index ba614f5c88578b03fa43d44bc919dce421f379c3..1662a015259d98cdbe1fc7593a06a07f8f7ec42e 100644 --- a/src/wolf_ros_subscriber_gnss.cpp +++ b/src/wolf_ros_subscriber_gnss.cpp @@ -6,19 +6,7 @@ using namespace wolf; WolfSubscriberWrapperGnss::WolfSubscriberWrapperGnss(const SensorBasePtr& sensor_ptr) : WolfSubscriberWrapper(sensor_ptr) { - prcopt_ = prcopt_default; - prcopt_.mode = PMODE_SINGLE; - prcopt_.soltype = 0; - prcopt_.nf = 1; - prcopt_.navsys = SYS_GPS; - prcopt_.sateph = EPHOPT_BRDC; - prcopt_.ionoopt = IONOOPT_OFF; - prcopt_.tropopt = TROPOPT_OFF; - prcopt_.dynamics = 0; - prcopt_.tidecorr = 0; - prcopt_.sbascorr = SBSOPT_FCORR; - - this->new_obs_=false; + } @@ -34,37 +22,25 @@ void WolfSubscriberWrapperGnss::callbackObservation(const iri_gnss_msgs::Observa ROS_INFO("callbackObs!"); if (last_nav_ptr_==nullptr) return; - - // std::shared_ptr<GNSSUtils::Observations> obs_ptr = std::make_shared<GNSSUtils::Observations>(); - // GNSSUtils::fillObservation(*(obs_ptr), msg); - - // GNSSUtils::Observations obs; - GNSSUtils::fillObservation(this->obs_, msg); - this->new_obs_=true; - // CaptureGnssPtr new_capture = std::make_shared<CaptureGnss>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), - // sensor_ptr_, - // obs_ptr, - // last_nav_ptr_); - // new_capture->process(); + std::shared_ptr<GNSSUtils::Observations> 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, + last_nav_ptr_); + new_capture->process(); } void WolfSubscriberWrapperGnss::callbackNavigation(const iri_gnss_msgs::Navigation::ConstPtr& msg) { ROS_INFO("callbackNav!"); - - std::cout << "Ephemeris size: " << msg->eph.size() << "\n"; - GNSSUtils::fillNavigation(this->last_nav_, msg); + last_nav_ptr_ = std::make_shared<GNSSUtils::Navigation>(); - this->last_nav_.print(); - - if (this->new_obs_) - { - std::cout << "Before computed!!! \n"; - this->get_pos_res_ = GNSSUtils::computePos(this->obs_, this->last_nav_, this->prcopt_); - std::cout << "Computed!!! \n"; - } + GNSSUtils::fillNavigation(*(last_nav_ptr_), msg); } + std::shared_ptr<WolfSubscriberWrapper> WolfSubscriberWrapperGnss::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr) { return std::make_shared<WolfSubscriberWrapperGnss>(_sensor_ptr);