From 08b213858baa7057961494fd86b14404640722e9 Mon Sep 17 00:00:00 2001
From: Kurt Wilson <kurt4wilson@gmail.com>
Date: Mon, 22 May 2023 11:07:45 -0400
Subject: [PATCH] quick test to make the the queue behaves

---
 .../priority_memory_strategy.hpp              |  5 +++-
 .../src/priority_memory_strategy.cpp          | 27 ++++++++++++++-----
 2 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp b/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp
index 17a6b24..4c78abc 100644
--- a/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp
+++ b/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp
@@ -95,6 +95,8 @@ public:
 
     static size_t num_executables;
     int executable_id = 0;
+
+    std::string name = "";
 };
 
 class PriorityExecutableComparator
@@ -266,11 +268,12 @@ public:
         priority_map[handle]->chain_id = chain_index;
     }
 
-    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, int chain_id = 0)
+    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, int chain_id = 0, std::string name = "")
     {
         // TODO: any sanity checks should go here
         priority_map[handle] = std::make_shared<PriorityExecutable>(handle, period, t, DEADLINE);
         priority_map[handle]->chain_id = chain_id;
+        priority_map[handle]->name = name;
     }
 
     int get_priority(std::shared_ptr<const void> executable)
diff --git a/src/priority_executor/src/priority_memory_strategy.cpp b/src/priority_executor/src/priority_memory_strategy.cpp
index 8f8ddf0..2c84e0d 100644
--- a/src/priority_executor/src/priority_memory_strategy.cpp
+++ b/src/priority_executor/src/priority_memory_strategy.cpp
@@ -293,7 +293,8 @@ bool PriorityMemoryStrategy<>::collect_entities(const WeakNodeList &weak_nodes)
         for (auto &weak_group : node->get_callback_groups())
         {
             auto group = weak_group.lock();
-            if (!group || !group->can_be_taken_from().load())
+            if (!group)
+            // if (!group || !group->can_be_taken_from().load())
             {
                 continue;
             }
@@ -429,11 +430,21 @@ void PriorityMemoryStrategy<>::get_next_executable(
 
     const PriorityExecutable *next_exec = nullptr;
     // std::cout << "all_executables_.size():" << all_executables_.size() << std::endl;
-    // print all_executables_
-    // for (auto exec : all_executables_)
-    // {
-    //     std::cout << exec << ": " << exec->handle << " : " << exec->sched_type << std::endl;
-    // }
+    // log contents of all_executables_
+    // std::cout << exec->name << ": " << exec->handle << " : " << exec->sched_type << std::endl;
+    std::ostringstream oss;
+    oss << "{\"operation\":\"get_next_executable\"";
+    // output names and handles of all_executables_ in json array
+    oss << ",\"all_executables\":[";
+    for (auto exec : all_executables_)
+    {
+        // if (exec->can_be_run)
+        oss << "{\"name\":\"" << exec->name << "\", \"sched_type\":\"" << exec->sched_type << "\", \"can_be_run\":\"" << exec->can_be_run << "\"},";
+    }
+    // remove trailing comma
+    oss.seekp(-1, oss.cur);
+    oss << "]}";
+    log_entry(logger_, oss.str());
 
     // while (!all_executables_.empty())
     for (auto exec : all_executables_)
@@ -596,7 +607,9 @@ void PriorityMemoryStrategy<>::get_next_executable(
         }
         // returning with an executable
         // remove from all_executables_ map
-        all_executables_.erase(exec);
+        all_executables_.erase(next_exec);
+        std::ostringstream oss;
+        oss << "{\"operation\": \"select_task\", \"task\": \"" << exec->name << "\"}";
         return;
     }
 }