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_