From 6d56dff794b346d9d15ef2eb998ee094e8987cc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20=C3=81ngel=20Gonz=C3=A1lez=20Santamarta?= Date: Wed, 30 Oct 2024 22:36:40 +0100 Subject: [PATCH] improving yasmin logs --- README.md | 136 +++++++++++------- .../include/yasmin/blackboard/blackboard.hpp | 5 + yasmin/include/yasmin/state.hpp | 21 ++- yasmin/include/yasmin/state_machine.hpp | 3 +- yasmin/src/yasmin/blackboard/blackboard.cpp | 6 + yasmin/src/yasmin/cb_state.cpp | 4 +- yasmin/src/yasmin/state.cpp | 4 + yasmin/src/yasmin/state_machine.cpp | 39 +++-- yasmin/yasmin/blackboard.py | 14 ++ yasmin/yasmin/state.py | 5 + yasmin/yasmin/state_machine.py | 47 ++++-- yasmin_demos/src/action_client_demo.cpp | 6 +- yasmin_demos/src/monitor_demo.cpp | 13 +- yasmin_demos/src/service_client_demo.cpp | 6 +- yasmin_demos/src/yasmin_demo.cpp | 13 +- .../yasmin_demos/action_client_demo.py | 19 ++- yasmin_demos/yasmin_demos/monitor_demo.py | 7 +- yasmin_demos/yasmin_demos/nav_demo.py | 30 +++- .../yasmin_demos/service_client_demo.py | 13 +- yasmin_demos/yasmin_demos/yasmin_demo.py | 26 +++- 20 files changed, 287 insertions(+), 130 deletions(-) diff --git a/README.md b/README.md index f1eb4d3..bee58e2 100644 --- a/README.md +++ b/README.md @@ -106,6 +106,7 @@ $ ros2 run yasmin_demos yasmin_demo.py import time import rclpy +import yasmin from yasmin import State from yasmin import Blackboard from yasmin import StateMachine @@ -120,7 +121,7 @@ class FooState(State): self.counter = 0 def execute(self, blackboard: Blackboard) -> str: - print("Executing state FOO") + yasmin.YASMIN_LOG_INFO("Executing state FOO") time.sleep(3) if self.counter < 3: @@ -137,17 +138,17 @@ class BarState(State): super().__init__(outcomes=["outcome3"]) def execute(self, blackboard: Blackboard) -> str: - print("Executing state BAR") + yasmin.YASMIN_LOG_INFO("Executing state BAR") time.sleep(3) - print(blackboard["foo_str"]) + yasmin.YASMIN_LOG_INFO(blackboard["foo_str"]) return "outcome3" # main def main(): - print("yasmin_demo") + yasmin.YASMIN_LOG_INFO("yasmin_demo") # init ROS 2 rclpy.init() @@ -160,16 +161,26 @@ def main(): # add states sm.add_state( - "FOO", FooState(), transitions={"outcome1": "BAR", "outcome2": "outcome4"} + "FOO", + FooState(), + transitions={ + "outcome1": "BAR", + "outcome2": "outcome4", + }, + ) + sm.add_state( + "BAR", + BarState(), + transitions={ + "outcome3": "FOO", + }, ) - sm.add_state("BAR", BarState(), transitions={"outcome3": "FOO"}) - # pub FSM info YasminViewerPub("yasmin_demo", sm) # execute FSM outcome = sm() - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() @@ -198,6 +209,7 @@ $ ros2 run yasmin_demos service_client_demo.py import rclpy from example_interfaces.srv import AddTwoInts +import yasmin from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -239,14 +251,14 @@ def set_ints(blackboard: Blackboard) -> str: def print_sum(blackboard: Blackboard) -> str: - print(f"Sum: {blackboard['sum']}") + yasmin.YASMIN_LOG_INFO(f"Sum: {blackboard['sum']}") return SUCCEED # main def main(): - print("yasmin_service_client_demo") + yasmin.YASMIN_LOG_INFO("yasmin_service_client_demo") # init ROS 2 rclpy.init() @@ -273,7 +285,11 @@ def main(): }, ) sm.add_state( - "PRINTING_SUM", CbState([SUCCEED], print_sum), transitions={SUCCEED: "outcome4"} + "PRINTING_SUM", + CbState([SUCCEED], print_sum), + transitions={ + SUCCEED: "outcome4", + }, ) # pub FSM info @@ -281,7 +297,7 @@ def main(): # execute FSM outcome = sm() - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() @@ -310,6 +326,7 @@ $ ros2 run yasmin_demos action_client_demo.py import rclpy from action_tutorials_interfaces.action import Fibonacci +import yasmin from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -346,18 +363,18 @@ class FibonacciState(ActionState): def print_feedback( self, blackboard: Blackboard, feedback: Fibonacci.Feedback ) -> None: - print(f"Received feedback: {list(feedback.partial_sequence)}") + yasmin.YASMIN_LOG_INFO(f"Received feedback: {list(feedback.partial_sequence)}") def print_result(blackboard: Blackboard) -> str: - print(f"Result: {blackboard['fibo_res']}") + yasmin.YASMIN_LOG_INFO(f"Result: {blackboard['fibo_res']}") return SUCCEED # main def main(): - print("yasmin_action_client_demo") + yasmin.YASMIN_LOG_INFO("yasmin_action_client_demo") # init ROS 2 rclpy.init() @@ -372,12 +389,18 @@ def main(): sm.add_state( "CALLING_FIBONACCI", FibonacciState(), - transitions={SUCCEED: "PRINTING_RESULT", CANCEL: "outcome4", ABORT: "outcome4"}, + transitions={ + SUCCEED: "PRINTING_RESULT", + CANCEL: "outcome4", + ABORT: "outcome4", + }, ) sm.add_state( "PRINTING_RESULT", CbState([SUCCEED], print_result), - transitions={SUCCEED: "outcome4"}, + transitions={ + SUCCEED: "outcome4", + }, ) # pub FSM info @@ -389,7 +412,7 @@ def main(): # execute FSM outcome = sm(blackboard) - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() @@ -415,6 +438,7 @@ import rclpy from rclpy.qos import qos_profile_sensor_data from nav_msgs.msg import Odometry +import yasmin from yasmin import Blackboard from yasmin import StateMachine from yasmin_ros import MonitorState @@ -438,7 +462,7 @@ class PrintOdometryState(MonitorState): self.times = times def monitor_handler(self, blackboard: Blackboard, msg: Odometry) -> str: - print(msg) + yasmin.YASMIN_LOG_INFO(msg) self.times -= 1 @@ -451,7 +475,7 @@ class PrintOdometryState(MonitorState): # main def main(): - print("yasmin_monitor_demo") + yasmin.YASMIN_LOG_INFO("yasmin_monitor_demo") # init ROS 2 rclpy.init() @@ -478,7 +502,7 @@ def main(): # execute FSM outcome = sm() - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() @@ -506,6 +530,7 @@ import rclpy from geometry_msgs.msg import Pose from nav2_msgs.action import NavigateToPose +import yasmin from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -578,7 +603,7 @@ def get_next_waypoint(blackboard: Blackboard) -> str: # main def main(): - print("yasmin_nav2_demo") + yasmin.YASMIN_LOG_INFO("yasmin_nav2_demo") # init ROS 2 rclpy.init() @@ -594,29 +619,34 @@ def main(): sm.add_state( "CREATING_WAYPOINTS", CbState([SUCCEED], create_waypoints), - transitions={SUCCEED: "TAKING_RANDOM_WAYPOINTS"}, + transitions={ + SUCCEED: "TAKING_RANDOM_WAYPOINTS", + }, ) sm.add_state( "TAKING_RANDOM_WAYPOINTS", CbState([SUCCEED], take_random_waypoint), - transitions={SUCCEED: "NAVIGATING"}, + transitions={ + SUCCEED: "NAVIGATING", + }, ) nav_sm.add_state( "GETTING_NEXT_WAYPOINT", CbState([END, HAS_NEXT], get_next_waypoint), - transitions={END: SUCCEED, HAS_NEXT: "NAVIGATING"}, + transitions={ + END: SUCCEED, + HAS_NEXT: "NAVIGATING", + }, ) nav_sm.add_state( "NAVIGATING", Nav2State(), - transitions={SUCCEED: "GETTING_NEXT_WAYPOINT", CANCEL: CANCEL, ABORT: ABORT}, - ) - - sm.add_state( - "NAVIGATING", - nav_sm, - transitions={SUCCEED: SUCCEED, CANCEL: CANCEL, ABORT: ABORT}, + transitions={ + SUCCEED: "GETTING_NEXT_WAYPOINT", + CANCEL: CANCEL, + ABORT: ABORT, + }, ) # pub FSM info @@ -626,7 +656,7 @@ def main(): blackboard = Blackboard() blackboard["waypoints_num"] = 2 outcome = sm(blackboard) - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() @@ -657,11 +687,14 @@ $ ros2 run yasmin_demos yasmin_demo #include "rclcpp/rclcpp.hpp" +#include "yasmin/logs.hpp" #include "yasmin/state.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/ros_logs.hpp" #include "yasmin_viewer/yasmin_viewer_pub.hpp" +using namespace yasmin; + // define state Foo class FooState : public yasmin::State { public: @@ -671,7 +704,7 @@ public: std::string execute(std::shared_ptr blackboard) { - std::cout << "Executing state FOO\n"; + YASMIN_LOG_INFO("Executing state FOO"); std::this_thread::sleep_for(std::chrono::seconds(3)); if (this->counter < 3) { @@ -693,10 +726,10 @@ public: std::string execute(std::shared_ptr blackboard) { - std::cout << "Executing state BAR\n"; + YASMIN_LOG_INFO("Executing state BAR"); std::this_thread::sleep_for(std::chrono::seconds(3)); - std::cout << blackboard->get("foo_str") << "\n"; + YASMIN_LOG_INFO(blackboard->get("foo_str").c_str()); return "outcome3"; } @@ -704,7 +737,7 @@ public: int main(int argc, char *argv[]) { - std::cout << "yasmin_demo\n"; + YASMIN_LOG_INFO("yasmin_demo"); rclcpp::init(argc, argv); // set ROS 2 logs @@ -724,7 +757,7 @@ int main(int argc, char *argv[]) { // execute std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + YASMIN_LOG_INFO(outcome.c_str()); rclcpp::shutdown(); @@ -756,6 +789,7 @@ $ ros2 run yasmin_demos service_client_demo #include "rclcpp/rclcpp.hpp" #include "yasmin/cb_state.hpp" +#include "yasmin/logs.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/basic_outcomes.hpp" #include "yasmin_ros/ros_logs.hpp" @@ -764,6 +798,7 @@ $ ros2 run yasmin_demos service_client_demo using std::placeholders::_1; using std::placeholders::_2; +using namespace yasmin; std::string set_ints(std::shared_ptr blackboard) { @@ -815,7 +850,7 @@ public: int main(int argc, char *argv[]) { - std::cout << "yasmin_service_client_demo\n"; + YASMIN_LOG_INFO("yasmin_service_client_demo"); rclcpp::init(argc, argv); // set ROS 2 logs @@ -848,7 +883,7 @@ int main(int argc, char *argv[]) { // execute std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + YASMIN_LOG_INFO(outcome.c_str()); rclcpp::shutdown(); @@ -872,8 +907,6 @@ $ ros2 run yasmin_demos action_client_demo Click to expand ```cpp -// along with this program. If not, see . - #include #include #include @@ -881,6 +914,7 @@ $ ros2 run yasmin_demos action_client_demo #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" @@ -891,6 +925,7 @@ $ ros2 run yasmin_demos action_client_demo 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 blackboard) { @@ -960,7 +995,7 @@ public: 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 @@ -992,7 +1027,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(); @@ -1020,6 +1055,7 @@ $ ros2 run yasmin_demos monitor_demo #include "nav_msgs/msg/odometry.hpp" +#include "yasmin/logs.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/basic_outcomes.hpp" #include "yasmin_ros/monitor_state.hpp" @@ -1028,6 +1064,7 @@ $ ros2 run yasmin_demos monitor_demo using std::placeholders::_1; using std::placeholders::_2; +using namespace yasmin; class PrintOdometryState : public yasmin_ros::MonitorState { @@ -1055,10 +1092,9 @@ public: (void)blackboard; - std::cout << "x: " << msg->pose.pose.position.x << "\n"; - std::cout << "y: " << msg->pose.pose.position.y << "\n"; - std::cout << "z: " << msg->pose.pose.position.z << "\n"; - std::cout << "\n"; + YASMIN_LOG_INFO("x: %d", msg->pose.pose.position.x); + YASMIN_LOG_INFO("y: %d", msg->pose.pose.position.y); + YASMIN_LOG_INFO("z: %d", msg->pose.pose.position.z); this->times--; @@ -1072,7 +1108,7 @@ public: int main(int argc, char *argv[]) { - std::cout << "yasmin_monitor_demo\n"; + YASMIN_LOG_INFO("yasmin_monitor_demo"); rclcpp::init(argc, argv); // set ROS 2 logs @@ -1093,7 +1129,7 @@ int main(int argc, char *argv[]) { // execute std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + YASMIN_LOG_INFO(outcome.c_str()); rclcpp::shutdown(); diff --git a/yasmin/include/yasmin/blackboard/blackboard.hpp b/yasmin/include/yasmin/blackboard/blackboard.hpp index bcce283..4d10ce8 100644 --- a/yasmin/include/yasmin/blackboard/blackboard.hpp +++ b/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 T get(std::string name) { + YASMIN_LOG_DEBUG("Getting '%s' from the blackboard", name.c_str()); + std::lock_guard lk(this->mutex); if (!this->contains(name)) { @@ -54,6 +57,8 @@ class Blackboard { template void set(std::string name, T value) { + YASMIN_LOG_DEBUG("Setting '%s' in the blackboard", name.c_str()); + std::lock_guard lk(this->mutex); if (!this->contains(name)) { diff --git a/yasmin/include/yasmin/state.hpp b/yasmin/include/yasmin/state.hpp index 6c4c253..80b5ee7 100644 --- a/yasmin/include/yasmin/state.hpp +++ b/yasmin/include/yasmin/state.hpp @@ -52,13 +52,20 @@ class State { std::vector 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; } }; diff --git a/yasmin/include/yasmin/state_machine.hpp b/yasmin/include/yasmin/state_machine.hpp index 89f0087..9606424 100644 --- a/yasmin/include/yasmin/state_machine.hpp +++ b/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> const &get_states(); std::map> 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(); diff --git a/yasmin/src/yasmin/blackboard/blackboard.cpp b/yasmin/src/yasmin/blackboard/blackboard.cpp index 5f1a894..e36da1a 100644 --- a/yasmin/src/yasmin/blackboard/blackboard.cpp +++ b/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 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 lk(this->mutex); return (this->values.find(name) != this->values.end()); } diff --git a/yasmin/src/yasmin/cb_state.cpp b/yasmin/src/yasmin/cb_state.cpp index b73ad32..e0b4245 100644 --- a/yasmin/src/yasmin/cb_state.cpp +++ b/yasmin/src/yasmin/cb_state.cpp @@ -25,9 +25,7 @@ using namespace yasmin; CbState::CbState( std::vector outcomes, std::string (*callback)(std::shared_ptr blackboard)) - : State(outcomes) { - this->callback = callback; -} + : State(outcomes), callback(callback) {} std::string CbState::execute(std::shared_ptr blackboard) { diff --git a/yasmin/src/yasmin/state.cpp b/yasmin/src/yasmin/state.cpp index 90c4470..132fc41 100644 --- a/yasmin/src/yasmin/state.cpp +++ b/yasmin/src/yasmin/state.cpp @@ -19,6 +19,7 @@ #include #include +#include "yasmin/logs.hpp" #include "yasmin/state.hpp" using namespace yasmin; @@ -27,6 +28,9 @@ State::State(std::vector outcomes) { this->outcomes = outcomes; } std::string State::operator()(std::shared_ptr blackboard) { + + YASMIN_LOG_DEBUG("Executing state %s", this->to_string().c_str()); + this->canceled.store(false); std::string outcome = this->execute(blackboard); diff --git a/yasmin/src/yasmin/state_machine.cpp b/yasmin/src/yasmin/state_machine.cpp index a60d5cf..0914fe2 100644 --- a/yasmin/src/yasmin/state_machine.cpp +++ b/yasmin/src/yasmin/state_machine.cpp @@ -70,11 +70,21 @@ void StateMachine::add_state(std::string name, std::shared_ptr 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 lock(*this->current_state_mutex.get()); - if (!this->current_state.empty()) { - this->states.at(this->current_state)->cancel_state(); - } -} - std::map> 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) { // 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 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"; diff --git a/yasmin/yasmin/blackboard.py b/yasmin/yasmin/blackboard.py index 63d9caa..7b88386 100644 --- a/yasmin/yasmin/blackboard.py +++ b/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 diff --git a/yasmin/yasmin/state.py b/yasmin/yasmin/state.py index 50b2540..c55cc98 100644 --- a/yasmin/yasmin/state.py +++ b/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: diff --git a/yasmin/yasmin/state_machine.py b/yasmin/yasmin/state_machine.py index a52f95c..0f80e1b 100644 --- a/yasmin/yasmin/state_machine.py +++ b/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 diff --git a/yasmin_demos/src/action_client_demo.cpp b/yasmin_demos/src/action_client_demo.cpp index effe5f5..f2946e8 100644 --- a/yasmin_demos/src/action_client_demo.cpp +++ b/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 blackboard) { @@ -99,7 +101,7 @@ class FibonacciState : public yasmin_ros::ActionState { 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(); diff --git a/yasmin_demos/src/monitor_demo.cpp b/yasmin_demos/src/monitor_demo.cpp index 1d2efa1..90eba0c 100644 --- a/yasmin_demos/src/monitor_demo.cpp +++ b/yasmin_demos/src/monitor_demo.cpp @@ -21,6 +21,7 @@ #include "nav_msgs/msg/odometry.hpp" +#include "yasmin/logs.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/basic_outcomes.hpp" #include "yasmin_ros/monitor_state.hpp" @@ -29,6 +30,7 @@ using std::placeholders::_1; using std::placeholders::_2; +using namespace yasmin; class PrintOdometryState : public yasmin_ros::MonitorState { @@ -56,10 +58,9 @@ class PrintOdometryState (void)blackboard; - std::cout << "x: " << msg->pose.pose.position.x << "\n"; - std::cout << "y: " << msg->pose.pose.position.y << "\n"; - std::cout << "z: " << msg->pose.pose.position.z << "\n"; - std::cout << "\n"; + YASMIN_LOG_INFO("x: %d", msg->pose.pose.position.x); + YASMIN_LOG_INFO("y: %d", msg->pose.pose.position.y); + YASMIN_LOG_INFO("z: %d", msg->pose.pose.position.z); this->times--; @@ -73,7 +74,7 @@ class PrintOdometryState int main(int argc, char *argv[]) { - std::cout << "yasmin_monitor_demo\n"; + YASMIN_LOG_INFO("yasmin_monitor_demo"); rclcpp::init(argc, argv); // set ROS 2 logs @@ -94,7 +95,7 @@ int main(int argc, char *argv[]) { // execute std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + YASMIN_LOG_INFO(outcome.c_str()); rclcpp::shutdown(); diff --git a/yasmin_demos/src/service_client_demo.cpp b/yasmin_demos/src/service_client_demo.cpp index 24d872f..111f14f 100644 --- a/yasmin_demos/src/service_client_demo.cpp +++ b/yasmin_demos/src/service_client_demo.cpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "yasmin/cb_state.hpp" +#include "yasmin/logs.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/basic_outcomes.hpp" #include "yasmin_ros/ros_logs.hpp" @@ -29,6 +30,7 @@ using std::placeholders::_1; using std::placeholders::_2; +using namespace yasmin; std::string set_ints(std::shared_ptr blackboard) { @@ -80,7 +82,7 @@ class AddTwoIntsState int main(int argc, char *argv[]) { - std::cout << "yasmin_service_client_demo\n"; + YASMIN_LOG_INFO("yasmin_service_client_demo"); rclcpp::init(argc, argv); // set ROS 2 logs @@ -113,7 +115,7 @@ int main(int argc, char *argv[]) { // execute std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + YASMIN_LOG_INFO(outcome.c_str()); rclcpp::shutdown(); diff --git a/yasmin_demos/src/yasmin_demo.cpp b/yasmin_demos/src/yasmin_demo.cpp index 3b6322d..3da1d65 100644 --- a/yasmin_demos/src/yasmin_demo.cpp +++ b/yasmin_demos/src/yasmin_demo.cpp @@ -20,11 +20,14 @@ #include "rclcpp/rclcpp.hpp" +#include "yasmin/logs.hpp" #include "yasmin/state.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/ros_logs.hpp" #include "yasmin_viewer/yasmin_viewer_pub.hpp" +using namespace yasmin; + // define state Foo class FooState : public yasmin::State { public: @@ -34,7 +37,7 @@ class FooState : public yasmin::State { std::string execute(std::shared_ptr blackboard) { - std::cout << "Executing state FOO\n"; + YASMIN_LOG_INFO("Executing state FOO"); std::this_thread::sleep_for(std::chrono::seconds(3)); if (this->counter < 3) { @@ -56,10 +59,10 @@ class BarState : public yasmin::State { std::string execute(std::shared_ptr blackboard) { - std::cout << "Executing state BAR\n"; + YASMIN_LOG_INFO("Executing state BAR"); std::this_thread::sleep_for(std::chrono::seconds(3)); - std::cout << blackboard->get("foo_str") << "\n"; + YASMIN_LOG_INFO(blackboard->get("foo_str").c_str()); return "outcome3"; } @@ -67,7 +70,7 @@ class BarState : public yasmin::State { int main(int argc, char *argv[]) { - std::cout << "yasmin_demo\n"; + YASMIN_LOG_INFO("yasmin_demo"); rclcpp::init(argc, argv); // set ROS 2 logs @@ -87,7 +90,7 @@ int main(int argc, char *argv[]) { // execute std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + YASMIN_LOG_INFO(outcome.c_str()); rclcpp::shutdown(); diff --git a/yasmin_demos/yasmin_demos/action_client_demo.py b/yasmin_demos/yasmin_demos/action_client_demo.py index 99347d9..1f82a4d 100755 --- a/yasmin_demos/yasmin_demos/action_client_demo.py +++ b/yasmin_demos/yasmin_demos/action_client_demo.py @@ -19,6 +19,7 @@ import rclpy from action_tutorials_interfaces.action import Fibonacci +import yasmin from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -55,18 +56,18 @@ def response_handler( def print_feedback( self, blackboard: Blackboard, feedback: Fibonacci.Feedback ) -> None: - print(f"Received feedback: {list(feedback.partial_sequence)}") + yasmin.YASMIN_LOG_INFO(f"Received feedback: {list(feedback.partial_sequence)}") def print_result(blackboard: Blackboard) -> str: - print(f"Result: {blackboard['fibo_res']}") + yasmin.YASMIN_LOG_INFO(f"Result: {blackboard['fibo_res']}") return SUCCEED # main def main(): - print("yasmin_action_client_demo") + yasmin.YASMIN_LOG_INFO("yasmin_action_client_demo") # init ROS 2 rclpy.init() @@ -81,12 +82,18 @@ def main(): sm.add_state( "CALLING_FIBONACCI", FibonacciState(), - transitions={SUCCEED: "PRINTING_RESULT", CANCEL: "outcome4", ABORT: "outcome4"}, + transitions={ + SUCCEED: "PRINTING_RESULT", + CANCEL: "outcome4", + ABORT: "outcome4", + }, ) sm.add_state( "PRINTING_RESULT", CbState([SUCCEED], print_result), - transitions={SUCCEED: "outcome4"}, + transitions={ + SUCCEED: "outcome4", + }, ) # pub FSM info @@ -98,7 +105,7 @@ def main(): # execute FSM outcome = sm(blackboard) - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demos/yasmin_demos/monitor_demo.py b/yasmin_demos/yasmin_demos/monitor_demo.py index 42915d6..59f2c62 100755 --- a/yasmin_demos/yasmin_demos/monitor_demo.py +++ b/yasmin_demos/yasmin_demos/monitor_demo.py @@ -20,6 +20,7 @@ from rclpy.qos import qos_profile_sensor_data from nav_msgs.msg import Odometry +import yasmin from yasmin import Blackboard from yasmin import StateMachine from yasmin_ros import MonitorState @@ -43,7 +44,7 @@ def __init__(self, times: int) -> None: self.times = times def monitor_handler(self, blackboard: Blackboard, msg: Odometry) -> str: - print(msg) + yasmin.YASMIN_LOG_INFO(msg) self.times -= 1 @@ -56,7 +57,7 @@ def monitor_handler(self, blackboard: Blackboard, msg: Odometry) -> str: # main def main(): - print("yasmin_monitor_demo") + yasmin.YASMIN_LOG_INFO("yasmin_monitor_demo") # init ROS 2 rclpy.init() @@ -83,7 +84,7 @@ def main(): # execute FSM outcome = sm() - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demos/yasmin_demos/nav_demo.py b/yasmin_demos/yasmin_demos/nav_demo.py index 4a126ea..1ba9f7b 100755 --- a/yasmin_demos/yasmin_demos/nav_demo.py +++ b/yasmin_demos/yasmin_demos/nav_demo.py @@ -22,6 +22,7 @@ from geometry_msgs.msg import Pose from nav2_msgs.action import NavigateToPose +import yasmin from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -94,7 +95,7 @@ def get_next_waypoint(blackboard: Blackboard) -> str: # main def main(): - print("yasmin_nav2_demo") + yasmin.YASMIN_LOG_INFO("yasmin_nav2_demo") # init ROS 2 rclpy.init() @@ -110,29 +111,44 @@ def main(): sm.add_state( "CREATING_WAYPOINTS", CbState([SUCCEED], create_waypoints), - transitions={SUCCEED: "TAKING_RANDOM_WAYPOINTS"}, + transitions={ + SUCCEED: "TAKING_RANDOM_WAYPOINTS", + }, ) sm.add_state( "TAKING_RANDOM_WAYPOINTS", CbState([SUCCEED], take_random_waypoint), - transitions={SUCCEED: "NAVIGATING"}, + transitions={ + SUCCEED: "NAVIGATING", + }, ) nav_sm.add_state( "GETTING_NEXT_WAYPOINT", CbState([END, HAS_NEXT], get_next_waypoint), - transitions={END: SUCCEED, HAS_NEXT: "NAVIGATING"}, + transitions={ + END: SUCCEED, + HAS_NEXT: "NAVIGATING", + }, ) nav_sm.add_state( "NAVIGATING", Nav2State(), - transitions={SUCCEED: "GETTING_NEXT_WAYPOINT", CANCEL: CANCEL, ABORT: ABORT}, + transitions={ + SUCCEED: "GETTING_NEXT_WAYPOINT", + CANCEL: CANCEL, + ABORT: ABORT, + }, ) sm.add_state( "NAVIGATING", nav_sm, - transitions={SUCCEED: SUCCEED, CANCEL: CANCEL, ABORT: ABORT}, + transitions={ + SUCCEED: SUCCEED, + CANCEL: CANCEL, + ABORT: ABORT, + }, ) # pub FSM info @@ -142,7 +158,7 @@ def main(): blackboard = Blackboard() blackboard["waypoints_num"] = 2 outcome = sm(blackboard) - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demos/yasmin_demos/service_client_demo.py b/yasmin_demos/yasmin_demos/service_client_demo.py index d0b31dc..d5c62a3 100755 --- a/yasmin_demos/yasmin_demos/service_client_demo.py +++ b/yasmin_demos/yasmin_demos/service_client_demo.py @@ -19,6 +19,7 @@ import rclpy from example_interfaces.srv import AddTwoInts +import yasmin from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -60,14 +61,14 @@ def set_ints(blackboard: Blackboard) -> str: def print_sum(blackboard: Blackboard) -> str: - print(f"Sum: {blackboard['sum']}") + yasmin.YASMIN_LOG_INFO(f"Sum: {blackboard['sum']}") return SUCCEED # main def main(): - print("yasmin_service_client_demo") + yasmin.YASMIN_LOG_INFO("yasmin_service_client_demo") # init ROS 2 rclpy.init() @@ -94,7 +95,11 @@ def main(): }, ) sm.add_state( - "PRINTING_SUM", CbState([SUCCEED], print_sum), transitions={SUCCEED: "outcome4"} + "PRINTING_SUM", + CbState([SUCCEED], print_sum), + transitions={ + SUCCEED: "outcome4", + }, ) # pub FSM info @@ -102,7 +107,7 @@ def main(): # execute FSM outcome = sm() - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demos/yasmin_demos/yasmin_demo.py b/yasmin_demos/yasmin_demos/yasmin_demo.py index 9304cbf..11536c0 100755 --- a/yasmin_demos/yasmin_demos/yasmin_demo.py +++ b/yasmin_demos/yasmin_demos/yasmin_demo.py @@ -19,6 +19,7 @@ import time import rclpy +import yasmin from yasmin import State from yasmin import Blackboard from yasmin import StateMachine @@ -33,7 +34,7 @@ def __init__(self) -> None: self.counter = 0 def execute(self, blackboard: Blackboard) -> str: - print("Executing state FOO") + yasmin.YASMIN_LOG_INFO("Executing state FOO") time.sleep(3) if self.counter < 3: @@ -50,17 +51,17 @@ def __init__(self) -> None: super().__init__(outcomes=["outcome3"]) def execute(self, blackboard: Blackboard) -> str: - print("Executing state BAR") + yasmin.YASMIN_LOG_INFO("Executing state BAR") time.sleep(3) - print(blackboard["foo_str"]) + yasmin.YASMIN_LOG_INFO(blackboard["foo_str"]) return "outcome3" # main def main(): - print("yasmin_demo") + yasmin.YASMIN_LOG_INFO("yasmin_demo") # init ROS 2 rclpy.init() @@ -73,16 +74,27 @@ def main(): # add states sm.add_state( - "FOO", FooState(), transitions={"outcome1": "BAR", "outcome2": "outcome4"} + "FOO", + FooState(), + transitions={ + "outcome1": "BAR", + "outcome2": "outcome4", + }, + ) + sm.add_state( + "BAR", + BarState(), + transitions={ + "outcome3": "FOO", + }, ) - sm.add_state("BAR", BarState(), transitions={"outcome3": "FOO"}) # pub FSM info YasminViewerPub("yasmin_demo", sm) # execute FSM outcome = sm() - print(outcome) + yasmin.YASMIN_LOG_INFO(outcome) # shutdown ROS 2 rclpy.shutdown()