[C++] Implement SRDF parsing for geometry data

This new feature allows to activate collision pairs according to an
SRDF file

src/multibody/geometry.hpp

@@ -350,6 +350,16 @@ namespace se3
     
     void desactivateCollisionPairs();
     void initializeListOfCollisionPairs();
+    
+    ///
+    /// \brief Active all possible collision pairs except those mentioned in the SRDF file.
+    ///        It throws if the SRDF file is incorrect.
+    ///
+    /// \param[in] filename The complete path to the SRDF file.
+    /// \param[in] verbose Verbosity mode.
+    ///
+    void addCollisionPairsFromSrdf(const std::string & filename,
+                                   const bool verbose = false) throw (std::invalid_argument);
 
     ///
     /// \brief Compute the collision status between two collision objects given by their indexes.

src/multibody/geometry.hxx

@@ -36,6 +36,12 @@
 #include <list>
 #include <utility>
 
+// Read XML file with boost
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <fstream>
+#include <boost/foreach.hpp>
+
 /// @cond DEV
 
 namespace se3
@@ -198,6 +204,16 @@ namespace se3
     
     return (Index) distance(collision_pairs.begin(), it);
   }
+  
+//  std::vector<Index> GeometryData::findCollisionPairsSupportedBy(const JointIndex joint_id) const
+//  {
+////    std::vector<Index> collision_pairs;
+////    for(CollisionPairsVector_t::const_iterator it = collision_pairs.begin();
+////        it != collision_pairs.end(); ++it)
+////    {
+////      if (geom_model.it->first )
+////    }
+//  }
 
   // TODO :  give a srdf file as argument, read it, and remove corresponding
   // pairs from list collision_pairs
@@ -284,6 +300,68 @@ namespace se3
   {
     std::fill(distance_results.begin(), distance_results.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
   }
+  
+  void GeometryData::addCollisionPairsFromSrdf(const std::string & filename,
+                                               const bool verbose) throw (std::invalid_argument)
+  {
+    std::ifstream srdf_stream(filename);
+    if (! srdf_stream.is_open())
+    {
+      const std::string exception_message (filename + " seems not to be a valid file.");
+      throw std::invalid_argument(exception_message);
+    }
+    
+    // Add all collision pairs
+    addAllCollisionPairs();
+    std::cout << "Num collision pairs " << nCollisionPairs << std::endl;
+    
+    // Read xml stream
+    using boost::property_tree::ptree;
+    ptree pt;
+    read_xml(srdf_stream, pt);
+    
+    // Iterate over collision pairs
+    const se3::Model & model = data_ref.model;
+    BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
+    {
+      if (v.first == "disable_collisions")
+      {
+        const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
+        const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");
+        
+        // Check first if the two bodies exist in model
+        if (!model.existBodyName(link1) || !model.existBodyName(link2))
+        {
+          if (verbose)
+            std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl;
+          continue;
+        }
+        
+        const Model::JointIndex id1 = model.getBodyId(link1);
+        const Model::JointIndex id2 = model.getBodyId(link2);
+        
+        typedef GeometryModel::GeomIndexList GeomIndexList;
+        const GeomIndexList & innerObject1 = model_geom.innerObjects.at(id1);
+        const GeomIndexList & innerObject2 = model_geom.innerObjects.at(id2);
+        
+        for(GeomIndexList::const_iterator it1 = innerObject1.begin();
+            it1 != innerObject1.end();
+            ++it1)
+        {
+          for(GeomIndexList::const_iterator it2 = innerObject2.begin();
+              it2 != innerObject2.end();
+              ++it2)
+          {
+            removeCollisionPair(CollisionPair(*it1, *it2));
+          }
+        }
+        
+      } // BOOST_FOREACH
+    }
+    
+    
+    std::cout << "Num collision pairs " << nCollisionPairs << std::endl;
+  }
 
 
 } // namespace se3