From b05ab22fba39ff4fc5ccfa1384aa1d5f53af0390 Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Fri, 25 Oct 2024 11:48:56 +0200
Subject: [PATCH] add helper function to load robot from package name + urdf +
 srdf (#3039)

---
 .../moveit/utils/robot_model_test_utils.h     | 11 +++++++++
 .../utils/src/robot_model_test_utils.cpp      | 24 +++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h
index f8fe1b052c..a419d4cc4e 100644
--- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h
+++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h
@@ -52,6 +52,17 @@ namespace moveit
 {
 namespace core
 {
+
+/** \brief Loads a robot model given a URDF and SRDF file in a package.
+ * \param[in] package_name The name of the package containing the URDF and SRDF files.
+ * \param[in] urdf_relative_path The relative path to the URDF file in the package installed directory.
+ * \param[in] srdf_relative_path The relative path to the SRDF file in the package installed directory.
+ * \returns a RobotModel constructed from the URDF and SRDF files, or nullptr if the files could not be loaded.
+ */
+moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& package_name,
+                                                  const std::string& urdf_relative_path,
+                                                  const std::string& srdf_relative_path);
+
 /** \brief Loads a robot from moveit_resources.
  * \param[in] robot_name The name of the robot in moveit_resources to load.
  *            This should be the prefix to many of the robot packages.
diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp
index 5c417f0c71..cc39f6caf8 100644
--- a/moveit_core/utils/src/robot_model_test_utils.cpp
+++ b/moveit_core/utils/src/robot_model_test_utils.cpp
@@ -60,6 +60,30 @@ rclcpp::Logger getLogger()
 }
 }  // namespace
 
+moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& package_name,
+                                                  const std::string& urdf_relative_path,
+                                                  const std::string& srdf_relative_path)
+{
+  const auto urdf_path =
+      std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)) / urdf_relative_path;
+  const auto srdf_path =
+      std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)) / srdf_relative_path;
+
+  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path.string());
+  if (urdf_model == nullptr)
+  {
+    return nullptr;
+  }
+
+  auto srdf_model = std::make_shared<srdf::Model>();
+  if (!srdf_model->initFile(*urdf_model, srdf_path.string()))
+  {
+    return nullptr;
+  }
+
+  return std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
+}
+
 moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
 {
   urdf::ModelInterfaceSharedPtr urdf = loadModelInterface(robot_name);