Publisher Demo (C++)

This tutorial demonstrates how to publish ROS 2 messages from within a YASMIN state machine using the PublisherState in C++. This is useful for sending commands to actuators, broadcasting status updates, or generating test data. Publishing states integrate with other state machine logic, allowing you to coordinate complex behaviors that involve both sensing (subscribing) and acting (publishing).

Background

The PublisherState allows states to:

• Publish messages to ROS 2 topics as part of state execution
• Generate message content dynamically from the blackboard
• Send periodic updates or commands as part of a control loop
• Coordinate publishing with other state machine operations
• Maintain state across multiple publish operations

1. Define the Publisher State

Create a class inheriting from yasmin_ros::PublisherState, templated with the message type std_msgs::msg::Int32. The constructor requires two parameters: the topic name ("count") and a callback function that creates the message to publish. The create_int_msg method is invoked each time the state executes. It retrieves the current counter from the blackboard, increments it, stores the updated value back in the blackboard, and creates an Int32 message with the new counter value. This demonstrates stateful publishing - the counter persists across state executions thanks to the blackboard, allowing you to track progress or maintain sequences.

class PublishIntState
    : public yasmin_ros::PublisherState<std_msgs::msg::Int32> {

public:
  PublishIntState()
      : yasmin_ros::PublisherState<std_msgs::msg::Int32>(
            "count", // topic name
            std::bind(&PublishIntState::create_int_msg, this,
                      _1) // create msg handler callback
        ) {};

  std_msgs::msg::Int32
  create_int_msg(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {

    int counter = blackboard->get<int>("counter");
    counter++;
    blackboard->set<int>("counter", counter);

    YASMIN_LOG_INFO("Creating message %d", counter);
    std_msgs::msg::Int32 msg;
    msg.data = counter;
    return msg;
  };
};

2. Create the State Machine

First, we define a check_count callback function that compares the current counter against a maximum value stored in the blackboard. This function introduces a small delay (1 second) to simulate processing time, then returns "outcome1" if the maximum is reached (to exit), or "outcome2" to continue publishing. In the main function, we create a state machine with two states: PUBLISHING_INT publishes a message and transitions to CHECKINNG_COUNTS, which evaluates the condition. If the maximum hasn't been reached, it loops back to PUBLISHING_INT, creating a publish-check cycle. We initialize the blackboard with counter = 0 and max_count = 10, so the machine will publish 10 messages before completing. This pattern is useful for sending sequences of commands, periodic status updates, or controlled test data generation.

std::string
check_count(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {

  rclcpp::sleep_for(std::chrono::seconds(1));
  YASMIN_LOG_INFO("Checking count: %d", blackboard->get<int>("counter"));

  if (blackboard->get<int>("counter") >= blackboard->get<int>("max_count")) {
    return "outcome1";
  } else {
    return "outcome2";
  }
}

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

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

  auto sm = std::make_shared<yasmin::StateMachine>(
      std::initializer_list<std::string>{yasmin_ros::basic_outcomes::SUCCEED});

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

  sm->add_state("PUBLISHING_INT", std::make_shared<PublishIntState>(),
                {
                    {yasmin_ros::basic_outcomes::SUCCEED,
                     "CHECKINNG_COUNTS"},
                });
  sm->add_state("CHECKINNG_COUNTS",
                std::make_shared<yasmin::CbState>(
                    std::initializer_list<std::string>{"outcome1", "outcome2"},
                    check_count),
                {{"outcome1", yasmin_ros::basic_outcomes::SUCCEED},
                 {"outcome2", "PUBLISHING_INT"}});

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

  std::shared_ptr<yasmin::blackboard::Blackboard> blackboard =
      std::make_shared<yasmin::blackboard::Blackboard>();
  blackboard->set<int>("counter", 0);
  blackboard->set<int>("max_count", 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

Execute the publisher demo and use ros2 topic echo /count in another terminal to observe the incrementing integer messages being published by the state machine.

ros2 run yasmin_demos publisher_demo