Program Listing for File parameter.hpp

↰ Return to documentation for file (/tmp/ws/src/fuse/fuse_core/include/fuse_core/parameter.hpp)

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef FUSE_CORE__PARAMETER_HPP_
#define FUSE_CORE__PARAMETER_HPP_

#include <map>
#include <stdexcept>
#include <string>
#include <unordered_set>
#include <vector>

#include <fuse_core/eigen.hpp>
#include <fuse_core/loss_loader.hpp>
#include <fuse_core/node_interfaces/node_interfaces.hpp>
#include <rclcpp/logging.hpp>
#include "rclcpp/parameter.hpp"

namespace fuse_core
{

// Helper function to get a namespace string with a '.' suffix, but only if not empty
std::string joinParameterName(const std::string & left, const std::string & right);

// NOTE(CH3): Some of these basically mimic the behavior from rclcpp's node.hpp, but for interfaces

template<class T>
T getParam(
  node_interfaces::NodeInterfaces<node_interfaces::Parameters> interfaces,
  const std::string & parameter_name,
  const T & default_value,
  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
  rcl_interfaces::msg::ParameterDescriptor(),
  bool ignore_override = false)
{
  auto params_interface = interfaces.get_node_parameters_interface();
  if (params_interface->has_parameter(parameter_name)) {
    return params_interface->get_parameter(parameter_name).get_parameter_value().get<T>();
  } else {
    try {
      return params_interface->declare_parameter(
        parameter_name, rclcpp::ParameterValue(default_value), parameter_descriptor, ignore_override
      ).get<T>();
    } catch (const rclcpp::ParameterTypeException & ex) {
      throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what());
    }
  }
}


template<class T>
T getParam(
  node_interfaces::NodeInterfaces<node_interfaces::Parameters> interfaces,
  const std::string & parameter_name,
  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
  rcl_interfaces::msg::ParameterDescriptor(),
  bool ignore_override = false)
{
  // get advantage of parameter value template magic to get the correct rclcpp::ParameterType from T
  // NOTE(CH3): For the same reason we can't defer to the overload of getParam
  rclcpp::ParameterValue value{T{}};
  try {
    return interfaces.get_node_parameters_interface()->declare_parameter(
      parameter_name, value.get_type(), parameter_descriptor, ignore_override
    ).get<T>();
  } catch (const rclcpp::ParameterTypeException & ex) {
    throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what());
  }
}

namespace detail
{
std::unordered_set<std::string>
list_parameter_override_prefixes(
  const std::map<std::string, rclcpp::ParameterValue> & overrides,
  std::string prefix);
}  // namespace detail

std::unordered_set<std::string>
list_parameter_override_prefixes(
  node_interfaces::NodeInterfaces<node_interfaces::Parameters> interfaces,
  std::string prefix);

inline
void getParamRequired(
  node_interfaces::NodeInterfaces<
    node_interfaces::Base,
    node_interfaces::Logging,
    node_interfaces::Parameters
  > interfaces,
  const std::string & key,
  std::string & value
)
{
  std::string default_value = "";
  value = getParam(interfaces, key, default_value);

  if (value == default_value) {
    const std::string error =
      "Could not find required parameter " + key + " in namespace " +
      interfaces.get_node_base_interface()->get_namespace();

    RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), error);
    throw std::runtime_error(error);
  }
}

template<typename T,
  typename = std::enable_if_t<std::is_integral<T>::value || std::is_floating_point<T>::value>>
void getPositiveParam(
  node_interfaces::NodeInterfaces<
    node_interfaces::Logging,
    node_interfaces::Parameters
  > interfaces,
  const std::string & parameter_name,
  T & default_value,
  const bool strict = true
)
{
  T value = getParam(interfaces, parameter_name, default_value);
  if (value < 0 || (strict && value == 0)) {
    RCLCPP_WARN_STREAM(
      interfaces.get_node_logging_interface()->get_logger(),
      "The requested " << parameter_name.c_str() << " is <" << (strict ? "=" : "")
                       << " 0. Using the default value (" << default_value << ") instead.");
  } else {
    default_value = value;
  }
}

inline void getPositiveParam(
  node_interfaces::NodeInterfaces<
    node_interfaces::Logging,
    node_interfaces::Parameters
  > interfaces,
  const std::string & parameter_name,
  rclcpp::Duration & default_value, const bool strict = true)
{
  double default_value_sec = default_value.seconds();
  getPositiveParam(interfaces, parameter_name, default_value_sec, strict);
  default_value = rclcpp::Duration::from_seconds(default_value_sec);
}

template<int Size, typename Scalar = double>
fuse_core::Matrix<Scalar, Size, Size> getCovarianceDiagonalParam(
  node_interfaces::NodeInterfaces<
    node_interfaces::Logging,
    node_interfaces::Parameters
  > interfaces,
  const std::string & parameter_name,
  Scalar default_value
)
{
  using Vector = typename Eigen::Matrix<Scalar, Size, 1>;

  std::vector<Scalar> diagonal(Size, default_value);
  diagonal = getParam(interfaces, parameter_name, diagonal);

  const auto diagonal_size = diagonal.size();
  if (diagonal_size != Size) {
    throw std::invalid_argument(
            "Invalid size of " + std::to_string(diagonal_size) + ", expected " +
            std::to_string(Size));
  }

  if (std::any_of(
      diagonal.begin(), diagonal.end(),
      [](const auto & value) {return value < Scalar(0);}))  // NOLINT(whitespace/braces)
  {
    throw std::invalid_argument(
            "Invalid negative diagonal values in " +
            fuse_core::to_string(Vector(diagonal.data())));
  }

  return Vector(diagonal.data()).asDiagonal();
}

inline fuse_core::Loss::SharedPtr loadLossConfig(
  node_interfaces::NodeInterfaces<
    node_interfaces::Base,
    node_interfaces::Logging,
    node_interfaces::Parameters
  > interfaces,
  const std::string & name
)
{
  if (!interfaces.get_node_parameters_interface()->has_parameter(
      name + ".type"))
  {
    return {};
  }

  std::string loss_type;
  getParamRequired(interfaces, name + ".type", loss_type);

  auto loss = fuse_core::createUniqueLoss(loss_type);
  loss->initialize(interfaces, interfaces.get_node_base_interface()->get_fully_qualified_name());

  return loss;
}

}  // namespace fuse_core

#endif  // FUSE_CORE__PARAMETER_HPP_