inFuse

h1. 03/14-15 : PoM/Prism Discussion (details)

*Participants :* 

Simon, Ellon, Quentin, Pierre

*Goal of the meeting :* Discuss the PoM/Prism Details. 

*Notes :* 

* After the first meeting, Prism design appeared more complicated than previously thought. Quentin and Pierre met afterward to think about a potential solution.

* Pose and transform are used interchangeably in the following and are always accompanied with a parent frame id, a child frame id and an uncertainty matrix.

* In the following, the word "Produce" mean a node create data but not necessarily "Publish" it for other nodes (Example : sensors node produce raw data but only publish stamped data). 

 

 

h2. 1. Summary of "starting point"

 

*1. "Mandatory" nodes producing raw data*

* Joint nodes : produce and publish poses between joint base and joint head (a "Joint node" here can represent a succession of several hardware joints, like an arm with several hinges. But a joint node has to publish only a single pose from the base to the head. Make several joint nodes if every hinge position has to be passed to Prism. */!\* Even a joint node representing a simple hinge has to publish a whole pose : [translation vector, rotation quaternion, uncertainty matrix, parent and child frame id ]).

* Sensor nodes : produce raw sensor data (point clouds, etc...) .

 

*2. "Mandatory" nodes receiving processed data*

* Localization DFPCs, receives stamped data : raw sensor data stamped with a time stamp and a pose(RobotBase->Sensor)

* MightlyLocalizer (part of POM) receives time stamp : time stamp at sensor data acquisition date.

 

*3. Constraints*

* Centralized time : Every time stamp has to come from a single node : POM (Both Prism and MightyLocalizer ? Or only one of the two ? More on that below).

* Publish/Subscribe only : No direct services between nodes allowed.

* Sensors only know their frame id and are not managing transform propagation between sensor frame and robot frame. These are managed elsewhere (Prism).

 

 

h2. 2. Prism design, second draw (see last meeting for first draw)

 

*Prism goal :* 

* Managing the internal transform tree of the robot (only for sensors). Receives information about sensor mount joint positions and update the internal tree. Publish poses(robotBase->sensors)

* Publish time stamp at sensor data acquisition (for use in sensor nodes and POM).

These two information have to be used by sensor nodes for them to publish time and pose stamped data. The problem is if it is Prism which has to publish time stamp at sensor data acquisition (as decided in the last meeting), the sensor node has to notify Prism when raw data has been produced. This will be problematic with only publish/subscribe communication.

 

After discussion (Quentin, Pierre), it appeared easier to let the sensor node publish a time stamp for POM instead of prism. (in addition to publish time stamped data for localization nodes). Prism will publish time stamped poses at (relatively) high frequency for sensor nodes regardless of data being produces by sensor nodes or not. This way a sensor node can read at data acquisition the last time stamped sensor pose published by Prism and proceed to publish stamped data, and a time stamp related only to a data acquisition.

!Prism2.png!

h2. 3. Prism implementation details

 

h3. ASN.1 messages

 

*Joint to Prism : ASN.1 Transform*

<pre><code class="Cpp">

ASN.1 Transform

{

    Vec3d position

    Quaterniond orientation

    Matrix6d cov

    T-String parentFrame

    T-String childFrame

};

</code></pre>

 

*Prism to sensors : ASN.1 Pom2Sensor :*

<pre><code class="Cpp">

ASN.1 Pom2Sensor

{

    Time ts

    SEQUENCE(SIZE (1...NbSensorMax)) OF Transform

};

</code></pre>

Note : Prism publish an array of sensor poses. It is up to the sensor to choose the correct sensor pose (the sensor now its own frameId)

 

*Sensor to Pom: ASN.1 Sensor2Pom :*

<pre><code class="Cpp">

ASN.1 Pom2Sensor

{

    Time ts

    T-String childFrame (= sensorFrame)

};

</code></pre>

 

h3. Prism implementation

<pre><code class="Cpp">

class Prism

{

    attributes:

    EnvireGraph _internalRobotGraph;

    BitStream(ASN1_Transform)* JointStateBuffer; //(circular buffer of ASN1 bitstreams to handle both high and low frequency joint pose publishing)

    ASN1_Transform(*) JointState;

    ASN1_Pom2Sensor sensorPoses*;

    members:

    updateGraph(jointState)

    decode_message(ASN1 bitstream) //decode jointState messages

    encode_messages                // encode array of sensor poses

};

</code></pre>