Monitor Demo (C++)

This tutorial demonstrates how to monitor ROS 2 topics using YASMIN's MonitorState in C++. This state subscribes to a topic and processes incoming messages, allowing your state machine to react to sensor data or other published information. The MonitorState is particularly valuable in robotics applications where you need to wait for specific sensor readings, check for environmental conditions, or synchronize with external events before proceeding with other tasks.

Background

The MonitorState is useful when you need to:

• Wait for a specific message to arrive on a topic
• Process a certain number of messages before transitioning
• React to changing data from sensors or other ROS nodes
• Implement timeout behavior if no messages arrive within a time limit
• Monitor robot state (odometry, battery, sensors) for decision making

1. Define the Monitor State

Create a class inheriting from yasmin_ros::MonitorState, templated with the message type nav_msgs::msg::Odometry. The constructor takes six parameters: the topic name ("odom"), a list of possible outcomes, the monitor handler callback, QoS settings (quality of service for subscription reliability), message queue size, and timeout in seconds. The monitor_handler method is called for each received message. It logs the robot's position coordinates (x, y, z) from the odometry message, decrements the counter, and returns "outcome2" when the desired number of messages have been processed, or "outcome1" to continue monitoring. If no message arrives within the timeout period, the state automatically returns the TIMEOUT outcome.

class PrintOdometryState
    : public yasmin_ros::MonitorState<nav_msgs::msg::Odometry> {

public:
  int times;

  PrintOdometryState(int times)
      : yasmin_ros::MonitorState<nav_msgs::msg::Odometry>(
            "odom",                   // topic name
            {"outcome1", "outcome2"}, // possible outcomes
            std::bind(&PrintOdometryState::monitor_handler, this, _1,
                      _2), // monitor handler callback
            10,            // QoS for the topic subscription
            10,            // queue size for the callback
            10             // timeout for receiving messages
        ) {
    this->times = times;
  };

  std::string
  monitor_handler(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
                  std::shared_ptr<nav_msgs::msg::Odometry> msg) {

    (void)blackboard;

    YASMIN_LOG_INFO("x: %f", msg->pose.pose.position.x);
    YASMIN_LOG_INFO("y: %f", msg->pose.pose.position.y);
    YASMIN_LOG_INFO("z: %f", msg->pose.pose.position.z);

    this->times--;

    if (this->times <= 0) {
      return "outcome2";
    }

    return "outcome1";
  };
};

2. Create the State Machine

In the main function, we create a simple state machine with one state that monitors odometry messages. The PrintOdometryState is initialized to process 5 messages before transitioning. The state machine defines four possible transitions: "outcome1" loops back to continue monitoring, "outcome2" exits successfully after processing all messages, TIMEOUT exits if no messages are received within 10 seconds, and CANCEL exits if the state is cancelled externally. This setup ensures the program handles all possible scenarios gracefully. The loop-back transition ("outcome1" → "PRINTING_ODOM") allows the state to process multiple messages without recreating the state object, which is efficient for continuous monitoring tasks.

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

  YASMIN_LOG_INFO("yasmin_monitor_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(
      "PRINTING_ODOM", std::make_shared<PrintOdometryState>(5),
      {
          {"outcome1", "PRINTING_ODOM"},
          {"outcome2", "outcome4"},
          {yasmin_ros::basic_outcomes::TIMEOUT, "outcome4"},
          {yasmin_ros::basic_outcomes::CANCEL, "outcome4"},
      });

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

3. Run the Demo

Run the monitor demo to observe how the state processes incoming odometry messages. Make sure a source is publishing to the /odom topic, or run a navigation simulation that provides odometry data.

ros2 run yasmin_demos monitor_demo