Skip to content
Snippets Groups Projects
Commit 788296d5 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

added infos for debugging

parent 84417724
No related branches found
No related tags found
2 merge requests!3After cmake and const refactor,!1new release
......@@ -17,11 +17,19 @@ void WolfSubscriberGnssUblox::initSubscriber(ros::NodeHandle& nh, const std::str
void WolfSubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
{
ROS_INFO("Ublox data stream callback!");
GNSSUtils::RawDataType res = ublox_raw_.addDataStream(msg.data);
// only create capture if observation is received
if (ublox_raw_.addDataStream(msg.data) == GNSSUtils::OBS)
if (res == GNSSUtils::OBS)
{
ROS_INFO("Observation received!");
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());
ROS_INFO("Navigation copied!");
ros::Time now = ros::Time::now();
......@@ -29,7 +37,10 @@ void WolfSubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
sensor_ptr_,
obs_ptr,
nav_ptr);
ROS_INFO("Capture GNSS created!");
cap_gnss_ptr->process();
ROS_INFO("Capture GNSS processed!");
}
}
std::shared_ptr<WolfSubscriber> WolfSubscriberGnssUblox::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment