parameter_event_handler.hpp

// Copyright 2019 Intel Corporation
//
// 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 RCLCPP__PARAMETER_EVENT_HANDLER_HPP_
#define RCLCPP__PARAMETER_EVENT_HANDLER_HPP_
 
#include <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "rclcpp/create_subscription.hpp"
#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
 
namespace rclcpp
{
 
struct ParameterCallbackHandle
{
  RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle)
 
  using ParameterCallbackType = std::function<void (const rclcpp::Parameter &)>;
 
  std::string parameter_name;
  std::string node_name;
  ParameterCallbackType callback;
};
 
struct ParameterEventCallbackHandle
{
  RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle)
 
  using ParameterEventCallbackType =
    std::function<void (const rcl_interfaces::msg::ParameterEvent &)>;
 
  ParameterEventCallbackType callback;
};
 
 
class ParameterEventHandler
{
public:
 
  template<typename NodeT>
  explicit ParameterEventHandler(
    NodeT node,
    const rclcpp::QoS & qos =
    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
  : node_base_(rclcpp::node_interfaces::get_node_base_interface(node))
  {
    auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);
 
    callbacks_ = std::make_shared<Callbacks>();
 
    event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
      node_topics, "/parameter_events", qos,
      [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) {
        callbacks->event_callback(event);
      });
  }
 
  using ParameterEventCallbackType =
    ParameterEventCallbackHandle::ParameterEventCallbackType;
 
 
  RCLCPP_PUBLIC
  ParameterEventCallbackHandle::SharedPtr
  add_parameter_event_callback(
    ParameterEventCallbackType callback);
 
 
  RCLCPP_PUBLIC
  void
  remove_parameter_event_callback(
    ParameterEventCallbackHandle::SharedPtr callback_handle);
 
  using ParameterCallbackType = ParameterCallbackHandle::ParameterCallbackType;
 
 
  RCLCPP_PUBLIC
  ParameterCallbackHandle::SharedPtr
  add_parameter_callback(
    const std::string & parameter_name,
    ParameterCallbackType callback,
    const std::string & node_name = "");
 
 
  RCLCPP_PUBLIC
  void
  remove_parameter_callback(
    ParameterCallbackHandle::SharedPtr callback_handle);
 
 
  RCLCPP_PUBLIC
  static bool
  get_parameter_from_event(
    const rcl_interfaces::msg::ParameterEvent & event,
    rclcpp::Parameter & parameter,
    const std::string & parameter_name,
    const std::string & node_name = "");
 
 
  RCLCPP_PUBLIC
  static rclcpp::Parameter
  get_parameter_from_event(
    const rcl_interfaces::msg::ParameterEvent & event,
    const std::string & parameter_name,
    const std::string & node_name = "");
 
 
  RCLCPP_PUBLIC
  static std::vector<rclcpp::Parameter>
  get_parameters_from_event(
    const rcl_interfaces::msg::ParameterEvent & event);
 
  using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
 
protected:
  // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
  // Hash function for string pair required in std::unordered_map
  // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values
  class StringPairHash
  {
  public:
    template<typename T>
    inline void hash_combine(std::size_t & seed, const T & v) const
    {
      std::hash<T> hasher;
      seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    }
 
    inline size_t operator()(const std::pair<std::string, std::string> & s) const
    {
      size_t seed = 0;
      hash_combine(seed, s.first);
      hash_combine(seed, s.second);
      return seed;
    }
  };
  // *INDENT-ON*
 
  struct Callbacks
  {
    std::recursive_mutex mutex_;
 
    // Map container for registered parameters
    std::unordered_map<
      std::pair<std::string, std::string>,
      CallbacksContainerType,
      StringPairHash
    > parameter_callbacks_;
 
    std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
 
    RCLCPP_PUBLIC
    void
    event_callback(const rcl_interfaces::msg::ParameterEvent & event);
  };
 
  std::shared_ptr<Callbacks> callbacks_;
 
  // Utility function for resolving node path.
  std::string resolve_path(const std::string & path);
 
  // Node interface used for base functionality
  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
 
  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
};
 
}  // namespace rclcpp
 
#endif  // RCLCPP__PARAMETER_EVENT_HANDLER_HPP_