parameter_event_handler.cpp

// 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.
 
#include <functional>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "rclcpp/parameter_event_handler.hpp"
#include "rcpputils/join.hpp"
 
namespace rclcpp
{
 
ParameterEventCallbackHandle::SharedPtr
ParameterEventHandler::add_parameter_event_callback(
  const ParameterEventCallbackType & callback)
{
  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
  auto handle = std::make_shared<ParameterEventCallbackHandle>();
  handle->callback = callback;
  callbacks_->event_callbacks_.emplace_front(handle);
 
  return handle;
}
 
void
ParameterEventHandler::remove_parameter_event_callback(
  const ParameterEventCallbackHandle::SharedPtr & callback_handle)
{
  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
  auto it = std::find_if(
    callbacks_->event_callbacks_.begin(),
    callbacks_->event_callbacks_.end(),
    [callback_handle](const auto & weak_handle) {
      return callback_handle.get() == weak_handle.lock().get();
    });
  if (it != callbacks_->event_callbacks_.end()) {
    callbacks_->event_callbacks_.erase(it);
  } else {
    throw std::runtime_error("Callback doesn't exist");
  }
}
 
ParameterCallbackHandle::SharedPtr
ParameterEventHandler::add_parameter_callback(
  const std::string & parameter_name,
  const ParameterCallbackType & callback,
  const std::string & node_name)
{
  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
  auto full_node_name = resolve_path(node_name);
 
  auto handle = std::make_shared<ParameterCallbackHandle>();
  handle->callback = callback;
  handle->parameter_name = parameter_name;
  handle->node_name = full_node_name;
  // the last callback registered is executed first.
  callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);
 
  return handle;
}
 
bool
ParameterEventHandler::configure_nodes_filter(const std::vector<std::string> & node_names)
{
  if (!event_subscription_->is_cft_supported()) {
    return false;
  }
 
  if (node_names.empty()) {
    // Clear content filter
    event_subscription_->set_content_filter(std::string());
    if (event_subscription_->is_cft_enabled()) {
      return false;
    }
    return true;
  }
 
  std::string filter_expression;
  size_t total = node_names.size();
  for (size_t i = 0; i < total; ++i) {
    filter_expression += "node = %" + std::to_string(i);
    if (i < total - 1) {
      filter_expression += " OR ";
    }
  }
 
  // Enclose each node name in "'".
  std::vector<std::string> quoted_node_names;
  const std::string delim("'");
  for (const auto & name : node_names) {
    quoted_node_names.push_back(delim + resolve_path(name) + delim);
  }
 
  event_subscription_->set_content_filter(filter_expression, quoted_node_names);
 
  return event_subscription_->is_cft_enabled();
}
 
void
ParameterEventHandler::remove_parameter_callback(
  const ParameterCallbackHandle::SharedPtr & callback_handle)
{
  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
  auto handle = callback_handle.get();
  auto & container = callbacks_->parameter_callbacks_[{handle->parameter_name, handle->node_name}];
  auto it = std::find_if(
    container.begin(),
    container.end(),
    [handle](const auto & weak_handle) {
      return handle == weak_handle.lock().get();
    });
  if (it != container.end()) {
    container.erase(it);
    if (container.empty()) {
      callbacks_->parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
    }
  } else {
    throw std::runtime_error("Callback doesn't exist");
  }
}
 
bool
ParameterEventHandler::get_parameter_from_event(
  const rcl_interfaces::msg::ParameterEvent & event,
  rclcpp::Parameter & parameter,
  const std::string & parameter_name,
  const std::string & node_name)
{
  if (event.node != node_name) {
    return false;
  }
 
  for (auto & new_parameter : event.new_parameters) {
    if (new_parameter.name == parameter_name) {
      parameter = rclcpp::Parameter::from_parameter_msg(new_parameter);
      return true;
    }
  }
 
  for (auto & changed_parameter : event.changed_parameters) {
    if (changed_parameter.name == parameter_name) {
      parameter = rclcpp::Parameter::from_parameter_msg(changed_parameter);
      return true;
    }
  }
 
  return false;
}
 
rclcpp::Parameter
ParameterEventHandler::get_parameter_from_event(
  const rcl_interfaces::msg::ParameterEvent & event,
  const std::string & parameter_name,
  const std::string & node_name)
{
  rclcpp::Parameter p;
  if (!get_parameter_from_event(event, p, parameter_name, node_name)) {
    if (event.node == node_name) {
      return rclcpp::Parameter(parameter_name, rclcpp::PARAMETER_NOT_SET);
    } else {
      throw std::runtime_error(
              "The node name '" + node_name + "' of parameter '" + parameter_name +
              +"' doesn't match the node name '" + event.node + "' in parameter event");
    }
  }
  return p;
}
 
std::vector<rclcpp::Parameter>
ParameterEventHandler::get_parameters_from_event(
  const rcl_interfaces::msg::ParameterEvent & event)
{
  std::vector<rclcpp::Parameter> params;
 
  for (auto & new_parameter : event.new_parameters) {
    params.push_back(rclcpp::Parameter::from_parameter_msg(new_parameter));
  }
 
  for (auto & changed_parameter : event.changed_parameters) {
    params.push_back(rclcpp::Parameter::from_parameter_msg(changed_parameter));
  }
 
  return params;
}
 
void
ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event)
{
  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
  for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) {
    rclcpp::Parameter p;
    if (get_parameter_from_event(event, p, it->first.first, it->first.second)) {
      for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) {
        auto shared_handle = cb->lock();
        if (nullptr != shared_handle) {
          shared_handle->callback(p);
        } else {
          cb = it->second.erase(cb);
        }
      }
    }
  }
 
  for (auto event_cb = event_callbacks_.begin(); event_cb != event_callbacks_.end(); ++event_cb) {
    auto shared_event_handle = event_cb->lock();
    if (nullptr != shared_event_handle) {
      shared_event_handle->callback(event);
    } else {
      event_cb = event_callbacks_.erase(event_cb);
    }
  }
}
 
std::string
ParameterEventHandler::resolve_path(const std::string & path)
{
  if (path.empty()) {
    return node_base_->get_fully_qualified_name();
  }
 
  if (*path.begin() != '/') {
    auto ns = node_base_->get_namespace();
    const std::vector<std::string> paths{ns, path};
    if(ns == std::string("/")) {
      return ns + path;
    }
 
    return rcpputils::join(paths, "/");
  }
 
  return path;
}
 
}  // namespace rclcpp