Pose

Introduction

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

Then new functions are available using this alias name, 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.

Maintained pose history

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.

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.

To get newest pose, or a pose at a given time, use something like:

#include <urob4/uresposehist.h>
...
void UResYourResource::yourFunction() 
{
  UResPoseHist * mapPose;
  UPoseTime currentPose;
  UPose pose;
  UTime t;
  //
  mapPose = (UResPoseHist *) getStaticResource("mapPose", false);
  if (mapPose != NULL)
    currentPose = mapPose->getNewest();
  ....
  t.now();  // current time
  t -= 5.0; // 5 seconds ago
  pose = mapPose->getPoseAtTime(t); // map pose 5 seconds ago
 }

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, just provide a pose and the time when the pose is valid

UPoseTime pt;    // structure with x, y, h and time
UTime t;         // linux time (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 plug-in, a positive number is a TCP/IP client. This number is then available to ease debugging in the help text:

mapPose help

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