Pose: Difference between revisions
(One intermediate revision by the same user not shown) | |||
Line 37: | Line 37: | ||
Further coordinate systems can be added with the command like: | Further coordinate systems can be added with the command like: | ||
module load=odoPose alias myPose | module load=odoPose alias=myPose | ||
Then new functions are available using this alias name, like: | |||
myPose log # start logging | myPose log # start logging | ||
myPose pose # get most current pose in this coordinate system | myPose pose # get most current pose in this coordinate system | ||
Line 60: | Line 60: | ||
The history buffer can be used to determine | 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. | These functions are usually used directly using C++ calls to this resource. |
Latest revision as of 07:44, 9 May 2009
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.