User/adambrett40/259 global waypoint cache

sailbot.code-workspace

@@ -370,7 +370,6 @@
         // for the documentation about the tasks.json format
         "version": "2.0.0",
         "tasks": [
-            // Build tasks
             {
                 "label": "Build All",
                 "detail": "Build all packages in the workspace",
@@ -401,7 +400,6 @@
                 "group": "build",
                 "problemMatcher": "$gcc"
             },
-            // Test tasks
             {
                 "label": "Test All",
                 "detail": "Run all unit tests and show results.",
@@ -410,7 +408,8 @@
                 "group": {
                     "kind": "test",
                     "isDefault": true
-                }
+                },
+                "problemMatcher": []
             },
             {
                 "label": "Test Package",
@@ -437,7 +436,6 @@
                 "command": "sudo rm -rf build install log; py3clean .",
                 "problemMatcher": []
             },
-            // Linting and static code analysis tasks
             {
                 "label": "lint_cmake",
                 "detail": "Run lint on cmake files.",
@@ -481,7 +479,7 @@
                 "type": "shell",
                 "command": "LINTER=mypy LOCAL_RUN=true scripts/ament-lint.sh",
                 "problemMatcher": [
-                    "$ament_mypy",
+                    "$ament_mypy"
                 ],
                 "presentation": {
                     "panel": "dedicated",
@@ -494,7 +492,7 @@
                 "type": "shell",
                 "command": "LINTER=xmllint LOCAL_RUN=true scripts/ament-lint.sh",
                 "problemMatcher": [
-                    "$ament_xmllint",
+                    "$ament_xmllint"
                 ],
                 "presentation": {
                     "panel": "dedicated",
@@ -508,19 +506,17 @@
                     "lint_cmake",
                     "flake8",
                     "mypy",
-                    "xmllint",
+                    "xmllint"
                 ],
                 "problemMatcher": []
             },
-            // Workspace editing tasks
             {
                 "label": "setup",
                 "detail": "Set up the workspace",
                 "type": "shell",
                 "command": "./scripts/setup.sh",
                 "problemMatcher": []
             }
-
         ],
         "inputs": [
             {

src/network_systems/lib/cmn_hdrs/shared_constants.h

@@ -17,6 +17,23 @@ static const std::string DEV  = "development";
 constexpr unsigned int MAX_LOCAL_TO_REMOTE_PAYLOAD_SIZE_BYTES = 340;
 constexpr unsigned int MAX_REMOTE_TO_LOCAL_PAYLOAD_SIZE_BYTES = 270;
 
+inline std::string getCachePath()
+{
+    const char * ros_ws = std::getenv("ROS_WORKSPACE");  //NOLINT (concurrency-mt-unsafe)
+    return (ros_ws != nullptr ? std::string(ros_ws) : "/workspaces/sailbot_workspace") +
+           "/build/network_systems/projects/local_transceiver/global_waypoint_cache";
+}
+
+inline std::string getCacheTempPath()
+{
+    const char * ros_ws = std::getenv("ROS_WORKSPACE");  //NOLINT (concurrency-mt-unsafe)
+    return (ros_ws != nullptr ? std::string(ros_ws) : "/workspaces/sailbot_workspace") +
+           "/build/network_systems/projects/local_transceiver/global_waypoint_cache_temp";
+}
+
+static const std::string CACHE_PATH      = getCachePath();
+static const std::string CACHE_TEMP_PATH = getCacheTempPath();
+
 constexpr int NUM_BATTERIES = []() constexpr
 {
     using batteries_arr = custom_interfaces::msg::Batteries::_batteries_type;

src/network_systems/projects/local_transceiver/inc/local_transceiver.h

@@ -111,6 +111,14 @@ class LocalTransceiver
      */
     std::optional<std::string> debugSend(const std::string & cmd);
 
+    /**
+     * @brief Asynchronously save the received serialized data to a local file
+     *
+     * @param receivedDataBuffer string to cache
+     * @return future object representing completion of the function
+     */
+    static std::future<void> cacheGlobalWaypointsAsync(std::string receivedDataBuffer);
+
     /**
      * @brief Retrieve the latest message from the remote server via the serial port
      *
@@ -121,6 +129,22 @@ class LocalTransceiver
     // TEST
     bool checkMailbox();
 
+    /**
+     * @brief Read and parse the data from the global waypoints file, if it exists
+     *
+     * @return The global waypoints from the cache
+     */
+    static std::optional<custom_interfaces::msg::Path> getCache();
+
+    //temp public for testing
+    /**
+     * @brief Parse the message received from the remote server
+     *
+     * @param msg message received from the remote server
+     * @return the data byte string payload from the message
+     */
+    static custom_interfaces::msg::Path parseInMsg(const std::string & msg);
+
 private:
     // Serial port read/write timeout
     constexpr static const struct timeval TIMEOUT
@@ -154,14 +178,6 @@ class LocalTransceiver
 
     std::optional<std::string> readRsp();
 
-    /**
-     * @brief Parse the message received from the remote server
-     *
-     * @param msg message received from the remote server
-     * @return the data byte string payload from the message
-     */
-    static custom_interfaces::msg::Path parseInMsg(const std::string & msg);
-
     /**
      * @brief Convert a boost::asio::streambuf into a string
      * @warning Flushes the streambuf object

src/network_systems/projects/local_transceiver/src/local_transceiver.cpp

@@ -21,6 +21,8 @@
 #include "at_cmds.h"
 #include "cmn_hdrs/ros_info.h"
 #include "cmn_hdrs/shared_constants.h"
+#include "filesystem"
+#include "fstream"
 #include "global_path.pb.h"
 #include "sensors.pb.h"
 #include "waypoint.pb.h"
@@ -205,6 +207,34 @@ std::optional<std::string> LocalTransceiver::debugSend(const std::string & cmd)
     return readRsp();
 }
 
+std::future<void> LocalTransceiver::cacheGlobalWaypointsAsync(std::string receivedDataBuffer)
+{
+    return std::async(std::launch::async, [receivedDataBuffer] {
+        try {
+            std::filesystem::path cache{CACHE_PATH};
+            if (std::filesystem::exists(cache)) {
+                std::filesystem::path cache_temp{CACHE_TEMP_PATH};
+                std::ofstream         writeFile(CACHE_TEMP_PATH, std::ios::binary);
+                if (!writeFile) {
+                    std::cerr << "Failed to create temp cache file" << std::endl;
+                }
+                writeFile.write(receivedDataBuffer.data(), static_cast<std::streamsize>(receivedDataBuffer.size()));
+                writeFile.close();
+                std::filesystem::rename(CACHE_TEMP_PATH, CACHE_PATH);
+            } else {
+                std::ofstream writeFile(CACHE_PATH, std::ios::binary);
+                if (!writeFile) {
+                    std::cerr << "Failed to create cache file" << std::endl;
+                }
+                writeFile.write(receivedDataBuffer.data(), static_cast<std::streamsize>(receivedDataBuffer.size()));
+                writeFile.close();
+            }
+        } catch (const std::exception & e) {
+            std::cerr << "Error caching global waypoints: " << e.what() << std::endl;
+        }
+    });
+}
+
 custom_interfaces::msg::Path LocalTransceiver::receive()
 {
     static constexpr int MAX_NUM_RETRIES = 20;
@@ -302,6 +332,8 @@ custom_interfaces::msg::Path LocalTransceiver::receive()
         break;
     }
 
+    cacheGlobalWaypointsAsync(receivedDataBuffer);
+
     custom_interfaces::msg::Path to_publish = parseInMsg(receivedDataBuffer);
     return to_publish;
 }
@@ -337,6 +369,18 @@ custom_interfaces::msg::Path LocalTransceiver::parseInMsg(const std::string & ms
     return soln;
 }
 
+std::optional<custom_interfaces::msg::Path> LocalTransceiver::getCache()
+{
+    std::filesystem::path cache{CACHE_PATH};
+    if (std::filesystem::exists(cache)) {
+        std::ifstream                input(CACHE_PATH, std::ios::binary);
+        std::string                  cachedDataBuffer(std::istreambuf_iterator<char>(input), {});
+        custom_interfaces::msg::Path to_publish = parseInMsg(cachedDataBuffer);
+        return to_publish;
+    }
+    return std::nullopt;
+}
+
 bool LocalTransceiver::rcvRsp(const AT::Line & expected_rsp)
 {
     bio::streambuf buf;

src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp

@@ -1,3 +1,5 @@
+#include <bits/stdc++.h>
+
 #include <chrono>
 #include <functional>
 #include <memory>
@@ -44,16 +46,27 @@ class LocalTransceiverIntf : public NetNode
             } else if (mode == SYSTEM_MODE::DEV) {
                 default_port                = LOCAL_TRANSCEIVER_TEST_PORT;
                 std::string run_iridium_cmd = "$ROS_WORKSPACE/scripts/run_virtual_iridium.sh";
-                int         result          = std::system(run_iridium_cmd.c_str());  //NOLINT(concurrency-mt-unsafe)
-                if (result != 0) {
-                    std::string msg = "Error: could not start virtual iridium";
-                    std::cerr << msg << std::endl;
-                    throw std::exception();
-                }
+                std::thread vi_thread(std::system, run_iridium_cmd.c_str());
+                //vi needs to run in background
+                vi_thread.detach();
+
+                const int   MAX_ATTEMPTS = 100;  // 100ms timeout, should only take <5
+                int         attempts     = 0;
+                const int   SLEEP_MS     = 1;
                 std::string set_baud_cmd = "stty 19200 < $LOCAL_TRANSCEIVER_TEST_PORT";
-                result                   = std::system(set_baud_cmd.c_str());  //NOLINT(concurrency-mt-unsafe)
-                if (result != 0) {
-                    std::string msg = "Error: could not set baud rate for virtual iridium";
+                while (attempts < MAX_ATTEMPTS) {
+                    if (std::filesystem::exists(LOCAL_TRANSCEIVER_TEST_PORT)) {
+                        int result = std::system(set_baud_cmd.c_str());  //NOLINT(concurrency-mt-unsafe)
+                        if (result == 0) {
+                            break;
+                        }
+                    }
+                    std::this_thread::sleep_for(std::chrono::milliseconds(SLEEP_MS));
+                    attempts++;
+                }
+
+                if (attempts == MAX_ATTEMPTS) {
+                    std::string msg = "Error: could not start virtual iridium";
                     std::cerr << msg << std::endl;
                     throw std::exception();
                 }
@@ -91,6 +104,9 @@ class LocalTransceiverIntf : public NetNode
             sub_local_path_data = this->create_subscription<custom_interfaces::msg::LPathData>(
               ros_topics::LOCAL_PATH, ROS_Q_SIZE,
               std::bind(&LocalTransceiverIntf::sub_local_path_data_cb, this, std::placeholders::_1));
+
+            std::thread cache_thread(&LocalTransceiverIntf::getAndPublishCache, this);
+            cache_thread.detach();
         }
     }
 
@@ -105,6 +121,16 @@ class LocalTransceiverIntf : public NetNode
     rclcpp::Subscription<custom_interfaces::msg::GPS>::SharedPtr            sub_gps;
     rclcpp::Subscription<custom_interfaces::msg::LPathData>::SharedPtr      sub_local_path_data;
 
+    // may want mutex to ensure cached waypoints can't be published after newly received waypoints,
+    // but don't think practically necessary since pub_cb won't fire until 5 minutes after cache call.
+    void getAndPublishCache()
+    {
+        auto msg = lcl_trns_->getCache();
+        if (msg) {
+            pub_->publish(*msg);
+        }
+    }
+
     /**
      * @brief Callback function to publish to onboard ROS network
      *

src/network_systems/projects/local_transceiver/test/test_local_transceiver.cpp

@@ -16,6 +16,7 @@
 
 #include "at_cmds.h"
 #include "cmn_hdrs/shared_constants.h"
+#include "filesystem"
 #include "global_path.pb.h"
 #include "local_transceiver.h"
 #include "sensors.pb.h"
@@ -45,7 +46,12 @@ class TestLocalTransceiver : public ::testing::Test
         }
     }
 
-    static void TearDownTestSuite() { http_echo_server_proc_.terminate(); }
+    static void TearDownTestSuite()
+    {
+        http_echo_server_proc_.terminate();
+        std::filesystem::remove(CACHE_PATH);
+        std::filesystem::remove(CACHE_TEMP_PATH);
+    }
 
     TestLocalTransceiver()
     {
@@ -227,6 +233,73 @@ TEST_F(TestLocalTransceiver, parseInMsgValid)
     EXPECT_EQ(parsed_test.waypoints[1].longitude, holder);
 }
 
+TEST_F(TestLocalTransceiver, checkCache)
+{
+    constexpr float                                   holder_lat   = 14.3;
+    constexpr float                                   holder_long  = -177.6;
+    constexpr float                                   updated_lat  = 17.9;
+    constexpr float                                   updated_long = 0.1;
+    std::vector<custom_interfaces::msg::HelperLatLon> waypoints;
+
+    Polaris::GlobalPath path;
+
+    Polaris::Waypoint * waypoint_a = path.add_waypoints();
+    waypoint_a->set_latitude(holder_lat);
+    waypoint_a->set_longitude(holder_long);
+
+    Polaris::Waypoint * waypoint_b = path.add_waypoints();
+    waypoint_b->set_latitude(holder_lat);
+    waypoint_b->set_longitude(holder_long);
+
+    std::string           serialized_test = path.SerializeAsString();
+    std::filesystem::path cache{CACHE_PATH};
+
+    ASSERT_FALSE(std::filesystem::exists(cache));
+
+    std::future<void> future = LocalTransceiver::cacheGlobalWaypointsAsync(serialized_test);
+    future.get();
+
+    ASSERT_TRUE(std::filesystem::exists(cache));
+
+    auto cache_obj = LocalTransceiver::getCache();
+
+    ASSERT_TRUE(cache_obj);
+
+    custom_interfaces::msg::Path parsed_cache = *cache_obj;
+    custom_interfaces::msg::Path parsed_test  = LocalTransceiver::parseInMsg(serialized_test);
+
+    EXPECT_EQ(parsed_test.waypoints[0].latitude, parsed_cache.waypoints[0].latitude);
+    EXPECT_EQ(parsed_test.waypoints[0].longitude, parsed_cache.waypoints[0].longitude);
+    EXPECT_EQ(parsed_test.waypoints[1].latitude, parsed_cache.waypoints[0].latitude);
+    EXPECT_EQ(parsed_test.waypoints[1].longitude, parsed_cache.waypoints[0].longitude);
+
+    waypoint_a->set_latitude(updated_lat);
+    waypoint_a->set_longitude(updated_long);
+
+    waypoint_b->set_latitude(updated_lat);
+    waypoint_b->set_longitude(updated_long);
+
+    serialized_test = path.SerializeAsString();
+    future          = LocalTransceiver::cacheGlobalWaypointsAsync(serialized_test);
+    future.get();
+
+    std::filesystem::path cache_temp{CACHE_TEMP_PATH};
+    ASSERT_TRUE(std::filesystem::exists(cache));
+    ASSERT_FALSE(std::filesystem::exists(CACHE_TEMP_PATH));
+
+    cache_obj = LocalTransceiver::getCache();
+
+    ASSERT_TRUE(std::filesystem::exists(cache));
+    ASSERT_TRUE(cache_obj);
+    parsed_cache = *cache_obj;
+
+    parsed_test = LocalTransceiver::parseInMsg(serialized_test);
+    EXPECT_EQ(parsed_test.waypoints[0].latitude, parsed_cache.waypoints[0].latitude);
+    EXPECT_EQ(parsed_test.waypoints[0].longitude, parsed_cache.waypoints[0].longitude);
+    EXPECT_EQ(parsed_test.waypoints[1].latitude, parsed_cache.waypoints[0].latitude);
+    EXPECT_EQ(parsed_test.waypoints[1].longitude, parsed_cache.waypoints[0].longitude);
+}
+
 std::mutex port_mutex;
 
 TEST_F(TestLocalTransceiver, testMailboxBlackbox)