Skip to content

Commit

Permalink
Fix missing namespace for joint_limits and check if parameter is decl…
Browse files Browse the repository at this point in the history 
…eared
  • Loading branch information
@sjahr @henningkayser
sjahr authored and henningkayser committed Nov 8, 2021
1 parent c97c840 commit 1908031
Show file tree
Hide file tree
Showing 3 changed files with 87 additions and 47 deletions.
115 changes: 78 additions & 37 deletions ..._motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,35 +39,60 @@
#include <limits>

// TODO(henning): Re-include when this is available, until then the headers content is copied below
// TODO(sjahr): Current implementation does not offer the desired API (08/2021). Use draft from dstogl instead
// (https://github.com/ros-controls/ros2_control/pull/462/files) #include <joint_limits_interface/joint_limits_rosparam.hpp>
// TODO(sjahr): Current implementation does not offer the desired API (08/2021). Use draft from dstogl instead but with
// slight modifications (https://github.com/ros-controls/ros2_control/pull/462/files)
// #include <joint_limits_interface/joint_limits_rosparam.hpp>
//////////////////////////////////////////////////////////////
// start of <joint_limits_interface/joint_limits_rosparam.hpp>
namespace joint_limits_interface
{
inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node)
template <typename T>
bool declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& parameter_name, T default_value)
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
node->declare_parameter<bool>(param_base_name + ".has_position_limits", false);
node->declare_parameter<double>(param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_velocity_limits", false);
node->declare_parameter<double>(param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_acceleration_limits", false);
node->declare_parameter<double>(param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_jerk_limits", false);
node->declare_parameter<double>(param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_effort_limits", false);
node->declare_parameter<double>(param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".angle_wraparound", false);
node->declare_parameter<bool>(param_base_name + ".has_soft_limits", false);
node->declare_parameter<double>(param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
if (!node->has_parameter(parameter_name))
{
node->declare_parameter<T>(parameter_name, default_value);
}
return true;
}
catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
{
RCLCPP_WARN_STREAM(node->get_logger(), "InvalidParameterTypeException(" << parameter_name << "): " << e.what());
return false;
}
}

inline bool declare_parameters(const std::string& joint_name, const std::string& param_ns,
const rclcpp::Node::SharedPtr& node)
{
std::string param_base_name = "joint_limits." + joint_name;
if (param_ns != "")
{
param_base_name = param_ns + "." + param_base_name;
}

try
{
declare_parameter<bool>(node, param_base_name + ".has_position_limits", false);
declare_parameter<double>(node, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
declare_parameter<double>(node, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
declare_parameter<bool>(node, param_base_name + ".has_velocity_limits", false);
declare_parameter<double>(node, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
declare_parameter<double>(node, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
declare_parameter<bool>(node, param_base_name + ".has_acceleration_limits", false);
declare_parameter<double>(node, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
declare_parameter<bool>(node, param_base_name + ".has_jerk_limits", false);
declare_parameter<double>(node, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
declare_parameter<bool>(node, param_base_name + ".has_effort_limits", false);
declare_parameter<double>(node, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
declare_parameter<bool>(node, param_base_name + ".angle_wraparound", false);
declare_parameter<bool>(node, param_base_name + ".has_soft_limits", false);
declare_parameter<double>(node, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
declare_parameter<double>(node, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
declare_parameter<double>(node, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
declare_parameter<double>(node, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
}
catch (const std::exception& ex)
{
Expand Down Expand Up @@ -106,14 +131,20 @@ inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node
*
* \param[in] joint_name Name of joint whose limits are to be fetched.
* \param[in] node NodeHandle where the joint limits are specified.
* \param[out] limits Where joint limit data gets written into. Limits specified in the node parameters will overwrite
* existing values. Values in \p limits not specified in the node parameters remain unchanged.
* \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node),
* false otherwise.
* \param[in] param_ns Parameter namespace of the joint i.e. robot_description_planning/ TODO(sjahr): Add API change
* upstream \param[out] limits Where joint limit data gets written into. Limits specified in the node parameters will
* overwrite existing values. Values in \p limits not specified in the node parameters remain unchanged. \return True if
* a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise.
*/
inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, JointLimits& limits)
inline bool getJointLimits(const std::string& joint_name, const std::string& param_ns,
const rclcpp::Node::SharedPtr& node, JointLimits& limits)
{
const std::string param_base_name = "joint_limits." + joint_name;
std::string param_base_name = "joint_limits." + joint_name;
if (param_ns != "")
{
param_base_name = param_ns + "." + param_base_name;
}

try
{
if (!node->has_parameter(param_base_name + ".has_position_limits") &&
Expand Down Expand Up @@ -254,10 +285,15 @@ inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::Sh
* \p k_velocity, \p soft_lower_limit and \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false
* otherwise.
*/
inline bool getSoftJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
SoftJointLimits& soft_limits)
inline bool getSoftJointLimits(const std::string& joint_name, const std::string& param_ns,
const rclcpp::Node::SharedPtr& node, SoftJointLimits& soft_limits)
{
const std::string param_base_name = "joint_limits." + joint_name;
std::string param_base_name = "joint_limits." + joint_name;
if (param_ns != "")
{
param_base_name = param_ns + "." + param_base_name;
}

try
{
if (!node->has_parameter(param_base_name + ".has_soft_limits") &&
Expand Down Expand Up @@ -310,24 +346,29 @@ namespace joint_limits_interface
/**
* @see joint_limits_inteface::getJointLimits(...)
*/
inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
joint_limits_interface::JointLimits& limits)
inline bool getJointLimits(const std::string& joint_name, const std::string& param_ns,
const rclcpp::Node::SharedPtr& node, joint_limits_interface::JointLimits& limits)
{
// Set the existing limits
if (!::joint_limits_interface::getJointLimits(joint_name, node, limits))
if (!::joint_limits_interface::getJointLimits(joint_name, param_ns, node, limits))
{
return false; // LCOV_EXCL_LINE // The case where getJointLimits returns
// false is covered above.
}
try
{
// Deceleration limits
const std::string limits_namespace = "joint_limits." + joint_name;
limits.has_deceleration_limits = node->declare_parameter(limits_namespace + ".has_deceleration_limits", false);
std::string param_base_name = "joint_limits." + joint_name;
if (param_ns != "")
{
param_base_name = param_ns + "." + param_base_name;
}

limits.has_deceleration_limits = node->declare_parameter(param_base_name + ".has_deceleration_limits", false);
if (limits.has_deceleration_limits)
{
limits.max_deceleration =
node->declare_parameter(limits_namespace + ".max_deceleration", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter(param_base_name + ".max_deceleration", std::numeric_limits<double>::quiet_NaN());
}
}
catch (const std::exception& ex)
Expand Down
7 changes: 3 additions & 4 deletions moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,18 +51,17 @@ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
const std::vector<const moveit::core::JointModel*>& joint_models)
{
JointLimitsContainer container;
// TODO(henningkayser): use param_namespace

RCLCPP_INFO_STREAM(LOGGER, "Reading limits from namespace " << node->get_namespace());
RCLCPP_INFO_STREAM(LOGGER, "Reading limits from namespace " << param_namespace);

// Iterate over all joint models and generate the map
for (auto joint_model : joint_models)
{
JointLimit joint_limit;

// If there is something defined for the joint in the node parameters
if (::joint_limits_interface::declare_parameters(joint_model->getName(), node) &&
joint_limits_interface::getJointLimits(joint_model->getName(), node, joint_limit))
if (::joint_limits_interface::declare_parameters(joint_model->getName(), param_namespace, node) &&
joint_limits_interface::getJointLimits(joint_model->getName(), param_namespace, node, joint_limit))
{
if (joint_limit.has_position_limits)
{
Expand Down
12 changes: 6 additions & 6 deletions moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,8 @@ TEST_F(JointLimitTest, SimpleRead)
JointLimits joint_limits_extended;

// TODO(sjahr): This might change once the fully ros2_control's joint limit interface is used again!
EXPECT_TRUE(::joint_limits_interface::declare_parameters("joint_1", node_));
EXPECT_TRUE(getJointLimits("joint_1", node_, joint_limits_extended));
EXPECT_TRUE(::joint_limits_interface::declare_parameters("joint_1", "", node_));
EXPECT_TRUE(getJointLimits("joint_1", "", node_, joint_limits_extended));

EXPECT_EQ(1, joint_limits_extended.max_acceleration);
EXPECT_EQ(-1, joint_limits_extended.max_deceleration);
Expand All @@ -72,7 +72,7 @@ TEST_F(JointLimitTest, readNonExistingJointLimit)
// Joints limits interface
JointLimits joint_limits_extended;

EXPECT_FALSE(getJointLimits("anything", node_, joint_limits_extended));
EXPECT_FALSE(getJointLimits("anything", "", node_, joint_limits_extended));
}

/**
Expand All @@ -85,7 +85,7 @@ TEST_F(JointLimitTest, readInvalidParameterName)
// Joints limits interface
JointLimits joint_limits_extended;

EXPECT_FALSE(getJointLimits("~anything", node_, joint_limits_extended));
EXPECT_FALSE(getJointLimits("~anything", "", node_, joint_limits_extended));
}

TEST_F(JointLimitTest, OldRead)
Expand All @@ -94,8 +94,8 @@ TEST_F(JointLimitTest, OldRead)
JointLimits joint_limits;

// TODO(sjahr): This might change once the fully ros2_control's joint limit interface is used again!
EXPECT_TRUE(::joint_limits_interface::declare_parameters("joint_1", node_));
EXPECT_TRUE(getJointLimits("joint_1", node_, joint_limits));
EXPECT_TRUE(::joint_limits_interface::declare_parameters("joint_1", "", node_));
EXPECT_TRUE(getJointLimits("joint_1", "", node_, joint_limits));

EXPECT_EQ(1, joint_limits.max_acceleration);
}
Expand Down

0 comments on commit 1908031

Please sign in to comment.