View source for Coordinate systems

Three coordinate systems are implemented at present:

* Odometry coordinates, this coordinate system may be a result of sensor fusion of high relative accuracy, but not sensors with an absolute measurement error relative to any coordinate system. That is, coordinates from a SLAM, Guidemark, Map or GPS may not be merged in this coordinate system. This coordinate system should be trustworthy when correlating local sensor information from a number of previous robot positions - e.g. image correlation or laser scan correlation.

* UTM coordinates, assumed to be available from one (or more) GPS receiver(s).
* Map coordinates, representing the best estimate of robot position in a map coordinate system. The map coordinate system may  be identical with UTM coordinates.

All coordinate systems are implemented in the same code, and thus has the same facilities.
These are:
* Providing the newest robot position and update time.
* Ability to log these updates (in e.g. odoPose.log)
* Ability to replay from a logfile
* Stores up to 2000 history poses - covers some 70m with the default update settings
* Can search in the history to get average heading or a position at a specific time or distance from current position.

The code is library code, so the functionality can be accessed by establishing a pointer directly to the resource modules.

'''Odometry pose'''

The odometry coordinate system is maintained by MRC (smrdemo) as shown in this figure

[[Image:odoPose-update.png]]

MRC (smrdemo) connects to the camera server (port 24920) and the laser scanner server (port 24919) if these are available when smrdemo is started.
If these two servers has the 'odoPose' module loaded, then this is automatically maintained with the odometry coordiante system.

If other servers - like the client or the behaviour server - needs an odometry position too, then this can be obtained from one of these two maintained servers. Obtaining updates from the laser scanner server:
 >> module load=odoPose
 >> module load=if alias=laser
 >> laser add=var
 >> laser connect=localhost:24919
 >> laser odoPosePush cmd="odoPose pose"

This sequence of commands first loads the odometry pose module 'odoPose', then an interface module given the name 'laser'.
To this interface module a data handler 'var' is added handling global variables and pose updates.
Then the interface is connected to the laser scanner server.
The final line specifies a command that is to be executed on the laser scanner server when the odometry pose is updated. When triggerd the newest pose (and time) is send to this server. The 'var' data handler forwards the pose to the odoPose module.

A more safe way of keeping the odometry pose updated is this slightly modified command sequence:
 >> module load=odoPose
 >> module load=if alias=laser
 >> laser add=var
 >> laserOnConnect cmd="laser odoPosePush flush"
 >> laserOnConnect cmd="laser odoPosePush cmd='odoPose pose'"
 >> laser connect=localhost:24919

The difference is that the update command is send to the laser scanner server whenever the laser scanner gets online. That is, if the laser scanner is loaded later, then the command sequence is send at that time. If this server was down and is restarted, then the laser scanner may not have noticed that the connection was down, and thus pushes odometry positions already, to avoid  double commands, any existing commands (from this client) is flushed first.

The behaviour server is assumed to establish a direct connection to the smrdemo port (e.g. 31001). This connection can be used to maintain the odometry coordinate as well. This could be established with the following command set:

 >> module load=odoPose
 >> module load=smr
 >> smr connect=localhost:31001
 >> smr stream=1

Assuming the MRC is started with the command
 $ ./smrdemo -t1

Once this is established, the behaviour server may act as source of odometry pose updates.


'''UTM pose'''

The UTM pose is intended to be maintained from a GPS module, so to get this running issue the following commads:
 >> module load=gps
 >> module load=utmPose
 >> gps dev="/dev/ttyUSB0" speed=9600
 >> gps open
 >> gps openLog

The commands shown should match the GPS on the MMR.

An alternative source is the RSK-GPS interfaces into MRC (smrdemo), the data from this can be streamed from the server holding the smrdemo connection.
The following script should ensure this UTM source (in addition to the odoPose) to be available:
 >> module load=utmPose
 >> module load=smr
 >> smr connect=localhost:31001
 >> smr stream=1

This loads the needed modules and starts the sreaming at every update interval - this gps is available on the HAKO tractor only, and here the update interval is 100ms.

NB! the UTM pose is a x,y (easting, northing) coordinate only, the heading and velocity can not be assumed to be valid. The coordinates are as received from the source, and no (additional) smoothing is performed.

'''Map pose'''

The map pose is intended for localization on an apriory map, where the coordinate system may or may not be in UTM coordinates. The assumed method is that a localization module combines the UTM position and sensor data with the map, and from this determines the most likely Map position, and load this into the Map coordinate system.

The code to update the map coordinate system is:
 {
  UPoseTime pt;
  double speed = 0.0;
  double x,y,th, time;
  UTime t;
  ...
  if (mapPose != NULL)
  {
   pt.x = x;
   pt.y = y;
   pt.h = th;
   t.now();
   pt.t = t;
   mapPose->addIfNeeded(pt, speed, -9);
  }
  ...
 }

Where the 'mapPose' is a pointer directly to the mapPose resource, the pointer should be defined in the resource class:

 class UResLocalization : public UResVarPool
 {
  public:
  ...
  bool setResource(UResBase * resource, bool remove);
  ...
  protected:
  UResPoseHist * mapPose;
 }

.. and will be set by the 'setResource(..)' function when the mapPose module is loaded, by:

 bool UResLocalization::setResource(UResBase * resource, bool remove)
 {
  bool result = true;
  if (resource->isA(UResPoseHist::getMapPoseID()))
  { // ressource may change
    if (remove)
      mapPose = NULL;
    else if (mapPose != resource)
      mapPose = (UResPoseHist *)resource;
    else
      result = false;
  }
  result &= UResVarPool::setResource(resource, remove);
  return result;
 }