fixup! Adapt to API changes in srdfdom

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

Hide file tree

Showing 4 changed files with 14 additions and 9 deletions.
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -156,9 +156,11 @@ void PlanningScene::initialize()
   // load collision defaults
   for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks())
     acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
-  // allow collisions for pairs that have been disabled
-  for (auto const& collision : getRobotModel()->getSRDF()->getCollisionPairs())
-    acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
+  // 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);
 
   setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }

diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
@@ -97,7 +97,11 @@ bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old,
   }
 
   if (!keep_old)
-    config_data.srdf_->collision_pairs_.clear();
+  {
+    config_data.srdf_->no_default_collision_links_.clear();
+    config_data.srdf_->enabled_collision_pairs_.clear();
+    config_data.srdf_->disabled_collision_pairs_.clear();
+  }
 
   return true;
 }

diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
@@ -641,7 +641,7 @@ bool ConfigurationFilesWidget::checkDependencies()
   }
 
   // Check that at least 1 link pair is disabled from collision checking
-  if (config_data_->srdf_->collision_pairs_.empty())
+  if (config_data_->srdf_->disabled_collision_pairs_.empty())
   {
     dependencies << "No self-collisions have been disabled";
   }

diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
@@ -676,7 +676,7 @@ void DefaultCollisionsWidget::checkedFilterChanged()
 void DefaultCollisionsWidget::linkPairsToSRDF()
 {
   // reset the data in the SRDF Writer class
-  config_data_->srdf_->collision_pairs_.clear();
+  config_data_->srdf_->disabled_collision_pairs_.clear();
 
   // Create temp disabled collision
   srdf::Model::CollisionPair dc;
@@ -691,8 +691,7 @@ void DefaultCollisionsWidget::linkPairsToSRDF()
       dc.link1_ = pair_it->first.first;
       dc.link2_ = pair_it->first.second;
       dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason);
-      dc.disabled_ = pair_it->second.disable_check;
-      config_data_->srdf_->collision_pairs_.push_back(dc);
+      config_data_->srdf_->disabled_collision_pairs_.push_back(dc);
     }
   }
 
@@ -719,7 +718,7 @@ void DefaultCollisionsWidget::linkPairsFromSRDF()
   std::pair<std::string, std::string> link_pair;
 
   // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created
-  for (const auto& disabled_collision : config_data_->srdf_->collision_pairs_)
+  for (const auto& disabled_collision : config_data_->srdf_->disabled_collision_pairs_)
   {
     // Set the link names
     link_pair.first = disabled_collision.link1_;