From 2e299ce5269393fec1bb67e99749b8b65976b10f Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Fri, 10 Nov 2023 19:28:30 +0100 Subject: [PATCH] Refactor out gnsstk_ros package. --- gnss_info/CMakeLists.txt | 1 + .../gnss_info/igs_satellite_metadata.h | 24 +-- gnss_info/package.xml | 2 + gnss_info/src/ethz_satdb_provider.cpp | 84 ++-------- gnss_info/src/igs_satellite_metadata.cpp | 112 ++----------- gnsstk_ros/CMakeLists.txt | 69 ++++++++ gnsstk_ros/LICENSE | 28 ++++ gnsstk_ros/README.md | 8 + .../include/gnsstk_ros/constellations.h | 69 ++++++++ gnsstk_ros/include/gnsstk_ros/position.h | 52 ++++++ gnsstk_ros/include/gnsstk_ros/time.h | 45 ++++++ gnsstk_ros/package.xml | 31 ++++ gnsstk_ros/src/constellations.cpp | 152 ++++++++++++++++++ gnsstk_ros/src/position.cpp | 58 +++++++ gnsstk_ros/src/time.cpp | 53 ++++++ 15 files changed, 593 insertions(+), 195 deletions(-) create mode 100644 gnsstk_ros/CMakeLists.txt create mode 100644 gnsstk_ros/LICENSE create mode 100644 gnsstk_ros/README.md create mode 100644 gnsstk_ros/include/gnsstk_ros/constellations.h create mode 100644 gnsstk_ros/include/gnsstk_ros/position.h create mode 100644 gnsstk_ros/include/gnsstk_ros/time.h create mode 100644 gnsstk_ros/package.xml create mode 100644 gnsstk_ros/src/constellations.cpp create mode 100644 gnsstk_ros/src/position.cpp create mode 100644 gnsstk_ros/src/time.cpp diff --git a/gnss_info/CMakeLists.txt b/gnss_info/CMakeLists.txt index d2bcd31..2691eb3 100644 --- a/gnss_info/CMakeLists.txt +++ b/gnss_info/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS cras_cpp_common geographic_msgs gnss_info_msgs + gnsstk_ros roscpp ) diff --git a/gnss_info/include/gnss_info/igs_satellite_metadata.h b/gnss_info/include/gnss_info/igs_satellite_metadata.h index c7de014..b78164a 100644 --- a/gnss_info/include/gnss_info/igs_satellite_metadata.h +++ b/gnss_info/include/gnss_info/igs_satellite_metadata.h @@ -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> 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 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. diff --git a/gnss_info/package.xml b/gnss_info/package.xml index 28365bb..30889e2 100644 --- a/gnss_info/package.xml +++ b/gnss_info/package.xml @@ -24,6 +24,7 @@ SPDX-FileCopyrightText: Czech Technical University in Prague cras_cpp_common eigen gnsstk + gnsstk_ros libcurl-dev libjsoncpp-dev yaml-cpp @@ -32,6 +33,7 @@ SPDX-FileCopyrightText: Czech Technical University in Prague cras_cpp_common cv_bridge gnsstk + gnsstk_ros libcurl libjsoncpp python3-matplotlib diff --git a/gnss_info/src/ethz_satdb_provider.cpp b/gnss_info/src/ethz_satdb_provider.cpp index 4350b78..19a4e6e 100644 --- a/gnss_info/src/ethz_satdb_provider.cpp +++ b/gnss_info/src/ethz_satdb_provider.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include @@ -26,6 +25,9 @@ #include #include #include +#include +#include +#include #include #include "tle.h" // NOLINT @@ -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; @@ -83,51 +85,6 @@ struct std::hash 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(rosTime.sec % ros::Duration::DAY.sec)); // NOLINT(runtime/int) - commonTime.addSeconds(rosTime.nsec * 1e-9); - return commonTime; -} - -cras::optional 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: @@ -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)) { @@ -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; @@ -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(navIn.epoch.sec * 1e9)); - navOut->timeStamp = rosTimeToGnssTime(epoch); + navOut->timeStamp = gnsstk_ros::convert(epoch); return true; } @@ -423,7 +380,7 @@ cras::expected 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); @@ -617,7 +574,7 @@ cras::expected, EthzSatdbProvider::getECEFPositions( const ros::Time& time, const std::unordered_map& 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); @@ -633,7 +590,7 @@ EthzSatdbProvider::getECEFPositions( std::list 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.", @@ -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()) @@ -682,16 +630,14 @@ EthzSatdbProvider::getSkyView( const ros::Time& time, const geographic_msgs::GeoPoint& receiverPosition, const double elevationMaskDeg, const std::unordered_map& 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 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; diff --git a/gnss_info/src/igs_satellite_metadata.cpp b/gnss_info/src/igs_satellite_metadata.cpp index 414f17b..69dac4d 100644 --- a/gnss_info/src/igs_satellite_metadata.cpp +++ b/gnss_info/src/igs_satellite_metadata.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include @@ -38,74 +40,7 @@ std::string longSinexTime(const gnsstk::Sinex::Time& t) return "20" + static_cast(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> 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 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; @@ -395,7 +330,7 @@ class IgsSinexBlock : public gnsstk::Sinex::Block class IgsSinexData : public gnsstk::Sinex::Data { public: - IgsSinexData() : gnsstk::Sinex::Data() + IgsSinexData() { gnsstk::Sinex::Data::initBlockFactory(); auto& factory = gnsstk::Sinex::Data::blockFactory; @@ -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(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 @@ -762,30 +690,8 @@ cras::optional 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]) @@ -853,8 +759,8 @@ cras::optional IGSSatelliteMetadata::getSatellite cras::optional 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); diff --git a/gnsstk_ros/CMakeLists.txt b/gnsstk_ros/CMakeLists.txt new file mode 100644 index 0000000..3c14bb7 --- /dev/null +++ b/gnsstk_ros/CMakeLists.txt @@ -0,0 +1,69 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +cmake_minimum_required(VERSION 3.10.2) +project(gnsstk_ros) + +set(CMAKE_CXX_STANDARD 17) + +find_package(catkin REQUIRED COMPONENTS + cras_cpp_common + geographic_msgs + geometry_msgs + gnss_info_msgs + roscpp +) + +find_package(GNSSTK REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS geographic_msgs geometry_msgs gnss_info_msgs roscpp +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} src/constellations.cpp src/position.cpp src/time.cpp) +target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} PRIVATE gnsstk) +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +if (CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) + + # catkin_lint - checks validity of package.xml and CMakeLists.txt + # ROS buildfarm calls this without any environment and with empty rosdep cache, + # so we have problems reading the list of packages from env + # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 + if(DEFINED ENV{ROS_HOME}) + #catkin_lint: ignore_once env_var + set(ROS_HOME "$ENV{ROS_HOME}") + else() + #catkin_lint: ignore_once env_var + set(ROS_HOME "$ENV{HOME}/.ros") + endif() + + #catkin_lint: ignore_once env_var + if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") + roslint_custom(catkin_lint "-W2" .) + endif() + + # Roslint C++ - checks formatting and some other rules for C++ files + file(GLOB_RECURSE ROSLINT_SRC src/*.cpp include/${PROJECT_NAME}/*.h) + + set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ + -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ + -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") + roslint_cpp(${ROSLINT_SRC}) + + roslint_add_test() +endif() \ No newline at end of file diff --git a/gnsstk_ros/LICENSE b/gnsstk_ros/LICENSE new file mode 100644 index 0000000..a5524fb --- /dev/null +++ b/gnsstk_ros/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2023, Czech Technical University in Prague + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/gnsstk_ros/README.md b/gnsstk_ros/README.md new file mode 100644 index 0000000..795df43 --- /dev/null +++ b/gnsstk_ros/README.md @@ -0,0 +1,8 @@ + + +# gnsstk_ros + +Bindings between GNSSTk library and ROS. \ No newline at end of file diff --git a/gnsstk_ros/include/gnsstk_ros/constellations.h b/gnsstk_ros/include/gnsstk_ros/constellations.h new file mode 100644 index 0000000..ffe3db9 --- /dev/null +++ b/gnsstk_ros/include/gnsstk_ros/constellations.h @@ -0,0 +1,69 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +namespace gnsstk_ros +{ + +/** + * \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, nullopt. + */ +cras::optional getRosConstellationFromPRN(const std::string& prn); + +/** + * \brief Get constellation name from the given SVN string like E203. + * \param[in] svn The SVN of the satellite. + * \return Constellation name (one of `gnss_info_msgs::Enums::CONSTELLATION_*` values). If invalid, nullopt. + */ +cras::optional getRosConstellationFromSVN(const std::string& svn); + +/** + * \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> 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 prnIntToString(int32_t prn, const std::string& constellation); + +/** + * \brief Convert the gnss_info_msgs constellation string to gnsstk constellation enum. + * \param[in] constellation The constellation string. One of `gnss_info_msgs::Enums::CONSTELLATION_*`. + * \return The equivalent gnsstk constellation. Return `Unknown` if there is no mapping. Return `nullopt` if + * `constellation` is empty. + */ +cras::optional rosConstellationToGnsstkSatelliteSystem(const std::string& constellation); + +/** + * \brief Convert the gnsstk constellation enum to gnss_info_msgs constellation string. + * \param constellation The gnsstk constellation enum value. + * \return The equivalent `gnss_info_msgs::Enums::CONSTELLATION_*`. Return `nullopt` if there is no mapping. + */ +cras::optional gnsstkSatelliteSystemRosConstellation(const gnsstk::SatelliteSystem& constellation); + +/** + * \brief Convert the given satellite info to non-wildcard gnsstk SatID. + * \param info Information about the satellite. + * \return The SatID object of the given satellite. `nullopt` if identifying the satellite is not possible. + */ +cras::optional satelliteInfoToSatID(const gnss_info_msgs::SatelliteInfo& info); + +} diff --git a/gnsstk_ros/include/gnsstk_ros/position.h b/gnsstk_ros/include/gnsstk_ros/position.h new file mode 100644 index 0000000..9f87412 --- /dev/null +++ b/gnsstk_ros/include/gnsstk_ros/position.h @@ -0,0 +1,52 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gnsstk_ros +{ + +/** + * \brief Convert the ROS GeoPoint message to gnsstk Position with Geodetic type. + * \param[in] position The WGS84 position to convert. + * \return The corresponding gnsstk position object. + */ +gnsstk::Position convert(const geographic_msgs::GeoPoint& position); + +/** + * \brief Convert the given gnsstk Position object to WGS84 ROS GeoPoint message. + * \param[in] position The gnsstk position to convert. + * \return The corresponding ROS GeoPoint. + */ +geographic_msgs::GeoPoint convertToGeographicMsg(const gnsstk::Position& position); + +/** + * \brief Convert the ROS Point ECEF position to gnsstk Position with Cartesian type. + * \param[in] position The ECEF position to convert. + * \return The corresponding gnsstk position object. + */ +gnsstk::Position convert(const geometry_msgs::Point& position); + +/** + * \brief Convert the given gnsstk Position object to ECEF ROS Point message. + * \param[in] position The gnsstk position to convert. + * \return The corresponding ROS Point in ECEF coordinates. + */ +geometry_msgs::Point convertToCartesianMsg(const gnsstk::Position& position); + +/** + * \brief Convert the given position and velocity to ROS SatellitePosition message. + * \param[in] xvt The position and velocity as determined by gnsstk. + * \param[in] satcatId ID of the satellite in the satellite catalog. + * \return The ROS SatellitePosition message corresponding to the given parameter. + */ +gnss_info_msgs::SatellitePosition convert(const gnsstk::Xvt& xvt, uint32_t satcatId); + +} diff --git a/gnsstk_ros/include/gnsstk_ros/time.h b/gnsstk_ros/include/gnsstk_ros/time.h new file mode 100644 index 0000000..772c549 --- /dev/null +++ b/gnsstk_ros/include/gnsstk_ros/time.h @@ -0,0 +1,45 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#pragma once + +#include + +#include + +namespace gnsstk_ros +{ + +/** + * \brief UNIX epoch (1 Jan 1970) represented in gnsstk CommonTime means. Static values for efficient computations. + */ +struct UnixEpoch +{ + static const gnsstk::CommonTime gnsstkCommonTime; //!< CommonTime of the UNIX epoch. + //! Number of days between CommonTime beginning of time and UNIX epoch start. + static const long gnsstkDaysOffset; // NOLINT(runtime/int) +}; + +/** + * \brief Get the number of days stored in the given time object. + * \param[in] time The time to get days of. + * \return The number of days. + */ +long getDays(const gnsstk::CommonTime& time); // NOLINT(runtime/int) + +/** + * \brief Convert the given gnsstk time to ROS time. + * \param[in] commonTime The time to convert. + * \return The equivalent ROS time. + * \throws std::runtime_error If the time cannot be represented in ROS. + */ +ros::Time convert(const gnsstk::CommonTime& commonTime); + +/** + * \brief Covnert the given ROS time to gnsstk time. + * \param[in] rosTime The ROS time to convert. + * \return The equivalent gnsstk time. + */ +gnsstk::CommonTime convert(const ros::Time& rosTime); + +} diff --git a/gnsstk_ros/package.xml b/gnsstk_ros/package.xml new file mode 100644 index 0000000..b457c6d --- /dev/null +++ b/gnsstk_ros/package.xml @@ -0,0 +1,31 @@ + + + + + gnsstk_ros + 1.0.0 + ROS bindings for GNSSTk library. + + Martin Pecka + Martin Pecka + + BSD + + catkin + + cras_cpp_common + geographic_msgs + geometry_msgs + gnss_info_msgs + roscpp + + gnsstk + gnsstk + + python-catkin-lint + python3-catkin-lint + roslint + diff --git a/gnsstk_ros/src/constellations.cpp b/gnsstk_ros/src/constellations.cpp new file mode 100644 index 0000000..79c4597 --- /dev/null +++ b/gnsstk_ros/src/constellations.cpp @@ -0,0 +1,152 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace gnsstk_ros +{ + +cras::optional getRosConstellationFromPRN(const std::string& prn) +{ + if (prn.empty()) + return cras::nullopt; + + 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::nullopt; + } +} + +cras::optional getRosConstellationFromSVN(const std::string& svn) +{ + // The implementation is actually the same. + return getRosConstellationFromPRN(svn); +} + +cras::optional> prnStringToInt(const std::string& prn) +{ + const auto constellation = getRosConstellationFromPRN(prn); + if (!constellation.has_value()) + return cras::nullopt; + + auto prnString = prn.substr(1); + + // Get rid of leading zeros, they would trick parseInt32 into parsing as octal + while (!prnString.empty() && prnString[0] == '0') + prnString = prnString.substr(1); + if (prnString.empty()) + return cras::nullopt; + + try + { + return std::make_pair(cras::parseInt32(prnString), *constellation); + } + catch (const std::invalid_argument&) // integer parsing failed + { + return cras::nullopt; + } +} + +cras::optional prnIntToString(const int32_t prn, const std::string& constellation) +{ + std::string prefix; + + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GPS) + prefix = "G"; + else if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GLONASS) + prefix = "R"; + else if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GALILEO) + prefix = "E"; + else if (constellation == gnss_info_msgs::Enums::CONSTELLATION_BEIDOU) + prefix = "C"; + else if (constellation == gnss_info_msgs::Enums::CONSTELLATION_QZSS) + prefix = "J"; + else if (constellation == gnss_info_msgs::Enums::CONSTELLATION_NAVIC) + prefix = "I"; + else + return cras::nullopt; + + return prefix + std::to_string(prn); +} + +cras::optional rosConstellationToGnsstkSatelliteSystem(const std::string& constellation) +{ + if (constellation.empty()) + return gnsstk::SatelliteSystem::Unknown; + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GPS) + return gnsstk::SatelliteSystem::GPS; + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GALILEO) + return gnsstk::SatelliteSystem::Galileo; + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_GLONASS) + return gnsstk::SatelliteSystem::Glonass; + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_BEIDOU) + return gnsstk::SatelliteSystem::BeiDou; + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_NAVIC) + return gnsstk::SatelliteSystem::IRNSS; + if (constellation == gnss_info_msgs::Enums::CONSTELLATION_QZSS) + return gnsstk::SatelliteSystem::QZSS; + return gnsstk::SatelliteSystem::Unknown; +} + +cras::optional gnsstkSatelliteSystemRosConstellation(const gnsstk::SatelliteSystem& constellation) +{ + switch (constellation) + { + case gnsstk::SatelliteSystem::GPS: + return gnss_info_msgs::Enums::CONSTELLATION_GPS; + case gnsstk::SatelliteSystem::Galileo: + return gnss_info_msgs::Enums::CONSTELLATION_GALILEO; + case gnsstk::SatelliteSystem::Glonass: + return gnss_info_msgs::Enums::CONSTELLATION_GLONASS; + case gnsstk::SatelliteSystem::BeiDou: + return gnss_info_msgs::Enums::CONSTELLATION_BEIDOU; + case gnsstk::SatelliteSystem::IRNSS: + return gnss_info_msgs::Enums::CONSTELLATION_NAVIC; + case gnsstk::SatelliteSystem::QZSS: + return gnss_info_msgs::Enums::CONSTELLATION_QZSS; + default: + return cras::nullopt; + } +} + +cras::optional satelliteInfoToSatID(const gnss_info_msgs::SatelliteInfo& info) +{ + const auto maybeConstellationAndPrn = prnStringToInt(info.prn); + if (!maybeConstellationAndPrn.has_value()) + return cras::nullopt; + + const auto& [prn, constellationStr] = *maybeConstellationAndPrn; + + auto maybeSatelliteSystem = rosConstellationToGnsstkSatelliteSystem(constellationStr); + if (!maybeSatelliteSystem.has_value()) + maybeSatelliteSystem = rosConstellationToGnsstkSatelliteSystem(info.constellation); + if (!maybeSatelliteSystem.has_value()) + return cras::nullopt; + + const auto system = *maybeSatelliteSystem; + return gnsstk::SatID(prn, system); +} + +} diff --git a/gnsstk_ros/src/position.cpp b/gnsstk_ros/src/position.cpp new file mode 100644 index 0000000..9bb4c8a --- /dev/null +++ b/gnsstk_ros/src/position.cpp @@ -0,0 +1,58 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#include + +#include +#include +#include + +namespace gnsstk_ros +{ + +gnsstk::Position convert(const geographic_msgs::GeoPoint& position) +{ + return {position.latitude, position.longitude, position.altitude, gnsstk::Position::CoordinateSystem::Geodetic}; +} + +geographic_msgs::GeoPoint convertToGeographicMsg(const gnsstk::Position& position) +{ + auto pos = position; + pos.transformTo(gnsstk::Position::CoordinateSystem::Geodetic); + geographic_msgs::GeoPoint msg; + msg.latitude = pos.geodeticLatitude(); + msg.longitude = pos.longitude(); + msg.altitude = pos.height(); + return msg; +} + +gnsstk::Position convert(const geometry_msgs::Point& position) +{ + return {position.x, position.y, position.z, gnsstk::Position::CoordinateSystem::Cartesian}; +} + +geometry_msgs::Point convertToCartesianMsg(const gnsstk::Position& position) +{ + auto pos = position; + pos.transformTo(gnsstk::Position::CoordinateSystem::Cartesian); + geometry_msgs::Point msg; + msg.x = pos.X(); + msg.y = pos.Y(); + msg.z = pos.Z(); + return msg; +} + +gnss_info_msgs::SatellitePosition convert(const gnsstk::Xvt& xvt, const uint32_t satcatId) +{ + 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]; + return position; +} + +} diff --git a/gnsstk_ros/src/time.cpp b/gnsstk_ros/src/time.cpp new file mode 100644 index 0000000..daadc7f --- /dev/null +++ b/gnsstk_ros/src/time.cpp @@ -0,0 +1,53 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#include + +#include +#include + +#include +#include +#include + +namespace gnsstk_ros +{ + +// Statically initialize the number of days since 1 Jan 1970 in gnsstk::CommonTime. +const gnsstk::CommonTime UnixEpoch::gnsstkCommonTime {gnsstk::YDSTime(1970, 0, 0.0).convertToCommonTime()}; +const long UnixEpoch::gnsstkDaysOffset {getDays(UnixEpoch::gnsstkCommonTime)}; // NOLINT(runtime/int) + +long getDays(const gnsstk::CommonTime& time) // NOLINT(runtime/int) +{ + long days, secs; // NOLINT(runtime/int) + double frac; + time.get(days, secs, frac); + return days; +} + +ros::Time convert(const gnsstk::CommonTime& commonTime) +{ + long days, secs; // NOLINT(runtime/int) + double frac; + commonTime.get(days, secs, frac); + + const auto daysSecs = ros::Duration::DAY.sec * (days - UnixEpoch::gnsstkDaysOffset); + const auto timeSecs = daysSecs + secs; + + using SecType = decltype(ros::Duration::sec); + if (timeSecs < std::numeric_limits::min() || timeSecs > std::numeric_limits::max()) + throw std::runtime_error("Duration is out of dual 32-bit range"); + + return ros::Time(static_cast(timeSecs), 0) + ros::Duration(frac); +} + +gnsstk::CommonTime convert(const ros::Time& rosTime) +{ + auto commonTime = UnixEpoch::gnsstkCommonTime; + commonTime.addDays(rosTime.sec / ros::Duration::DAY.sec); + commonTime.addSeconds(static_cast(rosTime.sec % ros::Duration::DAY.sec)); // NOLINT(runtime/int) + commonTime.addSeconds(rosTime.nsec * 1e-9); + return commonTime; +} + +}