improving yasmin logs

yasmin/include/yasmin/blackboard/blackboard.hpp

@@ -24,6 +24,7 @@
 
 #include "yasmin/blackboard/blackboard_value.hpp"
 #include "yasmin/blackboard/blackboard_value_interface.hpp"
+#include "yasmin/logs.hpp"
 
 namespace yasmin {
 namespace blackboard {
@@ -41,6 +42,8 @@ class Blackboard {
 
   template <class T> T get(std::string name) {
 
+    YASMIN_LOG_DEBUG("Getting '%s' from the blackboard", name.c_str());
+
     std::lock_guard<std::recursive_mutex> lk(this->mutex);
 
     if (!this->contains(name)) {
@@ -54,6 +57,8 @@ class Blackboard {
 
   template <class T> void set(std::string name, T value) {
 
+    YASMIN_LOG_DEBUG("Setting '%s' in the blackboard", name.c_str());
+
     std::lock_guard<std::recursive_mutex> lk(this->mutex);
 
     if (!this->contains(name)) {

yasmin/include/yasmin/state.hpp

@@ -52,13 +52,20 @@ class State {
   std::vector<std::string> const &get_outcomes();
 
   virtual std::string to_string() {
-    int status = 0;
-    char *demangledName =
-        abi::__cxa_demangle(typeid(*this).name(), nullptr, nullptr, &status);
-    std::string className =
-        (status == 0) ? demangledName : typeid(*this).name();
-    free(demangledName);
-    return className;
+    std::string name = typeid(*this).name();
+
+#ifdef __GNUG__ // if using GCC/G++
+    int status;
+    // demangle the name using GCC's demangling function
+    char *demangled =
+        abi::__cxa_demangle(name.c_str(), nullptr, nullptr, &status);
+    if (status == 0) {
+      name = demangled;
+    }
+    free(demangled);
+#endif
+
+    return name;
   }
 };
 

yasmin/include/yasmin/state_machine.hpp

@@ -51,8 +51,6 @@ class StateMachine : public State {
   void set_start_state(std::string state_name);
   std::string get_start_state();
 
-  void cancel_state() override;
-
   std::map<std::string, std::shared_ptr<State>> const &get_states();
   std::map<std::string, std::map<std::string, std::string>> const &
   get_transitions();
@@ -78,6 +76,7 @@ class StateMachine : public State {
   std::string execute();
   std::string operator()();
   using State::operator();
+  void cancel_state() override;
 
   std::string to_string();
 

yasmin/src/yasmin/blackboard/blackboard.cpp

@@ -35,12 +35,18 @@ Blackboard::~Blackboard() {
 }
 
 void Blackboard::remove(std::string name) {
+
+  YASMIN_LOG_DEBUG("Removing '%s' from the blackboard", name.c_str());
+
   std::lock_guard<std::recursive_mutex> lk(this->mutex);
   delete this->values.at(name);
   this->values.erase(name);
 }
 
 bool Blackboard::contains(std::string name) {
+
+  YASMIN_LOG_DEBUG("Checking if '%s' is in the blackboard", name.c_str());
+
   std::lock_guard<std::recursive_mutex> lk(this->mutex);
   return (this->values.find(name) != this->values.end());
 }

yasmin/src/yasmin/cb_state.cpp

@@ -25,9 +25,7 @@ using namespace yasmin;
 CbState::CbState(
     std::vector<std::string> outcomes,
     std::string (*callback)(std::shared_ptr<blackboard::Blackboard> blackboard))
-    : State(outcomes) {
-  this->callback = callback;
-}
+    : State(outcomes), callback(callback) {}
 
 std::string
 CbState::execute(std::shared_ptr<blackboard::Blackboard> blackboard) {

yasmin/src/yasmin/state.cpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "yasmin/logs.hpp"
 #include "yasmin/state.hpp"
 
 using namespace yasmin;
@@ -27,6 +28,9 @@ State::State(std::vector<std::string> outcomes) { this->outcomes = outcomes; }
 
 std::string
 State::operator()(std::shared_ptr<blackboard::Blackboard> blackboard) {
+
+  YASMIN_LOG_DEBUG("Executing state %s", this->to_string().c_str());
+
   this->canceled.store(false);
   std::string outcome = this->execute(blackboard);
 

yasmin/src/yasmin/state_machine.cpp

@@ -70,11 +70,21 @@ void StateMachine::add_state(std::string name, std::shared_ptr<State> state,
     }
   }
 
+  std::string transition_string = "";
+
+  for (auto t : transitions) {
+    transition_string += "\n\t" + t.first + " --> " + t.second;
+  }
+
+  YASMIN_LOG_DEBUG("Adding state '%s' of type '%s' with transitions: %s",
+                   name.c_str(), state->to_string().c_str(),
+                   transition_string.c_str());
+
   this->states.insert({name, state});
   this->transitions.insert({name, transitions});
 
   if (this->start_state.empty()) {
-    this->start_state = name;
+    this->set_start_state(name);
   }
 }
 
@@ -92,20 +102,13 @@ void StateMachine::set_start_state(std::string state_name) {
                                 "' is not in the state machine");
   }
 
+  YASMIN_LOG_DEBUG("Setting start state to '%s'", state_name.c_str());
+
   this->start_state = state_name;
 }
 
 std::string StateMachine::get_start_state() { return this->start_state; }
 
-void StateMachine::cancel_state() {
-  State::cancel_state();
-
-  const std::lock_guard<std::mutex> lock(*this->current_state_mutex.get());
-  if (!this->current_state.empty()) {
-    this->states.at(this->current_state)->cancel_state();
-  }
-}
-
 std::map<std::string, std::shared_ptr<State>> const &
 StateMachine::get_states() {
   return this->states;
@@ -185,6 +188,8 @@ void StateMachine::call_end_cbs(
 
 void StateMachine::validate() {
 
+  YASMIN_LOG_DEBUG("Validating state machine");
+
   // check initial state
   if (this->start_state.empty()) {
     throw std::runtime_error("No initial state set");
@@ -310,8 +315,9 @@ StateMachine::execute(std::shared_ptr<blackboard::Blackboard> blackboard) {
       // outcome is a state
     } else if (this->states.find(outcome) != this->states.end()) {
 
-      YASMIN_LOG_INFO("%s (%s) --> %s", this->current_state.c_str(),
-                      old_outcome.c_str(), outcome.c_str());
+      YASMIN_LOG_INFO("State machine transitioning '%s' : '%s' --> '%s'",
+                      this->current_state.c_str(), old_outcome.c_str(),
+                      outcome.c_str());
       this->call_transition_cbs(blackboard, this->get_start_state(), outcome,
                                 old_outcome);
 
@@ -346,6 +352,15 @@ std::string StateMachine::operator()() {
   return this->operator()(blackboard);
 }
 
+void StateMachine::cancel_state() {
+  State::cancel_state();
+
+  const std::lock_guard<std::mutex> lock(*this->current_state_mutex.get());
+  if (!this->current_state.empty()) {
+    this->states.at(this->current_state)->cancel_state();
+  }
+}
+
 std::string StateMachine::to_string() {
 
   std::string result = "State Machine\n";

yasmin/yasmin/blackboard.py

@@ -17,6 +17,8 @@
 from typing import Any, Dict
 from threading import Lock
 
+import yasmin
+
 
 class Blackboard(object):
 
@@ -27,18 +29,30 @@ def __init__(self, init: Dict[str, Any] = None) -> None:
             self._data.update(init)
 
     def __getitem__(self, key) -> Any:
+
+        yasmin.YASMIN_LOG_DEBUG(f"Getting '{key}' from the blackboard")
+
         with self.__lock:
             return self._data[key]
 
     def __setitem__(self, key, value) -> None:
+
+        yasmin.YASMIN_LOG_DEBUG(f"Setting '{key}' in the blackboard")
+
         with self.__lock:
             self._data[key] = value
 
     def __delitem__(self, key) -> None:
+
+        yasmin.YASMIN_LOG_DEBUG(f"Removing '{key}' from the blackboard")
+
         with self.__lock:
             del self._data[key]
 
     def __contains__(self, key) -> bool:
+
+        yasmin.YASMIN_LOG_DEBUG(f"Checking if '{key}' is in the blackboard")
+
         with self.__lock:
             return key in self._data
 

yasmin/yasmin/state.py

@@ -16,6 +16,8 @@
 
 from typing import List
 from abc import ABC, abstractmethod
+
+import yasmin
 from yasmin.blackboard import Blackboard
 
 
@@ -31,6 +33,9 @@ def __init__(self, outcomes: List[str]) -> None:
             raise ValueError("There must be at least one outcome")
 
     def __call__(self, blackboard: Blackboard = None) -> str:
+
+        yasmin.YASMIN_LOG_INFO(f"Executing state {self}")
+
         self._canceled = False
 
         if blackboard is None:

yasmin/yasmin/state_machine.py

@@ -60,30 +60,34 @@ def add_state(
                     f"State '{name}' references unregistered outcomes '{key}', available outcomes are {state.get_outcomes()}"
                 )
 
+        transition_string = ""
+        for key in transitions:
+            transition_string += "\n\t" + key + " --> " + transitions[key]
+
+        yasmin.YASMIN_LOG_DEBUG(
+            f"Adding state '{name}' of type '{state}' with transitions: {transition_string}"
+        )
+
         self._states[name] = {"state": state, "transitions": transitions}
 
         if not self._start_state:
-            self._start_state = name
+            self.set_start_state(name)
 
-    def set_start_state(self, name: str) -> None:
+    def set_start_state(self, state_name: str) -> None:
 
-        if not name:
+        if not state_name:
             raise ValueError("Initial state cannot be empty")
 
-        elif name not in self._states:
-            raise KeyError(f"Initial state '{name}' is not in the state machine")
+        elif state_name not in self._states:
+            raise KeyError(f"Initial state '{state_name}' is not in the state machine")
+
+        yasmin.YASMIN_LOG_DEBUG(f"Setting start state to '{state_name}'")
 
-        self._start_state = name
+        self._start_state = state_name
 
     def get_start_state(self) -> str:
         return self._start_state
 
-    def cancel_state(self) -> None:
-        super().cancel_state()
-        with self.__current_state_lock:
-            if self.__current_state:
-                self._states[self.__current_state]["state"].cancel_state()
-
     def get_states(self) -> Dict[str, Union[State, Dict[str, str]]]:
         return self._states
 
@@ -138,6 +142,8 @@ def _call_end_cbs(self, blackboard: Blackboard, outcome: str) -> None:
 
     def validate(self) -> None:
 
+        yasmin.YASMIN_LOG_DEBUG("Validating state machine")
+
         # check initial state
         if not self._start_state:
             raise RuntimeError("No initial state set")
@@ -227,7 +233,7 @@ def execute(self, blackboard: Blackboard) -> str:
             # outcome is a state
             elif outcome in self._states:
                 yasmin.YASMIN_LOG_INFO(
-                    f"{self.__current_state} ({old_outcome}) --> {outcome}"
+                    f"State machine transitioning '{self.__current_state}' : {old_outcome} --> {outcome}"
                 )
 
                 self._call_transition_cbs(
@@ -246,5 +252,18 @@ def execute(self, blackboard: Blackboard) -> str:
                     f"Outcome '{outcome}' is not a state neither a state machine outcome"
                 )
 
+    def cancel_state(self) -> None:
+        super().cancel_state()
+        with self.__current_state_lock:
+            if self.__current_state:
+                self._states[self.__current_state]["state"].cancel_state()
+
     def __str__(self) -> str:
-        return f"StateMachine: {self._states}"
+        result = "State Machine\n"
+
+        for key in self._states:
+            result += f"{key} ({self._states[key]['state']})\n"
+            for tkey in self._states[key]["transitions"]:
+                result += f"\t{tkey} --> {self._states[key]['transitions'][tkey]}\n"
+
+        return result

yasmin_demos/src/action_client_demo.cpp

@@ -20,6 +20,7 @@
 #include "action_tutorials_interfaces/action/fibonacci.hpp"
 
 #include "yasmin/cb_state.hpp"
+#include "yasmin/logs.hpp"
 #include "yasmin/state_machine.hpp"
 #include "yasmin_ros/action_state.hpp"
 #include "yasmin_ros/basic_outcomes.hpp"
@@ -30,6 +31,7 @@
 using std::placeholders::_1;
 using std::placeholders::_2;
 using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
+using namespace yasmin;
 
 std::string
 print_result(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
@@ -99,7 +101,7 @@ class FibonacciState : public yasmin_ros::ActionState<Fibonacci> {
 
 int main(int argc, char *argv[]) {
 
-  std::cout << "yasmin_action_client_demo\n";
+  YASMIN_LOG_INFO("yasmin_action_client_demo");
   rclcpp::init(argc, argv);
 
   // set ROS 2 logs
@@ -131,7 +133,7 @@ int main(int argc, char *argv[]) {
 
   // execute
   std::string outcome = (*sm.get())(blackboard);
-  std::cout << outcome << "\n";
+  YASMIN_LOG_INFO(outcome.c_str());
 
   rclcpp::shutdown();