From 264c4e7bded7465b0026c6a97cfdba5e75b5c3e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=A1clav=20Truhla=C5=99=C3=ADk?= Date: Fri, 24 May 2024 19:15:57 +0200 Subject: [PATCH] Visibility based GNSS receiver simulator --- gnss_info/CMakeLists.txt | 42 +- gnss_info/config/params.yaml | 8 + gnss_info/config/rviz_config.rviz | 133 ++++ gnss_info/include/gnss_info/DateTime.h | 99 +++ gnss_info/include/gnss_info/RinexDownloader.h | 13 + gnss_info/launch/simulator.launch | 23 + gnss_info/launch/sky_plot_view.launch | 22 + gnss_info/nodes/simulate_gnss_receiver.cpp | 690 ++++++++++++++++++ gnss_info/simulation/DemExtractor.cpp | 185 +++++ gnss_info/simulation/KN/model.config | 15 + gnss_info/simulation/KN/model.sdf | 391 ++++++++++ gnss_info/simulation/positionComparisons.cpp | 142 ++++ gnss_info/simulation/simulation_world.world | 29 + gnss_info/simulation/world_ray_controller.cc | 304 ++++++++ gnss_info/src/DateTime.cpp | 99 +++ gnss_info/src/RinexDownloader.cpp | 75 ++ 16 files changed, 2267 insertions(+), 3 deletions(-) create mode 100644 gnss_info/config/params.yaml create mode 100644 gnss_info/config/rviz_config.rviz create mode 100644 gnss_info/include/gnss_info/DateTime.h create mode 100644 gnss_info/include/gnss_info/RinexDownloader.h create mode 100644 gnss_info/launch/simulator.launch create mode 100644 gnss_info/launch/sky_plot_view.launch create mode 100644 gnss_info/nodes/simulate_gnss_receiver.cpp create mode 100644 gnss_info/simulation/DemExtractor.cpp create mode 100644 gnss_info/simulation/KN/model.config create mode 100644 gnss_info/simulation/KN/model.sdf create mode 100644 gnss_info/simulation/positionComparisons.cpp create mode 100644 gnss_info/simulation/simulation_world.world create mode 100644 gnss_info/simulation/world_ray_controller.cc create mode 100644 gnss_info/src/DateTime.cpp create mode 100644 gnss_info/src/RinexDownloader.cpp diff --git a/gnss_info/CMakeLists.txt b/gnss_info/CMakeLists.txt index 035c206..3c48a72 100644 --- a/gnss_info/CMakeLists.txt +++ b/gnss_info/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS gnss_info_msgs gnsstk_ros roscpp + gazebo_ros + geometry_msgs ) find_package(CURL REQUIRED) @@ -23,19 +25,22 @@ find_package(Filesystem REQUIRED COMPONENTS Final Experimental Boost) find_package(GNSSTK REQUIRED) find_package(jsoncpp REQUIRED) find_package(YAML-CPP REQUIRED) +find_package(gazebo REQUIRED) +find_package(ZLIB REQUIRED) #catkin_lint: ignore_once literal_project_name catkin_package( INCLUDE_DIRS include LIBRARIES orbital_data satellite_metadata - CATKIN_DEPENDS geographic_msgs gnss_info_msgs roscpp + CATKIN_DEPENDS geographic_msgs gnss_info_msgs roscpp gazebo_ros ) +include_directories(${GNSSTK_INCLUDE_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS} ${CURL_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS}) -add_library(${PROJECT_NAME}_common src/cache.cpp src/cache_index.cpp) +add_library(${PROJECT_NAME}_common src/cache.cpp src/cache_index.cpp src/DateTime.cpp src/RinexDownloader.cpp) add_dependencies(${PROJECT_NAME}_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_common PUBLIC ${catkin_LIBRARIES} PRIVATE ${CURL_LIBRARIES} std::filesystem) +target_link_libraries(${PROJECT_NAME}_common PUBLIC ${catkin_LIBRARIES} PRIVATE ${CURL_LIBRARIES} ${ZLIB_LIBRARIES} std::filesystem) add_library(satellite_metadata src/igs_satellite_metadata.cpp) add_dependencies(satellite_metadata ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -54,6 +59,37 @@ target_link_libraries(publish_all_satellites satellite_metadata) add_executable(publish_sky_view nodes/publish_sky_view.cpp) target_link_libraries(publish_sky_view orbital_data) +# truhlvac pridavky ---------------- + +add_executable(gnss_receiver_simulator nodes/simulate_gnss_receiver.cpp) +target_link_libraries(gnss_receiver_simulator gnsstk ${catkin_LIBRARIES} ${PROJECT_NAME}_common) + +add_executable(position_comparator simulation/positionComparisons.cpp) +target_link_libraries(position_comparator gnsstk ${catkin_LIBRARIES} ${PROJECT_NAME}_common) + +get_target_property(LIBA_INCLUDES gnsstk INCLUDE_DIRECTORIES) +message(WARNING "TEST ${LIBA_INCLUDES}") + +# Find ROS +find_package(roscpp REQUIRED) +find_package(std_msgs REQUIRED) +include_directories(${roscpp_INCLUDE_DIRS}) +include_directories(${std_msgs_INCLUDE_DIRS}) + +# Find Gazebo +find_package(gazebo REQUIRED) +include_directories(${GAZEBO_INCLUDE_DIRS}) +link_directories(${GAZEBO_LIBRARY_DIRS}) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS} -O0") + +set(CMAKE_CXX_STANDARD 11) + +# Build our plugin +add_library(world_ray_controller SHARED simulation/world_ray_controller.cc) +target_link_libraries(world_ray_controller ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${PROJECT_NAME}_common gnsstk) + +# ---------------------------------- + install(TARGETS publish_all_satellites publish_sky_view RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/gnss_info/config/params.yaml b/gnss_info/config/params.yaml new file mode 100644 index 0000000..9c2670a --- /dev/null +++ b/gnss_info/config/params.yaml @@ -0,0 +1,8 @@ +robot_position: + x: 0.0 + y: -15.0 + z: 0.7 + +only_signals: [] # 'GPS_L1', 'GAL_E1', 'BDS_B1' +only_systems: [] # 'GPS', 'GAL', 'BDS' +time: 1714476130 diff --git a/gnss_info/config/rviz_config.rviz b/gnss_info/config/rviz_config.rviz new file mode 100644 index 0000000..5b12bae --- /dev/null +++ b/gnss_info/config/rviz_config.rviz @@ -0,0 +1,133 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 355 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /sky_plot + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000231000000bc0000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 72 + Y: 27 diff --git a/gnss_info/include/gnss_info/DateTime.h b/gnss_info/include/gnss_info/DateTime.h new file mode 100644 index 0000000..bf52d5f --- /dev/null +++ b/gnss_info/include/gnss_info/DateTime.h @@ -0,0 +1,99 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#pragma once + +#include + +#include + +namespace gnss_info +{ + +/** + * \brief Object that can be used as an index into an ftp server to download hourly data. + */ +struct DateTime +{ + uint16_t year; //!< Year. + uint8_t month; //!< Month [0-11]. + uint8_t mday; //!< Day of month [1-31]. + uint16_t yday; //!< Day of year (starting with 1)[1-365]. + uint8_t hour; //!< Hour [0-23]. + uint8_t min; //!< Minutes [0-59]. + uint8_t sec; //!< Seconds [0-59]. + + /** + * \brief Create an entry for the given time. + * \param[in] time The time to create the entry for. + */ + explicit DateTime(const ros::Time& time); + + /** + * \brief Directly create an entry for the given hour. + * \param[in] year The year to represent. + * \param[in] month The month to represent (0-11). + * \param[in] mday The day of month to represent (1-31). + * \param[in] yday The day of year to represent (starting with 1). + * \param[in] hour The hour to represent (0-23). + * \param[in] min The minute to represent (0-59). + * \param[in] sec The second to represent (0-59). + */ + DateTime(uint16_t year, uint8_t month, uint8_t mday, uint16_t yday, uint8_t hour, uint8_t min, uint8_t sec); + + /** + * \brief Convert the index to a representative ROS Time. + */ + explicit operator ros::Time() const; + + /** + * \brief Compare with another index. + * \param[in] other The other index to compare. + * \return Whether this index represents the same day. + */ + bool operator==(const DateTime& other) const; + + /** + * \brief Compare with another index. + * \param[in] other The other index to compare. + * \return Whether this index represents more future time. + */ + bool operator>(const DateTime& other) const; + + /** + * \brief Compare with another index. + * \param[in] other The other index to compare. + * \return Whether this index represents more past time. + */ + // bool operator<(const DateTime& other) const; + + /** + * \brief Add seconds to this time index. + * \param[in] seconds Number of seconds to add. + * \return new struct with added seconds. + */ + DateTime operator+(const long seconds); + + + std::string toString() const { + std::stringstream stream; + stream << std::setfill('0') + << std::setw(4) << year << "-" // Year with 4 digits + << std::setw(2) << static_cast(month+1) << "-" // Month with 2 digits + << std::setw(2) << static_cast(mday) << " (" // Day of month with 2 digits + << std::setw(3) << yday << ") " // Day of year with 3 digits + << std::setw(2) << static_cast(hour) << ":" // Hour with 2 digits + << std::setw(2) << static_cast(min) << ":" // Minute with 2 digits + << std::setw(2) << static_cast(sec); // Second with 2 digits + return stream.str(); + } + +}; + + inline std::ostream& operator<<(std::ostream& s, const DateTime& dt) + { + s << dt.toString(); + return s; + } + +} \ No newline at end of file diff --git a/gnss_info/include/gnss_info/RinexDownloader.h b/gnss_info/include/gnss_info/RinexDownloader.h new file mode 100644 index 0000000..ce9da00 --- /dev/null +++ b/gnss_info/include/gnss_info/RinexDownloader.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +namespace gnss_info +{ + +class RinexDownloader { +public: + static bool downloadFile(const std::string& url, const std::string& outputPath); + static bool decompressFile(const std::string& inputPath, const std::string& outputPath); +}; +} \ No newline at end of file diff --git a/gnss_info/launch/simulator.launch b/gnss_info/launch/simulator.launch new file mode 100644 index 0000000..921501a --- /dev/null +++ b/gnss_info/launch/simulator.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gnss_info/launch/sky_plot_view.launch b/gnss_info/launch/sky_plot_view.launch new file mode 100644 index 0000000..aa7fe3d --- /dev/null +++ b/gnss_info/launch/sky_plot_view.launch @@ -0,0 +1,22 @@ + + + + + + + ["GPS", "GALILEO"] + + + + + + + + + + + + + + + diff --git a/gnss_info/nodes/simulate_gnss_receiver.cpp b/gnss_info/nodes/simulate_gnss_receiver.cpp new file mode 100644 index 0000000..9e44f21 --- /dev/null +++ b/gnss_info/nodes/simulate_gnss_receiver.cpp @@ -0,0 +1,690 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * An example node using IGSSatelliteMetadata to construct a list of all GNSS satellites that existed in a given + * time instant. Then based on the predefined position of the virtual receiver, saves RINEX data about visible satellites. + * + * Published topics: + * - `satellite_pos` (`gnss_info_msgs/SatellitesList`, latched): The list of all satellites for gazebo plugin. + * + * Parameters: + * - `~time` (float, defaults to ros::Time::now() ): The reference time for which satellites should be looked up. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "gnss_info/DateTime.h" +#include "gnss_info/RinexDownloader.h" + +using namespace std; +using namespace gnsstk; + +static const string dts("%Y/%03j/%02H:%02M:%02S %P"); + +map mapSelectedSysAndSigs + (unordered_set systems, unordered_set signals) +{ + // map> defaultMap = { + // {SatelliteSystem::GPS, + // { + // {"L1", WAVELENGTH_GPS_L1}, + // {"L2", WAVELENGTH_GPS_L2}, + // {"L5", WAVELENGTH_GPS_L5} + // } + // }, + // {SatelliteSystem::Galileo, + // { + // {"E1", WAVELENGTH_GALILEO_E1}, + // {"E5", WAVELENGTH_GALILEO_E5}, + // {"E5a", WAVELENGTH_GALILEO_E5a}, + // {"E5b", WAVELENGTH_GALILEO_E5b}, + // {"E6", WAVELENGTH_GALILEO_E6} + // } + // }, + // {SatelliteSystem::BeiDou, + // { + // {"L1", WAVELENGTH_BEIDOU_L1}, + // {"B1", WAVELENGTH_BEIDOU_B1}, + // {"B2", WAVELENGTH_BEIDOU_B2}, + // {"B2a", WAVELENGTH_BEIDOU_B2a}, + // {"B2b", WAVELENGTH_BEIDOU_B2b}, + // {"B3", WAVELENGTH_BEIDOU_B3} + // } + // }, + // {SatelliteSystem::QZSS, + // { + // {"L1", WAVELENGTH_QZSS_L1}, + // {"L2", WAVELENGTH_QZSS_L2}, + // {"L5", WAVELENGTH_QZSS_L5}, + // {"L6", WAVELENGTH_QZSS_L6} + // } + // }, + // {SatelliteSystem::IRNSS, + // { + // {"S", WAVELENGTH_NAVIC_S}, + // {"L5", WAVELENGTH_NAVIC_L5} + // } + // } + // }; + + map systemSignalMap; + + if (systems.empty() && signals.empty()) { + string supportedSystems = "GEJCI"; + for (const auto& supSys : supportedSystems) { + map sys = RinexObsID::validRinexTrackingCodes[supSys]; + string signalList = ""; + for (const auto& sig : sys) { + if (supSys == 'I' && sig.first == '1') continue; + signalList += sig.first; + } + systemSignalMap[supSys] = signalList; + } + } else { + for (const auto& sig : signals) { + char delimiter = '_'; + vector tokens; + istringstream str_stream(sig); + string token; + // Use std::getline to split the string + while (getline(str_stream, token, delimiter)) { + tokens.push_back(token); + } + + char realSys; + if (tokens[0].length() != 1) { + if (RinexObsID::map3to1sys.find(tokens[0]) != RinexObsID::map3to1sys.end()) { + realSys = RinexObsID::map3to1sys[tokens[0]][0]; + } + } else if (RinexObsID::map1to3sys.find(tokens[0]) != RinexObsID::map1to3sys.end()) { + realSys = tokens[0][0]; + } else { + cerr << "Unsupported system in signal definition " << sig << endl; + continue; + } + + char realSig; + string tokenSig = tokens[1]; + if (tokenSig == "L1" || tokenSig == "E1") realSig = '1'; + else if (tokenSig == "L2" || tokenSig == "B1") realSig = '2'; + else if (tokenSig == "L5" || tokenSig == "E5a" || tokenSig == "B2a") realSig = '5'; + else if (tokenSig == "E6" || tokenSig == "L6" || tokenSig == "B3") realSig = '6'; + else if (tokenSig == "E5b" || tokenSig == "B2b") realSig = '7'; + else if (tokenSig == "E5" || tokenSig == "B2") realSig = '8'; + else if (tokenSig == "S") realSig = '9'; + else { + cerr << "Unsupported signal in signal definition " << sig << endl; + continue; + } + + const auto foundSys = systemSignalMap.find(realSys); + if (foundSys != systemSignalMap.end()) { + // Expanding signal list for existing system + cout << "Found: " << foundSys->second + << "\t expanding of " << realSig << endl; + foundSys->second += realSig; + cout << systemSignalMap.find(realSys)->second << endl; + } else { + // Storing first signal for this system + systemSignalMap[realSys] = realSig; + } + } + if (systems.empty()) { + string supportedSystems = "GEJCI"; + for (const auto& supSys : supportedSystems) { + map sys = RinexObsID::validRinexTrackingCodes[supSys]; + if (systemSignalMap.find(supSys) != systemSignalMap.end()) continue; + string signalList = ""; + for (const auto& sig : sys) { + if (supSys == 'I' && sig.first == '1') continue; + signalList += sig.first; + } + systemSignalMap[supSys] = signalList; + } + } else { + string supportedSystems = "GEJCI"; + for (const auto& sys : systems) { + char realSys; + if (sys.length() != 1) { + if (RinexObsID::map3to1sys.find(sys) != RinexObsID::map3to1sys.end()) { + realSys = RinexObsID::map3to1sys[sys][0]; + } + } else if (RinexObsID::map1to3sys.find(sys) != RinexObsID::map1to3sys.end()) { + realSys = sys[0]; + } else { + cerr << "Unsupported system in system definition " << sys << endl; + continue; + } + + if (supportedSystems.find(realSys) == string::npos) continue; + + if (systemSignalMap.find(realSys) == systemSignalMap.end()) { + string signalList = ""; + for (const auto& sig : RinexObsID::validRinexTrackingCodes[realSys]) { + if (realSys == 'I' && sig.first == '1') continue; + signalList += sig.first; + } + systemSignalMap[realSys] = signalList; + } + } + } + } + + return systemSignalMap; +} + +double doppler3D(double freq, Triple pr, Triple ps, Triple vr, Triple vs) +{ + Triple n = (pr - ps).unitVector(); + double vrn = vr.dot(n); + double vsn = vs.dot(n); + double scale = (C_MPS - vrn) / (C_MPS - vsn); + double dopFreq = scale * freq; + double shift = dopFreq - freq; + // cout << "n: " << n + // << "\nvrn: " << vrn << "\tvsn: " << vsn + // << "\nscale: " << scale << "\tdF: " << dopFreq + // << "\nshift: " << shift + // << "\nWiki: " << vsn / WAVELENGTH_GPS_L1 << endl; + return shift; +} + +double getRinexDatum(RinexObsType rot, gnss_info_msgs::SatellitePosition sat, Xvt robPos) { + + +} + +string getCurrentDateUTC() { + // Get current time from ROS + ros::Time now = ros::Time::now(); + + // Convert ROS time to time_t + time_t now_time_t = (time_t)now.sec; + + // Convert time_t to tm as UTC + tm *utc_tm = gmtime(&now_time_t); + + std::ostringstream oss; + // Format date and time as yyyymmdd hhmmss + oss << std::put_time(utc_tm, "%Y%m%d %H%M%S"); + // Append UTC time zone code + oss << " UTC"; + + return oss.str(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "simulate_gnss_receiver"); + + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + const auto params = cras::nodeParams(pnh); + + auto pub = nh.advertise("satellite_positions", 1, true); + auto pubSatList = nh.advertise("satellites", 1, true); + + const auto time = params->getParam("/time", ros::Time::now()); + const auto onlySignals = params->getParam( + "/only_signals", + std::unordered_set() + // Supported: + // "GPS" or "G" + "_" +: "L1", "L2", "L5", + // "GAL" or "E" + "_" +: "E1", "E5", "E5a", "E5b", "E6", + // "QZS" or "J" + "_" +: "L1", "L2", "L5", "L6", + // "BDS" or "C" + "_" +: "L1", "B1", "B2", "B2a", "B2b", "B3", + // "IRN" or "I" + "_" +: "S", "L5" + ); + const auto onlySystems = params->getParam( + "/only_systems", + std::unordered_set() + // Supported: "GPS" or "G", "GAL" or "E", + // "QZS" or "J", "BDS" or "C", "IRN" or "I" + ); + + // Map with selected systems and their selected signals + map systemSignalMap = mapSelectedSysAndSigs(onlySystems, onlySignals); + for (const auto& i : systemSignalMap) { + cout << "System: " << i.first << "\tsignals: " << i.second << endl; + } + + Triple xpos(3972335.8991, 1021246.6248, 4868439.6474); + Triple xvel; // 0 + // Preset position of the robot + Xvt robPos; + robPos.x = xpos; + robPos.v = xvel; + // robPos.health = Xvt::HealthStatus::Healthy; + + NavLibrary navLib; + CorrectedEphemerisRange cer; + WGS84Ellipsoid ellipsoid; + CommonTime::tsConv = std::make_shared(); + ros::Time startTime = ros::Time::now(); + ros::Time offsetTime(time); + // ros::Time offsetTime(startTime.sec); + ros::Time diffTime(startTime.sec - offsetTime.sec); + cout << "startTime: " << startTime + << "\noffsetTime: " << offsetTime + << "\ndiffTime: " << diffTime << endl; + gnss_info::DateTime startDt(offsetTime); + gnss_info::DateTime currentDt(startTime); + cout << "startDt: " << startDt + << "\ncurrentDt: " << currentDt << endl; + + // string url = cras::format("https://igs.bkg.bund.de/root_ftp/MGEX/BRDC/%04u/%03u/BRDC00WRD_S_%04u%03u0000_01D_MN.rnx.gz", dt.year, dt.yday, dt.year, dt.yday); + // string url = cras::format("https://github.com/commaai/gnss-data-hourly/raw/master/%04u/%03u/hour%03u0.24n.gz", dt.year, dt.yday, dt.yday); + string url = cras::format("https://igs.bkg.bund.de/root_ftp/IGS/BRDC/%04u/%03u/BRDC00WRD_R_%04u%03u0000_01D_MN.rnx.gz", startDt.year, startDt.yday, startDt.year, startDt.yday); + // cout << "URL: " << url << endl; + + // string outputPath = cras::format("/home/truhlvac/Desktop/cache_simulator/%04u_%03u_hour%03u0.24n.gz", dt.year, dt.yday, dt.yday); + string outputPath = cras::format("/home/truhlvac/Desktop/cache_simulator/BRDC00WRD_R_%04u%03u0000_01D_MN.rnx.gz", startDt.year, startDt.yday); + string decompressedOutputPath = cras::format("/home/truhlvac/Desktop/cache_simulator/BRDC00WRD_R_%04u%03u0000_01D_MN.rnx", startDt.year, startDt.yday); + + if (gnss_info::RinexDownloader::downloadFile(url, outputPath)) { + cout << "Download completed successfully.\n"; + if (gnss_info::RinexDownloader::decompressFile(outputPath, decompressedOutputPath)) { + cout << "Decompression completed successfully.\n"; + } else { + cout << "Decompression failed.\n"; + return 2; + } + } else { + cout << "Download failed.\n"; + return 1; + } + + NavDataFactoryPtr ndfp(make_shared()); + navLib.addFactory(ndfp); + // cout << "Before: " << ndfp->getFactoryFormats() << endl; + if(!ndfp->addDataSource(decompressedOutputPath)) + // if(!ndfp->addDataSource("/home/truhlvac/catkin_ws/src/gnss-info-master/gnss_info/test/BRDC00WRD_R_20241230000_01D_MN.rnx")) + { + cout << "Could not add data source:\n" << decompressedOutputPath << endl; + return 3; + } + // if(!ndfp->addDataSource("/home/truhlvac/catkin_ws/src/gnss-info-master/gnss_info/test/IGS0OPSRAP_20241210000_01D_15M_ORB.SP3")) + // // if(!ndfp->addDataSource("/home/truhlvac/catkin_ws/src/gnss-info-master/gnss_info/test/BRDC00WRD_R_20241230000_01D_MN.rnx")) + // { + // cout << "Could not add data source SP3" << endl; + // return 3; + // } + // cout << "After: " << ndfp->getFactoryFormats() << endl; + cout << "Interval: (" << printTime(navLib.getInitialTime(), dts) << ", " << printTime(navLib.getFinalTime(), dts) << ")" << endl; + + auto automaticPublisher = [&pub, &pubSatList, &navLib, &diffTime, &systemSignalMap, &cer, &ellipsoid, &robPos](const ros::TimerEvent&) { + using namespace std; + using namespace gnsstk; + ros::Time timeNow = ros::Time::now(); + // cout << "Time: " << time << endl; + ros::Time time(timeNow.sec - diffTime.sec); + gnss_info::DateTime dt(time); + cout << "DT: " << dt << endl; + // cout << "DT+" << dt+100 << endl; + + CommonTime ct; + ct.set(convertCalendarToJD(dt.year, dt.month+1, dt.mday), + convertTimeToSOD(dt.hour, dt.min, dt.sec), 0.0, TimeSystem::GPS); + cout << "Ct when: " << printTime(ct, dts) << endl; + set sid_set = navLib.getIndexSet(navLib.getInitialTime(), navLib.getFinalTime()); + + // message for gazebo + gnss_info_msgs::SatellitesPositions msg; + msg.header.frame_id = "wgs84"; + msg.header.stamp = time; + + // mesage for sky_view + gnss_info_msgs::SatellitesList satListMsg; + for (const auto& id : sid_set) { + // Setup sky view message + gnss_info_msgs::SatelliteInfo satInfoMsg; + // Publish only selected systems + char systemChar; + switch (id.system) + { + case SatelliteSystem::GPS: + systemChar = 'G'; + satInfoMsg.constellation = gnss_info_msgs::Enums::CONSTELLATION_GPS; + break; + case SatelliteSystem::Galileo: + systemChar = 'E'; + satInfoMsg.constellation = gnss_info_msgs::Enums::CONSTELLATION_GALILEO; + break; + case SatelliteSystem::QZSS: + systemChar = 'J'; + satInfoMsg.constellation = gnss_info_msgs::Enums::CONSTELLATION_QZSS; + break; + case SatelliteSystem::BeiDou: + systemChar = 'C'; + satInfoMsg.constellation = gnss_info_msgs::Enums::CONSTELLATION_BEIDOU; + break; + case SatelliteSystem::IRNSS: + systemChar = 'I'; + satInfoMsg.constellation = gnss_info_msgs::Enums::CONSTELLATION_NAVIC; + break; + default: + systemChar = 'U'; + break; + } + if (systemSignalMap.find(systemChar) == systemSignalMap.end()) continue; + NavSatelliteID nsid(id); + Xvt xvt; + double threshold = 1; + double diff = 2000; + double deltaTime = 0; + double psuedorange = 0; + bool rv = true; + // cout << "Sat ID " << id << endl; + Position rxPos(robPos); + bool success; + CommonTime transmitEst; + tie(success, transmitEst) = RawRange::estTransmitFromReceive(rxPos, ct, navLib, id, ellipsoid); + // cout << "Success: " << success + // << "\nrxT: " << ct + // << "\ntxT: " << transmitEst + // << "\ndiT: " << ct.getSecondOfDay() - transmitEst.getSecondOfDay() << endl;; + if (!navLib.getXvt(nsid, transmitEst, xvt)) { // SVHealth::Healthy, NavValidityType::ValidOnly, NavSearchOrder::Nearest + continue; + } + + double range; + Xvt rotatedSvXvt; + tie(range, rotatedSvXvt) = RawRange::computeRange(rxPos, ct, xvt, transmitEst, ellipsoid, true); + // while (diff > threshold && rv) { + // CommonTime copyCt = ct; + // // cout << "diff: " << diff << "\tdetla: " << deltaTime + // // << "\trange: " << psuedorange + // // << "\ttime: " << copyCt << endl; + // copyCt.addSeconds(-deltaTime); + // rv = navLib.getXvt(nsid, copyCt, xvt, SVHealth::Healthy, NavValidityType::ValidOnly, NavSearchOrder::Nearest); + // if (!rv) break; + + // double newRange = (xvt.getPos() - robPos.getPos()).mag(); + // diff = abs(newRange - psuedorange); + // psuedorange = newRange; + // deltaTime = psuedorange / C_MPS; + // // cout << "diff: " << diff << "\tdetla: " << deltaTime + // // << "\trange: " << psuedorange + // // << "\ttime: " << copyCt << endl; + // } + + + // id.dump(cout); + // cout << "XVT: " << xvt << endl; + // cout << "Rotated: " << rotatedSvXvt << endl; + + + double compRange = cer.ComputeAtReceiveTime(ct, rxPos, id, navLib, NavSearchOrder::Nearest, SVHealth::Healthy, NavValidityType::ValidOnly, ellipsoid); + // cout << "Range: " << range << "\nCange: " << compRange << endl; + // range = cer.ComputeAtTransmitTime(ct, rxPos, id, navLib); + // cout << "Range: " << range << endl; + // double svrange = cer.ComputeAtTransmitSvTime(ct, range, rxPos, id, navLib); + // cout << "SVRange: " << svrange << endl; + + + // filling message for gazebo + gnss_info_msgs::SatellitePosition satPos; + satPos.position.x = xvt.getPos()[0]; + satPos.position.y = xvt.getPos()[1]; + satPos.position.z = xvt.getPos()[2]; + satPos.velocity.x = xvt.getVel()[0]; + satPos.velocity.y = xvt.getVel()[1]; + satPos.velocity.z = xvt.getVel()[2]; + satPos.satcat_id = id.id; // Satellite PRN + satPos.satcat_id += 1000* static_cast(id.system); // Satellite System + satPos.velocity_covariance[0] = compRange; + msg.satellites.push_back(satPos); + + // filling message for sky_view + satInfoMsg.satcat_id = satPos.satcat_id; + satInfoMsg.prn = string() + systemChar + to_string(id.id); + satListMsg.satellites.push_back(satInfoMsg); + } + + pub.publish(msg); + pubSatList.publish(satListMsg); + }; + + ros::Timer timer = nh.createTimer(ros::Duration(0.5), automaticPublisher); + + + Rinex3ObsStream rinStream( + cras::format("/home/truhlvac/Desktop/cache_simulator/%04u_%03u_%02u-%02u_at_%04u_%03u_%02u-%02u.obs", + startDt.year, startDt.yday, startDt.hour, startDt.min, currentDt.year, currentDt.yday, currentDt.hour, currentDt.min), + ios::out|ios::trunc); + Rinex3ObsHeader rinHead; + rinHead.version = Rinex3ObsBase::currentVersion; + rinHead.fileType = "O"; + rinHead.fileSysSat = SatID(SatelliteSystem::Mixed); + + rinHead.fileProgram = ros::this_node::getName(); + rinHead.fileAgency = "truhlvac_CTU"; + rinHead.date = getCurrentDateUTC(); + + rinHead.markerName = "Non_Physical"; + rinHead.markerNumber = "1"; + + rinHead.observer = "CTU"; + rinHead.agency = "Czech Technical University in Prague"; + + rinHead.recNo = "1"; + rinHead.recType = "Non_Physical"; + rinHead.recVers = "1.0"; + + rinHead.antNo = "1"; + rinHead.antType = "Non_Physical"; + + CivilTime startCt(startDt.year, startDt.month, startDt.mday, startDt.hour, startDt.min, startDt.sec, TimeSystem::UTC); + rinHead.firstObs = startCt; + + rinHead.sigStrengthUnit = "dbHz"; + + string observationCodes = "CLDS"; + for (const auto& sys : systemSignalMap) { + Rinex3ObsHeader::RinexObsVec rov; + for (const auto& sig : sys.second) { + char tc = RinexObsID::validRinexTrackingCodes[sys.first][sig][0]; + if (RinexObsID::validRinexTrackingCodes[sys.first][sig].find('C') != std::string::npos) { + tc = 'C'; + } + for (const auto& obs : observationCodes) { + // Construct ObsID string from system + type + signal + code ("GC1P") + RinexObsID roid(string() + sys.first + obs + sig + tc, Rinex3ObsBase::currentVersion); + rov.push_back(roid); + } + } + rinHead.mapObsTypes.insert(make_pair(string() + sys.first, rov)); + } + + rinHead.valid = Rinex3ObsHeader::allValid303; + rinHead.validEoH = true; + if (robPos.health == Xvt::HealthStatus::Healthy) { + rinHead.antennaPosition = robPos.getPos(); + rinHead.antennaDeltaHEN = Triple(); + rinStream << rinHead; + } + // rinHead.dump(cout); + map> lastPhaseLock; + + auto visibleSatsCB = [&rinStream, &robPos, &lastPhaseLock, &rinHead, &systemSignalMap](const gnss_info_msgs::SatellitesPositionsConstPtr& msg) { + // cout << "Robot: " << robPos << endl; + // Return if Robot position is uninitialized + if (robPos.health == Xvt::HealthStatus::Uninitialized) + { + return; + } + + Rinex3ObsData rinData; + map> newPhaseLock; + + // Time information to be written in the RinexData + gnss_info::DateTime dt(msg->header.stamp); + CommonTime ct; + ct.set(convertCalendarToJD(dt.year, dt.month+1, dt.mday), + convertTimeToSOD(dt.hour, dt.min, dt.sec), 0.0, TimeSystem::GPS); + rinData.time = ct; + rinData.epochFlag = 0; + + for (const auto& sat : msg->satellites) { + // ID and position and velocity of the satellite from message + uint prn = sat.satcat_id % 1000; + uint sys = sat.satcat_id / 1000; + SatID sid(prn, static_cast(sys)); + RinexSatID rsid(sid); + cout << "Sat: "; + rsid.dump(cout); + cout << endl; + Xvt xvt; + xvt.x = Triple(sat.position.x, sat.position.y, sat.position.z); + xvt.v = Triple(sat.velocity.x, sat.velocity.y, sat.velocity.z); + // cout << "XVT: " << xvt << endl; + + // Find system string + string systemString; + switch (rsid.system) + { + case SatelliteSystem::GPS: + systemString = "G"; + break; + case SatelliteSystem::Galileo: + systemString = "E"; + break; + case SatelliteSystem::QZSS: + systemString = "J"; + break; + case SatelliteSystem::BeiDou: + systemString = "C"; + break; + case SatelliteSystem::IRNSS: + systemString = "I"; + break; + default: + continue; + break; + } + + // Map for storing all RinexDatums for this SatID + vector datumVec; + + string observationCodes = "CLDS"; + for (const auto& sig : systemSignalMap[systemString[0]]) { + // Use correct frequencies for this signal + const double wave = getWavelength(rsid.system, int(sig) - 48); + const double freq = C_MPS / wave; + + // Calculations of all useful data + double dop_freq = doppler3D(freq, robPos.getPos(), xvt.getPos(), robPos.getVel(), xvt.getVel()); + // cout << "Doppler: " << dop_freq << endl; + // double range = (robPos.getPos() - xvt.getPos()).mag(); + double range = sat.velocity_covariance[0]; + // cout << "Range: " << range << endl; + bool lossOfLock = true; + double phase = range / wave; + double phaseLocked = phase; + if (lastPhaseLock.find(sid) != lastPhaseLock.end()) { + lossOfLock = false; + phaseLocked = lastPhaseLock[sid][sig]; + } + newPhaseLock[sid][sig] = phaseLocked; + double phaseIntegrated = phase - phaseLocked; + // cout << "Phase modulo: " << phase << "\tlast: " << phaseLocked << "\tintegrated: " << phaseIntegrated << endl; + // Calculated consts + double signalStrength = (-635.64 * log(range) + 10754.07) - sat.position_covariance[0]; + // cout << "Signal strength: " << signalStrength << endl; + + for (const auto& code : observationCodes) { + RinexDatum rd; + switch (code) + { + case 'C': + case 'P': + rd.data = range; + break; + case 'D': + rd.data = dop_freq; + break; + case 'L': + rd.data = phaseIntegrated; + break; + case 'S': + rd.data = signalStrength; + break; + + default: + cerr << "Invalid datum code: " << code << endl; + break; + } + rd.lli = lossOfLock; + datumVec.push_back(rd); + } + } + + rinData.obs[rsid] = datumVec; + } + // cout << "Stored Data" << endl; + rinData.numSVs = rinData.obs.size(); + rinStream << rinData; + lastPhaseLock = newPhaseLock; + + }; + + ros::Subscriber sub = nh.subscribe("visible_satellite_positions", 10, visibleSatsCB); + + + auto robPosCB = [&robPos, &rinHead, &rinStream](const geometry_msgs::PoseConstPtr& msg) { + + if (robPos.health == Xvt::HealthStatus::Healthy) return; + + robPos.x[0] = msg->position.x; + robPos.x[1] = msg->position.y; + robPos.x[2] = msg->position.z; + robPos.health = Xvt::HealthStatus::Healthy; + + rinHead.antennaPosition = robPos.getPos(); + rinHead.antennaDeltaHEN = Triple(); + + rinStream << rinHead; + cout << "POSITION CHANGED to: " << robPos.getPos() << endl; + }; + + ros::Subscriber subRobPos = nh.subscribe("/robot_position_ecef", 10, robPosCB); + + cout << "ROS Spin" << endl; + + ros::spin(); +} diff --git a/gnss_info/simulation/DemExtractor.cpp b/gnss_info/simulation/DemExtractor.cpp new file mode 100644 index 0000000..4dc64c1 --- /dev/null +++ b/gnss_info/simulation/DemExtractor.cpp @@ -0,0 +1,185 @@ +// Author: Martin Pecka ( martin.pecka@cvut.cz ) +// License: BSD + +#include "DemExtractor.h" + +// HACK: we need to access ODERayShape's private member geomID +#include +#include +#define private public +#include +#undef private + +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace safe_exploration; +using namespace std; + + +DemExtractor::DemExtractor(gazebo::physics::WorldPtr &world, const std::string demComputeFunction, const bool publishDEM, const double nanRatio, const std::string demGanModel, const std::string demGanCheckpoint) : + world(world), publishDem(publishDEM), nanRatio(nanRatio), demGanModel(""), demGanCheckpoint("") { + this->demGenerator.reset(new dem_generation::DEMGenerator(demComputeFunction, 1.0, 0.5, nanRatio)); + this->demGenerator->setResolution(-0.66, 1.34, -0.225, 0.225, -10.0, 1.0, 0.1, 0.1); + + if (this->publishDem) { + nh = ros::NodeHandle("~"); + pub = nh.advertise("dem", 1000); + msg.header.frame_id = "base_link"; + + gazeboNode.reset(new gazebo::transport::Node()); + gazeboNode->Init(world->Name()); + + gzPub = this->gazeboNode->Advertise("~/dem", 10); + } + + this->setDemGanModel(demGanModel, demGanCheckpoint); +} + +void DemExtractor::setDemGanModel(const string& demGanModel, const string& demGanCheckpoint) { + if (demGanModel == this->demGanModel && demGanCheckpoint == this->demGanCheckpoint) + return; + + this->demGanModel = demGanModel; + this->demGanCheckpoint = demGanCheckpoint; + + if (!demGanModel.empty()) { + this->demGanExecutor.reset(new dem_generation::DemGanExecutor()); + this->demGanExecutor->loadModel(demGanModel, demGanCheckpoint); + std::cout << "Using GAN DEM transform from " << demGanModel << std::endl; + } else { + this->demGanExecutor.reset(); + std::cout << "No GAN is used" << std::endl; + } +} + +const DEM& DemExtractor::getDEM(const ignition::math::Vector3d& center, const ignition::math::Vector3d& size, const flipper_control::Policy::INPUT_DATA& inputs) { + sensor_msgs::PointCloud2 cloud; + + // the following line is essential; otherwise, ODE segfaults on null pointer dereference when searching for + // a trimesh-trimesh collider + this->world->Physics()->InitForThread(); + + { + auto ray = boost::dynamic_pointer_cast ( + world->Physics()->CreateShape("ray", gazebo::physics::CollisionPtr())); + + // important so that the ray doesn't collide with robot body + dGeomID rayGeomID = ray->geomId; + dGeomSetCollideBits(rayGeomID, ~GZ_SENSOR_COLLIDE & GZ_ALL_COLLIDE); + + ignition::math::Pose3d startPose, endPose; + startPose.Rot() = ignition::math::Quaterniond::Identity; + endPose.Rot() = ignition::math::Quaterniond::Identity; + + gazebo::physics::ModelPtr robot = world->ModelByName("NIFTi"); + ignition::math::Pose3d robotPose(robot->WorldPose()); + ignition::math::Vector3d robotRotation = robotPose.Rot().Euler(); + robotRotation.X() = robotRotation.Y() = 0; // cancel out roll and pitch + robotPose.Rot() = ignition::math::Quaterniond::EulerToQuaternion(robotRotation); + + size_t numPoints = 0; + for (double x = this->demGenerator->getXMin(); x <= this->demGenerator->getXMax(); x += this->demGenerator->getDEMResolutionX()/2) + for (double y = this->demGenerator->getYMin(); y <= this->demGenerator->getYMax(); y += this->demGenerator->getDEMResolutionY() / 2) + numPoints++; + cloud.height = 1; + cloud.width = numPoints; + + sensor_msgs::PointCloud2Modifier mod(cloud); + mod.setPointCloud2FieldsByString(1, "xyz"); // also resizes the cloud + + cras::CloudIter cloudX(cloud, "x"); + cras::CloudIter cloudY(cloud, "y"); + cras::CloudIter cloudZ(cloud, "z"); + + double rayStartHeight = this->demGenerator->getZMax(); + double rayEndHeight = this->demGenerator->getZMin(); + double rayIntersectionDistance; + string entityName; + numPoints = 0; + + for (double x = this->demGenerator->getXMin(); x <= this->demGenerator->getXMax(); x += this->demGenerator->getDEMResolutionX()/2) + { + for (double y = this->demGenerator->getYMin(); y <= this->demGenerator->getYMax(); y += this->demGenerator->getDEMResolutionY()/2) + { + startPose.Pos().Set(x, y, rayStartHeight); + endPose.Pos().Set(x, y, rayEndHeight); + + ray->SetPoints(robotPose.CoordPositionAdd(startPose.Pos()), robotPose.CoordPositionAdd(endPose.Pos())); + ray->GetIntersection(rayIntersectionDistance, entityName); + + if (!entityName.empty()) { + *cloudX = static_cast(x); + *cloudY = static_cast(y); + *cloudZ = static_cast(rayStartHeight - rayIntersectionDistance); + ++numPoints; + ++cloudX; ++cloudY; ++cloudZ; + } + } + } + mod.resize(numPoints); + } + + const DEM& rawDem = this->demGenerator->pointCloud2DEM(cloud); + + if (this->demGanExecutor != nullptr) { + if (inputs.find(flipper_control::PolicyDataSource::PITCH) == inputs.end()) { + throw std::runtime_error("For DEM GAN executor, pitch is a required policy input."); + } + if (inputs.find(flipper_control::PolicyDataSource::FLIPPER_ANGLES) == inputs.end()) { + throw std::runtime_error("For DEM GAN executor, flipper angles are a required policy input."); + } + + const auto pitch = boost::any_cast(inputs.at(flipper_control::PolicyDataSource::PITCH)); + const auto flippers = + boost::any_cast >( + inputs.at(flipper_control::PolicyDataSource::FLIPPER_ANGLES)); + + this->mutableDem = rawDem; + this->demGanExecutor->transformDem(this->mutableDem, pitch, flippers); + } + + const DEM& dem = (this->demGanExecutor != nullptr ? this->mutableDem : rawDem); + + if (this->publishDem) { + msg.header.stamp = ros::Time::now(); + this->demGenerator->fillMsgMetadata(msg); + + msg.dem.resize(dem.getDemSize()); + msg.var.resize(dem.getDemSize()); + msg.points_counter.resize(dem.getDemSize()); + + gazebo::msgs::HeightmapGeom gzMsg; + gzMsg.set_width(dem.getCols()); + gzMsg.set_height(dem.getRows()); + gazebo::msgs::Set(gzMsg.mutable_size(), ignition::math::Vector3d(dem.getXMax() - dem.getXMin(), + dem.getYMax() - dem.getYMin(), + dem.getZMax() - dem.getZMin())); + gazebo::msgs::Set(gzMsg.mutable_origin(), ignition::math::Vector3d(dem.getXMin(), dem.getYMin(), dem.getZMin())); + + const auto map = dem.getDEM(); + const auto var = dem.getVar(); + const auto counter = dem.getCounter(); + + for (size_t i = 0; i < dem.getDemSize(); i++) { + msg.dem[i] = map[i]; + msg.var[i] = var[i]; + msg.points_counter[i] = counter[i]; + + gzMsg.add_heights(static_cast(map[i])); + } + + pub.publish(msg); + gzPub->Publish(gzMsg); + } + + return dem; +} diff --git a/gnss_info/simulation/KN/model.config b/gnss_info/simulation/KN/model.config new file mode 100644 index 0000000..d43f799 --- /dev/null +++ b/gnss_info/simulation/KN/model.config @@ -0,0 +1,15 @@ + + + KN yard + 1.0 + model.sdf + + + Vaclav Truhlarik + truhlvac@fel.cvut.cz + + + A model of a yard at the faculty at Karlovo Namesti. + + + diff --git a/gnss_info/simulation/KN/model.sdf b/gnss_info/simulation/KN/model.sdf new file mode 100644 index 0000000..d5080fb --- /dev/null +++ b/gnss_info/simulation/KN/model.sdf @@ -0,0 +1,391 @@ + + + true + + 0 0 0 0 0 0 + + + + 0 0 0.01 0 0 0 + + + + 0 0 1 + 200 200 + + + + + + + + + + 0 0 1 + 100 100 + + + + + + + + 8.5 -20 0.02 0 0 0 + + + + 0 0 1 + 17 40 + + + + + + + + + + 0 0 1 + 17 40 + + + + + + + + 8 -21.5 0.03 0 0 0.78 + + + + 0 0 1 + 2 27 + + + + + + + + + + 0 0 1 + 2 27 + + + + + + + 8.5 -7 0.03 0 0 0.78 + + + + 0 0 1 + 2 27 + + + + + + + + + + 0 0 1 + 2 27 + + + + + + + 17 -40 0.03 0 0 0 + + + + 0 0 1 + 10 20 + + + + + + + + + + 0 0 1 + 10 20 + + + + + + + + + -2.5 15 11 0 0 0 + + + + 95 10 22 + + + + 0.66 0.56 0.39 1 + 0.66 0.56 0.39 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 95 10 22 + + + + + + + + -10 -56 7.5 0 0 0 + + + + 80 10 15 + + + + 0.67 0.60 0.52 1 + 0.67 0.60 0.52 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 80 10 15 + + + + + + + + 35 -30.5 7.5 0 0 0 + + + + 10 41 15 + + + + 0.48 0.47 0.41 1 + 0.48 0.47 0.41 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 10 41 15 + + + + + + + + 40 0 10 0 0 0 + + + + 10 20 20 + + + + 0.67 0.60 0.52 1 + 0.67 0.60 0.52 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 10 20 20 + + + + + + + + -15 -22 4.5 0 0 0 + + + + 10 36 9 + + + + 0.66 0.56 0.39 1 + 0.66 0.56 0.39 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 10 36 9 + + + + + + + + -45 -20 11 0 0 0 + + + + 10 60 22 + + + + 0.48 0.47 0.41 1 + 0.48 0.47 0.41 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 10 60 22 + + + + + + + + -5 9 11 0 0 0 + + + + 10 2 22 + + + + + + + + + + 10 2 22 + + + + + + + + 28 0 2 0 0 0 + + + + 10 20 4 + + + + 0.71 0.40 0.16 1 + 0.71 0.40 0.16 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + 10 20 4 + + + + + + + + 29 0 5 0 0 0 + + + + 8 20 2 + + + + + + + + + + 8 20 2 + + + + + + + + 22.5 0 1 0 0 0 + + + + 1 20 0.3 + + + + + + + + + + 1 20 0.3 + + + + + + diff --git a/gnss_info/simulation/positionComparisons.cpp b/gnss_info/simulation/positionComparisons.cpp new file mode 100644 index 0000000..2838b76 --- /dev/null +++ b/gnss_info/simulation/positionComparisons.cpp @@ -0,0 +1,142 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "gnss_info/DateTime.h" + +using namespace std; +using namespace gnsstk; + +int main(int argc, char** argv) +{ + CommonTime::tsConv = std::make_shared(); + + PRSolution prs; + + ros::Time time(1714476230); + gnss_info::DateTime dt(time); + cout << "DT: " << dt << endl; + // cout << "DT+" << dt+100 << endl; + + CommonTime ct; + ct.set(convertCalendarToJD(dt.year, dt.month+1, dt.mday), + convertTimeToSOD(dt.hour, dt.min, dt.sec), 0.0, TimeSystem::GPS); + vector sats; + sats.push_back(SatID(16, SatelliteSystem::GPS)); + sats.push_back(SatID(18, SatelliteSystem::GPS)); + sats.push_back(SatID(23, SatelliteSystem::GPS)); + sats.push_back(SatID(26, SatelliteSystem::GPS)); + sats.push_back(SatID(13, SatelliteSystem::Galileo)); + // sats.push_back(SatID(14, SatelliteSystem::Galileo)); + sats.push_back(SatID(21, SatelliteSystem::Galileo)); + sats.push_back(SatID(26, SatelliteSystem::Galileo)); + cout << "Sats ok" << endl; + for (const auto& sat : sats) { + cout << sat << endl; + } + + vector ranges; + // Real ranges + // ranges.push_back(20811605.347); + // ranges.push_back(20837155.395); + // ranges.push_back(22281244.441); + // ranges.push_back(20990107.490); + // ranges.push_back(24300676.499); + // // ranges.push_back(25045197.864); + // ranges.push_back(25097084.721); + // ranges.push_back(23502129.162); + // Sim ranges + ranges.push_back(20767506.24200); + ranges.push_back(20793058.43800); + ranges.push_back(22237144.09500); + ranges.push_back(20946004.36000); + ranges.push_back(24256575.25700); + // ranges.push_back(25045197.864); + ranges.push_back(25052982.92600); + ranges.push_back(23458031.08600); + cout << "Ranges ok" << endl; + for (const auto& r : ranges) { + cout << r << endl; + } + + NavLibrary navLib; + NavDataFactoryPtr ndfp(make_shared()); + navLib.addFactory(ndfp); + // if(!ndfp->addDataSource(decompressedOutputPath)) + if(!ndfp->addDataSource("/home/truhlvac/Desktop/cache_simulator/treePartial/BRDC00WRD_R_20241210000_01D_MN.rnx")) + { + cout << "Could not add data source" << endl; + return 3; + } + cout << "NavLib ok" << endl; + // navLib.dump(cout, DumpDetail::OneLine); + + Matrix matrix(7, 4, 0.0); + cout << "Matrix ok" << endl; + + vector systems; + systems.push_back(SatelliteSystem::GPS); + systems.push_back(SatelliteSystem::Galileo); + prs.allowedGNSS = systems; + cout << "Systems ok" << endl; + for (const auto& sys : prs.allowedGNSS) { + cout << convertSatelliteSystemToString(sys) << endl; + } + + int ret = prs.PreparePRSolution(ct, sats, ranges, navLib, matrix); + cout << ret << endl; + cout << "Prepare solution: " << endl; + for (int r = 0; r < matrix.rows(); r++) { + for (int c = 0; c < matrix.cols(); c++) { + cout << matrix(r, c) << "\t"; + } + cout << "\n"; + } + + Matrix mat(0,0,0.0); + cout << "Mat: " << mat.rows() << endl; + int iters = 100000; + double limit = 1.0; + Vector residsOut; + Vector slopesOut; + SimpleTropModel trop; + int retSol = prs.SimplePRSolution(ct, sats, matrix, mat, &trop, iters, limit, residsOut, slopesOut); + cout << retSol << "\titers: " << iters << "\tlimit: " << limit << endl; + cout << "Resids: " << residsOut << endl; + + Vector solution = prs.Solution; + Matrix covariance = prs.Covariance; + cout << std::setprecision(std::numeric_limits::digits10 + 1); + cout << "Solution: " << prs.isValid() << "\n" << solution << endl; + cout << "Covariance:\n" << covariance << endl; +} + diff --git a/gnss_info/simulation/simulation_world.world b/gnss_info/simulation/simulation_world.world new file mode 100644 index 0000000..70771da --- /dev/null +++ b/gnss_info/simulation/simulation_world.world @@ -0,0 +1,29 @@ + + + + + 50.07655272 + 14.41788443 + 251.901 + 6.3 + + + model://ground_plane + + + + model://sun + + + + model://KN + + + + model://oak_tree + -9 -41 0 0 0 0 + + + + + \ No newline at end of file diff --git a/gnss_info/simulation/world_ray_controller.cc b/gnss_info/simulation/world_ray_controller.cc new file mode 100644 index 0000000..3f9723a --- /dev/null +++ b/gnss_info/simulation/world_ray_controller.cc @@ -0,0 +1,304 @@ +#include +#include +#include "gazebo/physics/physics.hh" +#include "gazebo/common/common.hh" +#include "gazebo/gazebo.hh" +// HACK: we need to access ODERayShape's private member geomID +#include +#include +#define private public +#include +#undef private + +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + +using namespace std; + +class WorldRaysController : public WorldPlugin +{ + private: physics::WorldPtr world; + private: ros::NodeHandle* rosNode; + private: ros::Subscriber SatPosSub; + private: ros::Publisher VisSatPosPub; + private: ros::Publisher RobPosPub; + private: ros::Publisher SkyViewPub; + private: vector renderSats; + private: gz::math::SphericalCoordinates coords; + private: ignition::math::Vector3d robPos; + private: ignition::math::Vector3d robPosECEF; + private: ignition::math::Vector3d robPosSpherical; + private: ros::Timer publishTimer; + private: boost::shared_ptr ray; + + public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/) + { + this->world = _parent; + + this->rosNode = new ros::NodeHandle("world_ray_controller"); + this->SatPosSub = this->rosNode->subscribe( + "/satellite_positions", 10, &WorldRaysController::SatPosCB, this); + + this->VisSatPosPub = this->rosNode->advertise( + "/visible_satellite_positions", 10); + + this->RobPosPub = this->rosNode->advertise( + "/robot_position_ecef", 10); + + this->SkyViewPub = this->rosNode->advertise( + "/sky_view", 10); + + double x = this->rosNode->param("/robot_position/x", 0.0); + double y = this->rosNode->param("/robot_position/y", 0.0); + double z = this->rosNode->param("/robot_position/z", 0.7); + robPos.Set(x, y, z); + // cerr << "Robot Position: " << robPos << endl; + + + const auto gzCoords = this->world->SphericalCoords(); + cout << "Reference coords:" + << "\nLat: " << gzCoords->LatitudeReference() + << "\nLon: " << gzCoords->LongitudeReference() + << "\nEle: " << gzCoords->GetElevationReference() + << "\nHea: " << gzCoords->HeadingOffset() << endl; + this->coords.SetLatitudeReference(gzCoords->LatitudeReference()); + this->coords.SetLongitudeReference(gzCoords->LongitudeReference()); + this->coords.SetElevationReference(gzCoords->GetElevationReference()); + this->coords.SetHeadingOffset(gzCoords->HeadingOffset()); + this->robPosECEF = this->coords.PositionTransform( + this->robPos, + this->coords.LOCAL2, this->coords.ECEF); + this->robPosSpherical = this->coords.PositionTransform( + this->robPosECEF, + this->coords.ECEF, this->coords.SPHERICAL); + cerr << std::setprecision(std::numeric_limits::digits10 + 1); + cerr << "Robot Position local: " << this->robPos + << "\nRobot Position ECEF: " << this->robPosECEF + << "\nRobot position Spherical: " << this->robPosSpherical << endl; + + ignition::math::Vector3d simEnd(3972333.340482479, 1021245.413622892, 4868439.193907659); + ignition::math::Vector3d realEnd(3972338.59978194, 1021248.239575014, 4868447.078602483); + ignition::math::Vector3d groundTruth((50.07640222 / 180) * M_PI, (14.41791814 / 180) * M_PI, 249.735); + cerr << "Sim: " << this->coords.PositionTransform(simEnd, this->coords.ECEF, this->coords.LOCAL2) + << "\nRel: " << this->coords.PositionTransform(realEnd, this->coords.ECEF, this->coords.LOCAL2) + << "\nGrT: " << this->coords.PositionTransform(groundTruth, this->coords.SPHERICAL, this->coords.LOCAL2) << endl; + + this->publishTimer = this->rosNode->createTimer(ros::Duration(1.0), + &WorldRaysController::PubRobPose, this); + + // Create a sphere model + sdf::SDF sphereSDF; + sphereSDF.SetFromString( + "" + " " + " true" + " " + std::to_string(robPos[0]) + " " + + std::to_string(robPos[1]) + " " + + std::to_string(0) + " 0 0 0" + "" + " " + " " + " " + " 0.7" + " " + " " + " " + " " + " " + " " + " " + ""); + + sdf::ElementPtr model = sphereSDF.Root()->GetElement("model"); + model->GetAttribute("name")->SetFromString("robot"); + // "sat_sphere_" + std::to_string(reinterpret_cast(&pose))); + this->world->InsertModelSDF(sphereSDF); + + this->ray = boost::dynamic_pointer_cast ( + world->Physics()->CreateShape("ray", gazebo::physics::CollisionPtr())); + + cerr << "Plugin loaded\n"; + } + + public: void SatPosCB(const gnss_info_msgs::SatellitesPositionsConstPtr &msg) + { + // the following line is essential; otherwise, ODE segfaults on null pointer dereference when searching for + // a trimesh-trimesh collider + this->world->Physics()->InitForThread(); + { + // auto ray = boost::dynamic_pointer_cast ( + // world->Physics()->CreateShape("ray", gazebo::physics::CollisionPtr())); + + ignition::math::Pose3d startPose, endPose; + startPose.Rot() = ignition::math::Quaterniond::Identity; + endPose.Rot() = ignition::math::Quaterniond::Identity; + + double rayIntersectionDistance; + string entityName; + + gnss_info_msgs::SatellitesPositions visSats; + visSats.header = msg->header; + // cerr << "Start pose: " << startPose << endl; + this->renderSats.clear(); + + // message for sky_view + gnss_info_msgs::SkyView skyView; + skyView.header.frame_id = "wgs84"; + skyView.header.stamp = msg->header.stamp; + skyView.reference_position.latitude = robPosSpherical.X() * 180 / M_PI; + skyView.reference_position.longitude = robPosSpherical.Y() * 180 / M_PI; + skyView.reference_position.altitude = robPosSpherical.Z(); + skyView.elevation_mask_deg = 5; + + for (const auto& sat : msg->satellites) { + startPose.Pos() = this->robPos; + ignition::math::Vector3d satPosECEF(sat.position.x, sat.position.y, sat.position.z); + ignition::math::Vector3d satPos = this->coords.PositionTransform( + satPosECEF, this->coords.ECEF, this->coords.LOCAL2); + endPose.Pos() = satPos; + // cerr << "Sat: " << sat.satcat_id + // << "\nSatPosECEF: " << satPosECEF + // << "\nSatPosENU: " << satPos << endl; + + this->ray->SetPoints(startPose.Pos(), endPose.Pos()); + this->ray->GetIntersection(rayIntersectionDistance, entityName); + + // cerr << "Intersection with: " << entityName << " at distance: " << rayIntersectionDistance << endl; + // ignition::math::Vector3d visSatVec = endPose.Pos().Normalize() * 10; + // ignition::math::Pose3d visSatPose(visSatVec, ignition::math::Quaterniond::Identity); + // this->renderSats.push_back(visSatPose); + + if (entityName.empty()) { + visSats.satellites.push_back(sat); + visSats.satellites.back().position_covariance[0] = 0.0; + + } else if (entityName[0] != 'K') { + double alpha = 0; + while (!entityName.empty()) { + ignition::math::Vector3d direction = satPos - this->robPos; + ignition::math::Vector3d step = (rayIntersectionDistance + 0.1) * direction.Normalize(); + startPose.Pos() += step; + this->ray->SetPoints(startPose.Pos(), endPose.Pos()); + this->ray->GetIntersection(rayIntersectionDistance, entityName); + // cerr << "Repeated intersection with: " << entityName << " at distance: " << rayIntersectionDistance << endl; + alpha += 1; + if (entityName[0] == 'K') break; + } + if (entityName[0] == 'K') continue; + + visSats.satellites.push_back(sat); + visSats.satellites.back().position_covariance[0] = max(0.0, min(10.0, alpha)); + } else { + continue; + } + + // std::cerr << "Alpha: " << visSats.satellites.back().position_covariance[0] << std::endl; + + // sky_view message + ignition::math::Vector3d relativeSatPos = endPose.Pos() - startPose.Pos(); + gnss_info_msgs::SatelliteSkyPosition satSkyPos = getSkyPos(relativeSatPos); + satSkyPos.satcat_id = sat.satcat_id; + skyView.satellites.push_back(satSkyPos); + } + + // this->AddSpheres(); + + // Publish the message + this->VisSatPosPub.publish(visSats); + this->SkyViewPub.publish(skyView); + } + + } + + public: void PubRobPose(const ros::TimerEvent&) + { + geometry_msgs::Pose msg; + msg.position.x = this->robPosECEF[0]; + msg.position.y = this->robPosECEF[1]; + msg.position.z = this->robPosECEF[2]; + + this->RobPosPub.publish(msg); + } + + public: gnss_info_msgs::SatelliteSkyPosition getSkyPos(ignition::math::Vector3d relativePos) + { + gnss_info_msgs::SatelliteSkyPosition skyPos; + double x = relativePos.X(); + double y = relativePos.Y(); + double z = relativePos.Z(); + double azimuth = atan2(x, y); + // Calculate elevation + double hypotenuse = sqrt(x * x + y * y); + double elevation = atan2(z, hypotenuse); + + // Convert radians to degrees + azimuth = azimuth * 180 / M_PI; + elevation = elevation * 180 / M_PI; + + // Normalize azimuth to be within the range [0, 360] degrees + if (azimuth < 0) { + azimuth += 360; + } + + skyPos.azimuth_deg = azimuth; + skyPos.elevation_deg = elevation; + + // cout << "Relative pose: " << relativePos + // << "\nazimuth: " << azimuth << "\televation: " << elevation << endl; + + return skyPos; + } + + public: void AddSpheres() + { + for (const auto& pose : renderSats) + { + cerr << "Adding sat at " << pose << endl; + // Create a sphere model + sdf::SDF sphereSDF; + sphereSDF.SetFromString( + "" + " " + " true" + " " + std::to_string(pose.X()) + " " + + std::to_string(pose.Y()) + " " + + std::to_string(pose.Z()) + " 0 0 0" + "" + " " + " " + " " + " 0.5" + " " + " " + " " + " " + " " + " " + " " + ""); + + sdf::ElementPtr model = sphereSDF.Root()->GetElement("model"); + model->GetAttribute("name")->SetFromString( + "sat_sphere_" + std::to_string(reinterpret_cast(&pose))); + this->world->InsertModelSDF(sphereSDF); + } + } + + +}; + +// Register this plugin with the simulator +GZ_REGISTER_WORLD_PLUGIN(WorldRaysController) +} \ No newline at end of file diff --git a/gnss_info/src/DateTime.cpp b/gnss_info/src/DateTime.cpp new file mode 100644 index 0000000..dccbb57 --- /dev/null +++ b/gnss_info/src/DateTime.cpp @@ -0,0 +1,99 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +#include +#include + +#include +#include + +namespace gnss_info +{ + +DateTime::DateTime(const ros::Time& time) +{ + const time_t nowSec = time.sec; + const auto t = *std::gmtime(&nowSec); + this->year = t.tm_year + 1900; // struct tm year is relative to year 1900 + this->month = t.tm_mon; + this->mday = t.tm_mday; + this->yday = t.tm_yday + 1; + this->hour = t.tm_hour; + this->min = t.tm_min; + this->sec = t.tm_sec; +} + +DateTime::DateTime(uint16_t year, uint8_t month, uint8_t mday, uint16_t yday, uint8_t hour, uint8_t min, uint8_t sec): + year(year), month(month), mday(mday), yday(yday), hour(hour), min(min), sec(sec) +{ +} + +DateTime::operator ros::Time() const +{ + tm start{}; + start.tm_year = this->year - 1900; + start.tm_yday = this->yday - 1; + start.tm_hour = this->hour; + start.tm_min = this->min; + start.tm_sec = this->sec; + return ros::Time(std::mktime(&start), 0); +} + +bool DateTime::operator==(const DateTime& other) const +{ + return this->year == other.year && this->yday == other.yday && this->hour == other.hour && this->min == other.min && this->sec == other.sec; +} + +bool DateTime::operator>(const DateTime& other) const +{ + if (this->year != other.year) return this->year > other.year; + if (this->yday != other.yday) return this->yday > other.yday; + if (this->hour != other.hour) return this->hour > other.hour; + if (this->min != other.min) return this->min > other.min; + return this->sec > other.sec; +} + +// bool DateTime::operator<(const DateTime& other) const +// { +// return other > this; +// } + +DateTime DateTime::operator+(const long seconds) { + // using namespace std::chrono; + + // Build a tm struct to use mktime + std::tm timeinfo = {}; + timeinfo.tm_year = this->year - 1900; // tm_year is years since 1900 + timeinfo.tm_mon = this->month - 1; // tm_mon is months since January [0-11] + timeinfo.tm_mday = this->mday; + timeinfo.tm_yday = this->yday - 1; + timeinfo.tm_hour = this->hour; + timeinfo.tm_min = this->min; + timeinfo.tm_sec = this->sec; + + std::cout << "timeinfo: " << timeinfo.tm_yday << std::endl; + + // Convert to time_t and add seconds + time_t rawtime = std::mktime(&timeinfo); + rawtime += seconds; + + // Convert back to tm struct + // localtime_r(&rawtime, &timeinfo); + const auto t = *std::gmtime(&rawtime); + + std::cout << "timeinfo: " << t.tm_yday << std::endl; + + + // Convert back to DateTime + return DateTime{ + static_cast(t.tm_year + 1900), + static_cast(t.tm_mon + 1), + static_cast(t.tm_mday), + static_cast(t.tm_yday + 1), // This may be incorrect after date manipulation + static_cast(t.tm_hour), + static_cast(t.tm_min), + static_cast(t.tm_sec) + }; +} + +} \ No newline at end of file diff --git a/gnss_info/src/RinexDownloader.cpp b/gnss_info/src/RinexDownloader.cpp new file mode 100644 index 0000000..3e9a01c --- /dev/null +++ b/gnss_info/src/RinexDownloader.cpp @@ -0,0 +1,75 @@ + +#include "gnss_info/RinexDownloader.h" +#include +#include +#include +#include +#include + +namespace gnss_info +{ + +// Callback function for writing data received by curl +size_t write_data(void *ptr, size_t size, size_t nmemb, void *stream) { + std::vector* receiveBuffer = (std::vector*)stream; + receiveBuffer->insert(receiveBuffer->end(), (unsigned char*)ptr, (unsigned char*)ptr + size * nmemb); + return size * nmemb; +} + +bool RinexDownloader::downloadFile(const std::string& url, const std::string& outputPath) { + CURL *curl; + CURLcode res; + curl = curl_easy_init(); + if (curl) { + std::vector receiveBuffer; + + curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, write_data); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, &receiveBuffer); + curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); + + res = curl_easy_perform(curl); + if (res != CURLE_OK) { + fprintf(stderr, "curl_easy_perform() failed: %s\n", curl_easy_strerror(res)); + curl_easy_cleanup(curl); + return false; + } + + std::ofstream file(outputPath, std::ios::out | std::ios::binary); + if (file.is_open()) { + file.write((const char*)&receiveBuffer[0], receiveBuffer.size()); + file.close(); + } + + curl_easy_cleanup(curl); + return true; + } + return false; +} + +bool RinexDownloader::decompressFile(const std::string& inputPath, const std::string& outputPath) { + gzFile inFile = gzopen(inputPath.c_str(), "rb"); + if (!inFile) { + std::cerr << "Could not open file for reading: " << inputPath << std::endl; + return false; + } + + std::ofstream outFile(outputPath, std::ios::out | std::ios::binary); + if (!outFile.is_open()) { + gzclose(inFile); + std::cerr << "Could not open file for writing: " << outputPath << std::endl; + return false; + } + + char buffer[128]; + int num_read = 0; + while ((num_read = gzread(inFile, buffer, sizeof(buffer))) > 0) { + outFile.write(buffer, num_read); + } + + gzclose(inFile); + outFile.close(); + return true; +} + +} \ No newline at end of file