Add substitution when loading yaml files

like in ros#1051

tools/roslaunch/src/roslaunch/loader.py

@@ -365,7 +365,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
         
@@ -401,6 +401,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: we have to import rosparam in oder to to configure the YAML constructors
             global rosparam

tools/roslaunch/src/roslaunch/xmlloader.py

@@ -235,9 +235,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())