Mission summary logger

.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/

src/subjugator/mission_planner/include/mission_summary_logger.hpp

@@ -0,0 +1,97 @@
+#pragma once
+
+#include <behaviortree_cpp/loggers/abstract_logger.h>
+
+#include <algorithm>
+#include <chrono>
+#include <cstdlib>
+#include <ctime>
+#include <filesystem>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+class MissionSummaryLogger : public BT::StatusChangeLogger
+{
+  public:
+    MissionSummaryLogger(const BT::Tree& tree) : BT::StatusChangeLogger(tree.rootNode())
+    {
+    }
+
+    ~MissionSummaryLogger() override
+    {
+        std::vector<std::pair<uint16_t, NodeRecord>> 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<double>(timestamp - record.start);
+        }
+    }
+
+    void flush() override
+    {
+    }
+
+  private:
+    struct NodeRecord
+    {
+        std::string name;
+        BT::Duration start{};
+        std::chrono::duration<double> elapsed{};
+        BT::NodeStatus final_status = BT::NodeStatus::IDLE;
+    };
+
+    std::unordered_map<uint16_t, NodeRecord> records_;
+};

src/subjugator/mission_planner/src/mission_planner_node.cpp

@@ -7,6 +7,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #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);