From 9bfb2c73cf70acef7ba9d127698831d3fe14bd04 Mon Sep 17 00:00:00 2001
From: Simon Schmeisser <simon.schmeisser@optonic.com>
Date: Fri, 10 Dec 2021 15:20:49 +0100
Subject: [PATCH] Allow restricting collision pairs to a group (#2987)

---
 .../moveit/planning_scene/planning_scene.h    | 29 +++++++++++--------
 .../planning_scene/src/planning_scene.cpp     |  4 ++-
 2 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
index 7187b47893..fb8c5336ec 100644
--- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
+++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
@@ -560,37 +560,42 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
     getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
   }
 
-  /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */
+  /** \brief Get the names of the links that are involved in collisions for the state \e robot_state.
+   *  Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         const moveit::core::RobotState& robot_state) const
+                         const moveit::core::RobotState& robot_state, const std::string& group_name = "") const
   {
-    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
+    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
   }
 
   /** \brief Get the names of the links that are involved in collisions for the state \e robot_state.
-      Update the link transforms for \e robot_state if needed. */
+      Update the link transforms for \e robot_state if needed.
+      Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         moveit::core::RobotState& robot_state) const
+                         moveit::core::RobotState& robot_state, const std::string& group_name = "") const
   {
     robot_state.updateCollisionBodyTransforms();
-    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
+    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix(),
+                      group_name);
   }
 
   /** \brief  Get the names of the links that are involved in collisions for the state \e robot_state given the
-      allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */
+      allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed.
+      Can be restricted to links part of or updated by \e group_name*/
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         moveit::core::RobotState& robot_state,
-                         const collision_detection::AllowedCollisionMatrix& acm) const
+                         moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm,
+                         const std::string& group_name = "") const
   {
     robot_state.updateCollisionBodyTransforms();
-    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm);
+    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm, group_name);
   }
 
   /** \brief  Get the names of the links that are involved in collisions for the state \e robot_state given the
-      allowed collision matrix (\e acm) */
+      allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                          const moveit::core::RobotState& robot_state,
-                         const collision_detection::AllowedCollisionMatrix& acm) const;
+                         const collision_detection::AllowedCollisionMatrix& acm,
+                         const std::string& group_name = "") const;
 
   /**@}*/
 
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index d59d952fa8..96db06e223 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -539,12 +539,14 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont
 
 void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                                       const moveit::core::RobotState& robot_state,
-                                      const collision_detection::AllowedCollisionMatrix& acm) const
+                                      const collision_detection::AllowedCollisionMatrix& acm,
+                                      const std::string& group_name) const
 {
   collision_detection::CollisionRequest req;
   req.contacts = true;
   req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
   req.max_contacts_per_pair = 1;
+  req.group_name = group_name;
   collision_detection::CollisionResult res;
   checkCollision(req, res, robot_state, acm);
   res.contacts.swap(contacts);