diff --git a/src/wolf_subscriber_gnss_ublox.cpp b/src/wolf_subscriber_gnss_ublox.cpp index c0755c82c444155a02e7acf5b725267c1b4d1b5f..58d304b147d19a6d9b9f3e3c45d88f60b0e7ad3d 100644 --- a/src/wolf_subscriber_gnss_ublox.cpp +++ b/src/wolf_subscriber_gnss_ublox.cpp @@ -11,25 +11,24 @@ WolfSubscriberGnssUblox::WolfSubscriberGnssUblox(const SensorBasePtr& sensor_ptr void WolfSubscriberGnssUblox::initSubscriber(ros::NodeHandle& nh, const std::string& topic) { - sub_ = nh.subscribe(topic, 1, &WolfSubscriberGnssUblox::callback, this); + sub_ = nh.subscribe(topic, 10, &WolfSubscriberGnssUblox::callback, this); } void WolfSubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg) { - ROS_INFO("Ublox data stream callback!"); - 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) { - ROS_INFO("Observation received!"); - + //ROS_INFO("Observation received!"); GNSSUtils::ObservationsPtr obs_ptr = std::make_shared<GNSSUtils::Observations>(ublox_raw_.getObservations()); - ROS_INFO("Observations copied!"); + //ROS_INFO("Observations copied!"); GNSSUtils::NavigationPtr nav_ptr = std::make_shared<GNSSUtils::Navigation>(ublox_raw_.getNavigation()); - ROS_INFO("Navigation copied!"); + //ROS_INFO("Navigation copied!"); ros::Time now = ros::Time::now(); @@ -38,9 +37,9 @@ void WolfSubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg) obs_ptr, nav_ptr); - ROS_INFO("Capture GNSS created!"); + //ROS_INFO("Capture GNSS created!"); cap_gnss_ptr->process(); - ROS_INFO("Capture GNSS processed!"); + //ROS_INFO("Capture GNSS processed!"); } } std::shared_ptr<WolfSubscriber> WolfSubscriberGnssUblox::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr)