Refactor joint properties data structure (#110)

include/srdfdom/model.h

@@ -52,6 +52,9 @@ namespace srdf
 class Model
 {
 public:
+  using PropertyMap = std::map<std::string, std::string>;       // property name -> value
+  using JointPropertyMap = std::map<std::string, PropertyMap>;  // joint name -> properties
+
   Model()
   {
   }
@@ -201,19 +204,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
   {
@@ -268,21 +258,20 @@ class Model
     return link_sphere_approximations_;
   }
 
-  /// Get the joint properties for a particular joint (empty vector if none)
-  const std::vector<JointProperty>& getJointProperties(const std::string& joint_name) const
+  /// Get the joint properties for a particular joint
+  const PropertyMap& getJointProperties(const std::string& joint_name) const
   {
-    std::map<std::string, std::vector<JointProperty>>::const_iterator 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 iter->second;
+    static const PropertyMap empty_map;
+
+    auto it = joint_properties_.find(joint_name);
+    if (it == joint_properties_.end())
+      return empty_map;
+    else
+      return it->second;
   }
 
-  /// Get the joint properties list
-  const std::map<std::string, std::vector<JointProperty>>& getJointProperties() const
+  /// Get the joint properties map
+  const JointPropertyMap& getJointProperties() const
   {
     return joint_properties_;
   }
@@ -314,10 +303,7 @@ 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_;
+  JointPropertyMap joint_properties_;
 };
 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_;
+  Model::JointPropertyMap joint_properties_;
 
   // Store the SRDF Model for updating the kinematic_model
   ModelSharedPtr srdf_model_;

src/model.cpp

@@ -45,8 +45,6 @@
 
 using namespace tinyxml2;
 
-const std::vector<srdf::Model::JointProperty> srdf::Model::empty_vector_;
-
 bool srdf::Model::isValidJoint(const urdf::ModelInterface& urdf_model, const std::string& name) const
 {
   if (urdf_model.getJoint(name))
@@ -612,6 +610,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 +629,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

@@ -509,18 +509,17 @@ void SRDFWriter::createJointPropertiesXML(tinyxml2::XMLElement* root)
 
   if (!joint_properties_.empty())
   {
-    XMLComment* comment = doc->NewComment(
-        "JOINT PROPERTIES: Purpose: Define a property for a particular joint (could be a virtual joint)");
+    XMLComment* comment = doc->NewComment("JOINT PROPERTIES: Define properties for individual joints");
     root->InsertEndChild(comment);
   }
   for (const auto& joint_properties : joint_properties_)
   {
     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/resources/pr2_desc.3-normalized.srdf

@@ -63,7 +63,7 @@
     <end_effector name="l_end_effector" parent_link="l_wrist_roll_link" group="l_end_effector"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="world_joint" type="planar" parent_frame="odom" child_link="base_footprint"/>
-    <!--JOINT PROPERTIES: Purpose: Define a property for a particular joint (could be a virtual joint)-->
+    <!--JOINT PROPERTIES: Define properties for individual joints-->
     <joint_property joint_name="world_joint" property_name="angular_distance_weight" value="0.5"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="r_shoulder_pan_link" link2="r_shoulder_lift_link" reason="adjacent"/>

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 srdf::Model::PropertyMap& 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 srdf::Model::PropertyMap& 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 srdf::Model::PropertyMap& 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)