From 2a6641392208f1ac28b4420d6c1323e4713c6c56 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Dec 2021 12:07:30 +0100 Subject: [PATCH] Move MoveItConfigData::setCollisionLinkPairs to collisions_updater.cpp This method is only used there to update disabled collision entries. --- .../tools/moveit_config_data.h | 7 --- .../src/collisions_updater.cpp | 48 ++++++++++++++++- .../src/tools/moveit_config_data.cpp | 53 ------------------- 3 files changed, 47 insertions(+), 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 index 7ce6f0caae..050ec293bf 100644 --- 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 index 48e2418cf7..3e6f36ad52 100644 --- 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& target_pairs, + const moveit_setup_assistant::LinkPairMap& source_pairs, size_t skip_mask) +{ + // remove duplicates + std::set 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 index a9f20cf945..77208dc5bb 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1203,59 +1203,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 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, 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 // ******************************************************************************************