Move MoveItConfigData::setCollisionLinkPairs to collisions_updater.cpp

This method is only used there to update disabled collision entries.
ubi-agni · Dec 28, 2021 · d89af78 · d89af78
1 parent f9d3eba
commit d89af78
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 61 deletions.
diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
@@ -330,13 +330,6 @@ class MoveItConfigData
    */
   std::string getGazeboCompatibleURDF();
 
-  /**
-   * \brief Set list of collision link pairs in SRDF; sorted; with optional filter
-   * \param link_pairs list of collision link pairs
-   * \param skip_mask mask of shifted moveit_setup_assistant::DisabledReason values that will be skipped
-   */
-  void setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask = 0);
-
   /**
    * \brief Decide the best two joints to be used for the projection evaluator
    * \param planning_group name of group to use

diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
@@ -115,6 +115,52 @@ moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfig
                                                           trials > 0, trials, min_collision_fraction, verbose);
 }
 
+// less operation for two CollisionPairs
+struct CollisionPairLess
+{
+  bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const
+  {
+    return left.link1_ < right.link1_ && left.link2_ < right.link2_;
+  }
+};
+
+// Update collision pairs
+void updateCollisionLinkPairs(std::vector<srdf::Model::CollisionPair>& target_pairs,
+                              const moveit_setup_assistant::LinkPairMap& source_pairs, size_t skip_mask)
+{
+  // remove duplicates
+  std::set<srdf::Model::CollisionPair, CollisionPairLess> filtered;
+  for (auto& p : target_pairs)
+  {
+    if (p.link1_ >= p.link2_)
+      std::swap(p.link1_, p.link2_);  // unify link1, link2 sorting
+    filtered.insert(p);
+  }
+
+  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
+  for (const auto& link_pair : source_pairs)
+  {
+    // Only copy those that are actually disabled
+    if (!link_pair.second.disable_check)
+      continue;
+
+    // Filter out pairs matching the skip_mask
+    if ((1 << link_pair.second.reason) & skip_mask)
+      continue;
+
+    srdf::Model::CollisionPair pair;
+    pair.link1_ = link_pair.first.first;
+    pair.link2_ = link_pair.first.second;
+    if (pair.link1_ >= pair.link2_)
+      std::swap(pair.link1_, pair.link2_);
+    pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason);
+
+    filtered.insert(pair);
+  }
+
+  target_pairs.assign(filtered.begin(), filtered.end());
+}
+
 int main(int argc, char* argv[])
 {
   std::string config_pkg_path;
@@ -205,7 +251,7 @@ int main(int argc, char* argv[])
   if (!include_always)
     skip_mask |= (1 << moveit_setup_assistant::ALWAYS);
 
-  config_data.setCollisionLinkPairs(link_pairs, skip_mask);
+  updateCollisionLinkPairs(config_data.srdf_->disabled_collision_pairs_, link_pairs, skip_mask);
 
   config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path);
 

diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -1198,59 +1198,6 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path)
   return true;  // file created successfully
 }
 
-// ******************************************************************************************
-// Set list of collision link pairs in SRDF; sorted; with optional filter
-// ******************************************************************************************
-
-class SortableCollisionPair
-{
-public:
-  SortableCollisionPair(const srdf::Model::CollisionPair& p)
-    : pair_(p), key_(p.link1_ < p.link2_ ? (p.link1_ + "|" + p.link2_) : (p.link2_ + "|" + p.link1_))
-  {
-  }
-  operator const srdf::Model::CollisionPair &() const
-  {
-    return pair_;
-  }
-  bool operator<(const SortableCollisionPair& other) const
-  {
-    return key_ < other.key_;
-  }
-
-private:
-  const srdf::Model::CollisionPair pair_;
-  const std::string key_;
-};
-
-void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask)
-{
-  // Create temp disabled collision
-  srdf::Model::CollisionPair pair;
-
-  std::set<SortableCollisionPair> collision_pairs;
-  collision_pairs.insert(srdf_->collision_pairs_.begin(), srdf_->collision_pairs_.end());
-
-  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
-  for (const std::pair<const std::pair<std::string, std::string>, LinkPairData>& link_pair : link_pairs)
-  {
-    // Only copy those that are actually disabled
-    if (link_pair.second.disable_check)
-    {
-      if ((1 << link_pair.second.reason) & skip_mask)
-        continue;
-
-      pair.link1_ = link_pair.first.first;
-      pair.link2_ = link_pair.first.second;
-      pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason);
-
-      collision_pairs.insert(SortableCollisionPair(pair));
-    }
-  }
-
-  srdf_->collision_pairs_.assign(collision_pairs.begin(), collision_pairs.end());
-}
-
 // ******************************************************************************************
 // Decide the best two joints to be used for the projection evaluator
 // ******************************************************************************************