moveit2
The MoveIt Motion Planning Framework for ROS 2.
joint_limits_rosparam.hpp
Go to the documentation of this file.
1 // Copyright 2020 PAL Robotics S.L.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
16 
17 #pragma once
18 
19 // TODO(henning): This file is copied from the DRAFT PR https://github.com/ros-controls/ros2_control/pull/462 since the
20 // current ros2_control implementation does not offer the desired joint limits API, yet. Remove when ros2_control has an
21 // upstream version of this.
22 
23 #include <limits>
24 #include <string>
25 
27 #include <rclcpp/rclcpp.hpp>
28 
29 namespace
30 {
32 template <typename T>
33 void declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value)
34 {
35  if (not node->has_parameter(name))
36  {
37  node->declare_parameter<T>(name, default_value);
38  }
39 }
40 } // namespace
41 
42 namespace joint_limits
43 {
44 inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
45  const std::string& param_ns)
46 {
47  const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name;
48 
49  try
50  {
51  declare_parameter(node, param_base_name + ".has_position_limits", false);
52  declare_parameter(node, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
53  declare_parameter(node, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
54  declare_parameter(node, param_base_name + ".has_velocity_limits", false);
55  declare_parameter(node, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
56  declare_parameter(node, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
57  declare_parameter(node, param_base_name + ".has_acceleration_limits", false);
58  declare_parameter(node, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
59  declare_parameter(node, param_base_name + ".has_jerk_limits", false);
60  declare_parameter(node, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
61  declare_parameter(node, param_base_name + ".has_effort_limits", false);
62  declare_parameter(node, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
63  declare_parameter(node, param_base_name + ".angle_wraparound", false);
64  declare_parameter(node, param_base_name + ".has_soft_limits", false);
65  declare_parameter(node, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
66  declare_parameter(node, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
67  declare_parameter(node, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
68  declare_parameter(node, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
69  }
70  catch (const std::exception& ex)
71  {
72  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
73  return false;
74  }
75  return true;
76 }
77 
79 
112 inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
113  const std::string& param_ns, JointLimits& limits)
114 {
115  const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name;
116  try
117  {
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"))
136  {
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());
141  return false;
142  }
143  }
144  catch (const std::exception& ex)
145  {
146  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
147  return false;
148  }
149 
150  // Position limits
151  bool has_position_limits = false;
152  if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits))
153  {
154  if (!has_position_limits)
155  {
156  limits.has_position_limits = false;
157  }
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))
161  {
162  limits.has_position_limits = true;
163  limits.min_position = min_pos;
164  limits.max_position = max_pos;
165  }
166 
167  bool angle_wraparound;
168  if (!has_position_limits && node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound))
169  {
170  limits.angle_wraparound = angle_wraparound;
171  }
172  }
173 
174  // Velocity limits
175  bool has_velocity_limits = false;
176  if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits))
177  {
178  if (!has_velocity_limits)
179  {
180  limits.has_velocity_limits = false;
181  }
182  double max_vel;
183  if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel))
184  {
185  limits.has_velocity_limits = true;
186  limits.max_velocity = max_vel;
187  }
188  }
189 
190  // Acceleration limits
191  bool has_acceleration_limits = false;
192  if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits))
193  {
194  if (!has_acceleration_limits)
195  {
196  limits.has_acceleration_limits = false;
197  }
198  double max_acc;
199  if (has_acceleration_limits && node->get_parameter(param_base_name + ".max_acceleration", max_acc))
200  {
201  limits.has_acceleration_limits = true;
202  limits.max_acceleration = max_acc;
203  }
204  }
205 
206  // Jerk limits
207  bool has_jerk_limits = false;
208  if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits))
209  {
210  if (!has_jerk_limits)
211  {
212  limits.has_jerk_limits = false;
213  }
214  double max_jerk;
215  if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk))
216  {
217  limits.has_jerk_limits = true;
218  limits.max_jerk = max_jerk;
219  }
220  }
221 
222  // Effort limits
223  bool has_effort_limits = false;
224  if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits))
225  {
226  if (!has_effort_limits)
227  {
228  limits.has_effort_limits = false;
229  }
230  double max_effort;
231  if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort))
232  {
233  limits.has_effort_limits = true;
234  limits.max_effort = max_effort;
235  }
236  }
237 
238  return true;
239 }
240 
242 
257 inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
258  const std::string& param_ns, SoftJointLimits& soft_limits)
259 {
260  const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name;
261  try
262  {
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"))
268  {
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());
273  return false;
274  }
275  }
276  catch (const std::exception& ex)
277  {
278  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
279  return false;
280  }
281 
282  // Override soft limits if complete specification is found
283  bool has_soft_limits;
284  if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits))
285  {
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"))
290  {
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);
295  return true;
296  }
297  }
298 
299  return false;
300 }
301 
302 } // namespace joint_limits
bool get_joint_limits(const std::string &joint_name, const rclcpp::Node::SharedPtr &node, const std::string &param_ns, JointLimits &limits)
Populate a JointLimits instance from the ROS parameter server.
bool declare_parameters(const std::string &joint_name, const rclcpp::Node::SharedPtr &node, const std::string &param_ns)
rclcpp::Parameter declare_parameter(const rclcpp::Node::SharedPtr &node, const std::string &parameter_name)
name
Definition: setup.py:7