Added joint limits urdf header for importing limits from URDF model

joint_limits/CMakeLists.txt

@@ -8,6 +8,7 @@ endif()
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   rclcpp_lifecycle
+  urdf
 )
 
 find_package(ament_cmake REQUIRED)
@@ -31,6 +32,9 @@ if(BUILD_TESTING)
   ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp)
   target_link_libraries(joint_limits_rosparam_test joint_limits)
 
+  ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp)
+  target_link_libraries(joint_limits_urdf_test joint_limits)
+
   add_launch_test(test/joint_limits_rosparam.launch.py)
   install(
     TARGETS joint_limits_rosparam_test

joint_limits/include/joint_limits/joint_limits_urdf.hpp

@@ -0,0 +1,85 @@
+// Copyright 2024 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.
+
+/// \author Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_LIMITS_URDF_HPP
+#define JOINT_LIMITS_URDF_HPP
+
+#include "joint_limits/joint_limits.hpp"
+#include "urdf_model/joint.h"
+
+namespace joint_limits
+{
+
+/**
+ * \brief Populate a JointLimits instance from URDF joint data.
+ * \param[in] urdf_joint URDF joint.
+ * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will
+ * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged.
+ * \return True if \e urdf_joint has a valid limits specification, false otherwise.
+ */
+inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits)
+{
+  if (!urdf_joint || !urdf_joint->limits)
+  {
+    return false;
+  }
+
+  limits.has_position_limits =
+    urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC;
+  if (limits.has_position_limits)
+  {
+    limits.min_position = urdf_joint->limits->lower;
+    limits.max_position = urdf_joint->limits->upper;
+  }
+
+  if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS)
+  {
+    limits.angle_wraparound = true;
+  }
+
+  limits.has_velocity_limits = true;
+  limits.max_velocity = urdf_joint->limits->velocity;
+
+  limits.has_acceleration_limits = false;
+
+  limits.has_effort_limits = true;
+  limits.max_effort = urdf_joint->limits->effort;
+
+  return true;
+}
+
+/**
+ * \brief Populate a SoftJointLimits instance from URDF joint data.
+ * \param[in] urdf_joint URDF joint.
+ * \param[out] soft_limits Where URDF soft joint limit data gets written into.
+ * \return True if \e urdf_joint has a valid soft limits specification, false otherwise.
+ */
+inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits)
+{
+  if (!urdf_joint || !urdf_joint->safety)
+  {
+    return false;
+  }
+
+  soft_limits.min_position = urdf_joint->safety->soft_lower_limit;
+  soft_limits.max_position = urdf_joint->safety->soft_upper_limit;
+  soft_limits.k_position = urdf_joint->safety->k_position;
+  soft_limits.k_velocity = urdf_joint->safety->k_velocity;
+
+  return true;
+}
+}  // namespace joint_limits
+#endif  // JOINT_LIMITS_URDF_HPP

joint_limits/package.xml

@@ -16,6 +16,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>urdf</depend>
 
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>