Action Client (C++)

This tutorial demonstrates how to integrate ROS 2 actions into a YASMIN state machine using the ActionState class in C++. Actions are perfect for long-running tasks that need to provide feedback during execution and can be canceled if needed - such as navigation, manipulation, or data processing tasks. We'll use the Fibonacci action as an example to show how to send goals, monitor feedback, and handle results. The ActionState automatically manages the action client lifecycle, handles timeouts, and provides clean integration with the state machine.

Prerequisites

Start the Fibonacci action server in a separate terminal before running this demo:

ros2 run yasmin_demos fibonacci_action_server

1. Define the Action State

Create a class inheriting from yasmin_ros::ActionState<Fibonacci>, specifying the Fibonacci action type as a template parameter. The constructor takes four parameters: the action name ("/fibonacci"), and three callback functions bound using std::bind. The create_goal_handler retrieves the Fibonacci order from the blackboard and creates a goal request. The response_handler processes the result by storing the computed sequence in the blackboard and returning SUCCEED. The print_feedback callback is invoked periodically during execution to display the partial sequence - this is one of the key advantages of actions over services, allowing us to monitor progress in real-time.

class FibonacciState : public yasmin_ros::ActionState<Fibonacci> {

public:
  FibonacciState()
      : yasmin_ros::ActionState<Fibonacci>(
            "/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)) {};

  Fibonacci::Goal create_goal_handler(
      std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {

    auto goal = Fibonacci::Goal();
    goal.order = blackboard->get<int>("n");
    return goal;
  };

  std::string
  response_handler(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
                   Fibonacci::Result::SharedPtr response) {

    blackboard->set<std::vector<int>>("fibo_res", response->sequence);
    return yasmin_ros::basic_outcomes::SUCCEED;
  };

  void
  print_feedback(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
                 std::shared_ptr<const Fibonacci::Feedback> feedback) {
    (void)blackboard;

    std::stringstream ss;
    ss << "Received feedback: [";
    for (size_t i = 0; i < feedback->sequence.size(); i++) {
      ss << feedback->sequence[i];
      if (i < feedback->sequence.size() - 1) {
        ss << ", ";
      }
    }
    ss << "]";

    YASMIN_LOG_INFO(ss.str().c_str());
  };
};

2. Create the State Machine

In the main function, we build a state machine with two states. The first state, CALLING_FIBONACCI, uses our FibonacciState to send the action goal and wait for completion. It can transition to PRINTING_RESULT on success, or exit with outcome4 if cancelled or aborted. The second state uses a CbState (callback state) to print the final result. We initialize the blackboard with n = 10 to request the first 10 Fibonacci numbers. The rclcpp::on_shutdown callback ensures graceful cancellation if the program is interrupted.

int main(int argc, char *argv[]) {

  YASMIN_LOG_INFO("yasmin_action_client_demo");
  rclcpp::init(argc, argv);
  yasmin_ros::set_ros_loggers();

  auto sm = std::make_shared<yasmin::StateMachine>(
      std::initializer_list<std::string>{"outcome4"});

  rclcpp::on_shutdown([sm]() {
    if (sm->is_running()) {
      sm->cancel_state();
    }
  });

  sm->add_state("CALLING_FIBONACCI", std::make_shared<FibonacciState>(),
                {
                    {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>(
                    std::initializer_list<std::string>{
                        yasmin_ros::basic_outcomes::SUCCEED},
                    print_result),
                {
                    {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"},
                });

  yasmin_viewer::YasminViewerPub yasmin_pub(sm, "YASMIN_ACTION_CLIENT_DEMO");

  std::shared_ptr<yasmin::blackboard::Blackboard> blackboard =
      std::make_shared<yasmin::blackboard::Blackboard>();
  blackboard->set<int>("n", 10);

  try {
    std::string outcome = (*sm.get())(blackboard);
    YASMIN_LOG_INFO(outcome.c_str());
  } catch (const std::exception &e) {
    YASMIN_LOG_WARN(e.what());
  }

  rclcpp::shutdown();
  return 0;
}

3. Run the Demo

First, start the Fibonacci action server, then run the client demo. The state machine will send the goal, display feedback during execution, and print the final result.

ros2 run yasmin_demos action_client_demo