Concurrence Demo (C++)

This tutorial demonstrates how to run multiple states concurrently in C++ using YASMIN's Concurrence container. Concurrency is essential in robotics for coordinating parallel operations like simultaneously monitoring sensors while executing actions, waiting for multiple conditions to be met, or performing independent tasks in parallel. The Concurrence container manages thread synchronization, outcome resolution, and blackboard access automatically, making it safe and easy to implement parallel state execution.

Background

The Concurrence container enables parallel execution of multiple states. It is useful when you need to:

1. Create the FooState

This state increments a counter and returns different outcomes based on the counter value. It executes relatively quickly (2 seconds) and stores a formatted string in the blackboard. The three different outcomes ("outcome1", "outcome2", "outcome3") demonstrate how concurrent states can progress through stages and produce varying results. The counter is incremented after determining the outcome, so the state's behavior evolves with each execution. This pattern is useful for tasks that have distinct phases or thresholds, like collecting sensor samples, performing iterations of a computation, or tracking progress toward a goal. 

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

  FooState()
      : yasmin::State({"outcome1", "outcome2", "outcome3"}), counter(0) {};

  std::string
  execute(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) override {
    YASMIN_LOG_INFO("Executing state FOO");
    std::this_thread::sleep_for(std::chrono::seconds(2));

    std::string outcome;

    blackboard->set<std::string>("foo_str",
                                 "Counter: " + std::to_string(this->counter));

    if (this->counter < 3) {
      outcome = "outcome1";
    } else if (this->counter < 5) {
      outcome = "outcome2";
    } else {
      outcome = "outcome3";
    }

    YASMIN_LOG_INFO("Finishing state FOO");
    this->counter += 1;
    return outcome;
  };
};

2. Create the BarState

Define a slower state (4 seconds) that checks if FooState has written to the blackboard. This demonstrates a key aspect of concurrent execution: timing matters. Since BarState takes longer than FooState, there's a race condition - sometimes BarState will execute before FooState has written foo_str to the blackboard. The contains() check safely handles this by checking if the key exists before accessing it. This pattern is crucial in concurrent systems where you need to handle asynchronous data availability. BarState always returns "outcome3", providing a consistent counterpoint to FooState's variable outcomes. 

class BarState : public yasmin::State {
public:
  BarState() : yasmin::State({"outcome3"}) {}

  std::string
  execute(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) override {
    YASMIN_LOG_INFO("Executing state BAR");
    std::this_thread::sleep_for(std::chrono::seconds(4));

    if (blackboard->contains("foo_str")) {
      YASMIN_LOG_INFO(blackboard->get<std::string>("foo_str").c_str());
    } else {
      YASMIN_LOG_INFO("blackboard does not yet contains 'foo_str'");
    }

    YASMIN_LOG_INFO("Finishing state BAR");
    return "outcome3";
  }
};

3. Create Concurrence Container

Create a Concurrence container that runs FooState and BarState in parallel. The constructor takes three parameters: a map of state names to state objects, a default outcome (returned when no outcome map matches), and an outcome map that defines how concurrent results map to container outcomes. The outcome map specifies: if FOO returns "outcome1" AND BAR returns "outcome3", the concurrence returns "outcome1"; if FOO returns "outcome2" AND BAR returns "outcome3", return "outcome2". If neither condition matches (e.g., FOO returns "outcome3"), the concurrence returns the default outcome "defaulted". This flexible outcome mapping allows you to implement AND, OR, or custom logic for combining parallel state results. 

int main(int argc, char *argv[]) {
  YASMIN_LOG_INFO("yasmin_concurrence_demo");
  rclcpp::init(argc, argv);
  yasmin_ros::set_ros_loggers();

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

  // Cancel state machine on ROS 2 shutdown
  rclcpp::on_shutdown([sm]() {
    if (sm->is_running()) {
      sm->cancel_state();
    }
  });

auto foo_state = std::make_shared<FooState>();
auto bar_state = std::make_shared<BarState>();

auto concurrent_state = std::make_shared<yasmin::Concurrence>(
    std::map<std::string, std::shared_ptr<yasmin::State>>{
        {"FOO", foo_state},
        {"BAR", bar_state}
    },
    "defaulted",
    yasmin::Concurrence::OutcomeMap{
        {"outcome1",
         yasmin::Concurrence::StateOutcomeMap{{"FOO", "outcome1"},
                                              {"BAR", "outcome3"}}},
        {"outcome2", yasmin::Concurrence::StateOutcomeMap{
                         {"FOO", "outcome2"}, {"BAR", "outcome3"}}}
    });

4. Add to State Machine

Add the concurrence container to the state machine with self-loops for outcome1 and outcome2, allowing it to execute multiple times. When the default outcome ("defaulted") is reached, the state machine exits with outcome4. This setup enables the demonstration to run through several concurrent executions before terminating. 

sm->add_state("CONCURRENCE", concurrent_state,
              {
                  {"outcome1", "CONCURRENCE"},
                  {"outcome2", "CONCURRENCE"},
                  {"defaulted", "outcome4"},
              });

yasmin_viewer::YasminViewerPub yasmin_pub(sm, "YASMIN_CONCURRENCE_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();

5. Run the Demo

Execute the concurrence demonstration to observe parallel state execution and outcome resolution. 

ros2 run yasmin_demos concurrence_demo