Fix collisions_updater CLI if no package is used

moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp

@@ -92,32 +92,45 @@ void URDFConfig::loadFromPath(const std::filesystem::path& urdf_file_path, const
 
 void URDFConfig::setPackageName()
 {
-  bool package_found = extractPackageNameFromPath(urdf_path_, urdf_pkg_name_, urdf_pkg_relative_path_);
-  if (!package_found)
-  {
-    urdf_pkg_name_ = "";
-    urdf_pkg_relative_path_ = urdf_path_;  // just the absolute path
-  }
-  // Check that ROS can find the package
-  const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_);
+  // Reset to defaults: no package name, relative path is set to absolute path
+  urdf_pkg_name_ = "";
+  urdf_pkg_relative_path_ = urdf_path_;
 
-  if (robot_desc_pkg_path.empty())
+  std::string pkg_name;
+  std::filesystem::path relative_path;
+  if (extractPackageNameFromPath(urdf_path_, pkg_name, relative_path))
   {
-    RCLCPP_WARN(*logger_,
-                "Package Not Found In ROS Workspace. ROS was unable to find the package name '%s'"
-                " within the ROS workspace. This may cause issues later.",
-                urdf_pkg_name_.c_str());
+    // Check that ROS can find the package, update members accordingly
+    const std::filesystem::path robot_desc_pkg_path = getSharePath(pkg_name);
+    if (!robot_desc_pkg_path.empty())
+    {
+      urdf_pkg_name_ = pkg_name;
+      urdf_pkg_relative_path_ = relative_path;
+    }
+    else
+    {
+      RCLCPP_WARN(*logger_,
+                  "Found package name '%s' but failed to resolve ROS package path."
+                  "Attempting to load the URDF from absolute path, instead.",
+                  pkg_name.c_str());
+    }
   }
 }
 
 void URDFConfig::loadFromPackage(const std::filesystem::path& package_name, const std::filesystem::path& relative_path,
                                  const std::string& xacro_args)
 {
+  const std::filesystem::path package_path = getSharePath(package_name);
+  if (package_path.empty())
+  {
+    throw std::runtime_error("URDF/COLLADA package not found: ''" + package_name.string());
+  }
+
   urdf_pkg_name_ = package_name;
   urdf_pkg_relative_path_ = relative_path;
   xacro_args_ = xacro_args;
 
-  urdf_path_ = getSharePath(urdf_pkg_name_) / relative_path;
+  urdf_path_ = package_path / relative_path;
   load();
 }
 
@@ -131,11 +144,6 @@ void URDFConfig::load()
     throw std::runtime_error("URDF/COLLADA file not found: " + urdf_path_.string());
   }
 
-  if (urdf_pkg_name_.empty())
-  {
-    throw std::runtime_error("URDF/COLLADA package not found for file path: " + urdf_path_.string());
-  }
-
   if (urdf_string_.empty() && rdf_loader::RDFLoader::isXacroFile(urdf_path_))
   {
     throw std::runtime_error("Running xacro failed.\nPlease check console for errors.");