User/adambrett40/259 global waypoint cache #493
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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)) { | ||
|
There was a problem hiding this comment. please look into creating the cache file in the constructor of is it possible to clear the file and write it directly instead of creating another file and renaming it? please look into using |
||
| 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); | ||
|
There was a problem hiding this comment. https://stackoverflow.com/questions/5645375/how-do-i-make-a-function-asynchronous-in-c try the 2019 answer in the link above and see if that works |
||
|
|
||
| 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() | ||
|
There was a problem hiding this comment. please make this function async as well |
||
| { | ||
| 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; | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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"; | ||
| 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"; | ||
| 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); | ||
|
There was a problem hiding this comment. since the get function is now async, we do not need to launch it in a thread |
||
| 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 | ||
| * | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
could you revert these small formatting changes? they dont seem to be related
if another lead request this be done please include it in another PR