Publish static transforms describing the staging area in world frame.

[peci1]

Solves #665.

Add publishing of the following static transforms to ROS:

• world->artifact_origin
• world->base_station
• world->staging_area

This is required e.g. to be able to precisely forbid exploration of the staging area after the robot left it. The relative positions of these three objects are not always the same, thus the transforms cannot be hard-coded.

Different from /subt/pose_from_artifact, this doesn't expose the Ignition services to the ROS interface, it just reads values from Ignition via service calls, and when the transform is known, publishes it as a static TF on the ROS side.

[peci1]

This might benefit from using the subt::kArtifactOrigin etc. constants from subt_ign in the subt_ros part, but that would create a circular dependency.

There's also room for simplifying the code if I could add build dependency of subt_ros on ros_ign_bridge. That would allow using the ign_to_ros() conversion function. I'm not sure if that's desired, as the original pose_from_artifact code also doesn't utilize it.

[osrf-jenkins]

Build finished. 15 tests run, 0 skipped, 0 failed.

[mjcarroll]

I think we may need some clarification against the rules, since it looks like this would replace the use of /subt/pose_from_artifact_origin. I believe the intention was to always make competitor robots use that service to get the artifact_origin location before leaving the staging area.

From the API page:

The artifact pose should be reported relative to the artifact origin. At this point, there's no physical object to represent the artifact origin. Instead, robots can query the ROS service /subt/pose_from_artifact_origin that returns the robot pose relative to the artifact origin. Note that this service is only available if the robot is in the staging area.

[peci1]

Well, if I understand it correctly, this shouldn't interfere with the rules. It doesn't add the possibility for a robot to query its own position in a different manner. What it adds is some "parent" or "neighbor" frames to artifact_origin. I'll try to explain why is which transform needed.

1. staging_area->artifact_origin: We want to be able to "mask out" the staging area from the areas allowed for exploration. If we don't know this, we have to somehow assume/derive the position of the area from the positions of the robots. That's mostly possible, but why not have it exactly. This would be needed both at competition and in testing.
2. base_station->artifact_origin: We'd want to know precisely where the base station is to have a better idea how to build the comms relay network, where to put the first breadcrumb and so on. Again, its position can be somehow estimated relative to the robot poses (or even detected visually), but it'd help to have the authoritative transform. Both at competition and in testing.
3. world->artifact_origin: This would only be helpful in testing when using ground truth. As the ground truth is published as transform world->robot, and all other transforms we have are artifact_origin->something, it's apparent that connecting these two subtrees is impossible without knowing the world->artifact_origin transform. In cave, it was the same for all worlds, but that doesn't hold generally. This transform would also be needed in case 1. and 2. are connected via the world frame (which they are as of now in this PR, but it might be possible to instead allow querying staging_area and base_station from /subt/pose_from_artifact_origin).

[peci1]

@adlarkin I've updated this PR with master (no conflifts).

This is the TF tree it produces:

[peci1]

I think merging this PR could still be desirable.