Introduce SMURFA-format

[hwiedPro]

Now we can define robot assemblies similar to xacro.
This will solve issue #78

Changes

The following sections define the changes made.

Changes in Key parsing

We use new keys and define old ones new.

• parent defines the frame in which position and rotationare given. So the FTS300 in the example below is rotated 90 degrees around the y-axis of the frame of the ur5 entity's ee_link node. parent's default value is "world"
• anchor defines to which node we create a fixed link to mount the next part of our assembly. parent's default value is "none"

The node for parent or anchor is always given in the way $ENTITY_NAME::$NODE_NAME.
There are a few special keywords that can be used as well:

• world: the world node/orign
• none (only for anchor): no fixed joint is created
• parent (only for anchor): a fixed joint to the node given as parent is created

SMURFA Format

Therefore we can define a *.smurfa file with the following exemplary content:

smurfa:
  - name: ur5
    file: ur5/smurf/ur5.smurf
    root: true
    position: [0,0,0]
    rotation: [1,0,0,0]
  - name: fts300
    file: robotiq_fts300/smurf/robotiq_fts300.smurf
    parent: "ur5::ee_link"
    anchor: parent
    rotation: [0,90,0]
  - name: gripper
    file: robotiq_2f-140/smurf/gripper_2f_140.smurf
    parent: "fts300::fts_toolside"
    anchor: parent
    position: [0,0,0]
    data_package: 0

In this example we have an ur5 robot, which is defined as the root of our assembly, by setting root: True.

It is possible to define multiple roots in one file, but they will belong to the same assembly. The placement of the assembly via the position and rotation parameters in the higher-level scene will only be applied for root entities. Also make sure to define all non-root objects relative to a root using parent.

The data_package entry defines how much data is sent to the DataBroker by the anchor joint. (0: none, 1: reduced, else or not given: full).

SMURFS Format

Old SMURFS files using the anchor can be used in the same way as before. SMURFS files using parent to define a fixation have to be adapted.

We can include SMURFA files in a scene (see below). In this case - just as before when including a robot - you can define the position and rotation. Both are applied on the whole assembly, giving you the ability to place the whole assembly with those two parameters. Additionally you should give an unique name for your assembly.
SMURFS allows the usage of parent and anchor like in SMURFA, too, but the entities are not linked in an assembly.

smurfs:
  - file: test.smurfa
    type: smurfa
    position: [1,1,1]
    rotation: 90
    anchor: world
    name: assembly
    data_package: 1

ToDo

• [Done] position and rotation (Exception: what(): Item is of wrong type.)
  Currently the application of the overall placement of the assembly is only possible if the root nodes position and rotation are given in the way:

  position: [$X, $Y $Z]
  rotation: [$W, $X, $Y, $Z]
  

  This is a problem while overwriting parts of the underlying configmap.
  Done -> f6f0c09

• [Done] zipped version of this format (ZSMURFA)
  Currently the entity_factory for ZSMURFA is not defined.
  -> Done with commit 1dbf3a4

• [Done] Sort all loaded entities before creating them, so that one's entities dependencies are loaded beforehand.
  -> Done with commit 16a33b1

[hwiedPro]

@malter: Please review commit f6f0c09. Is there a way to solve this more elegant with configmap methods?

[hwiedPro]

Set to WIP
--> merging with smurfs

[hwiedPro]

Loading another SMURFS into a first SMURFS now accepts parent and anchor definitions, too.
All entities in the second SMURFS, which are parented to the world, are transformed by the position and rotation given in the SMURFS include entry.
For SMURFS includes, anchor and parent entries from the include definition are only applied for entities parented to the world.

If a SMURFA is included the superior anchor/parent parameter is only applied to the entities defined as root.

[hwiedPro]

In testing...

[hwiedPro]

Corrected the smurfloader in such way that the entities are loaded in the order the smurfs files have been written.

It would be nice if it would load correctly independently from the order... --> To Do

[hwiedPro]

A further enhancement might be a field in a smurfa entry to define the type of the anchor joint. This also requires joint information, like limits and axis. This will be postponed until it is needed.

[malter]

Is the desired_id really useful? The whole application should work too, even if the id is not free. So, why is the desired_id required? In most cases where new id's are generated, the desired ones are not free anyway?

[hwiedPro]

This gives the possiblity to separate the ids and give the model a clearer structure, if not needed it can just be skipped.