Program Listing for File node.hpp

↰ Return to documentation for file (/tmp/ws/src/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/node.hpp)

// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// 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
//
//     http://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.

#ifndef GAZEBO_ROS__NODE_HPP_
#define GAZEBO_ROS__NODE_HPP_

#include <rclcpp/rclcpp.hpp>

#include <gazebo_ros/executor.hpp>
#include <gazebo_ros/node_visibility_control.h>
#include <gazebo_ros/qos.hpp>

#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
#include <unordered_set>

namespace gazebo_ros
{
// forward declare ExistingNodes
class ExistingNodes;


class GAZEBO_ROS_NODE_PUBLIC Node : public rclcpp::Node
{
public:
  using SharedPtr = std::shared_ptr<Node>;

  virtual ~Node();


  static SharedPtr Get();


  static SharedPtr Get(sdf::ElementPtr _sdf, std::string _node_name = "");


  template<typename ... Args>
  static SharedPtr CreateWithArgs(Args && ... args);

  /* \details SDF must have a type and name attribute
   *          where type is either 'int', 'float', 'double','bool' or 'string'.
   *          Note that 'float' and 'double' are both stored as double.
   * Examples:
   * \code{.xml}
   * <parameter type="int" name="my_int">55</parameter>
   * <parameter type="bool" name="my_bool">false</parameter>
   * <parameter type="double" name="my_double">13.37</parameter>
   * <parameter type="float" name="my_float">41.18</parameter>
   * <parameter type="string" name="my_string">Hello World</parameter>
   * \endcode
   * \param[in] _sdf An sdf element in the style mentioned above
   * \return The ROS parameter with the same name, type, and value as the sdf element
   *         On failure, the return parameter.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET
   */
  static rclcpp::Parameter sdf_to_ros_parameter(sdf::ElementPtr const & _sdf);

  inline const gazebo_ros::QoS & get_qos() const &
  {
    return this->qos_;
  }

  // binds to everything else
  inline gazebo_ros::QoS get_qos() &&
  {
    return this->qos_;
  }

private:
  using rclcpp::Node::Node;

  // A handler for the param change callback.
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handler_;

  std::shared_ptr<Executor> executor_;

  gazebo_ros::QoS qos_;

  static ExistingNodes static_existing_nodes_;

  static std::mutex lock_;

  static std::weak_ptr<Executor> static_executor_;

  static std::weak_ptr<Node> static_node_;

  static rclcpp::Logger internal_logger();
};

template<typename ... Args>
Node::SharedPtr Node::CreateWithArgs(Args && ... args)
{
  std::lock_guard<std::mutex> l(lock_);

  // Contruct Node by forwarding arguments
  // TODO(chapulina): use rclcpp::is_initialized() once that's available, see
  // https://github.com/ros2/rclcpp/issues/518
  Node::SharedPtr node;
  if (!rclcpp::ok()) {
    rclcpp::init(0, nullptr);
    RCLCPP_INFO(internal_logger(), "ROS was initialized without arguments.");
  }
  node = std::make_shared<Node>(std::forward<Args>(args) ...);
  node->set_parameter(rclcpp::Parameter("use_sim_time", true));

  // Store shared pointer to static executor in this object
  node->executor_ = static_executor_.lock();

  // If executor has not been contructed yet, do so now
  if (!node->executor_) {
    node->executor_ = std::make_shared<Executor>();
    static_executor_ = node->executor_;
  }

  // Generate warning on start up if use_sim_time parameter is set to false
  bool check_sim_time;
  node->get_parameter("use_sim_time", check_sim_time);
  if (!check_sim_time) {
    RCLCPP_WARN(
      node->get_logger(), "Startup warning: use_sim_time parameter will be ignored "
      "by default plugins and ROS messages will continue to use simulation timestamps");
  }

  std::weak_ptr<gazebo_ros::Node> node_weak_ptr;
  node_weak_ptr = node;
  // Parameter change callback
  auto param_change_callback =
    [&node_weak_ptr](std::vector<rclcpp::Parameter> parameters) {
      auto result = rcl_interfaces::msg::SetParametersResult();
      result.successful = true;

      for (const auto & parameter : parameters) {
        auto param_name = parameter.get_name();
        if (param_name == "use_sim_time") {
          if (auto node_shared_ptr = node_weak_ptr.lock()) {
            RCLCPP_WARN(
              node_shared_ptr->get_logger(),
              "use_sim_time parameter will be ignored by default plugins "
              "and ROS messages will continue to use simulation timestamps");
          }
        }
      }
      return result;
    };

  node->param_change_callback_handler_ =
    node->add_on_set_parameters_callback(param_change_callback);

  // Add new node to the executor so its callbacks are called
  node->executor_->add_node(node);

  return node;
}

// Class to hold the global set of tracked node names.
class ExistingNodes
{
public:
  // Methods need to be protected by internal mutex
  void add_node(const std::string & node_name);
  bool check_node(const std::string & node_name);
  void remove_node(const std::string & node_name);

private:
  std::unordered_set<std::string> set_;
  std::mutex internal_mutex_;
};
}  // namespace gazebo_ros
#endif  // GAZEBO_ROS__NODE_HPP_