Pose

Three static modules are available to maintain and service coordinate systems:

• odoPose intended for odometry pose as maintained from MRC (smrdemo)
• mapPose intended for a map coordinate system, e.g. oriented as a room, building or area coordinate reference
• utmPose intended to be maintained from a GPS and possibly backed by localization from known map positions in UTM coordinates (in WGS84 projection).

To add a module see (e.g. from the server window or in a script)

module help

To add all three coordinate system modules:

module add=mappose
module add=odoPose
module add=utmPose

Each coordinate system creates and maintains a set of global variables, including

time=0                 # time of last update (in sec since 1 jan 1970)
pose=450.2 -25.7 0.22  # Current pose (x y and heading)
vel=1.22               # Current velocity [m/s]
poseQ=0.7              # Pose quality (e.g. range 0..1)
     (for utmPose: 0=no data, 1=autonomous, 2=float, 3=fix, 4=DGPS)

A number of other values are maintained, including:

trip=466.12               # Total distance (should not be reset)
tripA=466.12              # Distance since reset - (mission distance)
tripB=466.12              # Distance since reset - (intended used by a mission rule)
estimateVel=0             # Estimate velocity from position updates
tripTime=401.1            # Time since restart [sec] (should not be reset)
tripTimeA=401.1           # Time since reset - (mission time)
tripTimeB=401.1           # Time since reset - (intended for use by the mission rule)
poseh5m=449.2 -25.6 0.15  # Average pose 1 m ago over the most recent 5 (but 1) meter
poseh5mUse=1              # Maintain the average 5m pose (1=yes, 0=no)

add a new coordinate system

Further coordinate systems can be added with the command like:

module load=odoPose alias myPose

The the then functions like

myPose log         # start logging
myPose pose        # get most current pose in this coordinate system
myPose help        # get help
myPosePush cmd="myPose pose"  # execute a command, when the pose in this coordinate system is updated

A set of variables will be maintained, as for the base coordinate systems.

Main function of pose information

The pose is information of robot position in som coordinate system at a point in time. The module stores a number of updates (currently 2000) and holds functions to maintain, query and do some calculations on this stack of positions. Update of the pose can trigger an event, that can be used to trigger some action, e.g. a sensor fusion update or transfer of pose to another server.

A few parameters determine if an update is stored as an update, determined on the movement and the time since last stored update. Default values are (if not changed):

minDist  =  0.03               // Adding update to history if distance is more than this
minTurn  =  1.0 * M_PI / 180.0 // Adding update to history if turned more than this (1 degree)
minTime  = 10.0                // Adding update to history if time passed is more than this

If either of these limitations are exceeded, then the new update is added to the update buffer. This means that if the distance criteria is the limiting criteria, then there could be 2000*0.03 = 60m of valid poses in the history buffer.

The history buffer can be used to determine

  * pose at a specific time (often used to determine pose at the time of some sensor data.)
  * pose at a distance from current position - e.g. 5m from current position, calculated as direct distance, not traveled distance.
  * Direct iteration into stored values.

These functions are usually used directly using C++ calls to this resource.

A number of global available values are maintained, such as the distance traveled. These are primarily intended to be used by a mission scheduler, as trip counter and time passed functions. See e.g.:

var mapPose

to get an overview of available global values available.

How to use (one of) the pose modules from your own plug-in

You can get access to the pose module using a resource pointer. The procedure is the same if you need it in an interface part of the plug-in or in a resource.

Create a pointer in the class definition to the common type UResPoseHist, e.g.:

#include <urob4/uresposehist.h>
...
protected:
UResPoseHist * mapPose;

This mapPose pointer needs to be initialized to NULL in the class constructor, as this usually is used as an indication of an invalid resource. e.g.:

UResYourClass::UResYourClass()
{
 setResID(getResID());
 resVersion = getResVersion();
 mapPose = NULL;
}

The server core will tell you about available resources - and changes in the resource situation, this is done with a call to the function setResource(UResBase * resource, bool remove), and if your program should listen to this call, to get the correct pointer, i.e. to get the mapPose pointer:

bool UResYourClass::setResource(UResBase * resource, bool remove)
{
  bool result = true;
  if (resource->isA(UResPoseHist::getMapPoseID()))   // *note
  { // ressource may be deleted or change
    if (remove)
      mapPose = NULL;
    else if (mapPose != resource)
      mapPose = (UResPoseHist *)resource;
    else
      result = false; // the resource is not neded by this class
  }
  // the base class may need the resource
  result &= UResVarPool::setResource(resource, remove);
  return result; // used or not
}

• note: The other alternatives are: UResPoseHist::getOdoPoseID() and UResPoseHist::getUtmPoseID() for the other coordinate systems.

Now the resource pointer is in place, ready for use.

Update with new positions

The main assumption is that one plugin maintains the pose and one or more other plugins (or the mission sequencer) uses the information.

To update the mapPose plugin, just provide a pose and the time when the pose is valid

UPoseTime pt;    // structure with x, y, h and time
UTime t;         // just time in linux time format (seconds since 1 jan 1970)
//
t.now();         // set time from linux clock
pt.set(xPosition, yPosition, headingInRadians); // set some pose
pt.t = t;        // add the time
mapPose->addIfNeeded(pt, 0.0, -12);  // add the new pose (if needed)

The call to addIfNeeded(...) has three parameters, the first is the pose, the second is an optional speed (not essential), the last is a number used to identify the updater - negative numbers are an update from a plugin, a positive number is a TCP/IP client. This number is available to ease debugging in the help text:

mapPose help

Use the resource pointer to get a pose

To get a pose will normally use getPoseAtTime(UTime atTime), like:

UPose pose;
UTime t;
//
t.now();
pose = mapPose->getPoseAtTime(t);

See the class documentation for UResPoseHist for other available methods and values.