Configure .world file when starting

[pschillinger]

This might be of interest for others as well. It would be nicer if we could just pass a .world string instead of a file to stage, but I wanted to touch as little as possible.

I needed a way to dynamically configure a .world file when starting stage from a .launch file, e.g., spawn a parametrizable number of robots (in principle, idea similar to xacro for urdf). Since I didn't find a way, I added the following new option:

-c <script> [args] <worldfile>
<script> is a custom script to configure the .world file (see below)
[args] are optional arguments to the script (all args following -c will be passed, so -u or -g need to preceed -c)
<worldfile> as usual, the used .world file stays the last argument and will be as well passed to the script as target

An arbitrary custom script can be used for configuration as long as it writes its result to <worldfile>. For convenience and reference, I added the script rosparam_instantiate.py to this PR:

Usage: rosparam_instantiate.py worldfile_in rosparam_ns worldfile_out
 - worldfile_in:  Path to initial .world file.
 - rosparam_ns:   Namespace which contains the objects to be instantiated.
 - worldfile_out: Path to resulting .world file (loaded by stage).

It accepts a rosparam namespace and instantiates all objects into the world worldfile_in. It can be used in a launchfile like the following example:

<arg name="test_color" value="red" />
<rosparam ns="/stage_objs" subst_value="True">
test:
    model_type: block
    pose: [0,1,0,0]
    color: $(arg test_color)
</rosparam>

<node pkg="stage_ros" name="stageros" type="stageros" args="-c $(find stage_ros)/scripts/rosparam_instantiate.py $(find stage_ros)/world/intense.world /stage_objs $(find stage_ros)/world/tmp.world"/>

which will spawn the following object:

block
(
    name "test"
    color "red"
    pose [ 0 1 0 0 ]
)

Note the following two aspects:

• As shown above, it is possible to use roslaunch's substitution args to dynamically set values.
• Since a namespace is expected instead of a param of type list, multiple different launchfiles can spawn objects into the same namespace. Just make sure the object names are unique, otherwise one will overwrite the other. If required, $(anon name) can help to keep names unique.

[wjwwood]

Thanks for the contribution! Someone will have a look at this for merge as soon as possible.

[wjwwood]

Sorry, I accidentally closed this when changing the branch layout of the repository. I've reopened it and updated the target base to be lunar-devel which is the new default branch. You can edit again to point at indigo-devel, which is what is used to release for ROS Indigo and Kinetic, if you like.