Sync with MoveIt1

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

@@ -105,6 +105,16 @@ class SRDFConfig : public SetupConfig
     return srdf_.disabled_collision_pairs_;
   }
 
+  std::vector<srdf::Model::CollisionPair>& getEnabledCollisions()
+  {
+    return srdf_.enabled_collision_pairs_;
+  }
+
+  std::vector<std::string>& getNoDefaultCollisionLinks()
+  {
+    return srdf_.no_default_collision_links_;
+  }
+
   std::vector<srdf::Model::EndEffector>& getEndEffectors()
   {
     return srdf_.end_effectors_;

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,9 +37,11 @@
 #pragma once
 
 #include <QAbstractTableModel>
+#include <srdfdom/srdf_writer.h>
 
 #ifndef Q_MOC_RUN
 #include <moveit_setup_srdf_plugins/compute_default_collisions.hpp>
+#include <moveit_setup_srdf_plugins/default_collisions.hpp>
 #endif
 
 #include <QItemSelection>
@@ -51,12 +53,12 @@ class CollisionMatrixModel : public QAbstractTableModel
 {
   Q_OBJECT
 public:
-  CollisionMatrixModel(LinkPairMap& pairs, const std::vector<std::string>& names, QObject* parent = nullptr);
+  CollisionMatrixModel(DefaultCollisions& default_collisions, 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 +70,7 @@ 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);
-  }
-
-private:
-  LinkPairMap& pairs_;
+  DefaultCollisions& default_collisions_;
   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

@@ -56,6 +56,10 @@ namespace srdf_setup
 class DefaultCollisions : public SRDFStep
 {
 public:
+  DefaultCollisions()
+  {
+  }
+
   std::string getName() const override
   {
     return "Self-Collisions";
@@ -84,6 +88,34 @@ class DefaultCollisions : public SRDFStep
     return link_pairs_;
   }
 
+  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;
+  };
+
+  /**
+   * \brief Enable/Disable link collisions by default
+   */
+  bool setDefault(const std::string& name, bool disabled);  // Is this needed?
+
+  bool setDefault(const std::string& link1, const std::string& link2, bool disable);
+
+  static inline const std::string COLLISION_DISABLING_REASON_ENABLED = "Explicitly enabled";
+  static inline const std::string COLLISION_DISABLING_REASON_DISABLED = "Disabled by default";
+  std::string getCollisionDisablingReason(const std::string& link1, const std::string& link2) const;
+
   // For Threaded Operations
   void startGenerationThread(unsigned int num_trials, double min_frac, bool verbose = true);
   void cancelGenerationThread();
@@ -99,6 +131,8 @@ class DefaultCollisions : public SRDFStep
   // For threaded operations
   boost::thread worker_;
   unsigned int progress_;
+
+  bool disabledByDefault(const std::string& link1, const std::string& link2) const;
 };
 }  // namespace srdf_setup
 }  // namespace moveit_setup

moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.hpp

@@ -194,18 +194,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

@@ -35,6 +35,7 @@
 /* Author: Robert Haschke */
 
 #include <moveit_setup_srdf_plugins/collision_matrix_model.hpp>
+#include <moveit_setup_srdf_plugins/default_collisions.hpp>
 #include <boost/assign.hpp>
 #include <QVector>
 #include <QBrush>
@@ -48,16 +49,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 +59,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(DefaultCollisions& default_collisions, const std::vector<std::string>& names,
+                                           QObject* parent)
+  : QAbstractTableModel(parent), default_collisions_(default_collisions), 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 +71,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 +81,89 @@ 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));
+}
+
 QVariant CollisionMatrixModel::data(const QModelIndex& index, int role) const
 {
-  if (index.isValid() && index.row() == index.column() && role == Qt::BackgroundRole)
-    return QApplication::palette().window();
+  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();
+    }
+  }
+
+  int r = visual_to_index_[index.row()], c = visual_to_index_[index.column()];
+  const std::string reason = default_collisions_.getCollisionDisablingReason(std_names_[r], std_names_[c]);
 
   switch (role)
   {
     case Qt::CheckStateRole:
-      return item->second.disable_check ? Qt::Checked : Qt::Unchecked;
+      return (reason.empty() || reason == DefaultCollisions::COLLISION_DISABLING_REASON_ENABLED) ? Qt::Unchecked :
+                                                                                                   Qt::Checked;
     case Qt::ToolTipRole:
-      return LONG_REASONS_TO_STRING.at(item->second.reason);
+      return !reason.empty() ? QString::fromStdString(reason) : QString();
     case Qt::BackgroundRole:
-      return LONG_REASONS_TO_BRUSH.at(item->second.reason);
+      if (reason.empty() || reason == DefaultCollisions::COLLISION_DISABLING_REASON_ENABLED)
+      {
+        return QVariant();
+      }
+      else if (reason == DefaultCollisions::COLLISION_DISABLING_REASON_DISABLED)
+      {
+        return default_collision_brush;
+      }
+      else
+      {
+        return LONG_REASONS_TO_BRUSH.at(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)
+  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;
+    return false;
+  }
 
-    item->second.disable_check = new_value;
-
-    // Handle USER Reasons: 1) pair is disabled by user
-    if (item->second.disable_check && item->second.reason == NOT_DISABLED)
-    {
-      item->second.reason = USER;
-
-      // Handle USER Reasons: 2) pair was disabled by user and now is enabled (not checked)
-    }
-    else if (!item->second.disable_check && item->second.reason == USER)
-    {
-      item->second.reason = NOT_DISABLED;
-    }
+  bool new_value = (value.toInt() == Qt::Checked);
+  bool changed = default_collisions_.setDefault(std_names_[visual_to_index_[index.row()]],
+                                                std_names_[visual_to_index_[index.column()]], new_value);
 
+  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)