diff --git a/README.md b/README.md index 4bff1db..f521742 100644 --- a/README.md +++ b/README.md @@ -103,13 +103,13 @@ $ ros2 run yasmin_demos yasmin_demo.py Click to expand ```python -#!/usr/bin/env python3 - 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 @@ -125,7 +125,7 @@ class FooState(State): if self.counter < 3: self.counter += 1 - blackboard.foo_str = f"Counter: {self.counter}" + blackboard["foo_str"] = f"Counter: {self.counter}" return "outcome1" else: return "outcome2" @@ -140,7 +140,7 @@ class BarState(State): print("Executing state BAR") time.sleep(3) - print(blackboard.foo_str) + print(blackboard["foo_str"]) return "outcome3" @@ -152,25 +152,17 @@ def main(): # init ROS 2 rclpy.init() + # set ROS 2 logs + set_ros_loggers() + # create a FSM sm = StateMachine(outcomes=["outcome4"]) # add states sm.add_state( - "FOO", - FooState(), - transitions={ - "outcome1": "BAR", - "outcome2": "outcome4" - } - ) - sm.add_state( - "BAR", - BarState(), - transitions={ - "outcome3": "FOO" - } + "FOO", FooState(), transitions={"outcome1": "BAR", "outcome2": "outcome4"} ) + sm.add_state("BAR", BarState(), transitions={"outcome3": "FOO"}) # pub FSM info YasminViewerPub("yasmin_demo", sm) @@ -205,10 +197,12 @@ $ ros2 run yasmin_demos service_client_demo.py ```python 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 @@ -220,34 +214,32 @@ class AddTwoIntsState(ServiceState): "/add_two_ints", # service name self.create_request_handler, # cb to create the request ["outcome1"], # outcomes. Includes (SUCCEED, ABORT) - self.response_handler # cb to process the response + self.response_handler, # cb to process the response ) def create_request_handler(self, blackboard: Blackboard) -> AddTwoInts.Request: req = AddTwoInts.Request() - req.a = blackboard.a - req.b = blackboard.b + req.a = blackboard["a"] + req.b = blackboard["b"] return req def response_handler( - self, - blackboard: Blackboard, - response: AddTwoInts.Response + self, blackboard: Blackboard, response: AddTwoInts.Response ) -> str: - blackboard.sum = response.sum + blackboard["sum"] = response.sum return "outcome1" def set_ints(blackboard: Blackboard) -> str: - blackboard.a = 10 - blackboard.b = 5 + blackboard["a"] = 10 + blackboard["b"] = 5 return SUCCEED def print_sum(blackboard: Blackboard) -> str: - print(f"Sum: {blackboard.sum}") + print(f"Sum: {blackboard['sum']}") return SUCCEED @@ -259,6 +251,9 @@ def main(): # init ROS 2 rclpy.init() + # set ROS 2 logs + set_ros_loggers() + # create a FSM sm = StateMachine(outcomes=["outcome4"]) @@ -266,9 +261,7 @@ def main(): sm.add_state( "SETTING_INTS", CbState([SUCCEED], set_ints), - transitions={ - SUCCEED: "ADD_TWO_INTS" - } + transitions={SUCCEED: "ADD_TWO_INTS"}, ) sm.add_state( "ADD_TWO_INTS", @@ -276,15 +269,11 @@ def main(): transitions={ "outcome1": "PRINTING_SUM", SUCCEED: "outcome4", - ABORT: "outcome4" - } + ABORT: "outcome4", + }, ) sm.add_state( - "PRINTING_SUM", - CbState([SUCCEED], print_sum), - transitions={ - SUCCEED: "outcome4" - } + "PRINTING_SUM", CbState([SUCCEED], print_sum), transitions={SUCCEED: "outcome4"} ) # pub FSM info @@ -320,10 +309,12 @@ $ ros2 run yasmin_demos action_client_demo.py ```python 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 @@ -336,34 +327,30 @@ class FibonacciState(ActionState): self.create_goal_handler, # cb to create the goal None, # outcomes. Includes (SUCCEED, ABORT, CANCEL) self.response_handler, # cb to process the response - self.print_feedback # cb to process the feedback + self.print_feedback, # cb to process the feedback ) def create_goal_handler(self, blackboard: Blackboard) -> Fibonacci.Goal: goal = Fibonacci.Goal() - goal.order = blackboard.n + goal.order = blackboard["n"] return goal def response_handler( - self, - blackboard: Blackboard, - response: Fibonacci.Result + self, blackboard: Blackboard, response: Fibonacci.Result ) -> str: - blackboard.fibo_res = response.sequence + blackboard["fibo_res"] = response.sequence return SUCCEED def print_feedback( - self, - blackboard: Blackboard, - feedback: Fibonacci.Feedback + self, blackboard: Blackboard, feedback: Fibonacci.Feedback ) -> None: print(f"Received feedback: {list(feedback.partial_sequence)}") def print_result(blackboard: Blackboard) -> str: - print(f"Result: {blackboard.fibo_res}") + print(f"Result: {blackboard['fibo_res']}") return SUCCEED @@ -375,6 +362,9 @@ def main(): # init ROS 2 rclpy.init() + # set ROS 2 logs + set_ros_loggers() + # create a FSM sm = StateMachine(outcomes=["outcome4"]) @@ -382,18 +372,12 @@ 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 @@ -401,7 +385,7 @@ def main(): # create an initial blackoard blackboard = Blackboard() - blackboard.n = 10 + blackboard["n"] = 10 # execute FSM outcome = sm(blackboard) @@ -434,7 +418,8 @@ from nav_msgs.msg import Odometry from yasmin import Blackboard from yasmin import StateMachine from yasmin_ros import MonitorState -from yasmin_ros.basic_outcomes import CANCEL +from yasmin_ros.ros_logs import set_ros_loggers +from yasmin_ros.basic_outcomes import TIMEOUT from yasmin_viewer import YasminViewerPub @@ -447,7 +432,7 @@ class PrintOdometryState(MonitorState): self.monitor_handler, # monitor handler callback qos=qos_profile_sensor_data, # qos for the topic sbscription msg_queue=10, # queue of the monitor handler callback - timeout=10 # timeout to wait for msgs in seconds + timeout=10, # timeout to wait for msgs in seconds # if not None, CANCEL outcome is added ) self.times = times @@ -471,6 +456,9 @@ def main(): # init ROS 2 rclpy.init() + # set ROS 2 logs + set_ros_loggers() + # create a FSM sm = StateMachine(outcomes=["outcome4"]) @@ -481,8 +469,8 @@ def main(): transitions={ "outcome1": "PRINTING_ODOM", "outcome2": "outcome4", - CANCEL: "outcome4" - } + TIMEOUT: "outcome4", + }, ) # pub FSM info @@ -522,6 +510,7 @@ 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 @@ -536,19 +525,19 @@ class Nav2State(ActionState): "/navigate_to_pose", # action name self.create_goal_handler, # cb to create the goal None, # outcomes. Includes (SUCCEED, ABORT, CANCEL) - None # cb to process the response + None, # cb to process the response ) def create_goal_handler(self, blackboard: Blackboard) -> NavigateToPose.Goal: goal = NavigateToPose.Goal() - goal.pose.pose = blackboard.pose + goal.pose.pose = blackboard["pose"] goal.pose.header.frame_id = "map" return goal def create_waypoints(blackboard: Blackboard) -> str: - blackboard.waypoints = { + blackboard["waypoints"] = { "entrance": [1.25, 6.30, -0.78, 0.67], "bathroom": [4.89, 1.64, 0.0, 1.0], "livingroom": [1.55, 4.03, -0.69, 0.72], @@ -559,19 +548,19 @@ def create_waypoints(blackboard: Blackboard) -> str: def take_random_waypoint(blackboard) -> str: - blackboard.random_waypoints = random.sample( - list(blackboard.waypoints.keys()), - blackboard.waypoints_num) + blackboard["random_waypoints"] = random.sample( + list(blackboard["waypoints"].keys()), blackboard["waypoints_num"] + ) return SUCCEED def get_next_waypoint(blackboard: Blackboard) -> str: - if not blackboard.random_waypoints: + if not blackboard["random_waypoints"]: return END - wp_name = blackboard.random_waypoints.pop(0) - wp = blackboard.waypoints[wp_name] + wp_name = blackboard["random_waypoints"].pop(0) + wp = blackboard["waypoints"][wp_name] pose = Pose() pose.position.x = wp[0] @@ -580,8 +569,8 @@ def get_next_waypoint(blackboard: Blackboard) -> str: pose.orientation.z = wp[2] pose.orientation.w = wp[3] - blackboard.pose = pose - blackboard.text = f"I have reached waypoint {wp_name}" + blackboard["pose"] = pose + blackboard["text"] = f"I have reached waypoint {wp_name}" return HAS_NEXT @@ -594,6 +583,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]) @@ -602,44 +594,29 @@ 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 @@ -647,7 +624,7 @@ def main(): # execute FSM blackboard = Blackboard() - blackboard.waypoints_num = 2 + blackboard["waypoints_num"] = 2 outcome = sm(blackboard) print(outcome) @@ -682,6 +659,7 @@ $ ros2 run yasmin_demos yasmin_demo #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 @@ -733,6 +711,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( std::initializer_list{"outcome4"}); @@ -781,6 +762,7 @@ $ ros2 run yasmin_demos service_client_demo #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" @@ -842,6 +824,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( std::initializer_list{"outcome4"}); @@ -893,6 +878,8 @@ $ ros2 run yasmin_demos action_client_demo Click to expand ```cpp +// along with this program. If not, see . + #include #include #include @@ -903,6 +890,7 @@ $ ros2 run yasmin_demos action_client_demo #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" @@ -983,6 +971,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( std::initializer_list{"outcome4"}); @@ -1029,21 +1020,6 @@ $ ros2 run yasmin_demos monitor_demo Click to expand ```cpp -// Copyright (C) 2023 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 . - #include #include #include @@ -1055,6 +1031,7 @@ $ ros2 run yasmin_demos monitor_demo #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; @@ -1108,6 +1085,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( std::initializer_list{"outcome4"}); diff --git a/yasmin/CMakeLists.txt b/yasmin/CMakeLists.txt index 75c747a..f61fef4 100644 --- a/yasmin/CMakeLists.txt +++ b/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 diff --git a/yasmin/include/yasmin/logs.hpp b/yasmin/include/yasmin/logs.hpp new file mode 100644 index 0000000..a28afae --- /dev/null +++ b/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 . + +#ifndef YASMIN_LOGS_HPP +#define YASMIN_LOGS_HPP + +#include +#include + +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 diff --git a/yasmin/include/yasmin/yasmin_logs.hpp b/yasmin/include/yasmin/yasmin_logs.hpp deleted file mode 100644 index 93bc298..0000000 --- a/yasmin/include/yasmin/yasmin_logs.hpp +++ /dev/null @@ -1,14 +0,0 @@ - -#ifndef YASMIN_LOGS_HPP -#define YASMIN_LOGS_HPP - -#define YASMIN_LOG_ERROR(text, ...) \ - fprintf(stderr, "[ERROR] " text "\n", ##__VA_ARGS__) -#define YASMIN_LOG_WARN(text, ...) \ - fprintf(stderr, "[WARN] " text "\n", ##__VA_ARGS__) -#define YASMIN_LOG_INFO(text, ...) \ - fprintf(stderr, "[INFO] " text "\n", ##__VA_ARGS__) -#define YASMIN_LOG_DEBUG(text, ...) \ - fprintf(stderr, "[DEBUG] " text "\n", ##__VA_ARGS__) - -#endif \ No newline at end of file diff --git a/yasmin/src/yasmin/logs.cpp b/yasmin/src/yasmin/logs.cpp new file mode 100644 index 0000000..9832888 --- /dev/null +++ b/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 . + +#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 diff --git a/yasmin/src/yasmin/state_machine.cpp b/yasmin/src/yasmin/state_machine.cpp index 40c8972..2f310cf 100644 --- a/yasmin/src/yasmin/state_machine.cpp +++ b/yasmin/src/yasmin/state_machine.cpp @@ -23,9 +23,9 @@ #include #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; diff --git a/yasmin/yasmin/__init__.py b/yasmin/yasmin/__init__.py index 75f6998..563e514 100644 --- a/yasmin/yasmin/__init__.py +++ b/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, diff --git a/yasmin/yasmin/yasmin_logs.py b/yasmin/yasmin/logs.py similarity index 98% rename from yasmin/yasmin/yasmin_logs.py rename to yasmin/yasmin/logs.py index 11cc37b..c3ff4b1 100644 --- a/yasmin/yasmin/yasmin_logs.py +++ b/yasmin/yasmin/logs.py @@ -16,7 +16,7 @@ import yasmin import logging -import yasmin.yasmin_logs +import yasmin.logs __all__ = [ "set_loggers", diff --git a/yasmin_demos/src/action_client_demo.cpp b/yasmin_demos/src/action_client_demo.cpp index a800715..85fbbba 100644 --- a/yasmin_demos/src/action_client_demo.cpp +++ b/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( std::initializer_list{"outcome4"}); diff --git a/yasmin_demos/src/monitor_demo.cpp b/yasmin_demos/src/monitor_demo.cpp index dca3152..69a0f3b 100644 --- a/yasmin_demos/src/monitor_demo.cpp +++ b/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( std::initializer_list{"outcome4"}); diff --git a/yasmin_demos/src/service_client_demo.cpp b/yasmin_demos/src/service_client_demo.cpp index 2d82072..6ab9445 100644 --- a/yasmin_demos/src/service_client_demo.cpp +++ b/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( std::initializer_list{"outcome4"}); diff --git a/yasmin_demos/src/yasmin_demo.cpp b/yasmin_demos/src/yasmin_demo.cpp index 7c662e0..d9710c0 100644 --- a/yasmin_demos/src/yasmin_demo.cpp +++ b/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( std::initializer_list{"outcome4"}); diff --git a/yasmin_demos/yasmin_demos/action_client_demo.py b/yasmin_demos/yasmin_demos/action_client_demo.py index ba5475b..99347d9 100755 --- a/yasmin_demos/yasmin_demos/action_client_demo.py +++ b/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"]) diff --git a/yasmin_demos/yasmin_demos/monitor_demo.py b/yasmin_demos/yasmin_demos/monitor_demo.py index 2d534b8..42915d6 100755 --- a/yasmin_demos/yasmin_demos/monitor_demo.py +++ b/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"]) diff --git a/yasmin_demos/yasmin_demos/nav_demo.py b/yasmin_demos/yasmin_demos/nav_demo.py index 06743d8..4a126ea 100755 --- a/yasmin_demos/yasmin_demos/nav_demo.py +++ b/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]) diff --git a/yasmin_demos/yasmin_demos/service_client_demo.py b/yasmin_demos/yasmin_demos/service_client_demo.py index 51660e9..d0b31dc 100755 --- a/yasmin_demos/yasmin_demos/service_client_demo.py +++ b/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"]) diff --git a/yasmin_demos/yasmin_demos/yasmin_demo.py b/yasmin_demos/yasmin_demos/yasmin_demo.py index 39d41aa..9304cbf 100755 --- a/yasmin_demos/yasmin_demos/yasmin_demo.py +++ b/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"]) diff --git a/yasmin_ros/CMakeLists.txt b/yasmin_ros/CMakeLists.txt index 3f7a09c..40ece41 100644 --- a/yasmin_ros/CMakeLists.txt +++ b/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 diff --git a/yasmin_ros/include/yasmin_ros/ros_logs.hpp b/yasmin_ros/include/yasmin_ros/ros_logs.hpp new file mode 100644 index 0000000..5ffd540 --- /dev/null +++ b/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 . + +#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 diff --git a/yasmin_ros/src/yasmin_ros/ros_logs.cpp b/yasmin_ros/src/yasmin_ros/ros_logs.cpp new file mode 100644 index 0000000..0173495 --- /dev/null +++ b/yasmin_ros/src/yasmin_ros/ros_logs.cpp @@ -0,0 +1,93 @@ +// 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 . + +#include +#include +#include + +#include "yasmin/logs.hpp" +#include "yasmin_ros/yasmin_node.hpp" + +namespace yasmin_ros { + +void ros_log_error(const char *text, ...) { + va_list args; + va_start(args, text); + + int size = vsnprintf(nullptr, 0, text, args) + 1; + va_end(args); + + std::string buffer(size, '\0'); + + va_start(args, text); + vsnprintf(&buffer[0], buffer.size(), text, args); + va_end(args); + + RCLCPP_ERROR(YasminNode::get_instance()->get_logger(), "%s", buffer.c_str()); +} + +void ros_log_warn(const char *text, ...) { + va_list args; + va_start(args, text); + + int size = vsnprintf(nullptr, 0, text, args) + 1; + va_end(args); + + std::string buffer(size, '\0'); + + va_start(args, text); + vsnprintf(&buffer[0], buffer.size(), text, args); + va_end(args); + + RCLCPP_WARN(YasminNode::get_instance()->get_logger(), "%s", buffer.c_str()); +} + +void ros_log_info(const char *text, ...) { + va_list args; + va_start(args, text); + + int size = vsnprintf(nullptr, 0, text, args) + 1; + va_end(args); + + std::string buffer(size, '\0'); + + va_start(args, text); + vsnprintf(&buffer[0], buffer.size(), text, args); + va_end(args); + + RCLCPP_INFO(YasminNode::get_instance()->get_logger(), "%s", buffer.c_str()); +} + +void ros_log_debug(const char *text, ...) { + va_list args; + va_start(args, text); + + int size = vsnprintf(nullptr, 0, text, args) + 1; + va_end(args); + + std::string buffer(size, '\0'); + + va_start(args, text); + vsnprintf(&buffer[0], buffer.size(), text, args); + va_end(args); + + RCLCPP_DEBUG(YasminNode::get_instance()->get_logger(), "%s", buffer.c_str()); +} + +void set_ros_loggers() { + yasmin::set_loggers(ros_log_info, ros_log_warn, ros_log_debug, ros_log_error); +} + +} // namespace yasmin_ros \ No newline at end of file diff --git a/yasmin_ros/yasmin_ros/ros_logs.py b/yasmin_ros/yasmin_ros/ros_logs.py new file mode 100644 index 0000000..c96c077 --- /dev/null +++ b/yasmin_ros/yasmin_ros/ros_logs.py @@ -0,0 +1,28 @@ +# 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 . + + +import yasmin +from yasmin_ros.yasmin_node import YasminNode + + +def set_ros_loggers() -> None: + node = YasminNode.get_instance() + yasmin.set_loggers( + node.get_logger().info, + node.get_logger().warn, + node.get_logger().debug, + node.get_logger().error, + ) diff --git a/yasmin_ros/yasmin_ros/yasmin_node.py b/yasmin_ros/yasmin_ros/yasmin_node.py index 3592e89..45bb9d1 100644 --- a/yasmin_ros/yasmin_ros/yasmin_node.py +++ b/yasmin_ros/yasmin_ros/yasmin_node.py @@ -20,8 +20,6 @@ from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor -import yasmin - class YasminNode(Node): @@ -46,13 +44,6 @@ def __init__(self) -> None: super().__init__(f"yasmin_{str(uuid.uuid4()).replace('-', '_')}_node") - yasmin.set_loggers( - self.get_logger().info, - self.get_logger().warn, - self.get_logger().debug, - self.get_logger().error, - ) - # executor self._executor = MultiThreadedExecutor() self._executor.add_node(self)