Use separate vectors for disabled/(re)enabled collision pairs

include/srdfdom/model.h

@@ -186,8 +186,6 @@ class Model
 
     /// The reason why the collision check was disabled/enabled
     std::string reason_;
-
-    bool disabled_;
   };
 
   // Some joints can be passive (not actuated). This structure specifies information about such joints
@@ -209,10 +207,16 @@ class Model
     return no_default_collision_links_;
   }
 
-  /// Get the list of pairs of links for which we explicitly disable/enable collision
-  const std::vector<CollisionPair>& getCollisionPairs() const
+  /// Get the list of pairs of links for which we explicitly re-enable collision (after having disabled it via a default)
+  const std::vector<CollisionPair>& getEnabledCollisionPairs() const
+  {
+    return enabled_collision_pairs_;
+  }
+
+  /// Get the list of pairs of links for which we explicitly disable collision
+  const std::vector<CollisionPair>& getDisabledCollisionPairs() const
   {
-    return collision_pairs_;
+    return disabled_collision_pairs_;
   }
 
   /// Get the list of groups defined for this model
@@ -272,7 +276,8 @@ class Model
   std::vector<EndEffector> end_effectors_;
   std::vector<LinkSpheres> link_sphere_approximations_;
   std::vector<std::string> no_default_collision_links_;
-  std::vector<CollisionPair> collision_pairs_;
+  std::vector<CollisionPair> disabled_collision_pairs_;
+  std::vector<CollisionPair> enabled_collision_pairs_;
   std::vector<PassiveJoint> passive_joints_;
 };
 typedef std::shared_ptr<Model> ModelSharedPtr;

include/srdfdom/srdf_writer.h

@@ -173,24 +173,26 @@ class SRDFWriter
    *
    * @param root  - TinyXML root element to attach sub elements to
    */
-  void createCollisionPairsXML(tinyxml2::XMLElement* root, const char* tag_name, bool disabled);
+  void createCollisionPairsXML(tinyxml2::XMLElement* root, const char* tag_name,
+                               const std::vector<Model::CollisionPair>& pairs);
 
 public:
   // ******************************************************************************************
   // Group Datastructures
   // ******************************************************************************************
 
-  std::vector<srdf::Model::Group> groups_;
-  std::vector<srdf::Model::GroupState> group_states_;
-  std::vector<srdf::Model::VirtualJoint> virtual_joints_;
-  std::vector<srdf::Model::EndEffector> end_effectors_;
-  std::vector<srdf::Model::LinkSpheres> link_sphere_approximations_;
+  std::vector<Model::Group> groups_;
+  std::vector<Model::GroupState> group_states_;
+  std::vector<Model::VirtualJoint> virtual_joints_;
+  std::vector<Model::EndEffector> end_effectors_;
+  std::vector<Model::LinkSpheres> link_sphere_approximations_;
   std::vector<std::string> no_default_collision_links_;
-  std::vector<srdf::Model::CollisionPair> collision_pairs_;
-  std::vector<srdf::Model::PassiveJoint> passive_joints_;
+  std::vector<Model::CollisionPair> disabled_collision_pairs_;
+  std::vector<Model::CollisionPair> enabled_collision_pairs_;
+  std::vector<Model::PassiveJoint> passive_joints_;
 
   // Store the SRDF Model for updating the kinematic_model
-  srdf::ModelSharedPtr srdf_model_;
+  ModelSharedPtr srdf_model_;
 
   // Robot name
   std::string robot_name_;

src/model.cpp

@@ -569,7 +569,7 @@ void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XML
     const char* reason = c_xml->Attribute("reason");
 
     CollisionPair pair{ boost::trim_copy(std::string(link1)), boost::trim_copy(std::string(link2)),
-                        reason ? reason : "", disabled };
+                        reason ? reason : "" };
     if (!urdf_model.getLink(pair.link1_))
     {
       CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link1);
@@ -580,7 +580,8 @@ void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XML
       CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link2);
       continue;
     }
-    collision_pairs_.push_back(pair);
+    auto& target_pairs = disabled ? disabled_collision_pairs_ : enabled_collision_pairs_;
+    target_pairs.push_back(pair);
   }
 }
 
@@ -696,6 +697,7 @@ void srdf::Model::clear()
   end_effectors_.clear();
   link_sphere_approximations_.clear();
   no_default_collision_links_.clear();
-  collision_pairs_.clear();
+  disabled_collision_pairs_.clear();
+  enabled_collision_pairs_.clear();
   passive_joints_.clear();
 }

src/srdf_writer.cpp

@@ -99,7 +99,8 @@ void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf::
 
   // Copy all read-only data from srdf model to this object
   no_default_collision_links_ = srdf_model_->getNoDefaultCollisionLinks();
-  collision_pairs_ = srdf_model_->getCollisionPairs();
+  disabled_collision_pairs_ = srdf_model_->getDisabledCollisionPairs();
+  enabled_collision_pairs_ = srdf_model_->getEnabledCollisionPairs();
   link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
   groups_ = srdf_model_->getGroups();
   virtual_joints_ = srdf_model_->getVirtualJoints();
@@ -338,21 +339,19 @@ void SRDFWriter::createCollisionDefaultsXML(XMLElement* root)
     root->InsertEndChild(entry);
   }
   // Write enabled collision pairs
-  createCollisionPairsXML(root, "enable_collisions", false);
+  createCollisionPairsXML(root, "enable_collisions", enabled_collision_pairs_);
 }
 
 // ******************************************************************************************
 // Generate XML for SRDF disabled/enabled collisions of robot link pairs
 // ******************************************************************************************
-void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name, bool disabled)
+void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name,
+                                         const std::vector<Model::CollisionPair>& pairs)
 {
   XMLDocument* doc = root->GetDocument();
 
-  for (const srdf::Model::CollisionPair& pair : collision_pairs_)
+  for (const srdf::Model::CollisionPair& pair : pairs)
   {
-    if (pair.disabled_ != disabled)
-      continue;
-
     // Create new element for each link pair
     XMLElement* entry = doc->NewElement(tag_name);
     entry->SetAttribute("link1", pair.link1_.c_str());
@@ -371,14 +370,14 @@ void SRDFWriter::createCollisionPairsXML(XMLElement* root)
   XMLDocument* doc = root->GetDocument();
 
   // Convenience comments
-  if (!collision_pairs_.empty())  // only show comments if there are corresponding elements
-  {
-    XMLComment* comment = doc->NewComment("DISABLE COLLISIONS: By default it is assumed that any link of the robot "
-                                          "could potentially come into collision with any other link in the robot. "
-                                          "This tag disables collision checking between a specified pair of links. ");
-    root->InsertEndChild(comment);
-  }
-  createCollisionPairsXML(root, "disable_collisions", true);
+  if (disabled_collision_pairs_.empty())
+    return;
+
+  XMLComment* comment = doc->NewComment("DISABLE COLLISIONS: By default it is assumed that any link of the robot "
+                                        "could potentially come into collision with any other link in the robot. "
+                                        "This tag disables collision checking between a specified pair of links. ");
+  root->InsertEndChild(comment);
+  createCollisionPairsXML(root, "disable_collisions", disabled_collision_pairs_);
 }
 
 // ******************************************************************************************