diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp index 0fa446ab57..74fd7ee166 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp @@ -305,8 +305,16 @@ void StartScreenWidget::onPackagePathChanged(const QString& /*path*/) void StartScreenWidget::onUrdfPathChanged(const QString& path) { - setup_step_.loadURDFFile(path.toStdString(), urdf_file_->getArgs().toStdString()); - urdf_file_->setArgsEnabled(setup_step_.isXacroFile()); + try + { + setup_step_.loadURDFFile(path.toStdString(), urdf_file_->getArgs().toStdString()); + urdf_file_->setArgsEnabled(setup_step_.isXacroFile()); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR(setup_step_.getLogger(), "Error Loading URDF Path. %s", e.what()); + QMessageBox::warning(this, "Error Loading URDF Path", QString(e.what())); + } } bool StartScreenWidget::loadPackageSettings(bool show_warnings) @@ -343,8 +351,8 @@ bool StartScreenWidget::loadExistingFiles() } catch (const std::runtime_error& e) { + RCLCPP_ERROR(setup_step_.getLogger(), "Error Loading SRDF. %s", e.what()); QMessageBox::warning(this, "Error Loading SRDF", QString(e.what())); - RCLCPP_ERROR(setup_step_.getLogger(), "%s", e.what()); return false; } diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp index 3fc15b301f..ace3237c6b 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include @@ -49,7 +50,14 @@ namespace moveit_setup */ inline std::filesystem::path getSharePath(const std::string& package_name) { - return std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)); + try + { + return std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)); + } + catch (const std::runtime_error& e) + { + return std::filesystem::path(); + } } /** 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 290c9e8a2a..164c7817c9 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp @@ -98,18 +98,15 @@ void URDFConfig::setPackageName() urdf_pkg_name_ = ""; urdf_pkg_relative_path_ = urdf_path_; // just the absolute path } - else + // Check that ROS can find the package + const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_); + + if (robot_desc_pkg_path.empty()) { - // Check that ROS can find the package - const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_); - - if (robot_desc_pkg_path.empty()) - { - 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()); - } + 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()); } } @@ -134,6 +131,11 @@ 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."); diff --git a/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp b/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp index de0c0c3857..3625d94594 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp @@ -53,7 +53,8 @@ bool extractPackageNameFromPath(const std::filesystem::path& path, std::string& } // truncate path step by step and check if it contains a package.xml - while (!sub_path.empty()) + // This runs until the path is either empty "" or at the root "/" or "C:\\" + while (!sub_path.empty() && sub_path != sub_path.root_path()) { if (std::filesystem::is_regular_file(sub_path / "package.xml")) {