Skip to content

Commit

Permalink
Refactor out gnsstk_ros package.
Browse files Browse the repository at this point in the history
  • Loading branch information
@peci1
peci1 committed Nov 10, 2023
1 parent be2f54f commit 2e299ce
Show file tree
Hide file tree
Showing 15 changed files with 593 additions and 195 deletions.
1 change: 1 addition & 0 deletions gnss_info/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
cras_cpp_common
geographic_msgs
gnss_info_msgs
gnsstk_ros
roscpp
)

Expand Down
24 changes: 1 addition & 23 deletions gnss_info/include/gnss_info/igs_satellite_metadata.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,29 +18,7 @@
namespace gnss_info
{

/**
* \brief Get constellation name from the given PRN string like E03.
* \param[in] prn String PRN.
* \return Constellation name (one of `gnss_info_msgs::Enums::CONSTELLATION_*` values). If invalid, empty string.
*/
std::string getConstellationFromPRN(const std::string& prn);

/**
* \brief Convert the given PRN string like E03 to integer like 3 and constellation name.
* \param[in] prn The PRN string.
* \return A pair containing the numeric PRN and constellation name.
*/
cras::optional<std::pair<int32_t, std::string>> prnStringToInt(const std::string& prn);

/**
* \brief Convert the given numeric PRN and constellation to string PRN like E03.
* \param[in] prn The numeric PRN.
* \param[in] constellation Constellation name (one of `gnss_info_msgs::Enums::CONSTELLATION_*` values).
* \return The PRN string.
*/
cras::optional<std::string> prnIntToString(int32_t prn, const std::string& constellation);

class IGSSatelliteMetadataPrivate;
struct IGSSatelliteMetadataPrivate;

/**
* \brief Wrapper around IGS satellite catalog which contains information about all public GNSS satellites.
Expand Down
2 changes: 2 additions & 0 deletions gnss_info/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ SPDX-FileCopyrightText: Czech Technical University in Prague
<build_depend>cras_cpp_common</build_depend>
<build_depend>eigen</build_depend>
<build_depend>gnsstk</build_depend>
<build_depend>gnsstk_ros</build_depend>
<build_depend>libcurl-dev</build_depend>
<build_depend>libjsoncpp-dev</build_depend>
<build_depend>yaml-cpp</build_depend>
Expand All @@ -32,6 +33,7 @@ SPDX-FileCopyrightText: Czech Technical University in Prague
<exec_depend>cras_cpp_common</exec_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>gnsstk</exec_depend>
<exec_depend>gnsstk_ros</exec_depend>
<exec_depend>libcurl</exec_depend>
<exec_depend>libjsoncpp</exec_depend>
<exec_depend>python3-matplotlib</exec_depend>
Expand Down
84 changes: 15 additions & 69 deletions gnss_info/src/ethz_satdb_provider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,16 @@
#include <gnsstk/NavLibrary.hpp>
#include <gnsstk/OrbitData.hpp>
#include <gnsstk/Position.hpp>
#include <gnsstk/YDSTime.hpp>
#include <jsoncpp/json/json.h>

#include <cras_cpp_common/expected.hpp>
#include <cras_cpp_common/string_utils.hpp>
#include <gnss_info/ethz_satdb_provider.h>
#include <gnss_info/igs_satellite_metadata.h>
#include <gnss_info_msgs/Enums.h>
#include <gnsstk_ros/constellations.h>
#include <gnsstk_ros/position.h>
#include <gnsstk_ros/time.h>
#include <ros/ros.h>

#include "tle.h" // NOLINT
Expand Down Expand Up @@ -55,7 +57,7 @@ struct DayIndex

explicit operator ros::Time() const
{
struct tm start{};
tm start{};
start.tm_year = this->year - 1900;
start.tm_mon = this->month - 1;
start.tm_mday = this->day;
Expand Down Expand Up @@ -83,51 +85,6 @@ struct std::hash<gnss_info::DayIndex>
namespace gnsstk
{

ros::Time gnssTimeToRosTime(const CommonTime& commonTime)
{
long days, secs, daysOffset; // NOLINT(runtime/int)
double frac;
YDSTime(1970, 0, 0.0).convertToCommonTime().get(daysOffset, secs, frac);
commonTime.get(days, secs, frac);
return ros::Time() +
ros::Duration::DAY * (days - daysOffset) +
ros::Duration::SECOND * secs +
ros::Duration::SECOND * frac;
}

CommonTime rosTimeToGnssTime(const ros::Time& rosTime)
{
auto commonTime = YDSTime(1970, 0, 0.0).convertToCommonTime();
commonTime.addDays(rosTime.sec / ros::Duration::DAY.sec);
commonTime.addSeconds(static_cast<long>(rosTime.sec % ros::Duration::DAY.sec)); // NOLINT(runtime/int)
commonTime.addSeconds(rosTime.nsec * 1e-9);
return commonTime;
}

cras::optional<SatID> satelliteInfoToSatID(const gnss_info_msgs::SatelliteInfo& info)
{
const auto maybeConstellationAndPrn = gnss_info::prnStringToInt(info.prn);
if (!maybeConstellationAndPrn.has_value())
return cras::nullopt;
const auto& [prn, constellationStr] = *maybeConstellationAndPrn;
SatelliteSystem system;
if (constellationStr == gnss_info_msgs::Enums::CONSTELLATION_GPS)
system = SatelliteSystem::GPS;
else if (constellationStr == gnss_info_msgs::Enums::CONSTELLATION_GALILEO)
system = SatelliteSystem::Galileo;
else if (constellationStr == gnss_info_msgs::Enums::CONSTELLATION_GLONASS)
system = SatelliteSystem::Glonass;
else if (constellationStr == gnss_info_msgs::Enums::CONSTELLATION_BEIDOU)
system = SatelliteSystem::BeiDou;
else if (constellationStr == gnss_info_msgs::Enums::CONSTELLATION_NAVIC)
system = SatelliteSystem::IRNSS;
else if (constellationStr == gnss_info_msgs::Enums::CONSTELLATION_QZSS)
system = SatelliteSystem::QZSS;
else
return cras::nullopt;
return SatID(prn, system);
}

class TLEOrbitData : public OrbitData
{
public:
Expand Down Expand Up @@ -228,7 +185,7 @@ class TLEOrbitData : public OrbitData
const tle_t tleList{1, 1, &this->tle};

double rs[6];
const auto rosWhen = gnssTimeToRosTime(when);
const auto rosWhen = gnsstk_ros::convert(when);
gtime_t gtime {rosWhen.sec, rosWhen.nsec * 1e-9};
if (!tle_pos(utc2gpst(gtime), "", this->tle.satno, "", &tleList, nullptr, rs))
{
Expand Down Expand Up @@ -334,7 +291,7 @@ class TLENavDataFactory : public NavDataFactoryWithStoreFile
return true;

const auto& info = TLENavDataFactory::satelliteInfo[satcatID];
const auto satId = satelliteInfoToSatID(info);
const auto satId = gnsstk_ros::satelliteInfoToSatID(info);
if (!satId.has_value())
return false;

Expand All @@ -345,7 +302,7 @@ class TLENavDataFactory : public NavDataFactoryWithStoreFile
navOut->signal.nav = NavType::Any;
navOut->signal.messageType = NavMessageType::Almanac;
ros::Time epoch(navIn.epoch.time, static_cast<uint32_t>(navIn.epoch.sec * 1e9));
navOut->timeStamp = rosTimeToGnssTime(epoch);
navOut->timeStamp = gnsstk_ros::convert(epoch);

return true;
}
Expand Down Expand Up @@ -423,7 +380,7 @@ cras::expected<bool, std::string> EthzSatdbProviderPrivate::download(const DayIn
};

std::stringstream readBuffer;
const auto url = cras::format(urlFormat, prefix.c_str(), startDate.c_str(), endDate.c_str());
const auto url = cras::format(this->urlFormat, prefix.c_str(), startDate.c_str(), endDate.c_str());
const auto curl = curl_easy_init();
curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1u);
Expand Down Expand Up @@ -617,7 +574,7 @@ cras::expected<std::unordered_map<uint32_t, gnss_info_msgs::SatellitePosition>,
EthzSatdbProvider::getECEFPositions(
const ros::Time& time, const std::unordered_map<uint32_t, gnss_info_msgs::SatelliteInfo>& satellites)
{
const auto when = gnsstk::rosTimeToGnssTime(time);
const auto when = gnsstk_ros::convert(time);

auto when1 = when, when2 = when;
when1.addSeconds(-ros::Duration::DAY.sec * 3.0);
Expand All @@ -633,7 +590,7 @@ EthzSatdbProvider::getECEFPositions(
std::list<std::string> errors;
for (const auto& [satcatID, info] : satellites)
{
const auto maybeSatID = gnsstk::satelliteInfoToSatID(info);
const auto maybeSatID = gnsstk_ros::satelliteInfoToSatID(info);
if (!maybeSatID.has_value())
{
errors.push_back(cras::format("Failed to determine PRN of satellite %u (%s) at time %s.",
Expand All @@ -655,16 +612,7 @@ EthzSatdbProvider::getECEFPositions(
satcatID, info.name.c_str(), cras::to_string(time).c_str()));
continue;
}

gnss_info_msgs::SatellitePosition position;
position.satcat_id = satcatID;
position.position.x = xvt.x[0];
position.position.y = xvt.x[1];
position.position.z = xvt.x[2];
position.velocity.x = xvt.v[0];
position.velocity.y = xvt.v[1];
position.velocity.z = xvt.v[2];
positions[satcatID] = position;
positions[satcatID] = gnsstk_ros::convert(xvt, satcatID);
}

if (!errors.empty() && positions.empty())
Expand All @@ -682,16 +630,14 @@ EthzSatdbProvider::getSkyView(
const ros::Time& time, const geographic_msgs::GeoPoint& receiverPosition, const double elevationMaskDeg,
const std::unordered_map<uint32_t, gnss_info_msgs::SatellitePosition>& satelliteECEFPositions)
{
const auto& Geodetic = gnsstk::Position::CoordinateSystem::Geodetic;
const auto& Cartesian = gnsstk::Position::CoordinateSystem::Cartesian;

gnsstk::Position recPos{receiverPosition.latitude, receiverPosition.longitude, receiverPosition.altitude, Geodetic};
recPos.transformTo(Cartesian); // all following computations use Cartesian internally
gnsstk::Position recPos = gnsstk_ros::convert(receiverPosition);
// all following computations use Cartesian internally
recPos.transformTo(gnsstk::Position::CoordinateSystem::Cartesian);

std::unordered_map<uint32_t, gnss_info_msgs::SatelliteSkyPosition> skyView;
for (const auto& [satcatID, ecefPose] : satelliteECEFPositions)
{
gnsstk::Position satPos {ecefPose.position.x, ecefPose.position.y, ecefPose.position.z, Cartesian};
const gnsstk::Position satPos = gnsstk_ros::convert(ecefPose.position);
const auto elDeg = recPos.elevation(satPos);
if (elDeg < elevationMaskDeg)
continue;
Expand Down
112 changes: 9 additions & 103 deletions gnss_info/src/igs_satellite_metadata.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
#include <gnss_info/igs_satellite_metadata.h>
#include <gnss_info_msgs/Enums.h>
#include <gnss_info_msgs/SatelliteInfo.h>
#include <gnsstk_ros/constellations.h>
#include <gnsstk_ros/time.h>
#include <ros/ros.h>
#include <ros/package.h>

Expand All @@ -38,74 +40,7 @@ std::string longSinexTime(const gnsstk::Sinex::Time& t)
return "20" + static_cast<std::string>(t);
}

std::string getConstellationFromPRN(const std::string& prn)
{
if (prn.empty())
return "";

switch (prn[0])
{
case 'G':
return gnss_info_msgs::Enums::CONSTELLATION_GPS;
case 'R':
return gnss_info_msgs::Enums::CONSTELLATION_GLONASS;
case 'E':
return gnss_info_msgs::Enums::CONSTELLATION_GALILEO;
case 'C':
return gnss_info_msgs::Enums::CONSTELLATION_BEIDOU;
case 'J':
return gnss_info_msgs::Enums::CONSTELLATION_QZSS;
case 'I':
return gnss_info_msgs::Enums::CONSTELLATION_NAVIC;
default:
return "";
}
}

cras::optional<std::pair<int32_t, std::string>> prnStringToInt(const std::string& prn)
{
auto prnString = prn;
if (std::isupper(prnString[0]))
prnString = prnString.substr(1);

const auto constellation = getConstellationFromPRN(prn);
if (constellation.empty())
return cras::nullopt;

try
{
// Get rid of leading zeros, they would trick parseInt32 into parsing as octal
while (!prnString.empty() && prnString[0] == '0')
prnString = prnString.substr(1);
return std::make_pair(cras::parseInt32(prnString), constellation);
}
catch (const std::invalid_argument& e)
{
return cras::nullopt;
}
}

cras::optional<std::string> prnIntToString(const int32_t prn, const std::string& constellation)
{
std::string prefix;

if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GPS)
prefix = "G";
if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GLONASS)
prefix = "R";
if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GALILEO)
prefix = "E";
if (constellation == gnss_info_msgs::Enums::CONSTELLATION_BEIDOU)
prefix = "C";
if (constellation == gnss_info_msgs::Enums::CONSTELLATION_QZSS)
prefix = "J";
if (constellation == gnss_info_msgs::Enums::CONSTELLATION_NAVIC)
prefix = "I";

return prefix + std::to_string(prn);
}

struct SatelliteIdentifier : public gnsstk::Sinex::DataType
struct SatelliteIdentifier : gnsstk::Sinex::DataType
{
static const std::string BLOCK_TITLE;
static const size_t MIN_LINE_LEN = 40;
Expand Down Expand Up @@ -395,7 +330,7 @@ class IgsSinexBlock : public gnsstk::Sinex::Block<T>
class IgsSinexData : public gnsstk::Sinex::Data
{
public:
IgsSinexData() : gnsstk::Sinex::Data()
IgsSinexData()
{
gnsstk::Sinex::Data::initBlockFactory();
auto& factory = gnsstk::Sinex::Data::blockFactory;
Expand Down Expand Up @@ -701,14 +636,7 @@ ros::Time sinexTimeToRosTime(const gnsstk::Sinex::Time& t)
{
if (t.year == 0 && t.doy == 0 && t.sod == 0) // special meaning of all zeros
return ros::Time::MAX;
long days, secs, daysOffset; // NOLINT(runtime/int)
double frac;
gnsstk::YDSTime(1970, 0, 0.0).convertToCommonTime().get(daysOffset, secs, frac);
static_cast<gnsstk::CommonTime>(t).get(days, secs, frac);
return ros::Time() +
ros::Duration::DAY * (days - daysOffset) +
ros::Duration::SECOND * secs +
ros::Duration::SECOND * frac;
return gnsstk_ros::convert(t);
}

std::unordered_map<uint32_t, gnss_info_msgs::SatelliteInfo>
Expand Down Expand Up @@ -762,30 +690,8 @@ cras::optional<gnss_info_msgs::SatelliteInfo> IGSSatelliteMetadata::getSatellite
msg.satcat_id = satcatID;
msg.name = cras::split(satinfo.comment, ";", 1).back();
cras::strip(msg.name);
switch (svn[0])
{
case 'G':
msg.constellation = gnss_info_msgs::Enums::CONSTELLATION_GPS;
break;
case 'R':
msg.constellation = gnss_info_msgs::Enums::CONSTELLATION_GLONASS;
break;
case 'E':
msg.constellation = gnss_info_msgs::Enums::CONSTELLATION_GALILEO;
break;
case 'C':
msg.constellation = gnss_info_msgs::Enums::CONSTELLATION_BEIDOU;
break;
case 'J':
msg.constellation = gnss_info_msgs::Enums::CONSTELLATION_QZSS;
break;
case 'I':
msg.constellation = gnss_info_msgs::Enums::CONSTELLATION_NAVIC;
break;
default:
msg.constellation = "";
break;
}
const auto maybeConstellation = gnsstk_ros::getRosConstellationFromSVN(svn);
msg.constellation = maybeConstellation ? *maybeConstellation : "";

// Find PRN valid for the given time
for (const auto& prnItem : this->data->svnToSatPRN[svn])
Expand Down Expand Up @@ -853,8 +759,8 @@ cras::optional<gnss_info_msgs::SatelliteInfo> IGSSatelliteMetadata::getSatellite
cras::optional<gnss_info_msgs::SatelliteInfo> IGSSatelliteMetadata::getSatelliteByPRN(
const int32_t prn, const std::string& constellation, const ros::Time& time)
{
const auto prnString = prnIntToString(prn, constellation);
if (!prnString)
const auto prnString = gnsstk_ros::prnIntToString(prn, constellation);
if (!prnString.has_value() || !prnString->empty())
return cras::nullopt;

return this->getSatelliteByPRN(*prnString, time);
Expand Down
Loading

0 comments on commit 2e299ce

Please sign in to comment.