Unify initialization of ACM from SRDF

ubi-agni · Dec 28, 2021 · f9d3eba · f9d3eba
1 parent fd5b6de
commit f9d3eba
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 32 deletions.
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -88,6 +88,9 @@ class AllowedCollisionMatrix
    *  will be ignored. */
   AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed = false);
 
+  /** @brief Construct from an SRDF representation */
+  AllowedCollisionMatrix(const srdf::Model& srdf);
+
   /** @brief Construct the structure from a message representation */
   AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg);
 

diff --git a/...core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/...core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
@@ -78,14 +78,7 @@ class CollisionDetectorPandaTest : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-
-    // load collision defaults
-    for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
-      acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER);
-    // allow collisions for pairs that have been disabled
-    for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs())
-      acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
 
     cenv_ = value_->allocateEnv(robot_model_);
 

diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -51,6 +51,19 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& n
       setEntry(names[i], names[j], allowed);
 }
 
+AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf)
+{
+  // load collision defaults
+  for (const std::string& name : srdf.getNoDefaultCollisionLinks())
+    setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
+  // re-enable specific collision pairs
+  for (auto const& collision : srdf.getEnabledCollisionPairs())
+    setEntry(collision.link1_, collision.link2_, false);
+  // *finally* disable selected collision pairs
+  for (auto const& collision : srdf.getDisabledCollisionPairs())
+    setEntry(collision.link1_, collision.link2_, true);
+}
+
 AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg)
 {
   if (msg.entry_names.size() != msg.entry_values.size() ||

diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
@@ -76,14 +76,7 @@ class BulletCollisionDetectionTester : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-
-    // load collision defaults
-    for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
-      acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER);
-    // allow collisions for pairs that have been disabled
-    for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs())
-      acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
 
     cenv_ = std::make_shared<collision_detection::CollisionEnvBullet>(robot_model_);
 

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -151,16 +151,7 @@ void PlanningScene::initialize()
   robot_state_->setToDefaultValues();
   robot_state_->update();
 
-  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-
-  // load collision defaults
-  for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks())
-    acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
-  // load collision pairs
-  for (auto const& collision : getRobotModel()->getSRDF()->getEnabledCollisionPairs())
-    acm_->setEntry(collision.link1_, collision.link2_, false);
-  for (auto const& collision : getRobotModel()->getSRDF()->getDisabledCollisionPairs())
-    acm_->setEntry(collision.link1_, collision.link2_, true);
+  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*getRobotModel()->getSRDF());
 
   setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }

diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -140,14 +140,17 @@ planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene()
 // ******************************************************************************************
 void MoveItConfigData::loadAllowedCollisionMatrix()
 {
-  // Clear the allowed collision matrix
   allowed_collision_matrix_.clear();
 
-  // Update the allowed collision matrix, in case there has been a change
-  for (const auto& collision : srdf_->collision_pairs_)
-  {
-    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, collision.disabled_);
-  }
+  // load collision defaults
+  for (const std::string& name : srdf_->no_default_collision_links_)
+    allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
+  // re-enable specific collision pairs
+  for (auto const& collision : srdf_->enabled_collision_pairs_)
+    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false);
+  // *finally* disable selected collision pairs
+  for (auto const& collision : srdf_->disabled_collision_pairs_)
+    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true);
 }
 
 // ******************************************************************************************