Add a SimpleCarStatePublisher system that feeds from the PoseAggregator

[apojomovsky]

Follow up of #484

Quoting a comment from @stonier:

Since our coin of currency when talking to the visualiser is SimpleCarState, I wonder if we could write a publisher that publishes all SimpleCarStates off PoseAggregator. This might scale better for us.

• We should write a system that connects to thePoseBundle output from the PoseAggregator, calculates and publish an ignition SimpleCarState for each of the members of the pose bundle.
• Remove the SimpleCarState publication machinery that's currently managed from within the agent's.

[stonier]

@apojomovsky I had to unwind the system you put in when rebasing #481. I didn't realise at the time that this was direct from pose/velocity -> ignition simple car state message (I incorrectly assumed it was to a drake simple car state object). If I was going to accomodate that, I would have had to reach across and use that for all agents within their diagrams...which would have to be unwound when you do this anyway.

Some other bullets for your list:

• Drop the State output from AgentDiagramBuilder (here)
• Drop the existing DrakeSimpleCarStateToIgn system since it will no longer be used

[apojomovsky]

No problem @stonier , will do! Thanks for the heads up!

[apojomovsky]

@stonier , I've been looking at this issue and I ended up with two possible approaches.

First (but probably somewhat trickier) approach:

                         --------------                                     ------------
                        |  PoseBundle  | --> SimpleCarState (agent_1) -->  |    Ign     |
         PoseBundle --> |  toSimpleCar | --> SimpleCarState (agent_2) -->  | Publisher  |
                        |  state       | --> ..                            |            |
                         --------------  --> SimpleCarState (agent_n) -->   ------------

• A system that takes a PoseBundle as input and allocates as many outputs as agents available in the diagram at configure time (the number of agents is assumed to not change during the simulation lifespan).
• It would also require us to whether have an IgnitionPublisher for each output, or to modify it to accept multiple inputs of the same type (similarly to what the PoseAggregator does).

Second approach:

                     --------------                                         ------------
                    |  PoseBundle  |                                       |    Ign     |
     PoseBundle --> |  toSimpleCar | --> SimpleCarState_V (all agents) --> | Publisher  |
                    |  state       |                                       |            |
                     --------------                                         ------------

• The system would have a single output and it'll be of the type SimpleCarState_V, a custom type defined in delphyne that is just a vector composed of all the SimpleCarState, one for each agent.
• It would be simpler to initialize, since it would only involve a single input and a single output.

General caveats:

• Due to some limitations with ign_msgs/ign_transport, the state message would not be fully displayed in the TopicViewer (since it corresponds to a message that doesn't belong to ignition-msgs, etc.) We're already hitting this issue with the SimpleCarState, but with the current approach we can (at least) see the name of the agent's state publisher, if we use a SimpleCarState_V we'll not be able to see this. Should that issue be tackled before this one? Just ignore this one, this was fixed here but a problem with paths was avoiding us to see the message's payload (fixed here).
• Although sufficient for our purposes of visualization/plotting, the elimination of the State output from AgentDiagramBuilder would limit the state information published by complex cars like the MaliputRailcar since the specialized car state (like the MaliputRailcarState) won't be available anymore. Just wanted to ensure we're considering this consequence.

Thoughts?

[stonier]

Although sufficient for our purposes of visualization/plotting, the elimination of the State output from AgentDiagramBuilder would limit the state information published by complex cars like the MaliputRailcar since the specialized car state (like the MaliputRailcarState) won't be available anymore. Just wanted to ensure we're considering this consequence.

I don't expect we're limiting it (am I wrong?). The agents are free to do whatever publishing custom to their implementation inside the agent diagram. We're just worrying about the common parts getting published with this message.

[stonier]

Command Line Use Case

Despite being trickier, there is some appeal with the first approach for the command line use case. It dumps the state into the agent's ignition namespace (e.g. /agents/bob/state). If there are other publishers custom to that agent, it is likely they will be published in the same place. This is easy to find for debugging, recording, playback.

Visualiser Use Case

Having said that, I can understand it being easier for the visualiser if all agent information comes in one stream. However, if you're going to do this, then it might make sense to dump more into this box than just the 2D state (3D state, maliput lane are two things that come immediately to mind). This means this system would have to be wired up to additional components beyond the aggregator as we need them.

In this case, a splitter could separate out the components if we so wished.

Moving Forward

Might not have much choice if we can't view/plot the information in the visualiser. I believe this is the only place we use the information? Next user would be a display plugin that paints the car information in a box above the car (and subs. follows the car around).

[stonier]

I'll be conferring with @clalancette on various points with respect to the Agent Architecture today. I'll make this one of them and update you afterwards.

[apojomovsky]

I don't expect we're limiting it (am I wrong?). The agents are free to do whatever publishing custom to their implementation inside the agent diagram. We're just worrying about the common parts getting published with this message.

Hmm, that's true. You are right! I didn't saw the whole picture while writing the proposition.

[stonier]

Ok, conclusions from our huddle on the topic here:

• Yes to a vectorised publisher for the visualiser
• Yes to a splitter sitting after the vectorised publisher
• Yes to renaming to something better that expressed the idea of publishing generic agent information
• Yes to publishing 2D and 3D pose/velocity information in the message

Basically, doing both will cover both workflows. We'll be able to satisfy the need for the visualiser to consume a single source of information instead of having to co-ordinate multiple strands (this need is imminent), but also we'll not lose easy introspection / debugging capabilities (important given it's questionable whether that will be actually fixed).

The only thing to de-risk here is to determine how expensive the splitter is (take the case of our N=20 golden example, run it with the splitter, run it without).

Potentially the splitter could be running at a slower rate since it does not necessarily have to keep up with neither the simulation, nor the rendering rate (premise: for introspection and debugging only).

[hidmic]

@stonier @apojomovsky I think there are two sides to this. IMHO one thing is exposing agent state to the rest of the systems in the diagram (Drake semantics, intra-process), and another thing is exposing agent state to external applications (Ignition semantics, inter-process).

Here we focus on access for external applications. Some external applications may be interested in individual agents (e.g. for agent debugging), and some may be interested in the whole agent set (e.g. for visualizer annotations).

With #497, @apojomovsky provides a scalable solution for the latter use case. It keeps topic count low, which is great if you have +100 agents. As (standard, common) agent state gets more rich, we'll need further aggregation, but that's fine. It remains scalable.

We still need to provide for the former use case though. But then why not just selectively enabling each agent to do the publication on its own? It surely allows (full) agents state to be as complete and specific as desired. With few changes to the current state of things.

All in all, I think pursuing a one-size-fits-all type of solution is a bit of a stretch, considering we could "easily" do as above without crippling the user in any way whatsoever.

[hidmic]

(Posted above's before reading last @stonier reply, but still applies.)

[stonier]

A splitter after or included in the efficient vectorised publisher system would be nicer - it will be an operation common to all agents. Otherwise it's extra work for all agent development and I can imagine half of them eventually not realising or bothering with it since it doesn't break anything in its absence. At that point it will be frustratingly inconsistent.