33void declareParameterTemplate(
const rclcpp::Node::SharedPtr& node,
const std::string& name, T default_value)
44inline bool declareParameters(
const std::string& joint_name,
const rclcpp::Node::SharedPtr& node,
45 const std::string& param_ns)
47 const std::string param_base_name = (param_ns.empty() ?
"" : param_ns +
".") +
"joint_limits." + joint_name;
51 declareParameterTemplate(node, param_base_name +
".has_position_limits",
false);
52 declareParameterTemplate(node, param_base_name +
".min_position", std::numeric_limits<double>::quiet_NaN());
53 declareParameterTemplate(node, param_base_name +
".max_position", std::numeric_limits<double>::quiet_NaN());
54 declareParameterTemplate(node, param_base_name +
".has_velocity_limits",
false);
55 declareParameterTemplate(node, param_base_name +
".min_velocity", std::numeric_limits<double>::quiet_NaN());
56 declareParameterTemplate(node, param_base_name +
".max_velocity", std::numeric_limits<double>::quiet_NaN());
57 declareParameterTemplate(node, param_base_name +
".has_acceleration_limits",
false);
58 declareParameterTemplate(node, param_base_name +
".max_acceleration", std::numeric_limits<double>::quiet_NaN());
59 declareParameterTemplate(node, param_base_name +
".has_jerk_limits",
false);
60 declareParameterTemplate(node, param_base_name +
".max_jerk", std::numeric_limits<double>::quiet_NaN());
61 declareParameterTemplate(node, param_base_name +
".has_effort_limits",
false);
62 declareParameterTemplate(node, param_base_name +
".max_effort", std::numeric_limits<double>::quiet_NaN());
63 declareParameterTemplate(node, param_base_name +
".angle_wraparound",
false);
64 declareParameterTemplate(node, param_base_name +
".has_soft_limits",
false);
65 declareParameterTemplate(node, param_base_name +
".k_position", std::numeric_limits<double>::quiet_NaN());
66 declareParameterTemplate(node, param_base_name +
".k_velocity", std::numeric_limits<double>::quiet_NaN());
67 declareParameterTemplate(node, param_base_name +
".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
68 declareParameterTemplate(node, param_base_name +
".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
70 catch (
const std::exception& ex)
72 RCLCPP_ERROR(node->get_logger(),
"%s", ex.what());
112inline bool getJointLimits(
const std::string& joint_name,
const rclcpp::Node::SharedPtr& node,
115 const std::string param_base_name = (param_ns.empty() ?
"" : param_ns +
".") +
"joint_limits." + joint_name;
118 if (!node->has_parameter(param_base_name +
".has_position_limits") &&
119 !node->has_parameter(param_base_name +
".min_position") &&
120 !node->has_parameter(param_base_name +
".max_position") &&
121 !node->has_parameter(param_base_name +
".has_velocity_limits") &&
122 !node->has_parameter(param_base_name +
".min_velocity") &&
123 !node->has_parameter(param_base_name +
".max_velocity") &&
124 !node->has_parameter(param_base_name +
".has_acceleration_limits") &&
125 !node->has_parameter(param_base_name +
".max_acceleration") &&
126 !node->has_parameter(param_base_name +
".has_jerk_limits") &&
127 !node->has_parameter(param_base_name +
".max_jerk") &&
128 !node->has_parameter(param_base_name +
".has_effort_limits") &&
129 !node->has_parameter(param_base_name +
".max_effort") &&
130 !node->has_parameter(param_base_name +
".angle_wraparound") &&
131 !node->has_parameter(param_base_name +
".has_soft_limits") &&
132 !node->has_parameter(param_base_name +
".k_position") &&
133 !node->has_parameter(param_base_name +
".k_velocity") &&
134 !node->has_parameter(param_base_name +
".soft_lower_limit") &&
135 !node->has_parameter(param_base_name +
".soft_upper_limit"))
137 RCLCPP_ERROR(node->get_logger(),
138 "No joint limits specification found for joint '%s' in the parameter server "
139 "(node: %s param name: %s).",
140 joint_name.c_str(), node->get_name(), param_base_name.c_str());
144 catch (
const std::exception& ex)
146 RCLCPP_ERROR(node->get_logger(),
"%s", ex.what());
151 bool has_position_limits =
false;
152 if (node->get_parameter(param_base_name +
".has_position_limits", has_position_limits))
154 if (!has_position_limits)
158 double min_pos, max_pos;
159 if (has_position_limits && node->get_parameter(param_base_name +
".min_position", min_pos) &&
160 node->get_parameter(param_base_name +
".max_position", max_pos))
167 bool angle_wraparound;
168 if (!has_position_limits && node->get_parameter(param_base_name +
".angle_wraparound", angle_wraparound))
175 bool has_velocity_limits =
false;
176 if (node->get_parameter(param_base_name +
".has_velocity_limits", has_velocity_limits))
178 if (!has_velocity_limits)
183 if (has_velocity_limits && node->get_parameter(param_base_name +
".max_velocity", max_vel))
191 bool has_acceleration_limits =
false;
192 if (node->get_parameter(param_base_name +
".has_acceleration_limits", has_acceleration_limits))
194 if (!has_acceleration_limits)
199 if (has_acceleration_limits && node->get_parameter(param_base_name +
".max_acceleration", max_acc))
207 bool has_jerk_limits =
false;
208 if (node->get_parameter(param_base_name +
".has_jerk_limits", has_jerk_limits))
210 if (!has_jerk_limits)
215 if (has_jerk_limits && node->get_parameter(param_base_name +
".max_jerk", max_jerk))
223 bool has_effort_limits =
false;
224 if (node->get_parameter(param_base_name +
".has_effort_limits", has_effort_limits))
226 if (!has_effort_limits)
231 if (has_effort_limits && node->get_parameter(param_base_name +
".max_effort", max_effort))
257inline bool getJointLimits(
const std::string& joint_name,
const rclcpp::Node::SharedPtr& node,
260 const std::string param_base_name = (param_ns.empty() ?
"" : param_ns +
".") +
"joint_limits." + joint_name;
263 if (!node->has_parameter(param_base_name +
".has_soft_limits") &&
264 !node->has_parameter(param_base_name +
".k_velocity") &&
265 !node->has_parameter(param_base_name +
".k_position") &&
266 !node->has_parameter(param_base_name +
".soft_lower_limit") &&
267 !node->has_parameter(param_base_name +
".soft_upper_limit"))
269 RCLCPP_DEBUG(node->get_logger(),
270 "No soft joint limits specification found for joint '%s' in the parameter server "
271 "(node: %s param name: %s).",
272 joint_name.c_str(), node->get_name(), param_base_name.c_str());
276 catch (
const std::exception& ex)
278 RCLCPP_ERROR(node->get_logger(),
"%s", ex.what());
283 bool has_soft_limits;
284 if (node->get_parameter(param_base_name +
".has_soft_limits", has_soft_limits))
286 if (has_soft_limits && node->has_parameter(param_base_name +
".k_position") &&
287 node->has_parameter(param_base_name +
".k_velocity") &&
288 node->has_parameter(param_base_name +
".soft_lower_limit") &&
289 node->has_parameter(param_base_name +
".soft_upper_limit"))
291 node->get_parameter(param_base_name +
".k_position", soft_limits.
k_position);
292 node->get_parameter(param_base_name +
".k_velocity", soft_limits.
k_velocity);
293 node->get_parameter(param_base_name +
".soft_lower_limit", soft_limits.
min_position);
294 node->get_parameter(param_base_name +
".soft_upper_limit", soft_limits.
max_position);