diff --git a/.gitignore b/.gitignore index b4a768c42..6baa6867f 100644 --- a/.gitignore +++ b/.gitignore @@ -192,4 +192,4 @@ cython_debug/ bag_files src/mil_common/perception/vision_stack/ -src/subjugator/mission_planner/logs/mission_debug.txt +src/subjugator/mission_planner/logs/ diff --git a/joe_shmore.py b/joe_shmore.py deleted file mode 100644 index f44975c7d..000000000 --- a/joe_shmore.py +++ /dev/null @@ -1,31 +0,0 @@ -import subprocess -import time - -import serial - -port = "/dev/ttyV0" -target = "vectornav" -baud = 921600 - - -def sub_online(): - try: - result = subprocess.run( - ["lsof", port], - stdout=subprocess.PIPE, - stderr=subprocess.DEVNULL, - text=True, - ) - return result.stdout - except Exception as e: - print(f"Error checking port: {e}") - - -# Run every second -while True: - if not sub_online(): - with serial.Serial("/dev/ttyV0", baud, timeout=1) as ser: - ser.reset_input_buffer() - ser.reset_output_buffer() - - time.sleep(0.5) diff --git a/src/subjugator/mission_planner/include/mission_summary_logger.hpp b/src/subjugator/mission_planner/include/mission_summary_logger.hpp new file mode 100644 index 000000000..07dddd19f --- /dev/null +++ b/src/subjugator/mission_planner/include/mission_summary_logger.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class MissionSummaryLogger : public BT::StatusChangeLogger +{ + public: + MissionSummaryLogger(const BT::Tree& tree) : BT::StatusChangeLogger(tree.rootNode()) + { + } + + ~MissionSummaryLogger() override + { + std::vector> rows(records_.begin(), records_.end()); + std::sort(rows.begin(), rows.end(), + [](auto const& a, auto const& b) { return a.second.start < b.second.start; }); + + std::ostringstream table; + table << "=== MISSION SUMMARY ===\n" + << std::left << std::setw(6) << "UID" << std::setw(26) << "Operation" << std::setw(10) << "Result" + << "Duration\n" + << std::string(54, '-') << "\n"; + + for (auto const& [uid, record] : rows) + { + table << std::left << std::setw(6) << uid << std::setw(26) << record.name << std::setw(10) + << BT::toStr(record.final_status) << std::fixed << std::setprecision(2) << record.elapsed.count() + << " s\n"; + } + std::cout << table.str(); + std::time_t now = std::time(nullptr); + char timestamp[64]; + std::strftime(timestamp, sizeof(timestamp), "mission_summary_%Y%m%d_%H%M%S.txt", std::localtime(&now)); + + std::string home = std::getenv("HOME"); + std::string log_dir = home + "/mil2/src/subjugator/mission_planner/logs"; + std::filesystem::create_directories(log_dir); + + std::string full_path = log_dir + "/" + timestamp; + std::ofstream file(full_path); + + if (file) + { + file << table.str(); + std::cout << "Summary written to " << full_path << "\n"; + } + } + + void callback(BT::Duration timestamp, const BT::TreeNode& node, BT::NodeStatus prev_status, + BT::NodeStatus status) override + { + if (prev_status == BT::NodeStatus::IDLE && status == BT::NodeStatus::RUNNING) + { + records_[node.UID()].name = node.name(); + records_[node.UID()].start = timestamp; + } + else if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE) + { + auto it = records_.find(node.UID()); + if (it == records_.end()) + { + return; + } + auto& record = it->second; + record.final_status = status; + record.elapsed = std::chrono::duration(timestamp - record.start); + } + } + + void flush() override + { + } + + private: + struct NodeRecord + { + std::string name; + BT::Duration start{}; + std::chrono::duration elapsed{}; + BT::NodeStatus final_status = BT::NodeStatus::IDLE; + }; + + std::unordered_map records_; +}; diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index d60046834..a564ab1b1 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -7,6 +7,7 @@ #include #include "context.hpp" +#include "mission_summary_logger.hpp" #include "start_coin_flip.hpp" #include "std_srvs/srv/set_bool.hpp" #include "subjugator_msgs/msg/thruster_efforts.hpp" @@ -124,6 +125,9 @@ int main(int argc, char** argv) // For live feed of tree BT::Groot2Publisher publisher(*tree_ptr); + // Summary logger for mission results + MissionSummaryLogger summary_logger(*tree_ptr); + // Log BT transitions to console BT::StdCoutLogger logger_cout(*tree_ptr);