add helper function to load robot from package name + urdf + srdf (#3039

)

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.

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);