monitor_state.hpp

Go to the documentation of this file.

// Copyright (C) 2023 Miguel Ángel González Santamarta
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef YASMIN_ROS__MONITOR_STATE_HPP
#define YASMIN_ROS__MONITOR_STATE_HPP
 
#include <chrono>
#include <condition_variable>
#include <functional>
#include <memory>
#include <set>
#include <string>
#include <vector>
 
#include "rclcpp/rclcpp.hpp"
 
#include "yasmin/blackboard.hpp"
#include "yasmin/logs.hpp"
#include "yasmin/state.hpp"
#include "yasmin_ros/basic_outcomes.hpp"
#include "yasmin_ros/yasmin_node.hpp"
 
using std::placeholders::_1;
 
namespace yasmin_ros {

template <typename MsgT> class MonitorState : public yasmin::State {

  using MonitorHandler = std::function<std::string(
      std::shared_ptr<yasmin::Blackboard>, std::shared_ptr<MsgT>)>;
 
public:
  MonitorState(const std::string &topic_name,
               const std::set<std::string> &outcomes,
               MonitorHandler monitor_handler, rclcpp::QoS qos = 10,
               int msg_queue = 10, int timeout = -1, int maximum_retry = 3)
      : MonitorState(nullptr, topic_name, outcomes, monitor_handler, qos,
                     nullptr, msg_queue, timeout, maximum_retry) {}

  MonitorState(const std::string &topic_name,
               const std::set<std::string> &outcomes,
               MonitorHandler monitor_handler, rclcpp::QoS qos = 10,
               rclcpp::CallbackGroup::SharedPtr callback_group = nullptr,
               int msg_queue = 10, int timeout = -1, int maximum_retry = 3)
      : MonitorState(nullptr, topic_name, outcomes, monitor_handler, qos,
                     callback_group, msg_queue, timeout, maximum_retry) {}

  MonitorState(const rclcpp::Node::SharedPtr &node,
               const std::string &topic_name,
               const std::set<std::string> &outcomes,
               MonitorHandler monitor_handler, rclcpp::QoS qos = 10,
               rclcpp::CallbackGroup::SharedPtr callback_group = nullptr,
               int msg_queue = 10, int timeout = -1, int maximum_retry = 3)
      : State({basic_outcomes::CANCEL}), topic_name(topic_name),
        monitor_handler(monitor_handler), qos(qos), msg_queue(msg_queue),
        timeout(timeout), maximum_retry(maximum_retry) {
 
    // set outcomes
    if (timeout > 0) {
      this->outcomes.insert(basic_outcomes::TIMEOUT);
    }
 
    if (outcomes.size() > 0) {
      for (const std::string &outcome : outcomes) {
        this->outcomes.insert(outcome);
      }
    }
 
    if (node == nullptr) {
      this->node_ = YasminNode::get_instance();
    } else {
      this->node_ = node;
    }
 
    // Create subscription
    rclcpp::SubscriptionOptions options;
    options.callback_group = callback_group;
    this->sub = this->node_->create_subscription<MsgT>(
        this->topic_name, this->qos,
        std::bind(&MonitorState::callback, this, _1), options);
  }

  std::string execute(std::shared_ptr<yasmin::Blackboard> blackboard) override {
    int retry_count = 0;
 
    while (this->msg_list.empty()) {
      std::unique_lock<std::mutex> lock(this->msg_mutex);
 
      std::cv_status wait_status = std::cv_status::no_timeout;
      if (this->timeout > 0) {
        wait_status =
            this->msg_cond.wait_for(lock, std::chrono::seconds(this->timeout));
      } else {
        this->msg_cond.wait(lock);
      }
 
      if (this->is_canceled()) {
        return basic_outcomes::CANCEL;
      }
 
      while (this->timeout > 0 && wait_status == std::cv_status::timeout) {
        YASMIN_LOG_WARN("Timeout reached, topic '%s' is not available",
                        this->topic_name.c_str());
 
        if (retry_count < this->maximum_retry) {
          retry_count++;
          YASMIN_LOG_WARN("Retrying to wait for topic '%s' (%d/%d)",
                          this->topic_name.c_str(), retry_count,
                          this->maximum_retry);
          wait_status = this->msg_cond.wait_for(
              lock, std::chrono::seconds(this->timeout));
        } else {
          return basic_outcomes::TIMEOUT;
        }
 
        if (this->is_canceled()) {
          return basic_outcomes::CANCEL;
        }
      }
    }
 
    YASMIN_LOG_INFO("Processing msg from topic '%s'", this->topic_name.c_str());
    std::string outcome =
        this->monitor_handler(blackboard, this->msg_list.at(0));
    this->msg_list.erase(this->msg_list.begin());
 
    return outcome;
  }

  void cancel_state() {
    this->msg_cond.notify_one();
    yasmin::State::cancel_state();
  }
 
protected:
  rclcpp::Node::SharedPtr node_;
 
private:
  std::shared_ptr<rclcpp::Subscription<MsgT>> sub;

  std::string topic_name;
  rclcpp::QoS qos;

  std::vector<std::shared_ptr<MsgT>> msg_list;
  MonitorHandler monitor_handler;
  int msg_queue;
  int timeout;
  int maximum_retry;

  std::condition_variable msg_cond;
  std::mutex msg_mutex;

  void callback(const typename MsgT::SharedPtr msg) {
 
    this->msg_list.push_back(msg);
 
    if ((int)this->msg_list.size() > this->msg_queue) {
      this->msg_list.erase(this->msg_list.begin());
    }
 
    this->msg_cond.notify_one();
  }
};
 
} // namespace yasmin_ros
 
#endif // YASMIN_ROS__MONITOR_STATE_HPP