diff --git a/src/subscriber_gnss_receiver.cpp b/src/subscriber_gnss_receiver.cpp index 01deaea4927224c97034d050aee61088cdd5ffb8..f58553fa667382f3d05fe5d20545dfca339686c0 100644 --- a/src/subscriber_gnss_receiver.cpp +++ b/src/subscriber_gnss_receiver.cpp @@ -241,6 +241,8 @@ void SubscriberGnssReceiver::publishFixComp() GnssUtils::Options comp_pos_opt = compute_pos_opt_; + WOLF_DEBUG("COMPARING RTKLIB pntpos CONFIGURATIONS /////////////////"); + for (auto comp : comp_options_) { if (publishers_pose_.at(comp).getNumSubscribers() == 0 and @@ -300,11 +302,32 @@ void SubscriberGnssReceiver::publishFixComp() else if (comp[2] == '2') comp_pos_opt.ionoopt = IONOOPT_IFLC; + WOLF_DEBUG("opt ", comp, ": ", + " RAIM: ", comp_pos_opt.raim, + " EPH: ", comp_pos_opt.sateph, + " IONO: ", comp_pos_opt.ionoopt, + " TROP: ", comp_pos_opt.tropopt) + //std::cout << "//////////// Computing fix comp = " << comp << std::endl; auto fix = GnssUtils::computePos(receiver_->getObservations(), receiver_->getNavigation(), comp_pos_opt); - WOLF_INFO_COND(not fix.success, "computePos failed (opt ", comp, ") with msg: ", fix.msg); + + // print + #ifdef _WOLF_DEBUG + std::string discarded_str, used_str; + for (auto sat : fix.discarded_sats) + discarded_str += std::to_string(sat) + " "; + for (auto sat : fix.used_sats) + used_str += std::to_string(sat) + " "; + #endif + + WOLF_INFO_COND(not fix.success, "computePos failed (opt ", comp, ") with msg: ", fix.msg, + "\n\tused sats: ", fix.used_sats.size(), " (", used_str, ")", + "\n\tdiscarded sats: ", fix.discarded_sats.size(), " (", discarded_str, ")"); + WOLF_DEBUG_COND(fix.success, "computePos succeed (opt ", comp, ")", + "\n\tused sats: ", fix.used_sats.size(), " (", used_str, ")", + "\n\tdiscarded sats: ", fix.discarded_sats.size(), " (", discarded_str, ")"); //std::cout << "//////////// Computed" << std::endl; @@ -327,6 +350,7 @@ void SubscriberGnssReceiver::publishFixComp() last_fixes_ok_.at(comp) = fix.success; last_fixes_pose_.at(comp) = pose_msg; } + WOLF_DEBUG("////////////////////////////////////////////////"); } void SubscriberGnssReceiver::fillMsgs(const geometry_msgs::Pose pose, const std::string& comp)