diff --git a/srdf/include/srdf/model.h b/srdf/include/srdf/model.h index ba7a0a2..930ae69 100644 --- a/srdf/include/srdf/model.h +++ b/srdf/include/srdf/model.h @@ -168,6 +168,19 @@ public: std::map > 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 >& getDisabledCollisions(void) const + const std::vector& getDisabledCollisionPairs(void) const { return disabled_collisions_; } + /// \deprecated{ Use the version returning DisabledCollision } + __attribute__ ((deprecated)) + std::vector > getDisabledCollisions(void) const; + /// Get the list of groups defined for this model const std::vector& 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::vector groups_; - std::vector group_states_; - std::vector virtual_joints_; - std::vector end_effectors_; - std::vector visual_sensors_; - std::vector > disabled_collisions_; + std::string name_; + std::vector groups_; + std::vector group_states_; + std::vector virtual_joints_; + std::vector end_effectors_; + std::vector visual_sensors_; + std::vector disabled_collisions_; }; } diff --git a/srdf/src/model.cpp b/srdf/src/model.cpp index ae20f0a..a1be04d 100644 --- a/srdf/src/model.cpp +++ b/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)); - std::string link2_str = boost::trim_copy(std::string(link2)); - if (!urdf_model.getLink(link1_str)) + DisabledCollision dc; + dc.link1_ = boost::trim_copy(std::string(link1)); + 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 > srdf::Model::getDisabledCollisions(void) const +{ + std::vector > 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; +} diff --git a/srdf/test/res/pr2_desc.3.srdf b/srdf/test/res/pr2_desc.3.srdf index 33208de..d567cc0 100644 --- a/srdf/test/res/pr2_desc.3.srdf +++ b/srdf/test/res/pr2_desc.3.srdf @@ -65,7 +65,7 @@ - + diff --git a/srdf/test/test_parser.cpp b/srdf/test/test_parser.cpp index 5e62f52..baa13d7 100644 --- a/srdf/test/test_parser.cpp +++ b/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");