diff --git a/include/srdfdom/model.h b/include/srdfdom/model.h index d95f44e..b889755 100644 --- a/include/srdfdom/model.h +++ b/include/srdfdom/model.h @@ -201,19 +201,6 @@ class Model return name_; } - // Some joints may have additional properties. - struct JointProperty - { - /// The name of the joint that this property belongs to - std::string joint_name_; - - /// The name of the property - std::string property_name_; - - /// The value of the property. Type not specified. - std::string value_; - }; - /// Get the list of links that should have collision checking disabled by default (and only selectively enabled) const std::vector& getNoDefaultCollisionLinks() const { @@ -269,20 +256,22 @@ class Model } /// Get the joint properties for a particular joint (empty vector if none) - const std::vector& getJointProperties(const std::string& joint_name) const + const std::map& getJointProperties(const std::string& joint_name) const { - std::map>::const_iterator iter = joint_properties_.find(joint_name); + auto iter = joint_properties_.find(joint_name); + if (iter == joint_properties_.end()) { - // We return a standard empty vector here rather than insert a new empty vector - // into the map in order to keep the method const - return empty_vector_; + // We return a standard empty map here rather than create a new empty map + // in order to keep the method const + return empty_map_; } + return iter->second; } /// Get the joint properties list - const std::map>& getJointProperties() const + const std::map>& getJointProperties() const { return joint_properties_; } @@ -314,10 +303,12 @@ class Model std::vector enabled_collision_pairs_; std::vector disabled_collision_pairs_; std::vector passive_joints_; - std::map> joint_properties_; - // Empty joint property vector - static const std::vector empty_vector_; + // joint name -> (property name -> property value) + std::map> joint_properties_; + + // Empty joint property map + static const std::map empty_map_; }; typedef std::shared_ptr ModelSharedPtr; typedef std::shared_ptr ModelConstSharedPtr; diff --git a/include/srdfdom/srdf_writer.h b/include/srdfdom/srdf_writer.h index 90ad43f..be0b19b 100644 --- a/include/srdfdom/srdf_writer.h +++ b/include/srdfdom/srdf_writer.h @@ -197,7 +197,7 @@ class SRDFWriter std::vector disabled_collision_pairs_; std::vector enabled_collision_pairs_; std::vector passive_joints_; - std::map> joint_properties_; + std::map> joint_properties_; // Store the SRDF Model for updating the kinematic_model ModelSharedPtr srdf_model_; diff --git a/src/model.cpp b/src/model.cpp index 8d587f4..accbf4c 100644 --- a/src/model.cpp +++ b/src/model.cpp @@ -45,7 +45,7 @@ using namespace tinyxml2; -const std::vector srdf::Model::empty_vector_; +const std::map srdf::Model::empty_map_; bool srdf::Model::isValidJoint(const urdf::ModelInterface& urdf_model, const std::string& name) const { @@ -612,6 +612,9 @@ void srdf::Model::loadJointProperties(const urdf::ModelInterface& urdf_model, XM const char* jname = prop_xml->Attribute("joint_name"); const char* pname = prop_xml->Attribute("property_name"); const char* pval = prop_xml->Attribute("value"); + + std::string jname_str = boost::trim_copy(std::string(jname)); + if (!jname) { CONSOLE_BRIDGE_logError("joint_property is missing a joint name"); @@ -628,18 +631,13 @@ void srdf::Model::loadJointProperties(const urdf::ModelInterface& urdf_model, XM continue; } - JointProperty jp; - jp.joint_name_ = boost::trim_copy(std::string(jname)); - jp.property_name_ = boost::trim_copy(std::string(pname)); - jp.value_ = std::string(pval); - - if (!isValidJoint(urdf_model, jp.joint_name_)) + if (!isValidJoint(urdf_model, jname_str)) { CONSOLE_BRIDGE_logError("Property defined for a joint '%s' that is not known to the URDF. Ignoring.", - jp.joint_name_.c_str()); + jname_str.c_str()); continue; } - joint_properties_[jp.joint_name_].push_back(jp); + joint_properties_[jname_str][boost::trim_copy(std::string(pname))] = std::string(pval); } } diff --git a/src/srdf_writer.cpp b/src/srdf_writer.cpp index d24787d..eb968a9 100644 --- a/src/srdf_writer.cpp +++ b/src/srdf_writer.cpp @@ -518,9 +518,9 @@ void SRDFWriter::createJointPropertiesXML(tinyxml2::XMLElement* root) for (const auto& joint_property : joint_properties.second) { XMLElement* p_joint = doc->NewElement("joint_property"); - p_joint->SetAttribute("joint_name", joint_property.joint_name_.c_str()); - p_joint->SetAttribute("property_name", joint_property.property_name_.c_str()); - p_joint->SetAttribute("value", joint_property.value_.c_str()); + p_joint->SetAttribute("joint_name", joint_properties.first.c_str()); + p_joint->SetAttribute("property_name", joint_property.first.c_str()); + p_joint->SetAttribute("value", joint_property.second.c_str()); root->InsertEndChild(p_joint); } } diff --git a/test/test_parser.cpp b/test/test_parser.cpp index 2e7344c..944b308 100644 --- a/test/test_parser.cpp +++ b/test/test_parser.cpp @@ -179,18 +179,17 @@ TEST(TestCpp, testComplex) EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link"); // Joint Properties - const std::vector& gripper_props = s.getJointProperties("r_gripper_tool_joint"); + const std::map& gripper_props = s.getJointProperties("r_gripper_tool_joint"); EXPECT_EQ(gripper_props.size(), 0u); // When parsing, this made up joint that is not present in the URDF is expected to print an error // AND the property should not be made available in the srdf::Model - const std::vector& made_up_props = s.getJointProperties("made_up_joint"); + const std::map& made_up_props = s.getJointProperties("made_up_joint"); EXPECT_EQ(made_up_props.size(), 0u); - const std::vector& world_props = s.getJointProperties("world_joint"); + const std::map& world_props = s.getJointProperties("world_joint"); ASSERT_EQ(world_props.size(), 1u); - EXPECT_EQ(world_props[0].property_name_, "angular_distance_weight"); - EXPECT_EQ(world_props[0].value_, "0.5"); + EXPECT_EQ(world_props.at("angular_distance_weight"), "0.5"); } TEST(TestCpp, testReadWrite)