diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt
index a788b6de4f..1ed74c7603 100644
--- a/joint_limits/CMakeLists.txt
+++ b/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
diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp
new file mode 100644
index 0000000000..b5db39d9fb
--- /dev/null
+++ b/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
diff --git a/joint_limits/package.xml b/joint_limits/package.xml
index e1832fa776..98890eb03a 100644
--- a/joint_limits/package.xml
+++ b/joint_limits/package.xml
@@ -16,6 +16,7 @@
rclcpp
rclcpp_lifecycle
+ urdf
launch_testing_ament_cmake
ament_cmake_gtest