Program Listing for File joint_limits_rosparam.hpp
↰ Return to documentation for file (include/joint_limits/joint_limits_rosparam.hpp
)
// Copyright 2020 PAL Robotics S.L.
//
// 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 JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
#include <limits>
#include <string>
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
namespace // utilities
{
template <typename ParameterT>
auto auto_declare(
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const std::string & name, const ParameterT & default_value)
{
if (!param_itf->has_parameter(name))
{
auto param_default_value = rclcpp::ParameterValue(default_value);
param_itf->declare_parameter(name, param_default_value);
}
return param_itf->get_parameter(name).get_value<ParameterT>();
}
} // namespace
namespace joint_limits
{
inline bool declare_parameters(
const std::string & joint_name,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf)
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
auto_declare<bool>(param_itf, param_base_name + ".has_position_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
auto_declare<double>(
param_itf, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_velocity_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
auto_declare<double>(
param_itf, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_acceleration_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_deceleration_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_deceleration", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_jerk_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_effort_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".angle_wraparound", false);
auto_declare<bool>(param_itf, param_base_name + ".has_soft_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
auto_declare<double>(
param_itf, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
auto_declare<double>(
param_itf, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
auto_declare<double>(
param_itf, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
}
catch (const std::exception & ex)
{
RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what());
return false;
}
return true;
}
inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node)
{
return declare_parameters(
joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface());
}
inline bool declare_parameters(
const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node)
{
return declare_parameters(
joint_name, lifecycle_node->get_node_parameters_interface(),
lifecycle_node->get_node_logging_interface());
}
inline bool get_joint_limits(
const std::string & joint_name,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
JointLimits & limits)
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
if (
!param_itf->has_parameter(param_base_name + ".has_position_limits") &&
!param_itf->has_parameter(param_base_name + ".min_position") &&
!param_itf->has_parameter(param_base_name + ".max_position") &&
!param_itf->has_parameter(param_base_name + ".has_velocity_limits") &&
!param_itf->has_parameter(param_base_name + ".min_velocity") &&
!param_itf->has_parameter(param_base_name + ".max_velocity") &&
!param_itf->has_parameter(param_base_name + ".has_acceleration_limits") &&
!param_itf->has_parameter(param_base_name + ".max_acceleration") &&
!param_itf->has_parameter(param_base_name + ".has_deceleration_limits") &&
!param_itf->has_parameter(param_base_name + ".max_deceleration") &&
!param_itf->has_parameter(param_base_name + ".has_jerk_limits") &&
!param_itf->has_parameter(param_base_name + ".max_jerk") &&
!param_itf->has_parameter(param_base_name + ".has_effort_limits") &&
!param_itf->has_parameter(param_base_name + ".max_effort") &&
!param_itf->has_parameter(param_base_name + ".angle_wraparound") &&
!param_itf->has_parameter(param_base_name + ".has_soft_limits") &&
!param_itf->has_parameter(param_base_name + ".k_position") &&
!param_itf->has_parameter(param_base_name + ".k_velocity") &&
!param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
!param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
{
RCLCPP_ERROR(
logging_itf->get_logger(),
"No joint limits specification found for joint '%s' in the parameter server "
"(param name: %s).",
joint_name.c_str(), param_base_name.c_str());
return false;
}
}
catch (const std::exception & ex)
{
RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what());
return false;
}
// Position limits
if (param_itf->has_parameter(param_base_name + ".has_position_limits"))
{
limits.has_position_limits =
param_itf->get_parameter(param_base_name + ".has_position_limits").as_bool();
if (
limits.has_position_limits && param_itf->has_parameter(param_base_name + ".min_position") &&
param_itf->has_parameter(param_base_name + ".max_position"))
{
limits.min_position = param_itf->get_parameter(param_base_name + ".min_position").as_double();
limits.max_position = param_itf->get_parameter(param_base_name + ".max_position").as_double();
}
else
{
limits.has_position_limits = false;
}
if (
!limits.has_position_limits &&
param_itf->has_parameter(param_base_name + ".angle_wraparound"))
{
limits.angle_wraparound =
param_itf->get_parameter(param_base_name + ".angle_wraparound").as_bool();
}
}
// Velocity limits
if (param_itf->has_parameter(param_base_name + ".has_velocity_limits"))
{
limits.has_velocity_limits =
param_itf->get_parameter(param_base_name + ".has_velocity_limits").as_bool();
if (limits.has_velocity_limits && param_itf->has_parameter(param_base_name + ".max_velocity"))
{
limits.max_velocity = param_itf->get_parameter(param_base_name + ".max_velocity").as_double();
}
else
{
limits.has_velocity_limits = false;
}
}
// Acceleration limits
if (param_itf->has_parameter(param_base_name + ".has_acceleration_limits"))
{
limits.has_acceleration_limits =
param_itf->get_parameter(param_base_name + ".has_acceleration_limits").as_bool();
if (
limits.has_acceleration_limits &&
param_itf->has_parameter(param_base_name + ".max_acceleration"))
{
limits.max_acceleration =
param_itf->get_parameter(param_base_name + ".max_acceleration").as_double();
}
else
{
limits.has_acceleration_limits = false;
}
}
// Deceleration limits
if (param_itf->has_parameter(param_base_name + ".has_deceleration_limits"))
{
limits.has_deceleration_limits =
param_itf->get_parameter(param_base_name + ".has_deceleration_limits").as_bool();
if (
limits.has_deceleration_limits &&
param_itf->has_parameter(param_base_name + ".max_deceleration"))
{
limits.max_deceleration =
param_itf->get_parameter(param_base_name + ".max_deceleration").as_double();
}
else
{
limits.has_deceleration_limits = false;
}
}
// Jerk limits
if (param_itf->has_parameter(param_base_name + ".has_jerk_limits"))
{
limits.has_jerk_limits =
param_itf->get_parameter(param_base_name + ".has_jerk_limits").as_bool();
if (limits.has_jerk_limits && param_itf->has_parameter(param_base_name + ".max_jerk"))
{
limits.max_jerk = param_itf->get_parameter(param_base_name + ".max_jerk").as_double();
}
else
{
limits.has_jerk_limits = false;
}
}
// Effort limits
if (param_itf->has_parameter(param_base_name + ".has_effort_limits"))
{
limits.has_effort_limits =
param_itf->get_parameter(param_base_name + ".has_effort_limits").as_bool();
if (limits.has_effort_limits && param_itf->has_parameter(param_base_name + ".max_effort"))
{
limits.has_effort_limits = true;
limits.max_effort = param_itf->get_parameter(param_base_name + ".max_effort").as_double();
}
else
{
limits.has_effort_limits = false;
}
}
return true;
}
inline bool get_joint_limits(
const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits)
{
return get_joint_limits(
joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(), limits);
}
inline bool get_joint_limits(
const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
JointLimits & limits)
{
return get_joint_limits(
joint_name, lifecycle_node->get_node_parameters_interface(),
lifecycle_node->get_node_logging_interface(), limits);
}
inline bool get_joint_limits(
const std::string & joint_name,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
SoftJointLimits & soft_limits)
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
if (
!param_itf->has_parameter(param_base_name + ".has_soft_limits") &&
!param_itf->has_parameter(param_base_name + ".k_velocity") &&
!param_itf->has_parameter(param_base_name + ".k_position") &&
!param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
!param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
{
RCLCPP_DEBUG(
logging_itf->get_logger(),
"No soft joint limits specification found for joint '%s' in the parameter server "
"(param name: %s).",
joint_name.c_str(), param_base_name.c_str());
return false;
}
}
catch (const std::exception & ex)
{
RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what());
return false;
}
// Override soft limits if complete specification is found
if (param_itf->has_parameter(param_base_name + ".has_soft_limits"))
{
if (
param_itf->get_parameter(param_base_name + ".has_soft_limits").as_bool() &&
param_itf->has_parameter(param_base_name + ".k_position") &&
param_itf->has_parameter(param_base_name + ".k_velocity") &&
param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
{
soft_limits.k_position =
param_itf->get_parameter(param_base_name + ".k_position").as_double();
soft_limits.k_velocity =
param_itf->get_parameter(param_base_name + ".k_velocity").as_double();
soft_limits.min_position =
param_itf->get_parameter(param_base_name + ".soft_lower_limit").as_double();
soft_limits.max_position =
param_itf->get_parameter(param_base_name + ".soft_upper_limit").as_double();
return true;
}
}
return false;
}
inline bool get_joint_limits(
const std::string & joint_name, const rclcpp::Node::SharedPtr & node,
SoftJointLimits & soft_limits)
{
return get_joint_limits(
joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(),
soft_limits);
}
inline bool get_joint_limits(
const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
SoftJointLimits & soft_limits)
{
return get_joint_limits(
joint_name, lifecycle_node->get_node_parameters_interface(),
lifecycle_node->get_node_logging_interface(), soft_limits);
}
} // namespace joint_limits
#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_