8 #include <unordered_map>
9 #include <unordered_set>
11 #include CXX_FILESYSTEM_INCLUDE
13 #include <gnsstk/FFData.hpp>
14 #include <gnsstk/SinexStream.hpp>
15 #include <gnsstk/SinexData.hpp>
16 #include <gnsstk/SinexTypes.hpp>
17 #include <gnsstk/SinexBlock.hpp>
18 #include <gnsstk/YDSTime.hpp>
19 #include <yaml-cpp/yaml.h>
25 #include <gnss_info_msgs/Enums.h>
26 #include <gnss_info_msgs/SatelliteInfo.h>
32 namespace fs = CXX_FILESYSTEM_NAMESPACE;
40 return "19" +
static_cast<std::string
>(
t);
41 return "20" +
static_cast<std::string
>(
t);
63 void dump(std::ostream& s)
const
66 s <<
" svn=" << this->svn << std::endl;
67 s <<
" cosparID=" << this->cosparID << std::endl;
68 s <<
" satcatID=" << this->
satcatID << std::endl;
69 s <<
" block=" << this->block << std::endl;
70 s <<
" comment=" << this->comment << std::endl;
73 operator std::string()
const override
77 std::ostringstream ss;
94 static int FIELD_DIVS[] = {0, 5, 15, 22, 38, -1};
98 this->svn = line.substr(1, 4);
99 this->cosparID = line.substr(6, 9);
101 this->block = line.substr(23, 15);
102 while (!this->block.empty() && this->block.back() ==
' ')
104 this->comment = line.substr(39, 41);
133 void dump(std::ostream& s)
const
136 s <<
" svn=" << this->svn << std::endl;
137 s <<
" validFrom=" <<
static_cast<std::string
>(this->
validFrom) << std::endl;
138 s <<
" validTo=" <<
static_cast<std::string
>(this->
validTo) << std::endl;
139 s <<
" PRN=" << this->prn << std::endl;
140 s <<
" comment=" << this->comment << std::endl;
143 operator std::string()
const override
147 std::ostringstream ss;
164 static int FIELD_DIVS[] = {0, 5, 20, 35, -1};
168 this->svn = line.substr(1, 4);
169 this->validFrom = line.substr(8, 12);
170 this->validTo = line.substr(23, 12);
171 this->prn = line.substr(36, 3);
172 if (line.length() > 40)
173 this->comment = line.substr(40, 40);
202 void dump(std::ostream& s)
const
205 s <<
" svn=" << this->svn << std::endl;
206 s <<
" validFrom=" <<
static_cast<std::string
>(this->
validFrom) << std::endl;
207 s <<
" validTo=" <<
static_cast<std::string
>(this->
validTo) << std::endl;
208 s <<
" channel=" << this->
channel << std::endl;
209 s <<
" comment=" << this->comment << std::endl;
212 operator std::string()
const override
216 std::ostringstream ss;
233 static int FIELD_DIVS[] = {0, 5, 20, 35, 39, -1};
237 this->svn = line.substr(1, 4);
238 this->validFrom = line.substr(8, 12);
239 this->validTo = line.substr(23, 12);
241 this->comment = line.substr(40, 40);
265 operator std::string()
const override
295 s.formattedGetLine(line);
298 this->
dataVec.push_back(T(line.insert(0u, 1u, c), lineNum));
302 gnsstk::FFStreamError
err(exc);
311 s.formattedGetLine(line);
353 double frequency {std::numeric_limits<double>::quiet_NaN()};
375 std::unordered_map<std::string, SatelliteIdentifier>
svnToSatInfo;
378 std::unordered_map<std::string, std::list<SatellitePRN>>
svnToSatPRN;
380 std::unordered_map<std::string, std::list<SatelliteFrequencyChannel>>
svnToSatChannel;
399 ROS_INFO(
"Downloading satellite metadata from %s.", this->
url.c_str());
401 if (!maybeReadBuffer.has_value())
403 ROS_ERROR(
"%s", maybeReadBuffer.error().c_str());
407 auto& readBuffer = *maybeReadBuffer;
408 std::ofstream outFile(this->
cacheFile.c_str());
409 for (std::string line; std::getline(readBuffer, line);)
413 if (line.find_first_not_of(
' ') != std::string::npos)
414 while (!line.empty() && line.back() ==
' ')
418 for (
const auto c : line)
420 if (!std::isprint(c))
427 outFile << line <<
"\r\n";
440 const auto signalsDirEnv = std::getenv(
"GNSS_INFO_SIGNALS_PATH");
441 const auto dir = (signalsDirEnv ==
nullptr) ? defaultDir : signalsDirEnv;
443 std::unordered_map<std::string, GNSSSignal> signals;
447 for (fs::directory_iterator it(
d); it != fs::directory_iterator(); ++it)
449 if (!fs::is_regular_file(*it))
451 const auto path = it->path().string();
456 auto yaml = YAML::LoadFile(path);
457 if (yaml[
"signals"] && yaml[
"signals"].IsMap())
459 for (
const auto& constellationAndData : yaml[
"signals"])
461 if (!constellationAndData.second.IsMap())
463 for (
const auto&
s : constellationAndData.second)
465 if (!
s.second.IsMap() || !
s.first.IsScalar() || !
s.second[
"frequency"])
467 const auto signalName =
s.first.as<std::string>();
469 signal.
frequency =
s.second[
"frequency"].as<
double>();
470 if (
s.second[
"channel_step"])
471 signal.channelStep =
s.second[
"channel_step"].as<
double>();
472 signals[signalName] = signal;
476 if (yaml[
"blocks"] && yaml[
"blocks"].IsMap())
478 for (
const auto& constellationAndData : yaml[
"blocks"])
480 if (!constellationAndData.second.IsMap())
482 for (
const auto&
s : constellationAndData.second)
484 if (!
s.second.IsMap() || !
s.first.IsScalar() || !
s.second[
"signals"] ||
485 !
s.second[
"signals"].IsSequence())
489 const auto blockName =
s.first.as<std::string>();
491 for (
const auto& signalName :
s.second[
"signals"])
493 if (!signalName.IsScalar())
495 const auto name = signalName.as<std::string>();
496 if (signals.find(name) == signals.end())
498 block.
signals.push_back(signals[name]);
505 catch (
const YAML::Exception& e)
507 ROS_ERROR(
"Error loading YAML file %s: %s.", path.c_str(), e.what());
511 return !signals.empty();
518 this->
data->url =
"https://files.igs.org/pub/station/general/igs_satellite_metadata.snx";
519 const auto envUrl = std::getenv(
"GNSS_INFO_IGS_METADATA_URL");
520 if (envUrl !=
nullptr)
521 this->
data->url = envUrl;
528 this->
data->url = url;
533 this->
data->cacheFile = file;
538 this->
data->cacheValidity = validity;
543 if (!
isCacheFileValid(this->
data->cacheFile, this->data->cacheValidity) && !this->data->downloadMetadata())
550 input.exceptions(std::fstream::eofbit | std::fstream::failbit);
551 input >> igsSinexData;
555 ROS_ERROR(
"Error loading satellite metadata from %s: %s.", this->
data->cacheFile.c_str(), e.
what().c_str());
559 for (
const auto& block : igsSinexData.
blocks)
566 this->
data->svnToSatcat[entry.svn] = entry.satcatID;
567 this->
data->satcatToSvn[entry.satcatID] = entry.svn;
568 this->
data->svnToSatInfo[entry.svn] = entry;
576 this->
data->svnToSatPRN[entry.svn].push_back(entry);
584 this->
data->svnToSatChannel[entry.svn].push_back(entry);
589 ROS_INFO(
"Satellite metadata loaded from %s.", this->
data->cacheFile.c_str());
595 if (
t.year == 0 &&
t.doy == 0 &&
t.sod == 0)
600 std::unordered_map<uint32_t, gnss_info_msgs::SatelliteInfo>
602 const std::unordered_set<std::string>& onlyConstellations,
603 const std::unordered_set<std::string>& onlySignals)
605 std::unordered_map<uint32_t, gnss_info_msgs::SatelliteInfo> result;
607 for (
const auto& [svn, satcatID] : this->
data->svnToSatcat)
610 if (maybeSatellite && (!onlyActive || maybeSatellite->active))
612 if (!onlyConstellations.empty())
614 if (onlyConstellations.find(maybeSatellite->constellation) == onlyConstellations.cend())
617 if (!onlySignals.empty())
619 maybeSatellite->signals.erase(std::remove_if(
620 maybeSatellite->signals.begin(), maybeSatellite->signals.end(),
621 [&onlySignals](
const gnss_info_msgs::SatelliteSignal&
s)
623 return onlySignals.find(s.name) == onlySignals.cend();
625 maybeSatellite->signals.end());
626 if (maybeSatellite->signals.empty())
629 result[satcatID] = *maybeSatellite;
637 const uint32_t& satcatID,
const ros::Time& time)
639 if (this->
data->satcatToSvn.find(satcatID) == this->data->satcatToSvn.cend())
640 return cras::nullopt;
642 const auto svn = this->
data->satcatToSvn[satcatID];
643 const auto& satinfo = this->
data->svnToSatInfo[svn];
645 gnss_info_msgs::SatelliteInfo
msg;
648 msg.satcat_id = satcatID;
652 msg.constellation = maybeConstellation ? *maybeConstellation :
"";
655 for (
const auto& prnItem : this->
data->svnToSatPRN[svn])
659 if (validFrom <=
time &&
time <= validTo)
661 msg.prn = prnItem.prn;
668 this->
data->loadSignals();
669 const auto& blockName = this->
data->svnToSatInfo[svn].block;
670 if (this->
data->blockSignals.find(blockName) != this->data->blockSignals.cend())
673 if (
msg.constellation == gnss_info_msgs::Enums::CONSTELLATION_GLONASS)
675 for (
const auto& satChannelItem : this->
data->svnToSatChannel[svn])
679 if (validFrom <=
time &&
time <= validTo)
681 channel = satChannelItem.channel;
686 for (
const auto& signal : this->
data->blockSignals[blockName].signals)
688 gnss_info_msgs::SatelliteSignal sigMsg;
689 sigMsg.constellation =
msg.constellation;
690 sigMsg.name = signal.name;
691 sigMsg.frequency =
static_cast<float>(signal.frequency + channel * signal.channelStep);
692 msg.signals.push_back(sigMsg);
700 const std::string& prn,
const ros::Time& time)
702 for (
const auto& [svn, satPRN] : this->
data->svnToSatPRN)
704 for (
const auto& prnItem : this->
data->svnToSatPRN[svn])
706 if (prnItem.prn != prn)
710 if (validFrom <=
time &&
time <= validTo)
714 return cras::nullopt;
718 const int32_t prn,
const std::string& constellation,
const ros::Time& time)
721 if (!prnString.has_value() || !prnString->empty())
722 return cras::nullopt;