Merge moveit/moveit#3093

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp

@@ -275,6 +275,11 @@ class SRDFConfig : public SetupConfig
     return changes_;
   }
 
+  srdf::SRDFWriter& getSRDFWriter()
+  {
+    return srdf_;
+  }
+
 protected:
   void getRelativePath();
   void loadURDFModel();

moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.hpp

@@ -40,10 +40,6 @@
 #include <QSortFilterProxyModel>
 #include <QVector>
 
-#ifndef Q_MOC_RUN
-#include <moveit_setup_srdf_plugins/compute_default_collisions.hpp>
-#endif
-
 #include <moveit_setup_srdf_plugins/collision_matrix_model.hpp>
 
 namespace moveit_setup
@@ -70,7 +66,6 @@ class CollisionLinearModel : public QAbstractProxyModel
   QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override;
   QModelIndex parent(const QModelIndex& child) const override;
   QVariant data(const QModelIndex& index, int role) const override;
-  DisabledReason reason(int row) const;
 
   bool setData(const QModelIndex& index, const QVariant& value, int role) override;
   void setEnabled(const QItemSelection& selection, bool value);

moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <QAbstractTableModel>
+#include <srdfdom/srdf_writer.h>
 
 #ifndef Q_MOC_RUN
 #include <moveit_setup_srdf_plugins/compute_default_collisions.hpp>
@@ -51,12 +52,11 @@ class CollisionMatrixModel : public QAbstractTableModel
 {
   Q_OBJECT
 public:
-  CollisionMatrixModel(LinkPairMap& pairs, const std::vector<std::string>& names, QObject* parent = nullptr);
+  CollisionMatrixModel(const srdf::SRDFWriter& srdf, const std::vector<std::string>& names, QObject* parent = nullptr);
   int rowCount(const QModelIndex& parent = QModelIndex()) const override;
   int columnCount(const QModelIndex& parent = QModelIndex()) const override;
   QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override;
   QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
-  DisabledReason reason(const QModelIndex& index) const;
 
   // for editing
   Qt::ItemFlags flags(const QModelIndex& index) const override;
@@ -68,14 +68,10 @@ public Q_SLOTS:
   void setFilterRegExp(const QString& filter);
 
 private:
-  LinkPairMap::iterator item(const QModelIndex& index);
-  LinkPairMap::const_iterator item(const QModelIndex& index) const
-  {
-    return const_cast<CollisionMatrixModel*>(this)->item(index);
-  }
+  bool disabledByDefault(const std::string& link1, const std::string& link2) const;
 
 private:
-  LinkPairMap& pairs_;
+  srdf::SRDFWriter srdf_;
   const std::vector<std::string> std_names_;  // names of links
   QList<QString> q_names_;                    // names of links
   QList<int> visual_to_index_;                // map from visual index to actual index

moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp

@@ -84,6 +84,11 @@ class DefaultCollisions : public SRDFStep
     return link_pairs_;
   }
 
+  srdf::SRDFWriter& getSRDFWriter()
+  {
+    return srdf_config_->getSRDFWriter();
+  }
+
   // For Threaded Operations
   void startGenerationThread(unsigned int num_trials, double min_frac, bool verbose = true);
   void cancelGenerationThread();

moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.hpp

@@ -183,6 +183,8 @@ private Q_SLOTS:
   MonitorThread* worker_;
 
   DefaultCollisions setup_step_;
+  /// Working copy of SRDF config
+  srdf::SRDFWriterPtr wip_srdf_;
 
   // ******************************************************************************************
   // Private Functions
@@ -194,18 +196,19 @@ private Q_SLOTS:
    */
   void disableControls(bool disable);
 
-  /**
-   * \brief Allow toggling of all checkboxes in selection by filtering <space> keypresses
-   */
+  /** Allow toggling of all checkboxes in selection by filtering <space> keypresses */
   bool eventFilter(QObject* object, QEvent* event) override;
 
-  /**
-   * \brief Show header's sections in logicalIndexes and everything in between
-   */
+  /** Return list of selected sections */
+  QList<int> selectedSections(QHeaderView*& header) const;
+
+  /** Show header's sections in logicalIndexes and everything in between */
   void showSections(QHeaderView* header, const QList<int>& logicalIndexes);
-  /**
-   * \brief Toggle enabled status of selection
-   */
+
+  /** Enable/Disable selected sections by default */
+  void setDefaults(bool enabled);
+
+  /** Toggle enabled status of selection */
   void toggleSelection(QItemSelection selection);
 };
 

moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp

@@ -140,22 +140,17 @@ QVariant CollisionLinearModel::data(const QModelIndex& index, int role) const
   return QVariant();
 }
 
-DisabledReason CollisionLinearModel::reason(int row) const
-{
-  QModelIndex src_index = mapToSource(index(row, 0));
-  return qobject_cast<CollisionMatrixModel*>(sourceModel())->reason(src_index);
-}
-
 bool CollisionLinearModel::setData(const QModelIndex& index, const QVariant& value, int role)
 {
-  QModelIndex src_index = mapToSource(index);
-
   if (role == Qt::CheckStateRole)
   {
-    sourceModel()->setData(src_index, value, role);
-    int r = index.row();
-    Q_EMIT dataChanged(this->index(r, 2), this->index(r, 3));  // reason changed too
-    return true;
+    QModelIndex src_index = this->mapToSource(index);
+    if (sourceModel()->setData(src_index, value, role))
+    {
+      int r = index.row();
+      Q_EMIT dataChanged(this->index(r, 2), this->index(r, 3));  // reason changed too
+      return true;
+    }
   }
   return false;  // reject all other changes
 }
@@ -260,8 +255,7 @@ void SortFilterProxyModel::setShowAll(bool show_all)
 bool SortFilterProxyModel::filterAcceptsRow(int source_row, const QModelIndex& source_parent) const
 {
   CollisionLinearModel* m = qobject_cast<CollisionLinearModel*>(sourceModel());
-  if (!(show_all_ || m->reason(source_row) <= ALWAYS ||
-        m->data(m->index(source_row, 2), Qt::CheckStateRole) == Qt::Checked))
+  if (!(show_all_ || m->data(m->index(source_row, 2), Qt::CheckStateRole) == Qt::Checked))
     return false;  // not accepted due to check state
 
   const QRegExp regexp = filterRegExp();

moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_matrix_model.cpp

@@ -48,16 +48,6 @@ namespace moveit_setup
 {
 namespace srdf_setup
 {
-/// Mapping of reasons for disabling a link pair to strings
-static const std::unordered_map<DisabledReason, const char*> LONG_REASONS_TO_STRING =
-    boost::assign::map_list_of  // clang-format off
-    ( NEVER, "Never in Collision" )
-    ( DEFAULT, "Collision by Default" )
-    ( ADJACENT, "Adjacent Links" )
-    ( ALWAYS, "Always in Collision" )
-    ( USER, "User Disabled" )
-    ( NOT_DISABLED, "");  // clang-format on
-
 /// Mapping of reasons to a background color
 static const std::unordered_map<DisabledReason, QVariant> LONG_REASONS_TO_BRUSH =
     boost::assign::map_list_of  // clang-format off
@@ -68,8 +58,9 @@ static const std::unordered_map<DisabledReason, QVariant> LONG_REASONS_TO_BRUSH
     ( USER, QBrush(QColor("yellow")) )
     ( NOT_DISABLED, QBrush());  // clang-format on
 
-CollisionMatrixModel::CollisionMatrixModel(LinkPairMap& pairs, const std::vector<std::string>& names, QObject* parent)
-  : QAbstractTableModel(parent), pairs_(pairs), std_names_(names)
+CollisionMatrixModel::CollisionMatrixModel(const srdf::SRDFWriter& srdf, const std::vector<std::string>& names,
+                                           QObject* parent)
+  : QAbstractTableModel(parent), srdf_(srdf), std_names_(names)
 {
   int idx = 0;
   for (std::vector<std::string>::const_iterator it = names.begin(), end = names.end(); it != end; ++it, ++idx)
@@ -79,20 +70,6 @@ CollisionMatrixModel::CollisionMatrixModel(LinkPairMap& pairs, const std::vector
   }
 }
 
-// return item in pairs map given a normalized index, use item(normalized(index))
-LinkPairMap::iterator CollisionMatrixModel::item(const QModelIndex& index)
-{
-  int r = visual_to_index_[index.row()], c = visual_to_index_[index.column()];
-  if (r == c)
-    return pairs_.end();
-
-  // setLinkPair() actually inserts the pair (A,B) where A < B
-  if (std_names_[r] >= std_names_[c])
-    std::swap(r, c);
-
-  return pairs_.find(std::make_pair(std_names_[r], std_names_[c]));
-}
-
 int CollisionMatrixModel::rowCount(const QModelIndex& /*parent*/) const
 {
   return visual_to_index_.size();
@@ -103,67 +80,143 @@ int CollisionMatrixModel::columnCount(const QModelIndex& /*parent*/) const
   return visual_to_index_.size();
 }
 
+struct PairMatcher
+{
+  PairMatcher(const std::string& link1, const std::string& link2)
+    : search(link1 < link2 ? std::make_pair(std::cref(link1), std::cref(link2)) :
+                             std::make_pair(std::cref(link2), std::cref(link1)))
+  {
+  }
+
+  bool operator()(const srdf::Model::CollisionPair& pair) const
+  {
+    return (pair.link1_ == search.first && pair.link2_ == search.second) ||
+           (pair.link2_ == search.first && pair.link1_ == search.second);
+  }
+
+  std::pair<const std::string&, const std::string&> search;
+};
+
+template <typename Container>
+auto find(Container& pairs, const std::string& link1, const std::string& link2)
+{
+  return std::find_if(pairs.begin(), pairs.end(), PairMatcher(link1, link2));
+}
+
+bool CollisionMatrixModel::disabledByDefault(const std::string& link1, const std::string& link2) const
+{
+  for (const auto& name : srdf_.no_default_collision_links_)
+    if (name == link1 || name == link2)
+      return true;
+  return false;
+}
+
 QVariant CollisionMatrixModel::data(const QModelIndex& index, int role) const
 {
-  if (index.isValid() && index.row() == index.column() && role == Qt::BackgroundRole)
-    return QApplication::palette().window();
+  static std::string enabled = "Explicitly enabled";
+  static std::string disabled = "Disabled by default";
+  static QBrush default_collision_brush(QColor("lightpink").darker(110));
 
-  LinkPairMap::const_iterator item = this->item(index);
-  if (item == pairs_.end())
-    return QVariant();
+  if (index.isValid() && index.row() == index.column())
+  {
+    switch (role)
+    {
+      case Qt::BackgroundRole:
+        return QApplication::palette().window();
+      default:
+        return QVariant();
+    }
+  }
+
+  const std::string* reason = nullptr;
+  int r = visual_to_index_[index.row()], c = visual_to_index_[index.column()];
+  auto it = find(srdf_.disabled_collision_pairs_, std_names_[r], std_names_[c]);
+  if (it != srdf_.disabled_collision_pairs_.end())
+    reason = &it->reason_;
+  else if (find(srdf_.enabled_collision_pairs_, std_names_[r], std_names_[c]) != srdf_.enabled_collision_pairs_.end())
+    reason = &enabled;
+  else if (disabledByDefault(std_names_[r], std_names_[c]))
+    reason = &disabled;
 
   switch (role)
   {
     case Qt::CheckStateRole:
-      return item->second.disable_check ? Qt::Checked : Qt::Unchecked;
+      return (!reason || reason == &enabled) ? Qt::Unchecked : Qt::Checked;
     case Qt::ToolTipRole:
-      return LONG_REASONS_TO_STRING.at(item->second.reason);
+      return reason ? QString::fromStdString(*reason) : QString();
     case Qt::BackgroundRole:
-      return LONG_REASONS_TO_BRUSH.at(item->second.reason);
+      if (!reason || reason == &enabled)
+        return QVariant();
+      else if (reason == &disabled)
+        return default_collision_brush;
+      else
+        return LONG_REASONS_TO_BRUSH.at(moveit_setup::srdf_setup::disabledReasonFromString(*reason));
   }
   return QVariant();
 }
 
-DisabledReason CollisionMatrixModel::reason(const QModelIndex& index) const
-{
-  LinkPairMap::const_iterator item = this->item(index);
-  if (item == pairs_.end())
-    return NOT_DISABLED;
-  return item->second.reason;
-}
-
 bool CollisionMatrixModel::setData(const QModelIndex& index, const QVariant& value, int role)
 {
-  if (role == Qt::CheckStateRole)
-  {
-    LinkPairMap::iterator item = this->item(index);
-    if (item == pairs_.end())
-      return false;
-
-    bool new_value = (value.toInt() == Qt::Checked);
-    if (item->second.disable_check == new_value)
-      return true;
+  if (role != Qt::CheckStateRole)
+    return false;
 
-    item->second.disable_check = new_value;
+  bool new_value = (value.toInt() == Qt::Checked);
+  srdf::Model::CollisionPair p{ std_names_[visual_to_index_[index.row()]], std_names_[visual_to_index_[index.column()]],
+                                std::string() };
+  if (p.link1_ > p.link2_)
+    std::swap(p.link1_, p.link2_);
 
-    // Handle USER Reasons: 1) pair is disabled by user
-    if (item->second.disable_check && item->second.reason == NOT_DISABLED)
+  auto enabled = find(srdf_.enabled_collision_pairs_, p.link1_, p.link2_);
+  auto disabled = find(srdf_.disabled_collision_pairs_, p.link1_, p.link2_);
+  bool changed = true;
+  if (disabledByDefault(p.link1_, p.link2_))
+  {
+    assert(disabled == srdf_.disabled_collision_pairs_.end());
+    auto& pairs = srdf_.enabled_collision_pairs_;
+    if (new_value)
     {
-      item->second.reason = USER;
-
-      // Handle USER Reasons: 2) pair was disabled by user and now is enabled (not checked)
+      if (enabled != pairs.end())  // delete all matching pairs, starting with enabled
+        pairs.erase(std::remove_if(enabled, pairs.end(), PairMatcher(p.link1_, p.link2_)), pairs.end());
+      else
+        changed = false;
     }
-    else if (!item->second.disable_check && item->second.reason == USER)
+    else
     {
-      item->second.reason = NOT_DISABLED;
+      p.reason_ = moveit_setup::srdf_setup::disabledReasonToString(moveit_setup::srdf_setup::NOT_DISABLED);
+      if (enabled == pairs.end())
+        srdf_.enabled_collision_pairs_.push_back(p);
+      else
+        changed = false;
     }
+  }
+  else
+  {
+    assert(enabled == srdf_.enabled_collision_pairs_.end());
+    auto& pairs = srdf_.disabled_collision_pairs_;
+    if (new_value)
+    {
+      p.reason_ = moveit_setup::srdf_setup::disabledReasonToString(moveit_setup::srdf_setup::USER);
+      if (disabled == pairs.end())
+        pairs.push_back(p);
+      else
+        changed = false;
+    }
+    else
+    {
+      if (disabled != pairs.end())  // delete all matching pairs, starting with disabled
+        pairs.erase(std::remove_if(disabled, pairs.end(), PairMatcher(p.link1_, p.link2_)), pairs.end());
+      else
+        changed = false;
+    }
+  }
 
+  if (changed)
+  {
     QModelIndex mirror = this->index(index.column(), index.row());
     Q_EMIT dataChanged(index, index);
     Q_EMIT dataChanged(mirror, mirror);
-    return true;
   }
-  return false;  // reject all other changes
+  return changed;
 }
 
 void CollisionMatrixModel::setEnabled(const QItemSelection& selection, bool value)

moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp

@@ -165,6 +165,9 @@ void DefaultCollisions::generateCollisionTable(unsigned int num_trials, double m
   // Find the default collision matrix - all links that are allowed to collide
   link_pairs_ = computeDefaultCollisions(srdf_config_->getPlanningScene(), &progress_, include_never_colliding,
                                          num_trials, min_frac, verbose);
+  // linkPairsToSRDF(link_pairs, *wip_srdf_);
+  // Update collision_matrix for robot pose's use
+  // config_data_->loadAllowedCollisionMatrix(*wip_srdf_);
 
   // End the progress bar loop
   progress_ = 100;