Skip to content

Fix collisions_updater CLI if no package is used (backport #2344) #2360

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
henningkayser merged 1 commit into from
Oct 24, 2023
Merged

Fix collisions_updater CLI if no package is used (backport #2344) #2360

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 27 additions & 19 deletions moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand All @@ -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.");
Expand Down