Merge pull request #2660 from MarqRazz/pr-fix_model_with_collision

Fix getLinkModelNamesWithCollisionGeometry to include the base link
moveit · Jan 25, 2024 · 4e58ff1 · 4e58ff1
2 parents 4c024bc + bb6b86f
commit 4e58ff1
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp
@@ -229,6 +229,14 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode
   {
     link_model_map_[link_model->getName()] = link_model;
     link_model_name_vector_.push_back(link_model->getName());
+    // if this is the first link of the group with a valid parent and includes geometry (for example `base_link`) it should included
+    if (link_model_with_geometry_vector_.empty() && link_model->getParentLinkModel() &&
+        !link_model->getParentLinkModel()->getShapes().empty())
+    {
+      link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel());
+      link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName());
+    }
+    // all child links with collision geometry should also be included
     if (!link_model->getShapes().empty())
     {
       link_model_with_geometry_vector_.push_back(link_model);