new ros logs for YASMIN_LOG

yasmin/CMakeLists.txt

@@ -20,6 +20,7 @@ include_directories(src)
 set(LIB ${CMAKE_PROJECT_NAME}_lib)
 set(SOURCES
   src/yasmin/blackboard/blackboard.cpp
+  src/yasmin/logs.cpp
   src/yasmin/state.cpp
   src/yasmin/cb_state.cpp
   src/yasmin/state_machine.cpp

yasmin/include/yasmin/logs.hpp

@@ -0,0 +1,46 @@
+// Copyright (C) 2024  Miguel Ángel González Santamarta
+
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#ifndef YASMIN_LOGS_HPP
+#define YASMIN_LOGS_HPP
+
+#include <cstdarg>
+#include <cstdio>
+
+namespace yasmin {
+
+// define raw function pointer type for logging
+typedef void (*LogFunction)(const char *, ...);
+
+// declare function pointers as extern
+extern LogFunction log_error;
+extern LogFunction log_warn;
+extern LogFunction log_info;
+extern LogFunction log_debug;
+
+// macros to use the function pointers for logging
+#define YASMIN_LOG_ERROR(text, ...) yasmin::log_error(text, ##__VA_ARGS__)
+#define YASMIN_LOG_WARN(text, ...) yasmin::log_warn(text, ##__VA_ARGS__)
+#define YASMIN_LOG_INFO(text, ...) yasmin::log_info(text, ##__VA_ARGS__)
+#define YASMIN_LOG_DEBUG(text, ...) yasmin::log_debug(text, ##__VA_ARGS__)
+
+// function to set custom log functions
+void set_loggers(LogFunction error, LogFunction warn, LogFunction info,
+                 LogFunction debug);
+
+void set_default_loggers();
+} // namespace yasmin
+
+#endif // YASMIN_LOGS_HPP

yasmin/src/yasmin/logs.cpp

@@ -0,0 +1,77 @@
+// Copyright (C) 2024  Miguel Ángel González Santamarta
+
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#include "yasmin/logs.hpp"
+
+namespace yasmin {
+
+// define the default log functions
+void default_log_error(const char *text, ...) {
+  va_list args;
+  va_start(args, text);
+  fprintf(stderr, "[ERROR] ");
+  vfprintf(stderr, text, args);
+  fprintf(stderr, "\n");
+  va_end(args);
+}
+
+void default_log_warn(const char *text, ...) {
+  va_list args;
+  va_start(args, text);
+  fprintf(stderr, "[WARN] ");
+  vfprintf(stderr, text, args);
+  fprintf(stderr, "\n");
+  va_end(args);
+}
+
+void default_log_info(const char *text, ...) {
+  va_list args;
+  va_start(args, text);
+  fprintf(stderr, "[INFO] ");
+  vfprintf(stderr, text, args);
+  fprintf(stderr, "\n");
+  va_end(args);
+}
+
+void default_log_debug(const char *text, ...) {
+  va_list args;
+  va_start(args, text);
+  fprintf(stderr, "[DEBUG] ");
+  vfprintf(stderr, text, args);
+  fprintf(stderr, "\n");
+  va_end(args);
+}
+
+// define the function pointers to be initialized with default log functions
+LogFunction log_error = default_log_error;
+LogFunction log_warn = default_log_warn;
+LogFunction log_info = default_log_info;
+LogFunction log_debug = default_log_debug;
+
+// implement the set_loggers function
+void set_loggers(LogFunction error, LogFunction warn, LogFunction info,
+                 LogFunction debug) {
+  log_error = error ? error : default_log_error;
+  log_warn = warn ? warn : default_log_warn;
+  log_info = info ? info : default_log_info;
+  log_debug = debug ? debug : default_log_debug;
+}
+
+void set_default_loggers() {
+  set_loggers(default_log_error, default_log_warn, default_log_info,
+              default_log_debug);
+}
+
+} // namespace yasmin

yasmin/src/yasmin/state_machine.cpp

@@ -23,9 +23,9 @@
 #include <vector>
 
 #include "yasmin/blackboard/blackboard.hpp"
+#include "yasmin/logs.hpp"
 #include "yasmin/state.hpp"
 #include "yasmin/state_machine.hpp"
-#include "yasmin/yasmin_logs.hpp"
 
 using namespace yasmin;
 

yasmin/yasmin/__init__.py

@@ -3,7 +3,7 @@
 from yasmin.blackboard import Blackboard
 from yasmin.state_machine import StateMachine
 
-from yasmin.yasmin_logs import (
+from yasmin.logs import (
     set_loggers,
     YASMIN_LOG_ERROR,
     YASMIN_LOG_WARN,

yasmin/yasmin/yasmin_logs.py → yasmin/yasmin/logs.py

@@ -16,7 +16,7 @@
 import yasmin
 import logging
 
-import yasmin.yasmin_logs
+import yasmin.logs
 
 __all__ = [
     "set_loggers",

yasmin_demos/src/action_client_demo.cpp

@@ -23,6 +23,7 @@
 #include "yasmin/state_machine.hpp"
 #include "yasmin_ros/action_state.hpp"
 #include "yasmin_ros/basic_outcomes.hpp"
+#include "yasmin_ros/ros_logs.hpp"
 #include "yasmin_ros/yasmin_node.hpp"
 #include "yasmin_viewer/yasmin_viewer_pub.hpp"
 
@@ -103,6 +104,9 @@ int main(int argc, char *argv[]) {
   std::cout << "yasmin_action_client_demo\n";
   rclcpp::init(argc, argv);
 
+  // set ROS 2 logs
+  yasmin_ros::set_ros_loggers();
+
   // create a state machine
   auto sm = std::make_shared<yasmin::StateMachine>(
       std::initializer_list<std::string>{"outcome4"});

yasmin_demos/src/monitor_demo.cpp

@@ -24,6 +24,7 @@
 #include "yasmin/state_machine.hpp"
 #include "yasmin_ros/basic_outcomes.hpp"
 #include "yasmin_ros/monitor_state.hpp"
+#include "yasmin_ros/ros_logs.hpp"
 #include "yasmin_viewer/yasmin_viewer_pub.hpp"
 
 using std::placeholders::_1;
@@ -77,6 +78,9 @@ int main(int argc, char *argv[]) {
   std::cout << "yasmin_monitor_demo\n";
   rclcpp::init(argc, argv);
 
+  // set ROS 2 logs
+  yasmin_ros::set_ros_loggers();
+
   // create a state machine
   auto sm = std::make_shared<yasmin::StateMachine>(
       std::initializer_list<std::string>{"outcome4"});

yasmin_demos/src/service_client_demo.cpp

@@ -23,6 +23,7 @@
 #include "yasmin/cb_state.hpp"
 #include "yasmin/state_machine.hpp"
 #include "yasmin_ros/basic_outcomes.hpp"
+#include "yasmin_ros/ros_logs.hpp"
 #include "yasmin_ros/service_state.hpp"
 #include "yasmin_viewer/yasmin_viewer_pub.hpp"
 
@@ -84,6 +85,9 @@ int main(int argc, char *argv[]) {
   std::cout << "yasmin_service_client_demo\n";
   rclcpp::init(argc, argv);
 
+  // set ROS 2 logs
+  yasmin_ros::set_ros_loggers();
+
   // create a state machine
   auto sm = std::make_shared<yasmin::StateMachine>(
       std::initializer_list<std::string>{"outcome4"});

yasmin_demos/src/yasmin_demo.cpp

@@ -22,6 +22,7 @@
 
 #include "yasmin/state.hpp"
 #include "yasmin/state_machine.hpp"
+#include "yasmin_ros/ros_logs.hpp"
 #include "yasmin_viewer/yasmin_viewer_pub.hpp"
 
 // define state Foo
@@ -73,6 +74,9 @@ int main(int argc, char *argv[]) {
   std::cout << "yasmin_demo\n";
   rclcpp::init(argc, argv);
 
+  // set ROS 2 logs
+  yasmin_ros::set_ros_loggers();
+
   // create a state machine
   auto sm = std::make_shared<yasmin::StateMachine>(
       std::initializer_list<std::string>{"outcome4"});

yasmin_demos/yasmin_demos/action_client_demo.py

@@ -18,10 +18,12 @@
 
 import rclpy
 from action_tutorials_interfaces.action import Fibonacci
+
 from yasmin import CbState
 from yasmin import Blackboard
 from yasmin import StateMachine
 from yasmin_ros import ActionState
+from yasmin_ros.ros_logs import set_ros_loggers
 from yasmin_ros.basic_outcomes import SUCCEED, ABORT, CANCEL
 from yasmin_viewer import YasminViewerPub
 
@@ -69,6 +71,9 @@ def main():
     # init ROS 2
     rclpy.init()
 
+    # set ROS 2 logs
+    set_ros_loggers()
+
     # create a FSM
     sm = StateMachine(outcomes=["outcome4"])
 

yasmin_demos/yasmin_demos/monitor_demo.py

@@ -23,6 +23,7 @@
 from yasmin import Blackboard
 from yasmin import StateMachine
 from yasmin_ros import MonitorState
+from yasmin_ros.ros_logs import set_ros_loggers
 from yasmin_ros.basic_outcomes import TIMEOUT
 from yasmin_viewer import YasminViewerPub
 
@@ -60,6 +61,9 @@ def main():
     # init ROS 2
     rclpy.init()
 
+    # set ROS 2 logs
+    set_ros_loggers()
+
     # create a FSM
     sm = StateMachine(outcomes=["outcome4"])
 

yasmin_demos/yasmin_demos/nav_demo.py

@@ -26,6 +26,7 @@
 from yasmin import Blackboard
 from yasmin import StateMachine
 from yasmin_ros import ActionState
+from yasmin_ros.ros_logs import set_ros_loggers
 from yasmin_ros.basic_outcomes import SUCCEED, ABORT, CANCEL
 from yasmin_viewer import YasminViewerPub
 
@@ -98,6 +99,9 @@ def main():
     # init ROS 2
     rclpy.init()
 
+    # set ROS 2 logs
+    set_ros_loggers()
+
     # create state machines
     sm = StateMachine(outcomes=[SUCCEED, ABORT, CANCEL])
     nav_sm = StateMachine(outcomes=[SUCCEED, ABORT, CANCEL])

yasmin_demos/yasmin_demos/service_client_demo.py

@@ -18,10 +18,12 @@
 
 import rclpy
 from example_interfaces.srv import AddTwoInts
+
 from yasmin import CbState
 from yasmin import Blackboard
 from yasmin import StateMachine
 from yasmin_ros import ServiceState
+from yasmin_ros.ros_logs import set_ros_loggers
 from yasmin_ros.basic_outcomes import SUCCEED, ABORT
 from yasmin_viewer import YasminViewerPub
 
@@ -70,6 +72,9 @@ def main():
     # init ROS 2
     rclpy.init()
 
+    # set ROS 2 logs
+    set_ros_loggers()
+
     # create a FSM
     sm = StateMachine(outcomes=["outcome4"])
 

yasmin_demos/yasmin_demos/yasmin_demo.py

@@ -18,9 +18,11 @@
 
 import time
 import rclpy
+
 from yasmin import State
 from yasmin import Blackboard
 from yasmin import StateMachine
+from yasmin_ros.ros_logs import set_ros_loggers
 from yasmin_viewer import YasminViewerPub
 
 
@@ -63,6 +65,9 @@ def main():
     # init ROS 2
     rclpy.init()
 
+    # set ROS 2 logs
+    set_ros_loggers()
+
     # create a FSM
     sm = StateMachine(outcomes=["outcome4"])
 

yasmin_ros/CMakeLists.txt

@@ -48,6 +48,7 @@ include_directories(src)
 set(LIB ${CMAKE_PROJECT_NAME}_lib)
 set(SOURCES
   src/yasmin_ros/yasmin_node.cpp
+  src/yasmin_ros/ros_logs.cpp
   src/yasmin_ros/action_state.cpp
   src/yasmin_ros/service_state.cpp
   src/yasmin_ros/monitor_state.cpp

yasmin_ros/include/yasmin_ros/ros_logs.hpp

@@ -0,0 +1,25 @@
+// Copyright (C) 2024  Miguel Ángel González Santamarta
+
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#ifndef YASMIN_ROS_LOGS_HPP
+#define YASMIN_ROS_LOGS_HPP
+
+namespace yasmin_ros {
+
+void set_ros_loggers();
+
+} // namespace yasmin_ros
+
+#endif // YASMIN_ROS_LOGS_HPP