User/adambrett40/259 global waypoint cache

src/network_systems/lib/cmn_hdrs/shared_constants.h

@@ -21,6 +21,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

@@ -26,6 +26,7 @@ class LocalTransceiver
     friend class TestLocalTransceiver_parseInMsgValid_Test;
     friend class TestLocalTransceiver_SendAndReceiveMessage;
     friend class TestLocalTransceiver_testMailboxBlackbox_Test;
+    friend class TestLocalTransceiver_checkCache_Test;
 
 public:
     /**
@@ -111,6 +112,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 void cacheGlobalWaypoints(std::string receivedDataBuffer);
+
     /**
      * @brief Retrieve the latest message from the remote server via the serial port
      *
@@ -121,6 +130,13 @@ 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();
+
 private:
     // Serial port read/write timeout
     constexpr static const struct timeval TIMEOUT

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"
@@ -85,6 +87,8 @@ LocalTransceiver::LocalTransceiver(const std::string & port_name, const uint32_t
     serial_.set_option(bio::serial_port_base::baud_rate(baud_rate));
     // Set a timeout for read/write operations on the serial port
     setsockopt(serial_.native_handle(), SOL_SOCKET, SO_RCVTIMEO, &TIMEOUT, sizeof(TIMEOUT));
+    std::ofstream cacheFile(CACHE_PATH);
+    cacheFile.close();
 };
 
 LocalTransceiver::~LocalTransceiver()
@@ -205,6 +209,19 @@ std::optional<std::string> LocalTransceiver::debugSend(const std::string & cmd)
     return readRsp();
 }
 
+void LocalTransceiver::cacheGlobalWaypoints(std::string receivedDataBuffer)
+{
+    std::filesystem::path cache{CACHE_PATH};
+    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);
+}
+
 custom_interfaces::msg::Path LocalTransceiver::receive()
 {
     static constexpr int MAX_NUM_RETRIES = 20;
@@ -305,7 +322,11 @@ custom_interfaces::msg::Path LocalTransceiver::receive()
         break;
     }
 
+    std::future<void> fut = std::async(std::launch::async, cacheGlobalWaypoints, receivedDataBuffer);
+
     custom_interfaces::msg::Path to_publish = parseInMsg(receivedDataBuffer);
+
+    fut.get();
     return to_publish;
 }
 
@@ -340,6 +361,17 @@ custom_interfaces::msg::Path LocalTransceiver::parseInMsg(const std::string & ms
     return soln;
 }
 
+std::optional<custom_interfaces::msg::Path> LocalTransceiver::getCache()
+{
+    if (std::filesystem::exists(CACHE_PATH) && std::filesystem::file_size(CACHE_PATH) > 0) {
+        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 std::make_optional(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>
@@ -53,16 +55,32 @@ 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;
+                const int   IOCTL_ERR_CODE = 256;
+                std::string set_baud_cmd   = "stty 19200 < $LOCAL_TRANSCEIVER_TEST_PORT 2>/dev/null";
+                //silence stderr to not clutter console while polling
+                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;
+                        }
+                        if (result != IOCTL_ERR_CODE) {
+                            std::cerr << "Failed to set baud rate with code: " << result << std::endl;
+                        }
+                    }
+                    std::this_thread::sleep_for(std::chrono::milliseconds(SLEEP_MS));
+                    attempts++;
                 }
-                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";
+
+                if (attempts == MAX_ATTEMPTS) {
+                    std::string msg = "Error: could not start virtual iridium";
                     std::cerr << msg << std::endl;
                     throw std::exception();
                 }
@@ -80,6 +98,9 @@ class LocalTransceiverIntf : public NetNode
               this->get_logger(), "Running Local Transceiver in mode: %s, with port: %s.", mode.c_str(), port.c_str());
             lcl_trns_ = std::make_unique<LocalTransceiver>(port, SATELLITE_BAUD_RATE);
 
+            std::future<std::optional<custom_interfaces::msg::Path>> fut =
+              std::async(std::launch::async, lcl_trns_->getCache);
+
             static constexpr int  ROS_Q_SIZE     = 5;
             static constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(300000);
             timer_ = this->create_wall_timer(TIMER_INTERVAL, std::bind(&LocalTransceiverIntf::pub_cb, this));
@@ -100,6 +121,11 @@ 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::optional<custom_interfaces::msg::Path> msg = fut.get();
+            if (msg) {
+                pub_->publish(*msg);
+            }
         }
     }
 

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_TRUE(std::filesystem::exists(cache));
+    ASSERT_TRUE(std::filesystem::file_size(CACHE_PATH) == 0);
+
+    LocalTransceiver::cacheGlobalWaypoints(serialized_test);
+
+    ASSERT_TRUE(std::filesystem::exists(cache));
+    ASSERT_TRUE(std::filesystem::file_size(CACHE_PATH) > 0);
+
+    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();
+    LocalTransceiver::cacheGlobalWaypoints(serialized_test);
+
+    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)