Refactor joint properties data structure (moveit#110)

- Refactored joint_properties to use a map of maps
  for streamlined access to joint properties
- Removed JointProperty struct as information contained is redundant.
- Updated tests to account for new getJointProperties signature

Author: scchow

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<std::string>& getNoDefaultCollisionLinks() const
   {
@@ -269,20 +256,22 @@ class Model
   }
 
   /// Get the joint properties for a particular joint (empty vector if none)
-  const std::vector<JointProperty>& getJointProperties(const std::string& joint_name) const
+  const std::map<std::string, std::string>& getJointProperties(const std::string& joint_name) const
   {
-    std::map<std::string, std::vector<JointProperty>>::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_;
+      return empty_map_;
     }
+
     return iter->second;
   }
 
   /// Get the joint properties list
-  const std::map<std::string, std::vector<JointProperty>>& getJointProperties() const
+  const std::map<std::string, std::map<std::string, std::string>>& getJointProperties() const
   {
     return joint_properties_;
   }
@@ -314,10 +303,12 @@ class Model
   std::vector<CollisionPair> enabled_collision_pairs_;
   std::vector<CollisionPair> disabled_collision_pairs_;
   std::vector<PassiveJoint> passive_joints_;
-  std::map<std::string, std::vector<JointProperty>> joint_properties_;
 
-  // Empty joint property vector
-  static const std::vector<JointProperty> empty_vector_;
+  // Map the name of the joint to a second map that maps joint property name to joint property
+  std::map<std::string, std::map<std::string, std::string>> joint_properties_;
+
+  // Empty joint property map
+  static const std::map<std::string, std::string> empty_map_;
 };
 typedef std::shared_ptr<Model> ModelSharedPtr;
 typedef std::shared_ptr<const Model> ModelConstSharedPtr;

include/srdfdom/srdf_writer.h

@@ -197,7 +197,7 @@ class SRDFWriter
   std::vector<Model::CollisionPair> disabled_collision_pairs_;
   std::vector<Model::CollisionPair> enabled_collision_pairs_;
   std::vector<Model::PassiveJoint> passive_joints_;
-  std::map<std::string, std::vector<srdf::Model::JointProperty>> joint_properties_;
+  std::map<std::string, std::map<std::string, std::string>> joint_properties_;
 
   // Store the SRDF Model for updating the kinematic_model
   ModelSharedPtr srdf_model_;

src/model.cpp

@@ -45,7 +45,7 @@
 
 using namespace tinyxml2;
 
-const std::vector<srdf::Model::JointProperty> srdf::Model::empty_vector_;
+const std::map<std::string, std::string> 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);
   }
 }
 

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);
     }
   }

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<srdf::Model::JointProperty>& gripper_props = s.getJointProperties("r_gripper_tool_joint");
+  const std::map<std::string, std::string>& 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<srdf::Model::JointProperty>& made_up_props = s.getJointProperties("made_up_joint");
+  const std::map<std::string, std::string>& made_up_props = s.getJointProperties("made_up_joint");
   EXPECT_EQ(made_up_props.size(), 0u);
 
-  const std::vector<srdf::Model::JointProperty>& world_props = s.getJointProperties("world_joint");
+  const std::map<std::string, std::string>& 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)