ros_publisher_node.hpp

// Copyright 2024 Robin Müller
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#pragma once
 
#include <memory>
#include <string>
 
#include "auto_apms_behavior_tree_core/exceptions.hpp"
#include "auto_apms_behavior_tree_core/node/ros_node_context.hpp"
#include "auto_apms_util/string.hpp"
#include "behaviortree_cpp/condition_node.h"
#include "rclcpp/qos.hpp"
 
namespace auto_apms_behavior_tree::core
{

template <class MessageT>
class RosPublisherNode : public BT::ConditionNode
{
  using Publisher = typename rclcpp::Publisher<MessageT>;
 
public:
  using MessageType = MessageT;
  using Config = BT::NodeConfig;
  using Context = RosNodeContext;

  explicit RosPublisherNode(
    const std::string & instance_name, const Config & config, Context context, const rclcpp::QoS & qos = {10});
 
  virtual ~RosPublisherNode() = default;

  static BT::PortsList providedBasicPorts(BT::PortsList addition)
  {
    BT::PortsList basic = {BT::InputPort<std::string>("topic", "Name of the ROS 2 topic to publish to.")};
    basic.insert(addition.begin(), addition.end());
    return basic;
  }

  static BT::PortsList providedPorts() { return providedBasicPorts({}); }

  virtual bool setMessage(MessageT & msg);

  bool createPublisher(const std::string & topic_name);

  std::string getTopicName() const;
 
protected:
  const Context context_;
  const rclcpp::Logger logger_;
 
  BT::NodeStatus tick() override final;
 
private:
  const rclcpp::QoS qos_;
  std::string topic_name_;
  bool dynamic_client_instance_ = false;
  std::shared_ptr<Publisher> publisher_;
};
 
// #####################################################################################################################
// ################################              DEFINITIONS              ##############################################
// #####################################################################################################################
 
template <class MessageT>
inline RosPublisherNode<MessageT>::RosPublisherNode(
  const std::string & instance_name, const Config & config, Context context, const rclcpp::QoS & qos)
: BT::ConditionNode(instance_name, config),
  context_(context),
  logger_(context.getChildLogger(auto_apms_util::toSnakeCase(instance_name))),
  qos_{qos}
{
  // Consider aliasing in ports and copy the values from aliased to original ports
  this->modifyPortsRemapping(context_.copyAliasedPortValuesToOriginalPorts(this));
 
  if (const BT::Expected<std::string> expected_name = context_.getTopicName(this)) {
    createPublisher(expected_name.value());
  } else {
    // We assume that determining the topic name requires a blackboard pointer, which cannot be evaluated at
    // construction time. The expression will be evaluated each time before the node is ticked the first time after
    // successful execution.
    dynamic_client_instance_ = true;
  }
}
 
template <class MessageT>
inline bool RosPublisherNode<MessageT>::createPublisher(const std::string & topic_name)
{
  if (topic_name.empty()) {
    throw exceptions::RosNodeError(
      context_.getFullyQualifiedTreeNodeName(this) + " - Argument topic_name is empty when trying to create a client.");
  }
 
  // Check if the publisher with given name is already set up
  if (publisher_ && topic_name_ == topic_name) return true;
 
  rclcpp::Node::SharedPtr node = context_.nh_.lock();
  if (!node) {
    throw exceptions::RosNodeError(
      context_.getFullyQualifiedTreeNodeName(this) +
      " - The weak pointer to the ROS 2 node expired. The tree node doesn't "
      "take ownership of it.");
  }
 
  publisher_ = node->template create_publisher<MessageT>(topic_name, qos_);
  topic_name_ = topic_name;
  RCLCPP_DEBUG(
    logger_, "%s - Created publisher for topic '%s'.", context_.getFullyQualifiedTreeNodeName(this).c_str(),
    topic_name.c_str());
 
  // Wait for at least one subscriber to be connected
  RCLCPP_DEBUG(
    logger_, "%s - Waiting for at least one subscriber to connect to topic '%s'...",
    context_.getFullyQualifiedTreeNodeName(this).c_str(), topic_name_.c_str());
 
  const auto start_time = context_.getCurrentTime();
  while (rclcpp::ok() && publisher_->get_subscription_count() == 0) {
    if ((context_.getCurrentTime() - start_time) > context_.registration_options_.wait_timeout) {
      RCLCPP_DEBUG(
        logger_, "%s - Timeout waiting for subscriber to connect to topic '%s' after %.2f seconds.",
        context_.getFullyQualifiedTreeNodeName(this).c_str(), topic_name_.c_str(),
        context_.registration_options_.wait_timeout.count());
      break;
    }
    rclcpp::sleep_for(std::chrono::milliseconds(5));
  }
 
  if (publisher_->get_subscription_count() > 0) {
    RCLCPP_DEBUG(
      logger_, "%s - At least one subscriber found.", context_.getFullyQualifiedTreeNodeName(this).c_str(),
      topic_name_.c_str());
  } else if (context_.registration_options_.allow_unreachable) {
    RCLCPP_WARN(
      logger_, "%s - No subscriber connected to topic '%s', but continuing as allow_unreachable is true.",
      context_.getFullyQualifiedTreeNodeName(this).c_str(), topic_name_.c_str());
  } else {
    throw exceptions::RosNodeError(
      context_.getFullyQualifiedTreeNodeName(this) + " - No subscriber connected to topic '" + topic_name_ +
      "' after waiting for " + std::to_string(context_.registration_options_.wait_timeout.count()) + " seconds.");
  }
  return true;
}
 
template <class MessageT>
inline BT::NodeStatus RosPublisherNode<MessageT>::tick()
{
  if (!rclcpp::ok()) {
    halt();
    return BT::NodeStatus::FAILURE;
  }
 
  // If client has been set up in derived constructor, event though this constructor couldn't, we discard the intention
  // of dynamically creating the client
  if (dynamic_client_instance_ && publisher_) {
    dynamic_client_instance_ = false;
  }
 
  // Try again to create the client on first tick if this was not possible during construction or if client should be
  // created from a blackboard entry on the start of every iteration
  if (status() == BT::NodeStatus::IDLE && dynamic_client_instance_) {
    const BT::Expected<std::string> expected_name = context_.getTopicName(this);
    if (expected_name) {
      createPublisher(expected_name.value());
    } else {
      throw exceptions::RosNodeError(
        context_.getFullyQualifiedTreeNodeName(this) +
        " - Cannot create the publisher because the topic name couldn't be resolved using "
        "the expression specified in the node's registration options (" +
        NodeRegistrationOptions::PARAM_NAME_ROS2TOPIC + ": " + context_.registration_options_.topic +
        "). Error message: " + expected_name.error());
    }
  }
 
  if (!publisher_) {
    throw exceptions::RosNodeError(context_.getFullyQualifiedTreeNodeName(this) + " - publisher_ is nullptr.");
  }
 
  MessageT msg;
  if (!setMessage(msg)) {
    return BT::NodeStatus::FAILURE;
  }
  publisher_->publish(msg);
  return BT::NodeStatus::SUCCESS;
}
 
template <class MessageT>
inline bool RosPublisherNode<MessageT>::setMessage(MessageT & /*msg*/)
{
  return true;
}
 
template <class MessageT>
inline std::string RosPublisherNode<MessageT>::getTopicName() const
{
  return topic_name_;
}
 
}  // namespace auto_apms_behavior_tree::core