Visibility based GNSS receiver simulator

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}
 )

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

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: <Fixed 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: <Fixed 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

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 <functional>
+
+#include <ros/time.h>
+
+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<int>(month+1) << "-"  // Month with 2 digits
+               << std::setw(2) << static_cast<int>(mday) << " ("  // Day of month with 2 digits
+               << std::setw(3) << yday << ") "   // Day of year with 3 digits
+               << std::setw(2) << static_cast<int>(hour) << ":"  // Hour with 2 digits
+               << std::setw(2) << static_cast<int>(min) << ":"   // Minute with 2 digits
+               << std::setw(2) << static_cast<int>(sec);         // Second with 2 digits
+        return stream.str();
+    }
+
+};
+
+    inline std::ostream& operator<<(std::ostream& s, const DateTime& dt)
+    {
+        s << dt.toString();
+        return s;
+    }
+
+}

gnss_info/include/gnss_info/RinexDownloader.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <string>
+
+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);
+};
+}

gnss_info/launch/simulator.launch

@@ -0,0 +1,23 @@
+<launch>
+    <!-- Load parameters from the YAML file -->
+    <rosparam file="$(find gnss_info)/config/params.yaml" command="load"/>
+
+    <!-- Launch the C++ ROS node with parameters -->
+    <node name="gnss_simulator" pkg="gnss_info" type="gnss_receiver_simulator" output="screen"/>
+
+    <!-- Launch the python ROS node with parameters -->
+    <node name="sky_view" pkg="gnss_info" type="sky_plot" output="screen"/>
+
+    <!-- Add RViz node -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gnss_info)/config/rviz_config.rviz" />
+
+    <!-- Launch Gazebo with a specific world and parameters -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+      <arg name="world_name" value="$(find gnss_info)/simulation/simulation_world.world"/>
+      <arg name="paused" value="false"/>
+      <arg name="use_sim_time" value="false"/>
+      <arg name="gui" value="true"/>
+      <arg name="headless" value="false"/>
+      <arg name="debug" value="false"/>
+    </include>
+  </launch>

gnss_info/launch/sky_plot_view.launch

@@ -0,0 +1,22 @@
+<launch>
+    <!-- Launch the first C++ node -->
+    <node name="satellite_publisher" pkg="gnss_info" type="publish_all_satellites">
+        <!-- Add any ROS parameters if needed -->
+        <param name="time" value="1713794970" />
+        <!-- <param name="only_constellations" value="['GPS']" /> -->
+        <rosparam param="only_constellations"> ["GPS", "GALILEO"]</rosparam>
+    </node>
+
+    <!-- Launch the second C++ node -->
+    <!-- <node name="sky_view_publisher" pkg="gnss_info" type="publish_sky_view"> -->
+        <!-- Add any ROS parameters if needed -->
+        <!-- <param name="some_param" value="param_value"/> -->
+    <!-- </node> -->
+
+    <!-- Launch the Python node -->
+    <!-- <node name="sky_plot" pkg="gnss_info" type="sky_plot"> -->
+        <!-- Ensure the python file is executable: chmod +x your_python_node.py -->
+        <!-- Add any ROS parameters if needed -->
+        <!-- <param name="some_param" value="param_value"/> -->
+    <!-- </node> -->
+</launch>