Parse <collision_default> and <enable_collisions> tags

include/srdfdom/model.h

@@ -175,17 +175,19 @@ class Model
     std::vector<Sphere> spheres_;
   };
 
-  /// The definition of a disabled collision between two links
-  struct DisabledCollision
+  /// The definition of a disabled/enabled collision between two links
+  struct CollisionPair
   {
     /// The name of the first link (as in URDF) of the disabled collision
     std::string link1_;
 
     /// The name of the second link (as in URDF) of the disabled collision
     std::string link2_;
 
-    /// The reason why the collision check was disabled
+    /// 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
@@ -201,15 +203,17 @@ class Model
     return name_;
   }
 
-  /// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the
-  /// geometry and kinematics of the robot)
-  const std::vector<DisabledCollision>& getDisabledCollisionPairs() const
+  /// Get the list of links that should have collision checking disabled by default (and only selectively enabled)
+  const std::vector<std::string>& getNoDefaultCollisionLinks() const
   {
-    return disabled_collisions_;
+    return no_default_collision_links_;
   }
 
-  /// \deprecated{ Use the version returning DisabledCollision }
-  [[deprecated]] std::vector<std::pair<std::string, std::string> > getDisabledCollisions() const;
+  /// Get the list of pairs of links for which we explicitly disable/enable collision
+  const std::vector<CollisionPair>& getCollisionPairs() const
+  {
+    return collision_pairs_;
+  }
 
   /// Get the list of groups defined for this model
   const std::vector<Group>& getGroups() const
@@ -256,7 +260,9 @@ class Model
   void loadGroupStates(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
   void loadEndEffectors(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
   void loadLinkSphereApproximations(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
-  void loadDisabledCollisions(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
+  void loadCollisionDefaults(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
+  void loadCollisionPairs(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml, const char* tag_name,
+                          bool disabled);
   void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
 
   std::string name_;
@@ -265,7 +271,8 @@ class Model
   std::vector<VirtualJoint> virtual_joints_;
   std::vector<EndEffector> end_effectors_;
   std::vector<LinkSpheres> link_sphere_approximations_;
-  std::vector<DisabledCollision> disabled_collisions_;
+  std::vector<std::string> no_default_collision_links_;
+  std::vector<CollisionPair> collision_pairs_;
   std::vector<PassiveJoint> passive_joints_;
 };
 typedef std::shared_ptr<Model> ModelSharedPtr;

include/srdfdom/srdf_writer.h

@@ -125,12 +125,19 @@ class SRDFWriter
    */
   void createLinkSphereApproximationsXML(tinyxml2::XMLElement* root);
 
+  /**
+   * Generate XML for SRDF collision defaults
+   *
+   * @param root  - TinyXML root element to attach sub elements to
+   */
+  void createCollisionDefaultsXML(tinyxml2::XMLElement* root);
+
   /**
    * Generate XML for SRDF disabled collisions of robot link pairs
    *
    * @param root  - TinyXML root element to attach sub elements to
    */
-  void createDisabledCollisionsXML(tinyxml2::XMLElement* root);
+  void createCollisionPairsXML(tinyxml2::XMLElement* root);
 
   /**
    * Generate XML for SRDF group states of each joint's position
@@ -160,6 +167,15 @@ class SRDFWriter
    */
   void createPassiveJointsXML(tinyxml2::XMLElement* root);
 
+protected:
+  /**
+   * Generate XML for SRDF disabled/enabled collisions of robot link pairs
+   *
+   * @param root  - TinyXML root element to attach sub elements to
+   */
+  void createCollisionPairsXML(tinyxml2::XMLElement* root, const char* tag_name, bool disabled);
+
+public:
   // ******************************************************************************************
   // Group Datastructures
   // ******************************************************************************************
@@ -169,7 +185,8 @@ class SRDFWriter
   std::vector<srdf::Model::VirtualJoint> virtual_joints_;
   std::vector<srdf::Model::EndEffector> end_effectors_;
   std::vector<srdf::Model::LinkSpheres> link_sphere_approximations_;
-  std::vector<srdf::Model::DisabledCollision> disabled_collisions_;
+  std::vector<std::string> no_default_collision_links_;
+  std::vector<srdf::Model::CollisionPair> collision_pairs_;
   std::vector<srdf::Model::PassiveJoint> passive_joints_;
 
   // Store the SRDF Model for updating the kinematic_model

src/model.cpp

@@ -37,6 +37,7 @@
 #include "srdfdom/model.h"
 #include <console_bridge/console.h>
 #include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string/case_conv.hpp>
 #include <algorithm>
 #include <fstream>
 #include <sstream>
@@ -532,35 +533,54 @@ void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface& urdf_
   }
 }
 
-void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml)
+void srdf::Model::loadCollisionDefaults(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml)
 {
-  for (XMLElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml;
-       c_xml = c_xml->NextSiblingElement("disable_collisions"))
+  for (XMLElement* xml = robot_xml->FirstChildElement("disable_default_collisions"); xml;
+       xml = xml->NextSiblingElement("disable_default_collisions"))
+  {
+    const char* link_ = xml->Attribute("link");
+    if (!link_)
+    {
+      CONSOLE_BRIDGE_logError("A disable_default_collisions tag needs to specify a link name");
+      continue;
+    }
+    std::string link = boost::trim_copy(std::string(link_));
+    if (!urdf_model.getLink(link))
+    {
+      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot specify collision default.", link_);
+      continue;
+    }
+    no_default_collision_links_.push_back(link);
+  }
+}
+
+void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml,
+                                     const char* tag_name, bool disabled)
+{
+  for (XMLElement* c_xml = robot_xml->FirstChildElement(tag_name); c_xml; c_xml = c_xml->NextSiblingElement(tag_name))
   {
     const char* link1 = c_xml->Attribute("link1");
     const char* link2 = c_xml->Attribute("link2");
     if (!link1 || !link2)
     {
-      CONSOLE_BRIDGE_logError("A pair of links needs to be specified to disable collisions");
+      CONSOLE_BRIDGE_logError("A pair of links needs to be specified to disable/enable collisions");
       continue;
     }
-    DisabledCollision dc;
-    dc.link1_ = boost::trim_copy(std::string(link1));
-    dc.link2_ = boost::trim_copy(std::string(link2));
-    if (!urdf_model.getLink(dc.link1_))
+    const char* reason = c_xml->Attribute("reason");
+
+    CollisionPair pair{ boost::trim_copy(std::string(link1)), boost::trim_copy(std::string(link2)),
+                        reason ? reason : "", disabled };
+    if (!urdf_model.getLink(pair.link1_))
     {
-      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
+      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link1);
       continue;
     }
-    if (!urdf_model.getLink(dc.link2_))
+    if (!urdf_model.getLink(pair.link2_))
     {
-      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
+      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link2);
       continue;
     }
-    const char* reason = c_xml->Attribute("reason");
-    if (reason)
-      dc.reason_ = std::string(reason);
-    disabled_collisions_.push_back(dc);
+    collision_pairs_.push_back(pair);
   }
 }
 
@@ -619,7 +639,9 @@ bool srdf::Model::initXml(const urdf::ModelInterface& urdf_model, XMLElement* ro
   loadGroupStates(urdf_model, robot_xml);
   loadEndEffectors(urdf_model, robot_xml);
   loadLinkSphereApproximations(urdf_model, robot_xml);
-  loadDisabledCollisions(urdf_model, robot_xml);
+  loadCollisionDefaults(urdf_model, robot_xml);
+  loadCollisionPairs(urdf_model, robot_xml, "enable_collisions", false);
+  loadCollisionPairs(urdf_model, robot_xml, "disable_collisions", true);
   loadPassiveJoints(urdf_model, robot_xml);
 
   return true;
@@ -673,14 +695,7 @@ void srdf::Model::clear()
   virtual_joints_.clear();
   end_effectors_.clear();
   link_sphere_approximations_.clear();
-  disabled_collisions_.clear();
+  no_default_collision_links_.clear();
+  collision_pairs_.clear();
   passive_joints_.clear();
 }
-
-std::vector<std::pair<std::string, std::string> > srdf::Model::getDisabledCollisions() const
-{
-  std::vector<std::pair<std::string, std::string> > result;
-  for (std::size_t i = 0; i < disabled_collisions_.size(); ++i)
-    result.push_back(std::make_pair(disabled_collisions_[i].link1_, disabled_collisions_[i].link2_));
-  return result;
-}

src/srdf_writer.cpp

@@ -98,7 +98,8 @@ void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf::
   }
 
   // Copy all read-only data from srdf model to this object
-  disabled_collisions_ = srdf_model_->getDisabledCollisionPairs();
+  no_default_collision_links_ = srdf_model_->getNoDefaultCollisionLinks();
+  collision_pairs_ = srdf_model_->getCollisionPairs();
   link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
   groups_ = srdf_model_->getGroups();
   virtual_joints_ = srdf_model_->getVirtualJoints();
@@ -195,7 +196,7 @@ void SRDFWriter::generateSRDF(XMLDocument& document)
   createLinkSphereApproximationsXML(robot_root);
 
   // Add Disabled Collisions
-  createDisabledCollisionsXML(robot_root);
+  createCollisionPairsXML(robot_root);
 }
 
 // ******************************************************************************************
@@ -315,32 +316,69 @@ void SRDFWriter::createLinkSphereApproximationsXML(XMLElement* root)
 }
 
 // ******************************************************************************************
-// Generate XML for SRDF disabled collisions of robot link pairs
+// Generate XML for SRDF collision defaults of robot links
 // ******************************************************************************************
-void SRDFWriter::createDisabledCollisionsXML(XMLElement* root)
+void SRDFWriter::createCollisionDefaultsXML(XMLElement* root)
 {
   XMLDocument* doc = root->GetDocument();
 
   // Convenience comments
-  if (disabled_collisions_.size())  // only show comments if there are corresponding elements
+  if (!no_default_collision_links_.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 "
+    XMLComment* comment = doc->NewComment("DEFAULT 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. ");
+                                          "This tag allows to revert this behavior and disable collisions by default.");
     root->InsertEndChild(comment);
   }
 
-  for (std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it = disabled_collisions_.begin();
-       pair_it != disabled_collisions_.end(); ++pair_it)
+  for (const std::string& name : no_default_collision_links_)
   {
+    XMLElement* entry = doc->NewElement("disable_default_collisions");
+    entry->SetAttribute("link", name.c_str());
+    root->InsertEndChild(entry);
+  }
+  // Write enabled collision pairs
+  createCollisionPairsXML(root, "enable_collisions", false);
+}
+
+// ******************************************************************************************
+// Generate XML for SRDF disabled/enabled collisions of robot link pairs
+// ******************************************************************************************
+void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name, bool disabled)
+{
+  XMLDocument* doc = root->GetDocument();
+
+  for (const srdf::Model::CollisionPair& pair : collision_pairs_)
+  {
+    if (pair.disabled_ != disabled)
+      continue;
+
     // Create new element for each link pair
-    XMLElement* link_pair = doc->NewElement("disable_collisions");
-    link_pair->SetAttribute("link1", pair_it->link1_.c_str());
-    link_pair->SetAttribute("link2", pair_it->link2_.c_str());
-    link_pair->SetAttribute("reason", pair_it->reason_.c_str());
+    XMLElement* entry = doc->NewElement(tag_name);
+    entry->SetAttribute("link1", pair.link1_.c_str());
+    entry->SetAttribute("link2", pair.link2_.c_str());
+    entry->SetAttribute("reason", pair.reason_.c_str());
 
-    root->InsertEndChild(link_pair);
+    root->InsertEndChild(entry);
+  }
+}
+
+// ******************************************************************************************
+// Generate XML for SRDF disabled collisions of robot link pairs
+// ******************************************************************************************
+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);
 }
 
 // ******************************************************************************************

test/test_parser.cpp

@@ -93,21 +93,21 @@ TEST(TestCpp, testSimple)
   EXPECT_TRUE(s.getVirtualJoints().size() == 0);
   EXPECT_TRUE(s.getGroups().size() == 0);
   EXPECT_TRUE(s.getGroupStates().size() == 0);
-  EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
+  EXPECT_TRUE(s.getCollisionPairs().empty());
   EXPECT_TRUE(s.getEndEffectors().size() == 0);
 
   EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf"));
   EXPECT_TRUE(s.getVirtualJoints().size() == 1);
   EXPECT_TRUE(s.getGroups().size() == 1);
   EXPECT_TRUE(s.getGroupStates().size() == 0);
-  EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
+  EXPECT_TRUE(s.getCollisionPairs().empty());
   EXPECT_TRUE(s.getEndEffectors().size() == 0);
 
   EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
   EXPECT_TRUE(s.getVirtualJoints().size() == 0);
   EXPECT_TRUE(s.getGroups().size() == 0);
   EXPECT_TRUE(s.getGroupStates().size() == 0);
-  EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
+  EXPECT_TRUE(s.getCollisionPairs().empty());
   EXPECT_TRUE(s.getEndEffectors().size() == 0);
 }
 
@@ -121,8 +121,8 @@ TEST(TestCpp, testComplex)
   EXPECT_TRUE(s.getVirtualJoints().size() == 1);
   EXPECT_TRUE(s.getGroups().size() == 7);
   EXPECT_TRUE(s.getGroupStates().size() == 2);
-  EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
-  EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
+  EXPECT_TRUE(s.getCollisionPairs().size() == 2);
+  EXPECT_TRUE(s.getCollisionPairs()[0].reason_ == "adjacent");
   EXPECT_TRUE(s.getEndEffectors().size() == 2);
 
   EXPECT_TRUE(s.getVirtualJoints()[0].name_ == "world_joint");