publisher_state.hpp

Go to the documentation of this file.

// Copyright (C) 2025 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__PUBLISHER_STATE_HPP_
#define YASMIN_ROS__PUBLISHER_STATE_HPP_
 
#include <functional>
#include <memory>
#include <string>
 
#include "rclcpp/rclcpp.hpp"
 
#include "yasmin/blackboard.hpp"
#include "yasmin/logs.hpp"
#include "yasmin/state.hpp"
#include "yasmin/types.hpp"
#include "yasmin_ros/basic_outcomes.hpp"
#include "yasmin_ros/ros_clients_cache.hpp"
#include "yasmin_ros/yasmin_node.hpp"
 
namespace yasmin_ros {

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

  using CreateMessageHandler =
      std::function<MsgT(yasmin::Blackboard::SharedPtr)>;
 
public:
  YASMIN_PTR_ALIASES(PublisherState)
 
  
  PublisherState(const std::string &topic_name,
                 CreateMessageHandler create_message_handler,
                 rclcpp::QoS qos = 10,
                 rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
      : PublisherState(nullptr, topic_name, create_message_handler, qos,
                       callback_group) {}

  PublisherState(const rclcpp::Node::SharedPtr &node,
                 const std::string &topic_name,
                 CreateMessageHandler create_message_handler,
                 rclcpp::QoS qos = 10,
                 rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
      : State({basic_outcomes::SUCCEED}), topic_name(topic_name),
        create_message_handler(create_message_handler) {
 
    if (node == nullptr) {
      this->node_ = YasminNode::get_instance();
    } else {
      this->node_ = node;
    }
 
    // create subscriber
    this->pub = ROSClientsCache::get_or_create_publisher<MsgT>(
        this->node_, topic_name, qos, callback_group);
 
    if (this->create_message_handler == nullptr) {
      throw std::invalid_argument("create_message_handler is needed");
    }
  }

  std::string execute(yasmin::Blackboard::SharedPtr blackboard) override {
 
    YASMIN_LOG_DEBUG("Publishing to topic '%s'", this->topic_name.c_str());
    MsgT msg = this->create_message_handler(blackboard);
    this->pub->publish(msg);
    return basic_outcomes::SUCCEED;
  }
 
protected:
  rclcpp::Node::SharedPtr node_;
 
private:
  std::shared_ptr<rclcpp::Publisher<MsgT>> pub;
  std::string topic_name;
  CreateMessageHandler create_message_handler;
};
 
} // namespace yasmin_ros
 
#endif // YASMIN_ROS__MONITOR_STATE_HPP_