Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[moveit_cpp] Fix double declaration #1097

Merged
merged 5 commits into from
Mar 18, 2022
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,16 @@
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/rclcpp.hpp"

/** declare a parameter if not already declared. */
#define DECLARE(node, name, type, default_value) \
do \
{ \
if (not node->has_parameter(name)) \
{ \
node->declare_parameter<type>(name, default_value); \
} \
} while (false);

namespace joint_limits
{
inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
Expand All @@ -35,24 +45,24 @@ inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node
const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "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());
DECLARE(node, param_base_name + ".has_position_limits", bool, false);
DECLARE(node, param_base_name + ".min_position", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".max_position", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".has_velocity_limits", bool, false);
DECLARE(node, param_base_name + ".min_velocity", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".max_velocity", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".has_acceleration_limits", bool, false);
DECLARE(node, param_base_name + ".max_acceleration", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".has_jerk_limits", bool, false);
DECLARE(node, param_base_name + ".max_jerk", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".has_effort_limits", bool, false);
DECLARE(node, param_base_name + ".max_effort", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".angle_wraparound", bool, false);
DECLARE(node, param_base_name + ".has_soft_limits", bool, false);
DECLARE(node, param_base_name + ".k_position", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".k_velocity", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".soft_lower_limit", double, std::numeric_limits<double>::quiet_NaN());
DECLARE(node, param_base_name + ".soft_upper_limit", double, std::numeric_limits<double>::quiet_NaN());
}
catch (const std::exception& ex)
{
Expand Down Expand Up @@ -288,4 +298,6 @@ inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node::

} // namespace joint_limits

#undef DECLARE

#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_