ROS Parameters Demo (C++)

This tutorial shows how to use ROS 2 parameters within a YASMIN state machine using the GetParametersState in C++. Parameters allow you to configure your state machine behavior at runtime without modifying code. This is essential for creating flexible, reusable robot behaviors that can be adapted to different scenarios, robots, or environments simply by changing parameter values. Instead of hardcoding values like loop counts, thresholds, or names, you can expose them as parameters that can be set via launch files, command-line arguments, or parameter files.

Background

ROS 2 parameters provide a way to:

1. Define the States

We define two simple states, FooState and BarState, that use the parameters retrieved from the blackboard. In FooState, instead of using hardcoded values, we retrieve max_counter from the blackboard to control how many iterations to perform. We also use the counter_str parameter to customize the log message prefix. This makes the state's behavior configurable - you can change how many iterations it performs or what prefix it uses in log messages without modifying the state's code. The same state class can be used in different state machines with different parameter values, promoting reusability. BarState simply logs the formatted string, demonstrating how parameter-driven data flows through the state machine naturally. 

class FooState : public yasmin::State {
public:
  int counter;

  FooState() : yasmin::State({"outcome1", "outcome2"}), counter(0) {
    this->set_description(
        "Increments a counter until the value from 'max_counter' is reached "
        "and writes the formatted counter string to the blackboard.");
    this->add_input_key(
        "max_counter",
        "Maximum number of iterations before the state finishes.");
    this->add_input_key<std::string>(
        "counter_str", "Prefix used when formatting the counter string.",
        std::string("Counter"));
    this->add_output_key("foo_str",
                         "Formatted counter string written to the blackboard.");
  };

  std::string
  execute(yasmin::Blackboard::SharedPtr blackboard) override {
    YASMIN_LOG_INFO("Executing state FOO");
    std::this_thread::sleep_for(std::chrono::seconds(3));

    if (this->counter < blackboard->get<int>("max_counter")) {
      this->counter += 1;
      blackboard->set<std::string>("foo_str",
                                   blackboard->get<std::string>("counter_str") +
                                       ": " + std::to_string(this->counter));
      return "outcome1";

    } else {
      return "outcome2";
    }
  };
};

class BarState : public yasmin::State {
public:
  BarState() : yasmin::State({"outcome3"}) {
    this->set_description("Reads and prints the formatted counter string "
                          "stored in 'foo_str' from the blackboard.");
    this->add_input_key("foo_str",
                        "Formatted counter string produced by FooState.");
  }

  std::string
  execute(yasmin::Blackboard::SharedPtr blackboard) override {
    YASMIN_LOG_INFO("Executing state BAR");
    std::this_thread::sleep_for(std::chrono::seconds(3));

    YASMIN_LOG_INFO(blackboard->get<std::string>("foo_str").c_str());

    return "outcome3";
  }
};

2. Create the State Machine

In the main function, we create a state machine that starts with a GetParametersState. This state is initialized with a map of parameter names and their default values: max_counter defaults to 3, and counter_str defaults to "Counter". The GetParametersState will first try to retrieve these parameters from the ROS 2 parameter server. If a parameter is set (via command line with --ros-args -p max_counter:=5 or in a parameter file), it uses that value; otherwise, it falls back to the default. Upon success, it stores all retrieved parameters in the blackboard and transitions to the FOO state. This pattern - parameter retrieval followed by execution - ensures your state machine is configured before running, preventing errors from missing configuration values. 

int main(int argc, char *argv[]) {
  // Initialize ROS 2
  rclcpp::init(argc, argv);

  // Set ROS 2 logs
  yasmin_ros::set_ros_loggers();
  YASMIN_LOG_INFO("yasmin_parameters_demo");

  // Create a state machine
  auto sm = yasmin::StateMachine::make_shared(
      std::initializer_list<std::string>{"outcome4"}, true);
  sm->set_description("Loads configuration values into the blackboard and then "
                      "runs a loop that formats and prints a counter string "
                      "until the configured maximum is reached.");
  sm->add_input_key<int>(
      "max_counter",
      "Maximum number of iterations before the state machine finishes.", 3);
  sm->add_input_key<std::string>(
      "counter_str", "Prefix used when formatting the counter string.",
      std::string("Counter"));
  sm->add_output_key("foo_str",
                     "Formatted counter string produced during execution.");

  auto getting_parameters_state = yasmin_ros::GetParametersState::make_shared(
      yasmin_ros::GetParametersState::Parameters{
          {"max_counter", 3},
          {"counter_str", std::string("Counter")},
      });
  getting_parameters_state->set_description(
      "Loads the configured parameters and stores them in the blackboard.");
  getting_parameters_state->add_output_key(
      "max_counter",
      "Maximum number of iterations before the state machine finishes.");
  getting_parameters_state->add_output_key(
      "counter_str", "Prefix used when formatting the counter string.");

  // Add states to the state machine
  sm->add_state("GETTING_PARAMETERS", getting_parameters_state,
                {
                    {yasmin_ros::basic_outcomes::SUCCEED, "FOO"},
                    {yasmin_ros::basic_outcomes::ABORT, "outcome4"},
                });
  sm->add_state("FOO", std::make_shared<FooState>(),
                {
                    {"outcome1", "BAR"},
                    {"outcome2", "outcome4"},
                });
  sm->add_state("BAR", std::make_shared<BarState>(),
                {
                    {"outcome3", "FOO"},
                });

  // Publish state machine updates
  yasmin_viewer::YasminViewerPub yasmin_pub(sm, "YASMIN_PARAMETERS_DEMO");

  // Execute the state machine
  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;
}

3. Run the Demo

Run the parameters demo to see how the state machine retrieves and uses ROS 2 parameters. Try running with and without custom parameters to observe the default value behavior. 

ros2 run yasmin_demos parameters_demo