Service Client (C++)

This tutorial demonstrates how to use the ServiceState in C++ to call ROS 2 services from a state machine. Services in ROS 2 are used for request-response communication - perfect for operations like calculations, queries, or triggering specific actions. We'll create a simple service client that adds two integers, showing how to prepare requests from blackboard data and process responses.

Prerequisites

You need a ROS 2 service server running. Start the example AddTwoInts service in a separate terminal:

ros2 run yasmin_demos add_two_ints_server

1. Define Helper Functions

First, we define callback functions to set the input values and print the result. The set_ints() function stores two integers (a=10, b=5) in the blackboard - these will be used as inputs for the service call. The print_sum() function retrieves the sum from the blackboard after the service returns and logs it. Both functions return SUCCEED to indicate successful completion. These helper functions demonstrate the callback state pattern, which is useful for simple operations that don't require a full custom state class - perfect for data preparation, logging, or simple conditional checks.

std::string
set_ints(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
  blackboard->set<int>("a", 10);
  blackboard->set<int>("b", 5);
  return yasmin_ros::basic_outcomes::SUCCEED;
}

std::string
print_sum(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
  std::stringstream ss;
  ss << "Sum: " << blackboard->get<int>("sum");
  YASMIN_LOG_INFO(ss.str().c_str());
  return yasmin_ros::basic_outcomes::SUCCEED;
}

2. Define the Service State

Create a class inheriting from yasmin_ros::ServiceState, templated with the service type AddTwoInts. The constructor initializes the service state with four parameters: the service name ("/add_two_ints"), the request creation callback, a list of additional outcomes (beyond the default SUCCEED and ABORT), and the response handler. The create_request_handler method retrieves the integers a and b from the blackboard and populates a service request. The response_handler processes the service response by extracting the sum and storing it back in the blackboard, then returns "outcome1" to signal successful completion. This state automatically handles service client creation, waiting for the service to become available, and error handling.

class AddTwoIntsState
    : public yasmin_ros::ServiceState<example_interfaces::srv::AddTwoInts> {
public:
  AddTwoIntsState()
      : yasmin_ros::ServiceState<example_interfaces::srv::AddTwoInts>(
            "/add_two_ints",
            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<yasmin::blackboard::Blackboard> blackboard) {

    auto request =
        std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
    request->a = blackboard->get<int>("a");
    request->b = blackboard->get<int>("b");
    return request;
  };

  std::string response_handler(
      std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
      example_interfaces::srv::AddTwoInts::Response::SharedPtr response) {

    blackboard->set<int>("sum", response->sum);
    return "outcome1";
  };
};

3. Create the State Machine

In the main function, we build a sequential state machine with three states. The first state, SETTING_INTS, uses a callback state to initialize the input values in the blackboard. Upon success, it transitions to ADD_TWO_INTS, our service state that performs the addition by calling the ROS 2 service. If the service call succeeds (outcome1), we transition to PRINTING_SUM to display the result. If the service fails (SUCCEED or ABORT from the default outcomes), we exit with outcome4. The YasminViewerPub allows visualization of the state machine execution flow. This pattern - prepare inputs, call service, process outputs - is common in robotics applications for tasks like motion planning, object recognition, or sensor calibration.

int main(int argc, char *argv[]) {
  YASMIN_LOG_INFO("yasmin_service_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("SETTING_INTS",
                std::make_shared<yasmin::CbState>(
                    std::initializer_list<std::string>{
                        yasmin_ros::basic_outcomes::SUCCEED},
                    set_ints),
                {
                    {yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"},
                });
  sm->add_state("ADD_TWO_INTS", std::make_shared<AddTwoIntsState>(),
                {
                    {"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>(
                    std::initializer_list<std::string>{
                        yasmin_ros::basic_outcomes::SUCCEED},
                    print_sum),
                {
                    {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"},
                });

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

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

  rclcpp::shutdown();
  return 0;
}

4. Run the Demo

First, start the service server in one terminal to provide the AddTwoInts service that the state machine will call:

ros2 run yasmin_demos add_two_ints_server

Then run the service client demo in another terminal to execute the state machine that calls the service:

ros2 run yasmin_demos service_client_demo