Add substitution when loading yaml files (PERCEPTION-1921)

tools/roslaunch/src/roslaunch/loader.py

@@ -358,7 +358,7 @@ def add_param(self, ros_config, param_name, param_value, verbose=True):
         else:
             ros_config.add_param(Param(param_name, param_value), verbose=verbose)
 
-    def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=True):
+    def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=True, subst_function=None):
         """
         Load rosparam setting
 
@@ -394,6 +394,8 @@ def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=Tr
                 with open(file_, 'r') as f:
                     text = f.read()
 
+            if subst_function is not None:
+                text = subst_function(text)
             # parse YAML text
             # - lazy import
             global yaml

tools/roslaunch/src/roslaunch/xmlloader.py

@@ -233,9 +233,10 @@ def _rosparam_tag(self, tag, context, ros_config, verbose=True):
             # load is the default command            
             cmd = cmd or 'load'
             value = _get_text(tag)
+            subst_function = None
             if subst_value:
-                value = self.resolve_args(value, context)
-            self.load_rosparam(context, ros_config, cmd, param, file, value, verbose=verbose)
+                subst_function = lambda x: self.resolve_args(x, context)
+            self.load_rosparam(context, ros_config, cmd, param, file, value, verbose=verbose, subst_function=subst_function)
 
         except ValueError as e:
             raise loader.LoadException("error loading <rosparam> tag: \n\t"+str(e)+"\nXML is %s"%tag.toxml())

tools/roslaunch/test/params_subst.yaml

@@ -0,0 +1 @@
+string1: $(anon foo)

tools/roslaunch/test/unit/test_xmlloader.py

@@ -242,6 +242,10 @@ def test_rosparam_valid(self):
         self.assertEquals('bar', p.value)
         p = [p for p in mock.params if p.key == '/node_rosparam/robots/childparam'][0]
         self.assertEquals('a child namespace parameter', p.value)
+
+        # test substitution in yaml files
+        p = [p for p in mock.params if p.key == '/rosparam_subst/string1'][0]
+        self.assertTrue('$(anon foo)' not in p.value)
 
         exes = [e for e in mock.executables if e.command == 'rosparam']
         self.assertEquals(len(exes), 2, "expected 2 rosparam exes, got %s"%len(exes))
@@ -274,6 +278,10 @@ def test_rosparam_valid(self):
         p = [p for p in mock.params if p.key == '/inline_dict2/key4'][0]
         self.assertEquals('value4', p.value)
 
+        # test substitution in inline yaml
+        p = [p for p in mock.params if p.key == '/inline_subst'][0]
+        self.assertTrue('$(anon foo)' not in p.value)
+
         # verify that later tags override 
         # - key2 is overriden
         self.assertEquals(1, len([p for p in mock.params if p.key == '/override/key1']))

tools/roslaunch/test/xml/test-rosparam-valid.xml

@@ -12,13 +12,19 @@
     <rosparam file="$(find roslaunch)/test/params.yaml" command="load" />      
   </node>
 
+  <group ns="rosparam_subst">
+    <rosparam file="$(find roslaunch)/test/params_subst.yaml" command="load" subst_value="true" />
+  </group>
+
   <rosparam param="inline_str">value1</rosparam>
   <rosparam param="inline_list">[1, 2, 3, 4]</rosparam>  
   <rosparam param="inline_dict">{key1: value1, key2: value2}</rosparam>    
   <rosparam param="inline_dict2">
      key3: value3
      key4: value4
   </rosparam>
+
+  <rosparam param="inline_subst" subst_value="true">$(anon foo)</rosparam>
 
   <rosparam param="override">{key1: value1, key2: value2}</rosparam>
   <rosparam param="override">{key1: override1}</rosparam>