diff --git a/README.md b/README.md index c14f951..4ca671e 100644 --- a/README.md +++ b/README.md @@ -296,7 +296,8 @@ class FibonacciState(ActionState): "/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 + self.response_handler, # cb to process the response + self.print_feedback ) def create_goal_handler(self, blackboard: Blackboard) -> Fibonacci.Goal: @@ -314,10 +315,12 @@ class FibonacciState(ActionState): blackboard.fibo_res = response.sequence return SUCCEED - -def set_int(blackboard: Blackboard) -> str: - blackboard.n = 3 - return SUCCEED + def print_feedback( + self, + blackboard: Blackboard, + feedback: Fibonacci.Feedback + ) -> None: + print(f"Received feedback: {list(feedback.partial_sequence)}") def print_result(blackboard: Blackboard) -> str: @@ -337,13 +340,6 @@ def main(): 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(), @@ -364,8 +360,12 @@ def main(): # pub FSM info YasminViewerPub("YASMIN_ACTION_CLIENT_DEMO", sm) + # create an initial blackoard + blackboard = Blackboard() + blackboard.n = 10 + # execute FSM - outcome = sm() + outcome = sm(blackboard) print(outcome) # shutdown ROS 2 @@ -865,19 +865,14 @@ $ ros2 run yasmin_demo action_client_demo 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; -} +using Fibonacci = action_tutorials_interfaces::action::Fibonacci; std::string print_result(std::shared_ptr blackboard) { auto fibo_res = blackboard->get>("sum"); - fprintf(stderr, "Sum:"); + fprintf(stderr, "Result received:"); for (auto ele : fibo_res) { fprintf(stderr, " %d,", ele); @@ -888,36 +883,48 @@ print_result(std::shared_ptr blackboard) { return yasmin_ros::basic_outcomes::SUCCEED; } -class FibonacciState : public yasmin_ros::ActionState< - action_tutorials_interfaces::action::Fibonacci> { +class FibonacciState : public yasmin_ros::ActionState { 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)){}; + : yasmin_ros::ActionState( + "/fibonacci", + std::bind(&FibonacciState::create_goal_handler, this, _1), + std::bind(&FibonacciState::response_handler, this, _1, _2), + std::bind(&FibonacciState::print_feedback, this, _1, _2)){}; - action_tutorials_interfaces::action::Fibonacci::Goal create_goal_handler( + Fibonacci::Goal create_goal_handler( std::shared_ptr blackboard) { - auto goal = action_tutorials_interfaces::action::Fibonacci::Goal(); + auto goal = 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) { + std::string + response_handler(std::shared_ptr blackboard, + Fibonacci::Result::SharedPtr response) { blackboard->set>("sum", response->sequence); return yasmin_ros::basic_outcomes::SUCCEED; } + void + print_feedback(std::shared_ptr blackboard, + std::shared_ptr feedback) { + (void)blackboard; + + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + + fprintf(stderr, "%s\n", ss.str().c_str()); + } + std::string to_string() { return "FibonacciState"; } }; @@ -931,10 +938,6 @@ int main(int argc, char *argv[]) { 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"}, @@ -947,8 +950,13 @@ int main(int argc, char *argv[]) { // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + // create an initial blackboard + std::shared_ptr blackboard = + std::make_shared(); + blackboard->set("n", 10); + // execute - std::string outcome = (*sm.get())(); + std::string outcome = (*sm.get())(blackboard); std::cout << outcome << "\n"; rclcpp::shutdown(); diff --git a/yasmin_demo/src/action_client_demo.cpp b/yasmin_demo/src/action_client_demo.cpp index d3b7b8e..286f165 100644 --- a/yasmin_demo/src/action_client_demo.cpp +++ b/yasmin_demo/src/action_client_demo.cpp @@ -28,19 +28,14 @@ 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; -} +using Fibonacci = action_tutorials_interfaces::action::Fibonacci; std::string print_result(std::shared_ptr blackboard) { auto fibo_res = blackboard->get>("sum"); - fprintf(stderr, "Sum:"); + fprintf(stderr, "Result received:"); for (auto ele : fibo_res) { fprintf(stderr, " %d,", ele); @@ -51,36 +46,48 @@ print_result(std::shared_ptr blackboard) { return yasmin_ros::basic_outcomes::SUCCEED; } -class FibonacciState : public yasmin_ros::ActionState< - action_tutorials_interfaces::action::Fibonacci> { +class FibonacciState : public yasmin_ros::ActionState { 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)){}; + : yasmin_ros::ActionState( + "/fibonacci", + std::bind(&FibonacciState::create_goal_handler, this, _1), + std::bind(&FibonacciState::response_handler, this, _1, _2), + std::bind(&FibonacciState::print_feedback, this, _1, _2)){}; - action_tutorials_interfaces::action::Fibonacci::Goal create_goal_handler( + Fibonacci::Goal create_goal_handler( std::shared_ptr blackboard) { - auto goal = action_tutorials_interfaces::action::Fibonacci::Goal(); + auto goal = 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) { + std::string + response_handler(std::shared_ptr blackboard, + Fibonacci::Result::SharedPtr response) { blackboard->set>("sum", response->sequence); return yasmin_ros::basic_outcomes::SUCCEED; } + void + print_feedback(std::shared_ptr blackboard, + std::shared_ptr feedback) { + (void)blackboard; + + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + + fprintf(stderr, "%s\n", ss.str().c_str()); + } + std::string to_string() { return "FibonacciState"; } }; @@ -94,10 +101,6 @@ int main(int argc, char *argv[]) { 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"}, @@ -110,8 +113,13 @@ int main(int argc, char *argv[]) { // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); + // create an initial blackboard + std::shared_ptr blackboard = + std::make_shared(); + blackboard->set("n", 10); + // execute - std::string outcome = (*sm.get())(); + std::string outcome = (*sm.get())(blackboard); std::cout << outcome << "\n"; rclcpp::shutdown(); diff --git a/yasmin_demo/yasmin_demo/action_client_demo.py b/yasmin_demo/yasmin_demo/action_client_demo.py index 1e3da21..8b7fb63 100755 --- a/yasmin_demo/yasmin_demo/action_client_demo.py +++ b/yasmin_demo/yasmin_demo/action_client_demo.py @@ -33,7 +33,8 @@ def __init__(self) -> None: "/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 + self.response_handler, # cb to process the response + self.print_feedback ) def create_goal_handler(self, blackboard: Blackboard) -> Fibonacci.Goal: @@ -51,10 +52,12 @@ def response_handler( blackboard.fibo_res = response.sequence return SUCCEED - -def set_int(blackboard: Blackboard) -> str: - blackboard.n = 3 - return SUCCEED + def print_feedback( + self, + blackboard: Blackboard, + feedback: Fibonacci.Feedback + ) -> None: + print(f"Received feedback: {list(feedback.partial_sequence)}") def print_result(blackboard: Blackboard) -> str: @@ -74,13 +77,6 @@ def main(): 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(), @@ -101,8 +97,12 @@ def main(): # pub FSM info YasminViewerPub("YASMIN_ACTION_CLIENT_DEMO", sm) + # create an initial blackoard + blackboard = Blackboard() + blackboard.n = 10 + # execute FSM - outcome = sm() + outcome = sm(blackboard) print(outcome) # shutdown ROS 2 diff --git a/yasmin_ros/include/yasmin_ros/action_state.hpp b/yasmin_ros/include/yasmin_ros/action_state.hpp index 8dc1633..01255d5 100644 --- a/yasmin_ros/include/yasmin_ros/action_state.hpp +++ b/yasmin_ros/include/yasmin_ros/action_state.hpp @@ -36,6 +36,7 @@ template class ActionState : public yasmin::State { using Goal = typename ActionT::Goal; using Result = typename ActionT::Result::SharedPtr; + using Feedback = typename ActionT::Feedback; using SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; @@ -46,29 +47,39 @@ template class ActionState : public yasmin::State { std::function)>; using ResutlHandler = std::function, Result)>; + using FeedbackHandler = + std::function, + std::shared_ptr)>; public: ActionState(std::string action_name, CreateGoalHandler create_goal_handler, std::vector outcomes, int timeout = -1.0) : ActionState(action_name, create_goal_handler, outcomes, nullptr, - timeout) {} + nullptr, timeout) {} ActionState(std::string action_name, CreateGoalHandler create_goal_handler, - ResutlHandler result_handler = nullptr, int timeout = -1.0) + ResutlHandler result_handler = nullptr, + FeedbackHandler feedback_handler = nullptr, int timeout = -1.0) : ActionState(action_name, create_goal_handler, {}, result_handler, - timeout) {} + feedback_handler, timeout) {} ActionState(std::string action_name, CreateGoalHandler create_goal_handler, std::vector outcomes, - ResutlHandler result_handler = nullptr, int timeout = -1.0) + ResutlHandler result_handler = nullptr, + FeedbackHandler feedback_handler = nullptr, int timeout = -1.0) : ActionState(nullptr, action_name, create_goal_handler, outcomes, - result_handler, timeout) {} + result_handler, feedback_handler, timeout) {} ActionState(rclcpp::Node *node, std::string action_name, CreateGoalHandler create_goal_handler, std::vector outcomes, - ResutlHandler result_handler = nullptr, int timeout = -1.0) - : State({}), action_name(action_name), timeout(timeout) { + ResutlHandler result_handler = nullptr, + FeedbackHandler feedback_handler = nullptr, int timeout = -1.0) + + : State({}), action_name(action_name), + create_goal_handler(create_goal_handler), + result_handler(result_handler), feedback_handler(feedback_handler), + timeout(timeout) { this->outcomes = {basic_outcomes::SUCCEED, basic_outcomes::ABORT, basic_outcomes::CANCEL}; @@ -88,9 +99,6 @@ template class ActionState : public yasmin::State { this->action_client = rclcpp_action::create_client(this->node, action_name); - this->create_goal_handler = create_goal_handler; - this->result_handler = result_handler; - if (this->create_goal_handler == nullptr) { throw std::invalid_argument("create_goal_handler is needed"); } @@ -129,6 +137,15 @@ template class ActionState : public yasmin::State { send_goal_options.result_callback = std::bind(&ActionState::result_callback, this, _1); + if (this->feedback_handler != nullptr) { + send_goal_options.feedback_callback = + [this, blackboard](typename GoalHandle::SharedPtr, + std::shared_ptr feedback) { + this->feedback_handler(blackboard, feedback); + }; + ; + } + RCLCPP_INFO(this->node->get_logger(), "Sending goal to action '%s'", this->action_name.c_str()); this->action_client->async_send_goal(goal, send_goal_options); @@ -158,7 +175,6 @@ template class ActionState : public yasmin::State { rclcpp::Node *node; std::string action_name; - int timeout; ActionClient action_client; std::condition_variable action_done_cond; @@ -172,6 +188,9 @@ template class ActionState : public yasmin::State { CreateGoalHandler create_goal_handler; ResutlHandler result_handler; + FeedbackHandler feedback_handler; + + int timeout; #if defined(FOXY) void goal_response_callback( diff --git a/yasmin_ros/yasmin_ros/action_state.py b/yasmin_ros/yasmin_ros/action_state.py index e08d4b0..8f8bb8c 100644 --- a/yasmin_ros/yasmin_ros/action_state.py +++ b/yasmin_ros/yasmin_ros/action_state.py @@ -45,6 +45,7 @@ class ActionState(State): _create_goal_handler: Callable _result_handler: Callable + _feedback_handler: Callable _timeout: float def __init__( @@ -54,6 +55,7 @@ def __init__( create_goal_handler: Callable, outcomes: List[str] = None, result_handler: Callable = None, + feedback_handler: Callable = None, node: Node = None, timeout: float = None ) -> None: @@ -82,6 +84,7 @@ def __init__( self._create_goal_handler = create_goal_handler self._result_handler = result_handler + self._feedback_handler = feedback_handler if not self._create_goal_handler: raise Exception("create_goal_handler is needed") @@ -113,7 +116,16 @@ def execute(self, blackboard: Blackboard) -> str: self._node.get_logger().info( f"Sending goal to action '{self._action_name}'") - send_goal_future = self._action_client.send_goal_async(goal) + + feedback_handler = None + + if self._feedback_handler is not None: + + def feedback_handler(feedback): + self._feedback_handler(blackboard, feedback.feedback) + + send_goal_future = self._action_client.send_goal_async( + goal, feedback_callback=feedback_handler) send_goal_future.add_done_callback(self._goal_response_callback) # Wait for action to be done