SDF: support plugin with repeated elements

pcg_gazebo/parsers/sdf/plugin.py

@@ -57,7 +57,17 @@ def _add_child_element(self, tag, value):
         assert is_string(tag), 'Input tag' \
             ' must be string or unicode, received={}, type={}'.format(
                 tag, type(tag))
-        self._value[tag] = value
+
+        # has multiple elements with the same tag
+        if tag in self._value:
+            if isinstance(self._value[tag], list):
+                self._value[tag].append(value)
+            else:
+                tmp = self._value[tag]
+                self._value[tag] = list([tmp, value])
+
+        else:
+            self._value[tag] = value
 
     def from_dict(self, sdf_data, ignore_tags=list()):
         XMLCustom.from_dict(self, sdf_data)

pcg_gazebo/parsers/types/custom.py

@@ -42,6 +42,36 @@ def _get_elem_as_xml(self, xml_elem, value):
                     child = Element(tag, attrib=value[tag]['attributes'])
                     self._get_elem_as_xml(child, value[tag]['value'])
                     xml_elem.append(child)
+
+                elif isinstance(value[tag], list):
+                    # check if the list contains repeated elements
+                    n_elem = 0
+                    for item in value[tag]:
+                        if isinstance(item, dict) and \
+                            'attributes' in item and \
+                            'value' in item:
+                            n_elem += 1
+
+                    # either all items are elements or none are 
+                    has_mult = n_elem == len(value[tag])
+                    is_valid = not (n_elem > 0 and not has_mult)
+
+                    assert is_valid, \
+                        "XML data has invalid repeated element for tag {}".format(tag)  
+
+                    if has_mult:
+                        # repeated elements
+                        for item in value[tag]:
+                            child = Element(tag, attrib=item['attributes'])
+                            self._get_elem_as_xml(child, item['value'])
+                            xml_elem.append(child)
+                    else:
+                        # single element containing a list
+                        child = Element(tag)
+                        output_str = ' '.join(['{}'] * len(value[tag]))
+                        child.text = output_str.format(*value[tag])
+                        xml_elem.append(child)
+
                 elif tag.startswith('@'):
                     xml_elem.set(tag.replace('@', ''), value[tag])
                 else: