adding support for reasons in disabled collisions

srdf/include/srdf/model.h

@@ -168,6 +168,19 @@ public:
     std::map<std::string, std::vector<double> > joint_values_;
   };
   
+  /// The definition of a disabled collision between two links
+  struct DisabledCollision
+  {
+    /// 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
+    std::string reason_;
+  };
+  
   /// Get the name of this model
   const std::string& getName(void) const
   {
@@ -175,11 +188,15 @@ public:
   }
   
   /// 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<std::pair<std::string, std::string> >& getDisabledCollisions(void) const
+  const std::vector<DisabledCollision>& getDisabledCollisionPairs(void) const
   {
     return disabled_collisions_;
   }
   
+  /// \deprecated{ Use the version returning DisabledCollision }
+  __attribute__ ((deprecated)) 
+  std::vector<std::pair<std::string, std::string> > getDisabledCollisions(void) const;
+  
   /// Get the list of groups defined for this model
   const std::vector<Group>& getGroups(void) const
   {
@@ -216,13 +233,13 @@ private:
   void loadVisualSensors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
   void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
 
-  std::string                                       name_;
+  std::string                    name_;
-  std::vector<Group>                                groups_;
+  std::vector<Group>             groups_;
-  std::vector<GroupState>                           group_states_;
+  std::vector<GroupState>        group_states_;
-  std::vector<VirtualJoint>                         virtual_joints_;
+  std::vector<VirtualJoint>      virtual_joints_;
-  std::vector<EndEffector>                          end_effectors_;
+  std::vector<EndEffector>       end_effectors_;
-  std::vector<VisualSensor>                         visual_sensors_;
+  std::vector<VisualSensor>      visual_sensors_;
-  std::vector<std::pair<std::string, std::string> > disabled_collisions_;
+  std::vector<DisabledCollision> disabled_collisions_;
 };
 
 }

srdf/src/model.cpp

@@ -477,19 +477,23 @@ void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model,
       ROS_ERROR("A pair of links needs to be specified to disable collisions");
       continue;
     }
-    std::string link1_str = boost::trim_copy(std::string(link1));
+    DisabledCollision dc;
-    std::string link2_str = boost::trim_copy(std::string(link2));
+    dc.link1_ = boost::trim_copy(std::string(link1));
-    if (!urdf_model.getLink(link1_str))
+    dc.link2_ = boost::trim_copy(std::string(link2));
+    if (!urdf_model.getLink(dc.link1_))
     {
       ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
       continue;
     }
-    if (!urdf_model.getLink(link2_str))
+    if (!urdf_model.getLink(dc.link2_))
     {
       ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
       continue;
     }
-    disabled_collisions_.push_back(std::make_pair(link1_str, link2_str));
+    const char *reason = c_xml->Attribute("reason");
+    if (reason)
+      dc.reason_ = std::string(reason);
+    disabled_collisions_.push_back(dc);
   }
 }
 
@@ -576,3 +580,11 @@ void srdf::Model::clear(void)
   visual_sensors_.clear();
   disabled_collisions_.clear();
 }
+
+std::vector<std::pair<std::string, std::string> > srdf::Model::getDisabledCollisions(void) 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;
+}

srdf/test/res/pr2_desc.3.srdf

@@ -65,7 +65,7 @@
    <end_effector name="r_end_effector" parent_link="r_wrist_roll_link" group="r_end_effector"/>
    <end_effector name="l_end_effector" parent_link="l_wrist_roll_link" group="l_end_effector"/>
 
-   <disable_collisions link1="r_shoulder_pan_link" link2="r_shoulder_lift_link" />
+   <disable_collisions link1="r_shoulder_pan_link" link2="r_shoulder_lift_link" reason="adjacent" />
    <disable_collisions link1="r_shoulder_pan_link" link2="l_gripper_palm_link" />
    <!-- and many more disable_collisions tags -->
 

srdf/test/test_parser.cpp

@@ -48,21 +48,21 @@ TEST(SRDF, Simple)
     EXPECT_TRUE(s.getVirtualJoints().size() == 0);
     EXPECT_TRUE(s.getGroups().size() == 0);
     EXPECT_TRUE(s.getGroupStates().size() == 0);
-    EXPECT_TRUE(s.getDisabledCollisions().empty());
+    EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
     EXPECT_TRUE(s.getEndEffectors().size() == 0);
 
     EXPECT_TRUE(s.initFile(u, "test/res/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.getDisabledCollisions().empty());
+    EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
     EXPECT_TRUE(s.getEndEffectors().size() == 0);
 
     EXPECT_TRUE(s.initFile(u, "test/res/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.getDisabledCollisions().empty());
+    EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
     EXPECT_TRUE(s.getEndEffectors().size() == 0);
 }
 
@@ -76,7 +76,8 @@ TEST(SRDF, Complex)
     EXPECT_TRUE(s.getVirtualJoints().size() == 1);
     EXPECT_TRUE(s.getGroups().size() == 7);
     EXPECT_TRUE(s.getGroupStates().size() == 2);
-    EXPECT_TRUE(s.getDisabledCollisions().size() == 2);
+    EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
+    EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
     EXPECT_TRUE(s.getEndEffectors().size() == 2);
 
     EXPECT_EQ(s.getVirtualJoints()[0].name_, "world_joint");