From 4e0ffb4242e0a8878c2c119cfc7a756fcbb95034 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Thu, 4 May 2023 09:42:22 +1200 Subject: [PATCH 1/4] Retrieve link collision geometry Signed-off-by: Emiliano Borghi --- .../urdf_geometry_parser.h | 9 +++++++++ src/urdf_geometry_parser.cpp | 18 ++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/include/urdf_geometry_parser/urdf_geometry_parser.h b/include/urdf_geometry_parser/urdf_geometry_parser.h index f4abb45..367ef17 100644 --- a/include/urdf_geometry_parser/urdf_geometry_parser.h +++ b/include/urdf_geometry_parser/urdf_geometry_parser.h @@ -78,6 +78,15 @@ class UrdfGeometryParser { bool getJointRadius(const std::string& joint_name, double& radius); + /** + * \brief Get link geometry from the URDF + * \param link_name Name of the link + * \param geometry Geometry of the link + * \return true if the geometry was found; false otherwise + */ + bool getLinkCollisionGeometry(const std::string& link_name, + urdf::GeometrySharedPtr& geometry); + /** * \brief Get joint steering limit from the URDF * considering the upper and lower limit is the same diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp index c375673..575ce68 100644 --- a/src/urdf_geometry_parser.cpp +++ b/src/urdf_geometry_parser.cpp @@ -178,6 +178,24 @@ namespace urdf_geometry_parser{ return false; } + bool UrdfGeometryParser::getLinkCollisionGeometry(const std::string& link_name, + urdf::GeometrySharedPtr& geometry) + { + if(model_) + { + urdf::LinkConstSharedPtr link(model_->getLink(link_name)); + if (!link) + { + ROS_ERROR_STREAM(link_name << " couldn't be retrieved from model description"); + return false; + } + geometry = link->collision->geometry; + return true; + } + else + return false; + } + bool UrdfGeometryParser::getJointSteeringLimits(const std::string& joint_name, double& steering_limit) { From a4ce5dcf31e01b32e9351eb78d6a98bf35257de8 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Tue, 1 Oct 2024 09:09:35 +1300 Subject: [PATCH 2/4] More robust geometry check Signed-off-by: Emiliano Borghi --- src/urdf_geometry_parser.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp index 575ce68..59172b3 100644 --- a/src/urdf_geometry_parser.cpp +++ b/src/urdf_geometry_parser.cpp @@ -189,6 +189,16 @@ namespace urdf_geometry_parser{ ROS_ERROR_STREAM(link_name << " couldn't be retrieved from model description"); return false; } + if (!link->collision) + { + ROS_ERROR_STREAM("Link " << link_name << " does not have collision description. Add collision description for link to urdf."); + return false; + } + if (!link->collision->geometry) + { + ROS_ERROR_STREAM("Link " << link_name << " does not have collision geometry description. Add collision geometry description for link to urdf."); + return false; + } geometry = link->collision->geometry; return true; } From a2723faf3c4e27200ea9e4e8fed59c6c865f8db0 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Fri, 4 Oct 2024 08:51:23 +1300 Subject: [PATCH 3/4] Add visual parser Signed-off-by: Emiliano Borghi --- .../urdf_geometry_parser.h | 11 ++++++- src/urdf_geometry_parser.cpp | 32 +++++++++++++++++-- 2 files changed, 40 insertions(+), 3 deletions(-) diff --git a/include/urdf_geometry_parser/urdf_geometry_parser.h b/include/urdf_geometry_parser/urdf_geometry_parser.h index 367ef17..5291597 100644 --- a/include/urdf_geometry_parser/urdf_geometry_parser.h +++ b/include/urdf_geometry_parser/urdf_geometry_parser.h @@ -79,7 +79,7 @@ class UrdfGeometryParser { double& radius); /** - * \brief Get link geometry from the URDF + * \brief Get link collision geometry from the URDF * \param link_name Name of the link * \param geometry Geometry of the link * \return true if the geometry was found; false otherwise @@ -87,6 +87,15 @@ class UrdfGeometryParser { bool getLinkCollisionGeometry(const std::string& link_name, urdf::GeometrySharedPtr& geometry); + /** + * \brief Get link visual geometry from the URDF + * \param link_name Name of the link + * \param geometry Geometry of the link + * \return true if the geometry was found; false otherwise + */ + bool getLinkVisualGeometry(const std::string& link_name, + urdf::GeometrySharedPtr& geometry); + /** * \brief Get joint steering limit from the URDF * considering the upper and lower limit is the same diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp index 59172b3..b62f5d7 100644 --- a/src/urdf_geometry_parser.cpp +++ b/src/urdf_geometry_parser.cpp @@ -191,12 +191,12 @@ namespace urdf_geometry_parser{ } if (!link->collision) { - ROS_ERROR_STREAM("Link " << link_name << " does not have collision description. Add collision description for link to urdf."); + ROS_ERROR_STREAM("Link " << link_name << " does not have collision description"); return false; } if (!link->collision->geometry) { - ROS_ERROR_STREAM("Link " << link_name << " does not have collision geometry description. Add collision geometry description for link to urdf."); + ROS_ERROR_STREAM("Link " << link_name << " does not have collision geometry description"); return false; } geometry = link->collision->geometry; @@ -206,6 +206,34 @@ namespace urdf_geometry_parser{ return false; } + bool UrdfGeometryParser::getLinkVisualGeometry(const std::string& link_name, + urdf::GeometrySharedPtr& geometry) + { + if(model_) + { + urdf::LinkConstSharedPtr link(model_->getLink(link_name)); + if (!link) + { + ROS_ERROR_STREAM(link_name << " couldn't be retrieved from model description"); + return false; + } + if (!link->visual) + { + ROS_ERROR_STREAM("Link " << link_name << " does not have visual description"); + return false; + } + if (!link->visual->geometry) + { + ROS_ERROR_STREAM("Link " << link_name << " does not have visual geometry description"); + return false; + } + geometry = link->visual->geometry; + return true; + } + else + return false; + } + bool UrdfGeometryParser::getJointSteeringLimits(const std::string& joint_name, double& steering_limit) { From 78eef9b57c79899540925ecb93909aee46bfb217 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Tue, 26 Nov 2024 15:21:30 +1300 Subject: [PATCH 4/4] Treat errors as messages Signed-off-by: Emiliano Borghi --- src/urdf_geometry_parser.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp index b62f5d7..e5bd9b0 100644 --- a/src/urdf_geometry_parser.cpp +++ b/src/urdf_geometry_parser.cpp @@ -53,19 +53,19 @@ namespace urdf_geometry_parser{ if (!link->collision) { - ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf."); + ROS_WARN_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf."); return false; } if (!link->collision->geometry) { - ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf."); + ROS_WARN_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf."); return false; } if (link->collision->geometry->type != urdf::Geometry::CYLINDER) { - ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry"); + ROS_WARN_STREAM("Link " << link->name << " does not have cylinder geometry"); return false; } @@ -82,7 +82,7 @@ namespace urdf_geometry_parser{ { if (!isCylinder(wheel_link)) { - ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!"); + ROS_WARN_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!"); return false; } @@ -100,13 +100,13 @@ namespace urdf_geometry_parser{ std::string robot_model_str=""; if (!res || !root_nh.getParam(model_param_name,robot_model_str)) { - ROS_ERROR("Robot descripion couldn't be retrieved from param server."); + ROS_ERROR("Robot description couldn't be retrieved from param server."); } else { model_ = urdf::parseURDF(robot_model_str); if(!model_) - ROS_ERROR_STREAM("Could not parse the urdf robot model "<collision) { - ROS_ERROR_STREAM("Link " << link_name << " does not have collision description"); + ROS_WARN_STREAM("Link " << link_name << " does not have collision description"); return false; } if (!link->collision->geometry) { - ROS_ERROR_STREAM("Link " << link_name << " does not have collision geometry description"); + ROS_WARN_STREAM("Link " << link_name << " does not have collision geometry description"); return false; } geometry = link->collision->geometry; @@ -219,12 +219,12 @@ namespace urdf_geometry_parser{ } if (!link->visual) { - ROS_ERROR_STREAM("Link " << link_name << " does not have visual description"); + ROS_WARN_STREAM("Link " << link_name << " does not have visual description"); return false; } if (!link->visual->geometry) { - ROS_ERROR_STREAM("Link " << link_name << " does not have visual geometry description"); + ROS_WARN_STREAM("Link " << link_name << " does not have visual geometry description"); return false; } geometry = link->visual->geometry;