From 764eb14e95e5b6697981287fd50b4c8fedb19e34 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 12 Sep 2023 15:25:59 +0200 Subject: [PATCH] Fix collisions_updater CLI if no package is used (#2344) * Fix collisions_updater CLI if no package is used The exceptions introduced with https://github.com/ros-planning/moveit2/pull/2032 prevented from running the collisions updater CLI without a ROS package. This fix makes ROS packages optional again, allowing to use the CLI with absolute paths only. * Improve warn message wording (cherry picked from commit 16ac53ce8e2f58358bb603ee1f1fc67907b5091f) --- .../src/urdf_config.cpp | 46 +++++++++++-------- 1 file changed, 27 insertions(+), 19 deletions(-) diff --git a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp index 4d48be3af8..2480848908 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp +++ b/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.");