diff --git a/README.md b/README.md index 7b0b606..2424c6b 100644 --- a/README.md +++ b/README.md @@ -31,7 +31,6 @@ YASMIN is a project focused on implementing robot behaviors using Finite State M ```shell # clone $ cd ~/ros2_ws/src -$ git clone https://github.com/uleroboticsgroup/simple_node.git $ git clone https://github.com/uleroboticsgroup/yasmin.git # dependencies @@ -45,40 +44,47 @@ $ colcon build ## Demos -A Python and a C++ demo are included. +There are some examples, for both Python and C++, that can be found in [yasmin_demo](./yasmin_demo/). + +### Python + +#### Vanilla Demo (FSM) + +```shell +$ ros2 run yasmin_demo yasmin_demo.py +```

-### Python +
+Click to expand ```python #!/usr/bin/env python3 import time import rclpy - -from simple_node import Node - from yasmin import State +from yasmin import Blackboard from yasmin import StateMachine from yasmin_viewer import YasminViewerPub # define state Foo class FooState(State): - def __init__(self): + def __init__(self) -> None: super().__init__(["outcome1", "outcome2"]) self.counter = 0 - def execute(self, blackboard): + def execute(self, blackboard: Blackboard) -> str: print("Executing state FOO") time.sleep(3) 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" @@ -86,10 +92,10 @@ class FooState(State): # define state Bar class BarState(State): - def __init__(self): + def __init__(self) -> None: super().__init__(outcomes=["outcome3"]) - def execute(self, blackboard): + def execute(self, blackboard: Blackboard) -> str: print("Executing state BAR") time.sleep(3) @@ -97,36 +103,270 @@ class BarState(State): return "outcome3" -class DemoNode(Node): +# main +def main(): + + print("yasmin_demo") + + # init ROS 2 + rclpy.init() + + # 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" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 + rclpy.shutdown() + + +if __name__ == "__main__": + main() +``` + +
+ +#### Service Demo (FSM + ROS 2 Service Client) + +```shell +$ ros2 run demo_nodes_py add_two_ints_server +``` + +```shell +$ ros2 run yasmin_demo service_client_demo.py +``` + +
+Click to expand + +```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.basic_outcomes import SUCCEED, ABORT +from yasmin_viewer import YasminViewerPub + + +class AddTwoIntsState(ServiceState): + def __init__(self) -> None: + super().__init__( + AddTwoInts, # srv type + "/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 + ) + + def create_request_handler(self, blackboard: Blackboard) -> AddTwoInts.Request: + + req = AddTwoInts.Request() + req.a = blackboard.a + req.b = blackboard.b + return req + + def response_handler( + self, + blackboard: Blackboard, + response: AddTwoInts.Response + ) -> str: - def __init__(self): - super().__init__("yasmin_node") + blackboard.sum = response.sum + return "outcome1" - # create a state machine - sm = StateMachine(outcomes=["outcome4"]) - # add states - sm.add_state("FOO", FooState(), - transitions={"outcome1": "BAR", - "outcome2": "outcome4"}) - sm.add_state("BAR", BarState(), - transitions={"outcome3": "FOO"}) +def set_ints(blackboard: Blackboard) -> str: + blackboard.a = 10 + blackboard.b = 5 + return SUCCEED - # pub - YasminViewerPub(self, "YASMIN_DEMO", sm) - # execute - outcome = sm() - print(outcome) +def print_sum(blackboard: Blackboard) -> str: + print(f"Sum: {blackboard.sum}") + return SUCCEED # main -def main(args=None): +def main(): + + print("yasmin_service_client_demo") + + # init ROS 2 + rclpy.init() + + # create a FSM + sm = StateMachine(outcomes=["outcome4"]) + + # add states + sm.add_state( + "SETTING_INTS", + CbState([SUCCEED], set_ints), + transitions={ + SUCCEED: "ADD_TWO_INTS" + } + ) + sm.add_state( + "ADD_TWO_INTS", + AddTwoIntsState(), + transitions={ + "outcome1": "PRINTING_SUM", + SUCCEED: "outcome4", + ABORT: "outcome4" + } + ) + sm.add_state( + "PRINTING_SUM", + CbState([SUCCEED], print_sum), + transitions={ + SUCCEED: "outcome4" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_SERVICE_CLIENT_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 + rclpy.shutdown() - print("yasmin_demo") - rclpy.init(args=args) - node = DemoNode() - node.join_spin() + +if __name__ == "__main__": + main() +``` + +
+ +#### Action Demo (FSM + ROS 2 Action) + +```shell +$ ros2 run action_tutorials_cpp fibonacci_action_server +``` + +```shell +$ ros2 run yasmin_demo action_client_demo.py +``` + +
+Click to expand + +```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.basic_outcomes import SUCCEED, ABORT, CANCEL +from yasmin_viewer import YasminViewerPub + + +class FibonacciState(ActionState): + def __init__(self) -> None: + super().__init__( + Fibonacci, # action type + "/fibonacci", # action name + self.create_goal_handler, # cb to create the goal + None, # outcomes. Includes (SUCCEED, ABORT, CANCEL) + self.response_handler # cb to process the response + ) + + def create_goal_handler(self, blackboard: Blackboard) -> Fibonacci.Goal: + + goal = Fibonacci.Goal() + goal.order = blackboard.n + return goal + + def response_handler( + self, + blackboard: Blackboard, + response: Fibonacci.Result + ) -> str: + + blackboard.fibo_res = response.sequence + return SUCCEED + + +def set_int(blackboard: Blackboard) -> str: + blackboard.n = 3 + return SUCCEED + + +def print_result(blackboard: Blackboard) -> str: + print(f"Result: {blackboard.fibo_res}") + return SUCCEED + + +# main +def main(): + + print("yasmin_action_client_demo") + + # init ROS 2 + rclpy.init() + + # create a FSM + sm = StateMachine(outcomes=["outcome4"]) + + # add states + sm.add_state( + "SETTING_INT", + CbState([SUCCEED], set_int), + transitions={ + SUCCEED: "CALLING_FIBONACCI" + } + ) + sm.add_state( + "CALLING_FIBONACCI", + FibonacciState(), + transitions={ + SUCCEED: "PRINTING_RESULT", + CANCEL: "outcome4", + ABORT: "outcome4" + } + ) + sm.add_state( + "PRINTING_RESULT", + CbState([SUCCEED], print_result), + transitions={ + SUCCEED: "outcome4" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_ACTION_CLIENT_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 rclpy.shutdown() @@ -134,15 +374,270 @@ if __name__ == "__main__": main() ``` +
+ +#### Monitor Demo (FSM + ROS 2 Subscriber) + +```shell +$ ros2 run yasmin_demo monitor_demo.py +``` + +
+Click to expand + +```python +import rclpy +from rclpy.qos import qos_profile_sensor_data +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_viewer import YasminViewerPub + + +class PrintOdometryState(MonitorState): + def __init__(self, times: int) -> None: + super().__init__( + Odometry, # msg type + "odom", # topic name + ["outcome1", "outcome2"], # outcomes + 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 + # if not None, CANCEL outcome is added + ) + self.times = times + + def monitor_handler(self, blackboard: Blackboard, msg: Odometry) -> str: + print(msg) + + self.times -= 1 + + if self.times <= 0: + return "outcome2" + + return "outcome1" + + +# main +def main(): + + print("yasmin_monitor_demo") + + # init ROS 2 + rclpy.init() + + # create a FSM + sm = StateMachine(outcomes=["outcome4"]) + + # add states + sm.add_state( + "PRINTING_ODOM", + PrintOdometryState(5), + transitions={ + "outcome1": "PRINTING_ODOM", + "outcome2": "outcome4", + CANCEL: "outcome4" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_MONITOR_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 + rclpy.shutdown() + + +if __name__ == "__main__": + main() +``` + +
+ +#### Nav2 Demo (Hierarchical FSM + ROS 2 Action) + +```shell +$ ros2 run yasmin_demo nav_demo.py +``` + +
+Click to expand + +```python +import random + +import rclpy +from geometry_msgs.msg import Pose +from nav2_msgs.action import NavigateToPose + +from yasmin import CbState +from yasmin import Blackboard +from yasmin import StateMachine +from yasmin_ros import ActionState +from yasmin_ros.basic_outcomes import SUCCEED, ABORT, CANCEL +from yasmin_viewer import YasminViewerPub + +HAS_NEXT = "has_next" +END = "end" + + +class Nav2State(ActionState): + def __init__(self) -> None: + super().__init__( + NavigateToPose, # action type + "/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 + ) + + def create_goal_handler(self, blackboard: Blackboard) -> NavigateToPose.Goal: + + goal = NavigateToPose.Goal() + goal.pose.pose = blackboard.pose + goal.pose.header.frame_id = "map" + return goal + + +def create_waypoints(blackboard: Blackboard) -> str: + 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], + "kitchen": [3.79, 6.77, 0.99, 0.12], + "bedroom": [7.50, 4.89, 0.76, 0.65], + } + return SUCCEED + + +def take_random_waypoint(blackboard) -> str: + 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: + return END + + wp_name = blackboard.random_waypoints.pop(0) + wp = blackboard.waypoints[wp_name] + + pose = Pose() + pose.position.x = wp[0] + pose.position.y = wp[1] + + pose.orientation.z = wp[2] + pose.orientation.w = wp[3] + + blackboard.pose = pose + blackboard.text = f"I have reached waypoint {wp_name}" + + return HAS_NEXT + + +# main +def main(): + + print("yasmin_nav2_demo") + + # init ROS 2 + rclpy.init() + + # create state machines + sm = StateMachine(outcomes=[SUCCEED, ABORT, CANCEL]) + nav_sm = StateMachine(outcomes=[SUCCEED, ABORT, CANCEL]) + + # add states + sm.add_state( + "CREATING_WAYPOINTS", + CbState([SUCCEED], create_waypoints), + transitions={ + SUCCEED: "TAKING_RANDOM_WAYPOINTS" + } + ) + sm.add_state( + "TAKING_RANDOM_WAYPOINTS", + CbState([SUCCEED], take_random_waypoint), + transitions={ + SUCCEED: "NAVIGATING" + } + ) + + nav_sm.add_state( + "GETTING_NEXT_WAYPOINT", + CbState([END, HAS_NEXT], get_next_waypoint), + 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 + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_NAV_DEMO", sm) + + # execute FSM + blackboard = Blackboard() + blackboard.waypoints_num = 2 + outcome = sm(blackboard) + print(outcome) + + # shutdown ROS 2 + rclpy.shutdown() + + +if __name__ == "__main__": + main() +``` + +
+ ### Cpp +#### Vanilla Demo + +```shell +$ ros2 run yasmin_demo yasmin_demo +``` + +
+Click to expand + ```cpp #include #include #include #include -#include "simple_node/node.hpp" +#include "rclcpp/rclcpp.hpp" #include "yasmin/state.hpp" #include "yasmin/state_machine.hpp" @@ -192,43 +687,375 @@ public: std::string to_string() { return "BarState"; } }; -class DemoNode : public simple_node::Node { +int main(int argc, char *argv[]) { + + std::cout << "yasmin_demo\n"; + rclcpp::init(argc, argv); + + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); + + // add states + sm->add_state("FOO", std::make_shared(), + {{"outcome1", "BAR"}, {"outcome2", "outcome4"}}); + sm->add_state("BAR", std::make_shared(), {{"outcome3", "FOO"}}); + + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; + + rclcpp::shutdown(); + + return 0; +} +``` + +
+ +#### Service Demo (FSM + ROS 2 Service Client) + +```shell +$ ros2 run demo_nodes_py add_two_ints_server +``` + +```shell +$ ros2 run yasmin_demo service_client_demo +``` + +
+Click to expand + +```cpp +#include +#include +#include + +#include "example_interfaces/srv/add_two_ints.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "yasmin/cb_state.hpp" +#include "yasmin/state_machine.hpp" +#include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/service_state.hpp" +#include "yasmin_viewer/yasmin_viewer_pub.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +std::string +set_ints(std::shared_ptr blackboard) { + blackboard->set("a", 10); + blackboard->set("b", 5); + return yasmin_ros::basic_outcomes::SUCCEED; +} + +std::string +print_sum(std::shared_ptr blackboard) { + fprintf(stderr, "Sum: %d\n", blackboard->get("sum")); + return yasmin_ros::basic_outcomes::SUCCEED; +} + +class AddTwoIntsState + : public yasmin_ros::ServiceState { + public: - std::unique_ptr yamin_pub; + AddTwoIntsState() + : yasmin_ros::ServiceState // msg + ( // node + "/add_two_ints", // srv name + std::bind(&AddTwoIntsState::create_request_handler, this, _1), + {"outcome1"}, + std::bind(&AddTwoIntsState::response_handler, this, _1, _2)){}; + + example_interfaces::srv::AddTwoInts::Request::SharedPtr + create_request_handler( + std::shared_ptr blackboard) { - DemoNode() : simple_node::Node("yasmin_node") { + auto request = + std::make_shared(); - // create a state machine - auto sm = std::make_shared( - yasmin::StateMachine({"outcome4"})); + request->a = blackboard->get("a"); + request->b = blackboard->get("b"); - // add states - sm->add_state("FOO", std::make_shared(), - {{"outcome1", "BAR"}, {"outcome2", "outcome4"}}); - sm->add_state("BAR", std::make_shared(), {{"outcome3", "FOO"}}); + return request; + } + + std::string response_handler( + std::shared_ptr blackboard, + example_interfaces::srv::AddTwoInts::Response::SharedPtr response) { - // pub - this->yamin_pub = std::make_unique( - yasmin_viewer::YasminViewerPub(this, "YASMIN_DEMO", sm)); + blackboard->set("sum", response->sum); - // execute - std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; + return "outcome1"; } + + std::string to_string() { return "AddTwoIntsState"; } }; int main(int argc, char *argv[]) { - std::cout << "yasmin_demo\n"; + std::cout << "yasmin_service_client_demo\n"; rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->join_spin(); + + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); + + // add states + sm->add_state("SETTING_INTS", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, set_ints)), + {{yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}}); + sm->add_state("ADD_TWO_INTS", std::make_shared(), + {{"outcome1", "PRINTING_SUM"}, + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + sm->add_state("PRINTING_SUM", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, print_sum)), + {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; + rclcpp::shutdown(); return 0; } ``` +
+ +#### Action Demo (FSM + ROS 2 Action) + +```shell +$ ros2 run action_tutorials_cpp fibonacci_action_server +``` + +```shell +$ ros2 run yasmin_demo action_client_demo +``` + +
+Click to expand + +```cpp +#include +#include +#include + +#include "action_tutorials_interfaces/action/fibonacci.hpp" + +#include "yasmin/cb_state.hpp" +#include "yasmin/state_machine.hpp" +#include "yasmin_ros/action_state.hpp" +#include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/yasmin_node.hpp" +#include "yasmin_viewer/yasmin_viewer_pub.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +std::string +set_int(std::shared_ptr blackboard) { + blackboard->set("n", 3); + return yasmin_ros::basic_outcomes::SUCCEED; +} + +std::string +print_result(std::shared_ptr blackboard) { + + auto fibo_res = blackboard->get>("sum"); + + fprintf(stderr, "Sum:"); + + for (auto ele : fibo_res) { + fprintf(stderr, " %d,", ele); + } + + fprintf(stderr, "\n"); + + return yasmin_ros::basic_outcomes::SUCCEED; +} + +class FibonacciState : public yasmin_ros::ActionState< + action_tutorials_interfaces::action::Fibonacci> { + +public: + FibonacciState() + : yasmin_ros::ActionState< + action_tutorials_interfaces::action::Fibonacci> // msg + ("/fibonacci", // action name + std::bind(&FibonacciState::create_goal_handler, this, _1), + std::bind(&FibonacciState::response_handler, this, _1, _2)){}; + + action_tutorials_interfaces::action::Fibonacci::Goal create_goal_handler( + std::shared_ptr blackboard) { + + auto goal = action_tutorials_interfaces::action::Fibonacci::Goal(); + goal.order = blackboard->get("n"); + + return goal; + } + + std::string response_handler( + std::shared_ptr blackboard, + action_tutorials_interfaces::action::Fibonacci::Result::SharedPtr + response) { + + blackboard->set>("sum", response->sequence); + + return yasmin_ros::basic_outcomes::SUCCEED; + } + + std::string to_string() { return "FibonacciState"; } +}; + +int main(int argc, char *argv[]) { + + std::cout << "yasmin_action_client_demo\n"; + rclcpp::init(argc, argv); + + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); + + // add states + sm->add_state("SETTING_INT", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, set_int)), + {{yasmin_ros::basic_outcomes::SUCCEED, "CALLING_FIBONACCI"}}); + sm->add_state("CALLING_FIBONACCI", std::make_shared(), + {{yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, + {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + sm->add_state("PRINTING_RESULT", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, print_result)), + {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; + + rclcpp::shutdown(); + + return 0; +} +``` + +
+ +#### Monitor Demo (FSM + ROS 2 Subscriber) + +```shell +$ ros2 run yasmin_demo monitor_demo +``` + +
+Click to expand + +```cpp +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "nav_msgs/msg/odometry.hpp" + +#include "yasmin/state_machine.hpp" +#include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/monitor_state.hpp" +#include "yasmin_viewer/yasmin_viewer_pub.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +class PrintOdometryState + : public yasmin_ros::MonitorState { + +public: + int times; + + PrintOdometryState(int times) + : yasmin_ros::MonitorState // msg type + ("odom", // topic name + {"outcome1", "outcome2"}, // outcomes + std::bind(&PrintOdometryState::monitor_handler, this, _1, + _2), // monitor handler callback + 10, // qos for the topic sbscription + 10, // queue of the monitor handler callback + 10 // timeout to wait for msgs in seconds + // if >0, CANCEL outcome is added + ) { + this->times = times; + }; + + std::string + monitor_handler(std::shared_ptr blackboard, + std::shared_ptr msg) { + + (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"; + + this->times--; + + if (this->times <= 0) { + return "outcome2"; + } + + return "outcome1"; + } + + std::string to_string() { return "PrintOdometryState"; } +}; + +int main(int argc, char *argv[]) { + + std::cout << "yasmin_monitor_demo\n"; + rclcpp::init(argc, argv); + + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); + + // add states + sm->add_state("PRINTING_ODOM", std::make_shared(5), + {{"outcome1", "PRINTING_ODOM"}, + {"outcome2", "outcome4"}, + {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}}); + + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; + + rclcpp::shutdown(); + + return 0; +} +``` + +
+ ## YASMIN Viewer @@ -245,7 +1072,7 @@ $ ros2 run yasmin_viewer yasmin_viewer_node http://localhost:5000/ -### Also you can custom host and port +### Custom host and port ```shell $ ros2 run yasmin_viewer yasmin_viewer_node --ros-args -p host:=127.0.0.1 -p port:=5032 diff --git a/yasmin_demo/CMakeLists.txt b/yasmin_demo/CMakeLists.txt index 6409843..494d0a0 100644 --- a/yasmin_demo/CMakeLists.txt +++ b/yasmin_demo/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(rclpy REQUIRED) find_package(yasmin REQUIRED) find_package(yasmin_ros REQUIRED) find_package(yasmin_viewer REQUIRED) -find_package(simple_node REQUIRED) find_package(nav_msgs REQUIRED) find_package(example_interfaces REQUIRED) find_package(action_tutorials_interfaces REQUIRED) @@ -33,7 +32,6 @@ set(DEPENDENCIES yasmin yasmin_ros yasmin_viewer - simple_node nav_msgs example_interfaces action_tutorials_interfaces @@ -117,4 +115,9 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS + yasmin_demo/nav_demo.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/yasmin_demo/package.xml b/yasmin_demo/package.xml index cb8528d..a25051d 100644 --- a/yasmin_demo/package.xml +++ b/yasmin_demo/package.xml @@ -15,7 +15,6 @@ yasmin yasmin_ros yasmin_viewer - simple_node nav_msgs example_interfaces action_tutorials_interfaces diff --git a/yasmin_demo/src/action_client_demo.cpp b/yasmin_demo/src/action_client_demo.cpp index 6d82651..d3b7b8e 100644 --- a/yasmin_demo/src/action_client_demo.cpp +++ b/yasmin_demo/src/action_client_demo.cpp @@ -18,12 +18,12 @@ #include #include "action_tutorials_interfaces/action/fibonacci.hpp" -#include "simple_node/node.hpp" #include "yasmin/cb_state.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/action_state.hpp" #include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/yasmin_node.hpp" #include "yasmin_viewer/yasmin_viewer_pub.hpp" using std::placeholders::_1; @@ -55,22 +55,18 @@ class FibonacciState : public yasmin_ros::ActionState< action_tutorials_interfaces::action::Fibonacci> { public: - FibonacciState(simple_node::Node *node) + FibonacciState() : yasmin_ros::ActionState< action_tutorials_interfaces::action::Fibonacci> // msg - (node, // node - "/fibonacci", // action name + ("/fibonacci", // action name std::bind(&FibonacciState::create_goal_handler, this, _1), std::bind(&FibonacciState::response_handler, this, _1, _2)){}; - action_tutorials_interfaces::action::Fibonacci::Goal::SharedPtr - create_goal_handler( + action_tutorials_interfaces::action::Fibonacci::Goal create_goal_handler( std::shared_ptr blackboard) { - auto goal = std::make_shared< - action_tutorials_interfaces::action::Fibonacci::Goal>(); - - goal->order = blackboard->get("n"); + auto goal = action_tutorials_interfaces::action::Fibonacci::Goal(); + goal.order = blackboard->get("n"); return goal; } @@ -88,46 +84,36 @@ class FibonacciState : public yasmin_ros::ActionState< std::string to_string() { return "FibonacciState"; } }; -class ActionClientDemoNode : public simple_node::Node { -public: - std::unique_ptr yamin_pub; - - ActionClientDemoNode() : simple_node::Node("yasmin_node") { - - // create a state machine - auto sm = std::make_shared( - yasmin::StateMachine({"outcome4"})); - - // add states - sm->add_state("SETTING_INT", - std::make_shared(yasmin::CbState( - {yasmin_ros::basic_outcomes::SUCCEED}, set_int)), - {{yasmin_ros::basic_outcomes::SUCCEED, "CALLING_FIBONACCI"}}); - sm->add_state("CALLING_FIBONACCI", std::make_shared(this), - {{yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, - {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, - {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); - sm->add_state("PRINTING_RESULT", - std::make_shared(yasmin::CbState( - {yasmin_ros::basic_outcomes::SUCCEED}, print_result)), - {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); - - // pub - this->yamin_pub = std::make_unique( - yasmin_viewer::YasminViewerPub(this, "YASMIN_ACTION_CLIENT_DEMO", sm)); - - // execute - std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; - } -}; - int main(int argc, char *argv[]) { std::cout << "yasmin_action_client_demo\n"; rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->join_spin(); + + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); + + // add states + sm->add_state("SETTING_INT", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, set_int)), + {{yasmin_ros::basic_outcomes::SUCCEED, "CALLING_FIBONACCI"}}); + sm->add_state("CALLING_FIBONACCI", std::make_shared(), + {{yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, + {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + sm->add_state("PRINTING_RESULT", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, print_result)), + {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; + rclcpp::shutdown(); return 0; diff --git a/yasmin_demo/src/monitor_demo.cpp b/yasmin_demo/src/monitor_demo.cpp index 38df38c..c842f18 100644 --- a/yasmin_demo/src/monitor_demo.cpp +++ b/yasmin_demo/src/monitor_demo.cpp @@ -20,7 +20,6 @@ #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "simple_node/node.hpp" #include "yasmin/state_machine.hpp" #include "yasmin_ros/basic_outcomes.hpp" @@ -36,17 +35,16 @@ class PrintOdometryState public: int times; - PrintOdometryState(simple_node::Node *node, int times) + PrintOdometryState(int times) : yasmin_ros::MonitorState // msg type - (node, // node - "odom", // topic name + ("odom", // topic name {"outcome1", "outcome2"}, // outcomes std::bind(&PrintOdometryState::monitor_handler, this, _1, - _2), // monitor handler callback - rclcpp::SensorDataQoS(), // qos for the topic sbscription - 10, // queue of the monitor handler callback - 10 // timeout to wait for msgs in seconds - // if >0, CANCEL outcome is added + _2), // monitor handler callback + 10, // qos for the topic sbscription + 10, // queue of the monitor handler callback + 10 // timeout to wait for msgs in seconds + // if >0, CANCEL outcome is added ) { this->times = times; }; @@ -55,6 +53,8 @@ class PrintOdometryState monitor_handler(std::shared_ptr blackboard, std::shared_ptr msg) { + (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"; @@ -72,39 +72,28 @@ class PrintOdometryState std::string to_string() { return "PrintOdometryState"; } }; -class MonitorDemoNode : public simple_node::Node { -public: - std::unique_ptr yamin_pub; +int main(int argc, char *argv[]) { - MonitorDemoNode() : simple_node::Node("yasmin_node") { + std::cout << "yasmin_monitor_demo\n"; + rclcpp::init(argc, argv); - // create a state machine - auto sm = std::make_shared( - yasmin::StateMachine({"outcome4"})); + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); - // add states - sm->add_state("PRINTING_ODOM", - std::make_shared(this, 5), - {{"outcome1", "PRINTING_ODOM"}, - {"outcome2", "outcome4"}, - {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}}); + // add states + sm->add_state("PRINTING_ODOM", std::make_shared(5), + {{"outcome1", "PRINTING_ODOM"}, + {"outcome2", "outcome4"}, + {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}}); - // pub - this->yamin_pub = std::make_unique( - yasmin_viewer::YasminViewerPub(this, "YASMIN_MONITOR_DEMO", sm)); + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); - // execute - std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; - } -}; - -int main(int argc, char *argv[]) { + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; - std::cout << "yasmin_monitor_demo\n"; - rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->join_spin(); rclcpp::shutdown(); return 0; diff --git a/yasmin_demo/src/service_client_demo.cpp b/yasmin_demo/src/service_client_demo.cpp index f13ac8d..96eea66 100644 --- a/yasmin_demo/src/service_client_demo.cpp +++ b/yasmin_demo/src/service_client_demo.cpp @@ -18,7 +18,7 @@ #include #include "example_interfaces/srv/add_two_ints.hpp" -#include "simple_node/node.hpp" +#include "rclcpp/rclcpp.hpp" #include "yasmin/cb_state.hpp" #include "yasmin/state_machine.hpp" @@ -46,13 +46,13 @@ class AddTwoIntsState : public yasmin_ros::ServiceState { public: - AddTwoIntsState(simple_node::Node *node) + AddTwoIntsState() : yasmin_ros::ServiceState // msg - (node, // node - "/add_two_ints", // srv name - std::bind(&AddTwoIntsState::create_request_handler, this, _1), - {"outcome1"}, - std::bind(&AddTwoIntsState::response_handler, this, _1, _2)){}; + ( // node + "/add_two_ints", // srv name + std::bind(&AddTwoIntsState::create_request_handler, this, _1), + {"outcome1"}, + std::bind(&AddTwoIntsState::response_handler, this, _1, _2)){}; example_interfaces::srv::AddTwoInts::Request::SharedPtr create_request_handler( @@ -79,46 +79,36 @@ class AddTwoIntsState std::string to_string() { return "AddTwoIntsState"; } }; -class ServiceClientDemoNode : public simple_node::Node { -public: - std::unique_ptr yamin_pub; - - ServiceClientDemoNode() : simple_node::Node("yasmin_node") { - - // create a state machine - auto sm = std::make_shared( - yasmin::StateMachine({"outcome4"})); - - // add states - sm->add_state("SETTING_INTS", - std::make_shared(yasmin::CbState( - {yasmin_ros::basic_outcomes::SUCCEED}, set_ints)), - {{yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}}); - sm->add_state("ADD_TWO_INTS", std::make_shared(this), - {{"outcome1", "PRINTING_SUM"}, - {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, - {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); - sm->add_state("PRINTING_SUM", - std::make_shared(yasmin::CbState( - {yasmin_ros::basic_outcomes::SUCCEED}, print_sum)), - {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); - - // pub - this->yamin_pub = std::make_unique( - yasmin_viewer::YasminViewerPub(this, "YASMIN_SERVICE_CLIENT_DEMO", sm)); - - // execute - std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; - } -}; - int main(int argc, char *argv[]) { std::cout << "yasmin_service_client_demo\n"; rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->join_spin(); + + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); + + // add states + sm->add_state("SETTING_INTS", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, set_ints)), + {{yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}}); + sm->add_state("ADD_TWO_INTS", std::make_shared(), + {{"outcome1", "PRINTING_SUM"}, + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + sm->add_state("PRINTING_SUM", + std::make_shared(yasmin::CbState( + {yasmin_ros::basic_outcomes::SUCCEED}, print_sum)), + {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; + rclcpp::shutdown(); return 0; diff --git a/yasmin_demo/src/yasmin_demo.cpp b/yasmin_demo/src/yasmin_demo.cpp index 4f9323c..9830857 100644 --- a/yasmin_demo/src/yasmin_demo.cpp +++ b/yasmin_demo/src/yasmin_demo.cpp @@ -18,7 +18,7 @@ #include #include -#include "simple_node/node.hpp" +#include "rclcpp/rclcpp.hpp" #include "yasmin/state.hpp" #include "yasmin/state_machine.hpp" @@ -68,37 +68,27 @@ class BarState : public yasmin::State { std::string to_string() { return "BarState"; } }; -class DemoNode : public simple_node::Node { -public: - std::unique_ptr yamin_pub; - - DemoNode() : simple_node::Node("yasmin_node") { +int main(int argc, char *argv[]) { - // create a state machine - auto sm = std::make_shared( - yasmin::StateMachine({"outcome4"})); + std::cout << "yasmin_demo\n"; + rclcpp::init(argc, argv); - // add states - sm->add_state("FOO", std::make_shared(), - {{"outcome1", "BAR"}, {"outcome2", "outcome4"}}); - sm->add_state("BAR", std::make_shared(), {{"outcome3", "FOO"}}); + // create a state machine + auto sm = std::make_shared( + yasmin::StateMachine({"outcome4"})); - // pub - this->yamin_pub = std::make_unique( - yasmin_viewer::YasminViewerPub(this, "YASMIN_DEMO", sm)); + // add states + sm->add_state("FOO", std::make_shared(), + {{"outcome1", "BAR"}, {"outcome2", "outcome4"}}); + sm->add_state("BAR", std::make_shared(), {{"outcome3", "FOO"}}); - // execute - std::string outcome = (*sm.get())(); - std::cout << outcome << "\n"; - } -}; + // pub + yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); -int main(int argc, char *argv[]) { + // execute + std::string outcome = (*sm.get())(); + std::cout << outcome << "\n"; - std::cout << "yasmin_demo\n"; - rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->join_spin(); rclcpp::shutdown(); return 0; diff --git a/yasmin_demo/yasmin_demo/action_client_demo.py b/yasmin_demo/yasmin_demo/action_client_demo.py index 440bf4a..1e3da21 100755 --- a/yasmin_demo/yasmin_demo/action_client_demo.py +++ b/yasmin_demo/yasmin_demo/action_client_demo.py @@ -17,10 +17,7 @@ import rclpy - -from simple_node import Node from action_tutorials_interfaces.action import Fibonacci - from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -30,9 +27,8 @@ class FibonacciState(ActionState): - def __init__(self, node: Node) -> None: + def __init__(self) -> None: super().__init__( - node, # node Fibonacci, # action type "/fibonacci", # action name self.create_goal_handler, # cb to create the goal @@ -66,39 +62,50 @@ def print_result(blackboard: Blackboard) -> str: return SUCCEED -class ActionClientDemoNode(Node): - - def __init__(self): - super().__init__("yasmin_node") - - # create a state machine - sm = StateMachine(outcomes=["outcome4"]) - - # add states - sm.add_state("SETTING_INT", CbState([SUCCEED], set_int), - transitions={SUCCEED: "CALLING_FIBONACCI"}) - sm.add_state("CALLING_FIBONACCI", FibonacciState(self), - transitions={SUCCEED: "PRINTING_RESULT", - CANCEL: "outcome4", - ABORT: "outcome4"}) - sm.add_state("PRINTING_RESULT", CbState([SUCCEED], print_result), - transitions={SUCCEED: "outcome4"}) - - # pub - YasminViewerPub(self, "YASMIN_ACTION_CLIENT_DEMO", sm) - - # execute - outcome = sm() - print(outcome) - - # main -def main(args=None): +def main(): print("yasmin_action_client_demo") - rclpy.init(args=args) - node = ActionClientDemoNode() - node.join_spin() + + # init ROS 2 + rclpy.init() + + # create a FSM + sm = StateMachine(outcomes=["outcome4"]) + + # add states + sm.add_state( + "SETTING_INT", + CbState([SUCCEED], set_int), + transitions={ + SUCCEED: "CALLING_FIBONACCI" + } + ) + sm.add_state( + "CALLING_FIBONACCI", + FibonacciState(), + transitions={ + SUCCEED: "PRINTING_RESULT", + CANCEL: "outcome4", + ABORT: "outcome4" + } + ) + sm.add_state( + "PRINTING_RESULT", + CbState([SUCCEED], print_result), + transitions={ + SUCCEED: "outcome4" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_ACTION_CLIENT_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demo/yasmin_demo/monitor_demo.py b/yasmin_demo/yasmin_demo/monitor_demo.py index 58fd070..a7be8c0 100755 --- a/yasmin_demo/yasmin_demo/monitor_demo.py +++ b/yasmin_demo/yasmin_demo/monitor_demo.py @@ -18,8 +18,6 @@ import rclpy from rclpy.qos import qos_profile_sensor_data - -from simple_node import Node from nav_msgs.msg import Odometry from yasmin import Blackboard @@ -30,17 +28,17 @@ class PrintOdometryState(MonitorState): - def __init__(self, node: Node, times: int) -> None: - super().__init__(node, # node - Odometry, # msg type - "odom", # topic name - ["outcome1", "outcome2"], # outcomes - 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 - # if not None, CANCEL outcome is added - ) + def __init__(self, times: int) -> None: + super().__init__( + Odometry, # msg type + "odom", # topic name + ["outcome1", "outcome2"], # outcomes + 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 + # if not None, CANCEL outcome is added + ) self.times = times def monitor_handler(self, blackboard: Blackboard, msg: Odometry) -> str: @@ -54,35 +52,36 @@ def monitor_handler(self, blackboard: Blackboard, msg: Odometry) -> str: return "outcome1" -class MonitorDemoNode(Node): - - def __init__(self): - super().__init__("yasmin_node") +# main +def main(): - # create a state machine - sm = StateMachine(outcomes=["outcome4"]) + print("yasmin_monitor_demo") - # add states - sm.add_state("PRINTING_ODOM", PrintOdometryState(self, 5), - transitions={"outcome1": "PRINTING_ODOM", - "outcome2": "outcome4", - CANCEL: "outcome4"}) + # init ROS 2 + rclpy.init() - # pub - YasminViewerPub(self, "YASMIN_MONITOR_DEMO", sm) + # create a FSM + sm = StateMachine(outcomes=["outcome4"]) - # execute - outcome = sm() - print(outcome) + # add states + sm.add_state( + "PRINTING_ODOM", + PrintOdometryState(5), + transitions={ + "outcome1": "PRINTING_ODOM", + "outcome2": "outcome4", + CANCEL: "outcome4" + } + ) + # pub FSM info + YasminViewerPub("YASMIN_MONITOR_DEMO", sm) -# main -def main(args=None): + # execute FSM + outcome = sm() + print(outcome) - print("yasmin_monitor_demo") - rclpy.init(args=args) - node = MonitorDemoNode() - node.join_spin() + # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demo/yasmin_demo/nav_demo.py b/yasmin_demo/yasmin_demo/nav_demo.py new file mode 100755 index 0000000..8a115c3 --- /dev/null +++ b/yasmin_demo/yasmin_demo/nav_demo.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 + +# 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 random + +import rclpy +from geometry_msgs.msg import Pose +from nav2_msgs.action import NavigateToPose + +from yasmin import CbState +from yasmin import Blackboard +from yasmin import StateMachine +from yasmin_ros import ActionState +from yasmin_ros.basic_outcomes import SUCCEED, ABORT, CANCEL +from yasmin_viewer import YasminViewerPub + +HAS_NEXT = "has_next" +END = "end" + + +class Nav2State(ActionState): + def __init__(self) -> None: + super().__init__( + NavigateToPose, # action type + "/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 + ) + + def create_goal_handler(self, blackboard: Blackboard) -> NavigateToPose.Goal: + + goal = NavigateToPose.Goal() + goal.pose.pose = blackboard.pose + goal.pose.header.frame_id = "map" + return goal + + +def create_waypoints(blackboard: Blackboard) -> str: + 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], + "kitchen": [3.79, 6.77, 0.99, 0.12], + "bedroom": [7.50, 4.89, 0.76, 0.65], + } + return SUCCEED + + +def take_random_waypoint(blackboard) -> str: + 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: + return END + + wp_name = blackboard.random_waypoints.pop(0) + wp = blackboard.waypoints[wp_name] + + pose = Pose() + pose.position.x = wp[0] + pose.position.y = wp[1] + + pose.orientation.z = wp[2] + pose.orientation.w = wp[3] + + blackboard.pose = pose + blackboard.text = f"I have reached waypoint {wp_name}" + + return HAS_NEXT + + +# main +def main(): + + print("yasmin_nav2_demo") + + # init ROS 2 + rclpy.init() + + # create state machines + sm = StateMachine(outcomes=[SUCCEED, ABORT, CANCEL]) + nav_sm = StateMachine(outcomes=[SUCCEED, ABORT, CANCEL]) + + # add states + sm.add_state( + "CREATING_WAYPOINTS", + CbState([SUCCEED], create_waypoints), + transitions={ + SUCCEED: "TAKING_RANDOM_WAYPOINTS" + } + ) + sm.add_state( + "TAKING_RANDOM_WAYPOINTS", + CbState([SUCCEED], take_random_waypoint), + transitions={ + SUCCEED: "NAVIGATING" + } + ) + + nav_sm.add_state( + "GETTING_NEXT_WAYPOINT", + CbState([END, HAS_NEXT], get_next_waypoint), + 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 + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_NAV_DEMO", sm) + + # execute FSM + blackboard = Blackboard() + blackboard.waypoints_num = 2 + outcome = sm(blackboard) + print(outcome) + + # shutdown ROS 2 + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/yasmin_demo/yasmin_demo/service_client_demo.py b/yasmin_demo/yasmin_demo/service_client_demo.py index c4aa4ed..0858ece 100755 --- a/yasmin_demo/yasmin_demo/service_client_demo.py +++ b/yasmin_demo/yasmin_demo/service_client_demo.py @@ -17,10 +17,7 @@ import rclpy - -from simple_node import Node from example_interfaces.srv import AddTwoInts - from yasmin import CbState from yasmin import Blackboard from yasmin import StateMachine @@ -30,9 +27,8 @@ class AddTwoIntsState(ServiceState): - def __init__(self, node: Node) -> None: + def __init__(self) -> None: super().__init__( - node, # node AddTwoInts, # srv type "/add_two_ints", # service name self.create_request_handler, # cb to create the request @@ -68,39 +64,50 @@ def print_sum(blackboard: Blackboard) -> str: return SUCCEED -class ServiceClientDemoNode(Node): - - def __init__(self): - super().__init__("yasmin_node") - - # create a state machine - sm = StateMachine(outcomes=["outcome4"]) - - # add states - sm.add_state("SETTING_INTS", CbState([SUCCEED], set_ints), - transitions={SUCCEED: "ADD_TWO_INTS"}) - sm.add_state("ADD_TWO_INTS", AddTwoIntsState(self), - transitions={"outcome1": "PRINTING_SUM", - SUCCEED: "outcome4", - ABORT: "outcome4"}) - sm.add_state("PRINTING_SUM", CbState([SUCCEED], print_sum), - transitions={SUCCEED: "outcome4"}) - - # pub - YasminViewerPub(self, "YASMIN_SERVICE_CLIENT_DEMO", sm) - - # execute - outcome = sm() - print(outcome) - - # main -def main(args=None): +def main(): print("yasmin_service_client_demo") - rclpy.init(args=args) - node = ServiceClientDemoNode() - node.join_spin() + + # init ROS 2 + rclpy.init() + + # create a FSM + sm = StateMachine(outcomes=["outcome4"]) + + # add states + sm.add_state( + "SETTING_INTS", + CbState([SUCCEED], set_ints), + transitions={ + SUCCEED: "ADD_TWO_INTS" + } + ) + sm.add_state( + "ADD_TWO_INTS", + AddTwoIntsState(), + transitions={ + "outcome1": "PRINTING_SUM", + SUCCEED: "outcome4", + ABORT: "outcome4" + } + ) + sm.add_state( + "PRINTING_SUM", + CbState([SUCCEED], print_sum), + transitions={ + SUCCEED: "outcome4" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_SERVICE_CLIENT_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_demo/yasmin_demo/yasmin_demo.py b/yasmin_demo/yasmin_demo/yasmin_demo.py index 6c73348..b547376 100755 --- a/yasmin_demo/yasmin_demo/yasmin_demo.py +++ b/yasmin_demo/yasmin_demo/yasmin_demo.py @@ -18,9 +18,6 @@ import time import rclpy - -from simple_node import Node - from yasmin import State from yasmin import Blackboard from yasmin import StateMachine @@ -58,36 +55,42 @@ def execute(self, blackboard: Blackboard) -> str: return "outcome3" -class DemoNode(Node): - - def __init__(self) -> None: - super().__init__("yasmin_node") - - # create a state machine - sm = StateMachine(outcomes=["outcome4"]) - - # add states - sm.add_state("FOO", FooState(), - transitions={"outcome1": "BAR", - "outcome2": "outcome4"}) - sm.add_state("BAR", BarState(), - transitions={"outcome3": "FOO"}) - - # pub - YasminViewerPub(self, "YASMIN_DEMO", sm) - - # execute - outcome = sm() - print(outcome) - - # main -def main(args=None): +def main(): print("yasmin_demo") - rclpy.init(args=args) - node = DemoNode() - node.join_spin() + + # init ROS 2 + rclpy.init() + + # 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" + } + ) + + # pub FSM info + YasminViewerPub("YASMIN_DEMO", sm) + + # execute FSM + outcome = sm() + print(outcome) + + # shutdown ROS 2 rclpy.shutdown() diff --git a/yasmin_ros/CMakeLists.txt b/yasmin_ros/CMakeLists.txt index f6ad8ec..639e1d6 100644 --- a/yasmin_ros/CMakeLists.txt +++ b/yasmin_ros/CMakeLists.txt @@ -14,8 +14,8 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rclpy REQUIRED) -find_package(simple_node REQUIRED) find_package(yasmin REQUIRED) # C++ @@ -24,6 +24,7 @@ include_directories(src) set(LIB ${CMAKE_PROJECT_NAME}_lib) set(SOURCES + src/yasmin_ros/yasmin_node.cpp src/yasmin_ros/action_state.cpp src/yasmin_ros/service_state.cpp src/yasmin_ros/monitor_state.cpp @@ -31,7 +32,7 @@ set(SOURCES set(DEPENDENCIES rclcpp - simple_node + rclcpp_action yasmin ) diff --git a/yasmin_ros/include/yasmin_ros/action_state.hpp b/yasmin_ros/include/yasmin_ros/action_state.hpp index 7a67a98..fb2bb47 100644 --- a/yasmin_ros/include/yasmin_ros/action_state.hpp +++ b/yasmin_ros/include/yasmin_ros/action_state.hpp @@ -16,42 +16,56 @@ #ifndef YASMIN_ROS_ACTION_STATE_HPP #define YASMIN_ROS_ACTION_STATE_HPP +#include #include #include -#include "simple_node/actions/action_client.hpp" -#include "simple_node/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "yasmin/blackboard/blackboard.hpp" #include "yasmin/state.hpp" #include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/yasmin_node.hpp" + +using namespace std::placeholders; namespace yasmin_ros { template class ActionState : public yasmin::State { - using Goal = typename ActionT::Goal::SharedPtr; + using Goal = typename ActionT::Goal; using Result = typename ActionT::Result::SharedPtr; + + using SendGoalOptions = + typename rclcpp_action::Client::SendGoalOptions; + using ActionClient = typename rclcpp_action::Client::SharedPtr; + using GoalHandle = rclcpp_action::ClientGoalHandle; + using CreateGoalHandler = std::function)>; using ResutlHandler = std::function, Result)>; public: - ActionState(simple_node::Node *node, std::string action_name, - CreateGoalHandler create_goal_handler, - std::vector outcomes) - : ActionState(node, action_name, create_goal_handler, outcomes, nullptr) {} - - ActionState(simple_node::Node *node, std::string action_name, - CreateGoalHandler create_goal_handler, - ResutlHandler result_handler) - : ActionState(node, action_name, create_goal_handler, {}, result_handler) { - } - - ActionState(simple_node::Node *node, std::string action_name, - CreateGoalHandler create_goal_handler, - std::vector outcomes, ResutlHandler result_handler) + ActionState(std::string action_name, CreateGoalHandler create_goal_handler, + std::vector outcomes) + : ActionState(action_name, create_goal_handler, outcomes, nullptr) {} + + ActionState(std::string action_name, CreateGoalHandler create_goal_handler, + ResutlHandler result_handler = nullptr) + : ActionState(action_name, create_goal_handler, {}, result_handler) {} + + ActionState(std::string action_name, CreateGoalHandler create_goal_handler, + std::vector outcomes, + ResutlHandler result_handler = nullptr) + : ActionState(nullptr, action_name, create_goal_handler, outcomes, + result_handler) {} + + ActionState(rclcpp::Node *node, std::string action_name, + CreateGoalHandler create_goal_handler, + std::vector outcomes, + ResutlHandler result_handler = nullptr) : State({}) { this->outcomes = {basic_outcomes::SUCCEED, basic_outcomes::ABORT, @@ -63,7 +77,14 @@ template class ActionState : public yasmin::State { } } - this->action_client = node->create_action_client(action_name); + if (node == nullptr) { + this->node = YasminNode::get_instance(); + } else { + this->node = node; + } + + this->action_client = + rclcpp_action::create_client(this->node, action_name); this->create_goal_handler = create_goal_handler; this->result_handler = result_handler; @@ -74,31 +95,42 @@ template class ActionState : public yasmin::State { } void cancel_state() { - this->action_client->cancel_goal(); + // this->action_client->cancel_goal(); yasmin::State::cancel_state(); } std::string execute(std::shared_ptr blackboard) { - Goal goal = this->create_goal(blackboard); + std::unique_lock lock(this->action_done_mutex); + + Goal goal = this->create_goal_handler(blackboard); this->action_client->wait_for_action_server(); - this->action_client->send_goal(*goal); - this->action_client->wait_for_result(); - if (this->action_client->is_canceled()) { + // options + auto send_goal_options = SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&ActionState::goal_response_callback, this, _1); + + send_goal_options.result_callback = + std::bind(&ActionState::result_callback, this, _1); + + this->action_client->async_send_goal(goal, send_goal_options); + + // wait for results + this->action_done_cond.wait(lock); + + if (this->action_status == rclcpp_action::ResultCode::CANCELED) { return basic_outcomes::CANCEL; - } else if (this->action_client->is_aborted()) { + } else if (this->action_status == rclcpp_action::ResultCode::ABORTED) { return basic_outcomes::ABORT; - } else if (this->action_client->is_succeeded()) { - Result result = this->action_client->get_result(); + } else if (this->action_status == rclcpp_action::ResultCode::SUCCEEDED) { if (this->result_handler != nullptr) { - std::string outcome = this->result_handler(blackboard, result); - return outcome; + return this->result_handler(blackboard, this->action_result); } return basic_outcomes::SUCCEED; @@ -108,12 +140,31 @@ template class ActionState : public yasmin::State { } private: - std::shared_ptr> action_client; + rclcpp::Node *node; + + ActionClient action_client; + std::condition_variable action_done_cond; + std::mutex action_done_mutex; + + Result action_result; + rclcpp_action::ResultCode action_status; + + std::shared_ptr goal_handle; + std::mutex goal_handle_mutex; + CreateGoalHandler create_goal_handler; ResutlHandler result_handler; - Goal create_goal(std::shared_ptr blackboard) { - return this->create_goal_handler(blackboard); + void + goal_response_callback(const typename GoalHandle::SharedPtr &goal_handle) { + std::lock_guard lock(this->goal_handle_mutex); + this->goal_handle = goal_handle; + } + + void result_callback(const typename GoalHandle::WrappedResult &result) { + this->action_result = result.result; + this->action_status = result.code; + this->action_done_cond.notify_one(); } }; diff --git a/yasmin_ros/include/yasmin_ros/monitor_state.hpp b/yasmin_ros/include/yasmin_ros/monitor_state.hpp index db4397a..a53651b 100644 --- a/yasmin_ros/include/yasmin_ros/monitor_state.hpp +++ b/yasmin_ros/include/yasmin_ros/monitor_state.hpp @@ -21,11 +21,11 @@ #include #include "rclcpp/rclcpp.hpp" -#include "simple_node/node.hpp" #include "yasmin/blackboard/blackboard.hpp" #include "yasmin/state.hpp" #include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/yasmin_node.hpp" using std::placeholders::_1; @@ -37,18 +37,22 @@ template class MonitorState : public yasmin::State { std::shared_ptr, std::shared_ptr)>; public: - MonitorState(simple_node::Node *node, std::string topic_name, - std::vector outcomes, + MonitorState(std::string topic_name, std::vector outcomes, MonitorHandler monitor_handler, rclcpp::QoS qos, int msg_queue) - : MonitorState(node, topic_name, outcomes, monitor_handler, qos, - msg_queue, -1) {} + : MonitorState(topic_name, outcomes, monitor_handler, qos, msg_queue, + -1) {} - MonitorState(simple_node::Node *node, std::string topic_name, - std::vector outcomes, + MonitorState(std::string topic_name, std::vector outcomes, MonitorHandler monitor_handler) - : MonitorState(node, topic_name, outcomes, monitor_handler, 10, 10, -1) {} + : MonitorState(topic_name, outcomes, monitor_handler, 10, 10, -1) {} + + MonitorState(std::string topic_name, std::vector outcomes, + MonitorHandler monitor_handler, rclcpp::QoS qos, int msg_queue, + int timeout) + : MonitorState(nullptr, topic_name, outcomes, monitor_handler, qos, + msg_queue, timeout) {} - MonitorState(simple_node::Node *node, std::string topic_name, + MonitorState(rclcpp::Node *node, std::string topic_name, std::vector outcomes, MonitorHandler monitor_handler, rclcpp::QoS qos, int msg_queue, int timeout) @@ -67,6 +71,12 @@ template class MonitorState : public yasmin::State { } } + if (node == nullptr) { + this->node = YasminNode::get_instance(); + } else { + this->node = node; + } + // other attributes this->monitor_handler = monitor_handler; this->msg_queue = msg_queue; @@ -75,7 +85,7 @@ template class MonitorState : public yasmin::State { this->monitoring = false; // create subscriber - this->sub = node->create_subscription( + this->sub = this->node->create_subscription( topic_name, qos, std::bind(&MonitorState::callback, this, _1)); } @@ -110,6 +120,7 @@ template class MonitorState : public yasmin::State { } private: + rclcpp::Node *node; std::shared_ptr> sub; MonitorHandler monitor_handler; diff --git a/yasmin_ros/include/yasmin_ros/service_state.hpp b/yasmin_ros/include/yasmin_ros/service_state.hpp index 4f87b55..fcdf9fb 100644 --- a/yasmin_ros/include/yasmin_ros/service_state.hpp +++ b/yasmin_ros/include/yasmin_ros/service_state.hpp @@ -20,11 +20,11 @@ #include #include "rclcpp/rclcpp.hpp" -#include "simple_node/node.hpp" #include "yasmin/blackboard/blackboard.hpp" #include "yasmin/state.hpp" #include "yasmin_ros/basic_outcomes.hpp" +#include "yasmin_ros/yasmin_node.hpp" namespace yasmin_ros { @@ -38,13 +38,19 @@ template class ServiceState : public yasmin::State { std::shared_ptr, Response)>; public: - ServiceState(simple_node::Node *node, std::string srv_name, + ServiceState(std::string srv_name, CreateRequestHandler create_request_handler, std::vector outcomes) - : ServiceState(node, srv_name, create_request_handler, outcomes, - nullptr) {} + : ServiceState(srv_name, create_request_handler, outcomes, nullptr) {} - ServiceState(simple_node::Node *node, std::string srv_name, + ServiceState(std::string srv_name, + CreateRequestHandler create_request_handler, + std::vector outcomes, + ResponseHandler response_handler) + : ServiceState(nullptr, srv_name, create_request_handler, outcomes, + response_handler) {} + + ServiceState(rclcpp::Node *node, std::string srv_name, CreateRequestHandler create_request_handler, std::vector outcomes, ResponseHandler response_handler) @@ -58,7 +64,13 @@ template class ServiceState : public yasmin::State { } } - this->service_client = node->create_client(srv_name); + if (node == nullptr) { + this->node = YasminNode::get_instance(); + } else { + this->node = node; + } + + this->service_client = this->node->create_client(srv_name); this->create_request_handler = create_request_handler; this->response_handler = response_handler; @@ -94,6 +106,7 @@ template class ServiceState : public yasmin::State { } private: + rclcpp::Node *node; std::shared_ptr> service_client; CreateRequestHandler create_request_handler; ResponseHandler response_handler; diff --git a/yasmin_ros/include/yasmin_ros/yasmin_node.hpp b/yasmin_ros/include/yasmin_ros/yasmin_node.hpp new file mode 100644 index 0000000..e1ee644 --- /dev/null +++ b/yasmin_ros/include/yasmin_ros/yasmin_node.hpp @@ -0,0 +1,50 @@ +// 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_YASMIN_NODE_HPP +#define YASMIN_ROS_YASMIN_NODE_HPP + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "yasmin/blackboard/blackboard.hpp" +#include "yasmin/state.hpp" +#include "yasmin_ros/basic_outcomes.hpp" + +namespace yasmin_ros { + +class YasminNode : public rclcpp::Node { + +public: + explicit YasminNode(); + YasminNode(YasminNode &other) = delete; + void operator=(const YasminNode &) = delete; + ~YasminNode() {} + + static YasminNode *get_instance() { + static YasminNode instance{}; + return &instance; + } + +private: + rclcpp::executors::MultiThreadedExecutor executor; + std::unique_ptr spin_thread; +}; + +} // namespace yasmin_ros + +#endif diff --git a/yasmin_ros/package.xml b/yasmin_ros/package.xml index 0ebd3dc..a4e43e8 100644 --- a/yasmin_ros/package.xml +++ b/yasmin_ros/package.xml @@ -12,7 +12,6 @@ rclcpp rclpy - simple_node yasmin ament_copyright diff --git a/yasmin_ros/src/yasmin_ros/yasmin_node.cpp b/yasmin_ros/src/yasmin_ros/yasmin_node.cpp new file mode 100644 index 0000000..ad17afe --- /dev/null +++ b/yasmin_ros/src/yasmin_ros/yasmin_node.cpp @@ -0,0 +1,48 @@ +// 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_ros/yasmin_node.hpp" + +using namespace yasmin_ros; + +std::string generateUUID() { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(0, 15); + auto rand_hex_digit = [&gen, &dis]() { + constexpr char hex_digits[] = "0123456789abcdef"; + return hex_digits[dis(gen)]; + }; + + std::stringstream ss; + for (int i = 0; i < 32; ++i) { + if (i == 8 || i == 12 || i == 16 || i == 20) + ss << "_"; + ss << rand_hex_digit(); + } + return ss.str(); +} + +YasminNode::YasminNode() : rclcpp::Node("yasmin_" + generateUUID() + "_node") { + + this->executor.add_node(this->get_node_base_interface()); + this->spin_thread = std::make_unique( + &rclcpp::executors::MultiThreadedExecutor::spin, &this->executor); + this->spin_thread->detach(); +} diff --git a/yasmin_ros/tests/python/test_yasmin_ros.py b/yasmin_ros/tests/python/test_yasmin_ros.py index e5b3520..e324772 100644 --- a/yasmin_ros/tests/python/test_yasmin_ros.py +++ b/yasmin_ros/tests/python/test_yasmin_ros.py @@ -18,65 +18,92 @@ import time from threading import Thread -from yasmin_ros import ActionState, ServiceState +from yasmin_ros import ActionState, ServiceState, MonitorState from yasmin_ros.basic_outcomes import SUCCEED, CANCEL, ABORT from example_interfaces.action import Fibonacci from example_interfaces.srv import AddTwoInts +from std_msgs.msg import String -from simple_node import Node import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.action import CancelResponse, GoalResponse class AuxNode(Node): def __init__(self): super().__init__("test_node") - self.action_server = self.create_action_server( - Fibonacci, "test", self.execute_action) + self.action_server = ActionServer( + self, Fibonacci, "test", + goal_callback=self.goal_callback, + handle_accepted_callback=self.handle_accepted_callback, + execute_callback=self.execute_action, + cancel_callback=self.cancel_action, + callback_group=ReentrantCallbackGroup() + ) self.service_server = self.create_service( AddTwoInts, "test", self.execute_service) + self.pub = self.create_publisher(String, "test", 10) + self.timer = self.create_timer(1, self.publis_msgs) + + def goal_callback(self, goal_request) -> int: + return GoalResponse.ACCEPT + + def handle_accepted_callback(self, goal_handle) -> None: + goal_handle.execute() + def execute_action(self, goal_handle): result = Fibonacci.Result() request = goal_handle.request - if request.order >= 0: + if request.order < 0: + goal_handle.abort() - counter = 0 - while not goal_handle.is_cancel_requested and counter < request.order: - time.sleep(1) + else: + time.sleep(3) if goal_handle.is_cancel_requested: goal_handle.canceled() - else: goal_handle.succeed() - else: - goal_handle.abort() - return result + def cancel_action(self, goal_handle) -> int: + return CancelResponse.ACCEPT + def execute_service(self, request, response): response.sum = request.a + request.b return response + def publis_msgs(self) -> None: + msg = String() + msg.data = "data" + self.pub.publish(msg) + class TestYasminRos(unittest.TestCase): - def setUp(self): - super().setUp() + @classmethod + def setUpClass(cls): rclpy.init() - self.node = Node("test_yasmin_node") - self.aux_node = AuxNode() - # states + cls.aux_node = AuxNode() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.aux_node) + + cls.spin_thread = Thread(target=cls.executor.spin) + cls.spin_thread.start() - def tearDown(self): - super().tearDown() + @classmethod + def tearDownClass(cls): rclpy.shutdown() def test_yasmin_ros_action_succeed(self): @@ -86,7 +113,7 @@ def create_goal_cb(blackboard): goal.order = 0 return goal - state = ActionState(self.node, Fibonacci, "test", create_goal_cb) + state = ActionState(Fibonacci, "test", create_goal_cb) self.assertEqual(SUCCEED, state()) def test_yasmin_ros_action_result_handler(self): @@ -99,8 +126,10 @@ def create_goal_cb(blackboard): def result_handler(blackboard, result): return "new_outcome" - state = ActionState(self.node, Fibonacci, "test", create_goal_cb, [ - "new_outcome"], result_handler) + state = ActionState(Fibonacci, "test", + create_goal_cb, + ["new_outcome"], + result_handler) self.assertEqual("new_outcome", state()) def test_yasmin_ros_action_cancel(self): @@ -114,8 +143,8 @@ def cancel_state(state, seconds): time.sleep(seconds) state.cancel_state() - state = ActionState(self.node, Fibonacci, "test", create_goal_cb) - thread = Thread(target=cancel_state, args=(state, 5,)) + state = ActionState(Fibonacci, "test", create_goal_cb) + thread = Thread(target=cancel_state, args=(state, 1,)) thread.start() self.assertEqual(CANCEL, state()) thread.join() @@ -127,7 +156,7 @@ def create_goal_cb(blackboard): goal.order = -1 return goal - state = ActionState(self.node, Fibonacci, "test", create_goal_cb) + state = ActionState(Fibonacci, "test", create_goal_cb) self.assertEqual(ABORT, state()) def test_yasmin_ros_service(self): @@ -138,7 +167,7 @@ def create_request_cb(blackboard): request.b = 3 return request - state = ServiceState(self.node, AddTwoInts, "test", create_request_cb) + state = ServiceState(AddTwoInts, "test", create_request_cb) self.assertEqual(SUCCEED, state()) def test_yasmin_ros_service_response_handler(self): @@ -152,6 +181,18 @@ def create_request_cb(blackboard): def response_handler(blackboard, response): return "new_outcome" - state = ServiceState(self.node, AddTwoInts, "test", - create_request_cb, ["new_outcome"], response_handler) + state = ServiceState(AddTwoInts, "test", + create_request_cb, + ["new_outcome"], + response_handler) self.assertEqual("new_outcome", state()) + + def test_yasmin_ros_monitor_timeout(self): + + def monitor_handler(blackboard, msg): + return SUCCEED + + state = MonitorState(String, "test1", [SUCCEED], + monitor_handler=monitor_handler, + timeout=2) + self.assertEqual(CANCEL, state()) diff --git a/yasmin_ros/yasmin_ros/action_state.py b/yasmin_ros/yasmin_ros/action_state.py index d547522..f68dc8a 100644 --- a/yasmin_ros/yasmin_ros/action_state.py +++ b/yasmin_ros/yasmin_ros/action_state.py @@ -15,22 +15,44 @@ from typing import List, Callable, Type, Any +from threading import RLock, Event + +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle +from rclpy.callback_groups import ReentrantCallbackGroup +from action_msgs.msg import GoalStatus + from yasmin import State from yasmin import Blackboard -from simple_node import Node +from yasmin_ros.yasmin_node import YasminNode from yasmin_ros.basic_outcomes import SUCCEED, ABORT, CANCEL class ActionState(State): + _node: Node + + _action_client: ActionClient + _action_done_event: Event = Event() + + _action_result: Any + _action_status: GoalStatus + + _goal_handle: ClientGoalHandle + _goal_handle_lock: RLock = RLock() + + _create_goal_handler: Callable + _result_handler: Callable + def __init__( self, - node: Node, action_type: Type, action_name: str, create_goal_handler: Callable, outcomes: List[str] = None, - result_handler: Callable = None + result_handler: Callable = None, + node: Node = None ) -> None: _outcomes = [SUCCEED, ABORT, CANCEL] @@ -38,44 +60,68 @@ def __init__( if outcomes: _outcomes = _outcomes + outcomes - self.__action_client = node.create_action_client( - action_type, action_name) + if node is None: + self._node = YasminNode.get_instance() + else: + self._node = node + + self._action_client = ActionClient( + self._node, + action_type, + action_name, + callback_group=ReentrantCallbackGroup() + ) - self.__create_goal_handler = create_goal_handler - self.__result_handler = result_handler + self._create_goal_handler = create_goal_handler + self._result_handler = result_handler - if not self.__create_goal_handler: + if not self._create_goal_handler: raise Exception("create_goal_handler is needed") super().__init__(_outcomes) - def _create_goal(self, blackboard: Blackboard) -> Any: - return self.__create_goal_handler(blackboard) - def cancel_state(self) -> None: - self.__action_client.cancel_goal() + + with self._goal_handle_lock: + if self._goal_handle is not None: + self._goal_handle.cancel_goal() + super().cancel_state() def execute(self, blackboard: Blackboard) -> str: - goal = self._create_goal(blackboard) - self.__action_client.wait_for_server() - self.__action_client.send_goal(goal) - self.__action_client.wait_for_result() + goal = self._create_goal_handler(blackboard) + self._action_client.wait_for_server() + + self._action_done_event.clear() + send_goal_future = self._action_client.send_goal_async(goal) + send_goal_future.add_done_callback(self._goal_response_callback) - if self.__action_client.is_canceled(): + # Wait for action to be done + self._action_done_event.wait() + + if self._action_status == GoalStatus.STATUS_CANCELED: return CANCEL - elif self.__action_client.is_aborted(): + elif self._action_status == GoalStatus.STATUS_ABORTED: return ABORT - elif self.__action_client.is_succeeded(): - result = self.__action_client.get_result() + elif self._action_status == GoalStatus.STATUS_SUCCEEDED: - if self.__result_handler is not None: - outcome = self.__result_handler(blackboard, result) - return outcome + if self._result_handler is not None: + return self._result_handler(blackboard, self._action_result) return SUCCEED return ABORT + + def _goal_response_callback(self, future) -> None: + with self._goal_handle_lock: + self._goal_handle = future.result() + get_result_future = self._goal_handle.get_result_async() + get_result_future.add_done_callback(self._get_result_callback) + + def _get_result_callback(self, future) -> None: + self._action_result = future.result().result + self._action_status = future.result().status + self._action_done_event.set() diff --git a/yasmin_ros/yasmin_ros/monitor_state.py b/yasmin_ros/yasmin_ros/monitor_state.py index c756888..113c88e 100644 --- a/yasmin_ros/yasmin_ros/monitor_state.py +++ b/yasmin_ros/yasmin_ros/monitor_state.py @@ -17,42 +17,54 @@ import time from typing import List, Callable, Union, Type +from rclpy.node import Node +from rclpy.subscription import Subscription from rclpy.qos import QoSProfile from yasmin import State from yasmin import Blackboard +from yasmin_ros.yasmin_node import YasminNode from yasmin_ros.basic_outcomes import CANCEL -from simple_node import Node class MonitorState(State): + _node: Node + _sub: Subscription + _monitor_handler: Callable + + msg_list: List = [] + msg_queue: int + timeout: int + time_to_wait: float = 0.001 + monitoring: bool = False + def __init__( self, - node: Node, msg_type: Type, topic_name: str, outcomes: List[str], monitor_handler: Callable, qos: Union[QoSProfile, int] = 10, msg_queue: int = 10, - timeout: int = None + timeout: int = None, + node: Node = None ) -> None: if not timeout is None: outcomes = [CANCEL] + outcomes super().__init__(outcomes) - self.monitor_handler = monitor_handler - self.msg_list = [] + self._monitor_handler = monitor_handler self.msg_queue = msg_queue self.timeout = timeout - self.time_to_wait = 0.001 - self.monitoring = False - self.outcomes = outcomes - self.node = node - self._sub = node.create_subscription( + if node is None: + self._node = YasminNode.get_instance() + else: + self._node = node + + self._sub = self._node.create_subscription( msg_type, topic_name, self.__callback, qos) def __callback(self, msg) -> None: @@ -82,11 +94,11 @@ def execute(self, blackboard: Blackboard) -> str: elapsed_time += self.time_to_wait if self.msg_list: - outcome = self.monitor_handler(blackboard, self.msg_list[0]) + outcome = self._monitor_handler(blackboard, self.msg_list[0]) if outcome is None: - self.node.get_logger().warn("Transition undeclared or declared but unhandled.") + self._node.get_logger().warn("Transition undeclared or declared but unhandled.") self.msg_list.pop(0) - if outcome is not None and outcome in self.outcomes: + if outcome is not None and outcome in self.get_outcomes(): valid_transition = True break diff --git a/yasmin_ros/yasmin_ros/service_state.py b/yasmin_ros/yasmin_ros/service_state.py index b990ab8..f329a85 100644 --- a/yasmin_ros/yasmin_ros/service_state.py +++ b/yasmin_ros/yasmin_ros/service_state.py @@ -16,22 +16,30 @@ from typing import List, Callable, Type, Any +from rclpy.node import Node +from rclpy.client import Client + from yasmin import State from yasmin import Blackboard -from simple_node import Node +from yasmin_ros.yasmin_node import YasminNode from yasmin_ros.basic_outcomes import SUCCEED, ABORT class ServiceState(State): + _node: Node + _service_client: Client + _create_request_handler: Callable + _response_handler: Callable + def __init__( self, - node: Node, srv_type: Type, srv_name: str, create_request_handler: Callable, outcomes: List[str] = None, - response_handler: Callable = None + response_handler: Callable = None, + node: Node = None ) -> None: _outcomes = [SUCCEED, ABORT] @@ -39,31 +47,33 @@ def __init__( if outcomes: _outcomes = _outcomes + outcomes - self.__service_client = node.create_client(srv_type, srv_name) + if node is None: + self._node = YasminNode.get_instance() + else: + self._node = node - self.__create_request_handler = create_request_handler - self.__response_handler = response_handler + self._service_client = self._node.create_client(srv_type, srv_name) - if not self.__create_request_handler: + self._create_request_handler = create_request_handler + self._response_handler = response_handler + + if not self._create_request_handler: raise Exception("create_request_handler is needed") super().__init__(_outcomes) - def _create_request(self, blackboard: Blackboard) -> Any: - return self.__create_request_handler(blackboard) - def execute(self, blackboard: Blackboard) -> str: - request = self._create_request(blackboard) - self.__service_client.wait_for_service() + request = self._create_request_handler(blackboard) + self._service_client.wait_for_service() try: - response = self.__service_client.call(request) + response = self._service_client.call(request) except: return ABORT - if self.__response_handler: - outcome = self.__response_handler(blackboard, response) + if self._response_handler: + outcome = self._response_handler(blackboard, response) return outcome return SUCCEED diff --git a/yasmin_ros/yasmin_ros/yasmin_node.py b/yasmin_ros/yasmin_ros/yasmin_node.py new file mode 100644 index 0000000..2442496 --- /dev/null +++ b/yasmin_ros/yasmin_ros/yasmin_node.py @@ -0,0 +1,52 @@ +# 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 . + + +import uuid +from threading import Thread, RLock + +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor + + +class YasminNode(Node): + + _instance: "YasminNode" = None + _lock: RLock = RLock() + _executor: MultiThreadedExecutor = None + _spin_thread: Thread = None + + @staticmethod + def get_instance() -> "YasminNode": + + with YasminNode._lock: + if YasminNode._instance == None: + YasminNode._instance = YasminNode() + + return YasminNode._instance + + def __init__(self) -> None: + + if not YasminNode._instance is None: + raise Exception("This class is a Singleton") + + super().__init__( + f"yasmin_{str(uuid.uuid4()).replace('-', '_')}_node") + + # executor + self._executor = MultiThreadedExecutor() + self._executor.add_node(self) + self._spin_thread = Thread(target=self._executor.spin) + self._spin_thread.start() diff --git a/yasmin_viewer/CMakeLists.txt b/yasmin_viewer/CMakeLists.txt index a219ad3..c2d3add 100644 --- a/yasmin_viewer/CMakeLists.txt +++ b/yasmin_viewer/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) find_package(yasmin REQUIRED) +find_package(yasmin_ros REQUIRED) find_package(yasmin_msgs REQUIRED) # C++ @@ -29,6 +30,7 @@ set(SOURCES set(DEPENDENCIES rclcpp yasmin + yasmin_ros yasmin_msgs ) diff --git a/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp b/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp index c3a7b3b..e8c6d19 100644 --- a/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp +++ b/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp @@ -38,6 +38,9 @@ class YasminViewerPub { YasminViewerPub(rclcpp::Node *node, std::string fsm_name, std::shared_ptr fsm); + YasminViewerPub(std::string fsm_name, + std::shared_ptr fsm); + std::vector parse_transitions(std::map transitions); @@ -47,12 +50,15 @@ class YasminViewerPub { int parent); protected: - void start_publisher(std::string fsm_name, - std::shared_ptr fsm); + void start_publisher(); private: rclcpp::Node *node; - std::unique_ptr thread; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; + + std::string fsm_name; + std::shared_ptr fsm; }; } // namespace yasmin_viewer diff --git a/yasmin_viewer/package.xml b/yasmin_viewer/package.xml index 1647df1..363ce8d 100644 --- a/yasmin_viewer/package.xml +++ b/yasmin_viewer/package.xml @@ -13,6 +13,7 @@ rclcpp rclpy yasmin + yasmin_ros yasmin_msgs ament_copyright diff --git a/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp b/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp index a910889..711b2b8 100644 --- a/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp +++ b/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp @@ -15,19 +15,32 @@ #include +#include "yasmin_ros/yasmin_node.hpp" #include "yasmin_viewer/yasmin_viewer_pub.hpp" using namespace yasmin_viewer; using namespace std::chrono_literals; +YasminViewerPub::YasminViewerPub(std::string fsm_name, + std::shared_ptr fsm) + : YasminViewerPub(nullptr, fsm_name, fsm) {} + YasminViewerPub::YasminViewerPub(rclcpp::Node *node, std::string fsm_name, - std::shared_ptr fsm) { + std::shared_ptr fsm) + : fsm_name(fsm_name), fsm(fsm) { + + if (node == nullptr) { + this->node = yasmin_ros::YasminNode::get_instance(); + } else { + this->node = node; + } - this->node = node; + this->publisher = + this->node->create_publisher( + "/fsm_viewer", 10); - this->thread = std::make_unique( - std::thread(&YasminViewerPub::start_publisher, this, fsm_name, fsm)); - this->thread->detach(); + this->timer = this->node->create_wall_timer( + 250ms, std::bind(&YasminViewerPub::start_publisher, this)); } std::vector YasminViewerPub::parse_transitions( @@ -87,20 +100,13 @@ void YasminViewerPub::parse_state( } } -void YasminViewerPub::start_publisher( - std::string fsm_name, std::shared_ptr fsm) { +void YasminViewerPub::start_publisher() { - auto publisher = this->node->create_publisher( - "/fsm_viewer", 10); + std::vector states_list; + this->parse_state(this->fsm_name, this->fsm, {}, states_list, -1); - while (rclcpp::ok()) { - std::vector states_list; - this->parse_state(fsm_name, fsm, {}, states_list, -1); + auto state_machine_msg = yasmin_msgs::msg::StateMachine(); + state_machine_msg.states = states_list; - auto state_machine_msg = yasmin_msgs::msg::StateMachine(); - state_machine_msg.states = states_list; - - publisher->publish(state_machine_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } + this->publisher->publish(state_machine_msg); } diff --git a/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py b/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py index 4a26047..4e7ce82 100644 --- a/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py +++ b/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py @@ -16,18 +16,15 @@ # along with this program. If not, see . -from typing import List, Dict - import time -from threading import Thread - import json +from threading import Thread +from typing import List, Dict import rclpy +from rclpy.node import Node import ament_index_python -from simple_node import Node - from yasmin_msgs.msg import ( StateMachine, State, @@ -44,7 +41,7 @@ class YasminFsmViewer(Node): def __init__(self) -> None: super().__init__("yasmin_viewer") - + self.declare_parameters( namespace="", parameters=[ @@ -52,7 +49,7 @@ def __init__(self) -> None: ("port", 5000), ] ) - + self.__started = False self.__fsm_dict = ExpiringDict(max_len=300, max_age_seconds=3) @@ -87,19 +84,21 @@ def get_fsm(fsm_name): self.__started = True # app.run(host="localhost", port=5000) - + _host = str(self.get_parameter('host').value) _port = int(self.get_parameter('port').value) print(f"Started Yasmin viewer on {_host}:{str(_port)}") - serve(app, - host = _host, - port = _port) + serve(app, + host=_host, + port=_port) def start_subscriber(self) -> None: - self.create_subscription(StateMachine, - "/fsm_viewer", - self.fsm_viewer_cb, - 10) + self.create_subscription( + StateMachine, + "/fsm_viewer", + self.fsm_viewer_cb, + 10 + ) rclpy.spin(self) @@ -142,8 +141,8 @@ def fsm_viewer_cb(self, msg: StateMachine) -> None: self.__fsm_dict[msg.states[0].name] = self.msg_to_dict(msg) -def main(args=None): - rclpy.init(args=args) +def main(): + rclpy.init() YasminFsmViewer() rclpy.shutdown() diff --git a/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py b/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py index d797c02..618a79a 100644 --- a/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py +++ b/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py @@ -15,26 +15,41 @@ from typing import Dict, List -from threading import Thread -import rclpy from rclpy.node import Node from yasmin_msgs.msg import ( State as StateMsg, StateMachine as StateMachineMsg, Transition as TransitionMsg ) +from yasmin_ros.yasmin_node import YasminNode from yasmin import StateMachine, State class YasminViewerPub: - def __init__(self, node: Node, fsm_name: str, fsm: StateMachine) -> None: - self.__fsm = fsm - self.__fsm_name = fsm_name - self.__node = node + _node: Node + _fsm: StateMachine + _fsm_name: str - thread = Thread(target=self._start_publisher) - thread.start() + def __init__( + self, + fsm_name: str, + fsm: StateMachine, + rate: int = 4, + node: Node = None + ) -> None: + + self._fsm = fsm + self._fsm_name = fsm_name + + if node is None: + self._node = YasminNode.get_instance() + else: + self._node = node + + self.pub = self._node.create_publisher( + StateMachineMsg, "/fsm_viewer", 10) + self._timer = self._node.create_timer(1 / rate, self._start_publisher) def parse_transitions(self, transitions: Dict[str, str]) -> List[TransitionMsg]: @@ -50,10 +65,10 @@ def parse_transitions(self, transitions: Dict[str, str]) -> List[TransitionMsg]: def parse_state( self, - state_name: str, - state_info: Dict[str, str], - states_list: List[StateMsg], - parent: int = -1 + state_name: str, + state_info: Dict[str, str], + states_list: List[StateMsg], + parent: int = -1 ) -> None: state_msg = StateMsg() @@ -90,29 +105,23 @@ def parse_state( for child_state in states_list: if ( - child_state.name == current_state and - child_state.parent == state_msg.id + child_state.name == current_state and + child_state.parent == state_msg.id ): state_msg.current_state = child_state.id break def _start_publisher(self) -> None: - publisher = self.__node.create_publisher( - StateMachineMsg, "/fsm_viewer", 10) - - rate = self.__node.create_rate(4) - while rclpy.ok(): - states_list = [] - self.parse_state(self.__fsm_name, - { - "state": self.__fsm, - "transitions": {} - }, - states_list) + states_list = [] + self.parse_state(self._fsm_name, + { + "state": self._fsm, + "transitions": {} + }, + states_list) - state_machine_msg = StateMachineMsg() - state_machine_msg.states = states_list + state_machine_msg = StateMachineMsg() + state_machine_msg.states = states_list - publisher.publish(state_machine_msg) - rate.sleep() + self.pub.publish(state_machine_msg)