Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Pilot Class Reference

The Pilot is the top-level navigation engine for Tekkotsu. More...

#include <Pilot.h>

Inheritance diagram for Pilot:

Detailed Description

The Pilot is the top-level navigation engine for Tekkotsu.

The Pilot accepts and executes PilotRequest instances.

Navigation involves a combination of path planning, odometry, and landmark-based localization. The Pilot uses the ShapeSpacePlannerXYTheta planner to find a path from the robot's current position to a target location that avoids collisions with worldShS shapes marked as obstacles.

Once a path is found, the Pilot formulates a navigation plan consisting of a series of travel steps, turn steps, and localization steps. The navigation planner is somewhat clever about where it places localization steps in order to avoid slowing down the robot's progress too much, but additional optimizations are possible.

Finally, the Pilot executes the navigation plan. Because both motion and odometry are subject to error, the robot can drift off course as it travels. When this is detected via a localization step, the execution engine attempts a correction by inserting a new turn step in the navigation plan and altering the travel step that follows.

Definition at line 88 of file Pilot.h.

List of all members.

Classes

struct  BearingLessThan
 Functor used to sort gaze points by their relative bearing from the robot. More...
class  ClearOldPath
class  CollisionChecker
 Collision checker service; its behavior is model-dependent. More...
class  CollisionDispatch
 Used by Pilot walk machines to decide how to respond to a collision. More...
class  ConstructNavPlan
 Construct a navigation plan: mixture of traveling, turning, and localizing steps. More...
class  Dispatch
class  ExecutePlan
 Execute a navigation plan; used for both goToShape and pushObject. More...
class  GoToShapeMachine
class  LocalizationMachine
class  LocalizationUtility
 State machine called by various Pilot functions to perform a landmark search and localization. More...
class  LookForObject
class  LookForTarget
class  PlanPath
class  PushObjectMachine
class  ReportCompletion
class  RunMotionModel
 Run the particle filter's motion model every few hundred milliseconds to provide odometry. More...
class  RunOpticalFlow
 Compute optical flow for visual odometry. Currently disabled. More...
class  SetOdometryMachine
class  SetupLandmarkExtractor
class  SetVelocityMachine
class  TerminateDueToCollision
 Terminate a walk operation and indicate that a collision was responsible. More...
class  VisualSearchMachine
class  WalkMachine
class  WaypointWalkMachine

Public Types

typedef unsigned int PilotVerbosity_t

Public Member Functions

 Pilot (const std::string &nodename="Pilot")
virtual void doStart ()
virtual void doStop ()
virtual void doEvent ()
void unwindForNextRequest ()
unsigned int executeRequest (BehaviorBase *requestingBehavior, const PilotRequest &req)
void executeRequest ()
void requestComplete (PilotTypes::ErrorType_t errorType)
void requestComplete (const PilotEvent &e)
void pilotPop ()
void pilotAbort ()
void changeVelocity (float vx, float vy=0, float va=0)
 Change velocity for walk or setVelocity requests.
void cancelVelocity ()
 forces completion of a setVelocity operation
void setWorldBounds (float minX, float width, float minY, float height)
void setWorldBounds (const Shape< PolygonData > &bounds)
 Set world bounds to constrain the path planner.
void randomizeParticles (float widthIncr=1000, float heightIncr=1000)
 Randomize the particles before localization.
void computeParticleBounds (float widthIncr=0, float heightIncr=0)
 Compute bounds for random particle distribution based on world shapes.
void setDefaultLandmarks (const std::vector< ShapeRoot > &landmarks)
const std::vector< ShapeRoot > & getDefaultLandmarks ()
void setDefaultLandmarkExtractor (const MapBuilderRequest &mapreq)
MapBuilderRequestgetDefaultLandmarkExtractor () const
MotionManager::MC_ID getWalk_id () const
MotionManager::MC_ID getWaypointwalk_id () const
void setupLandmarkExtractor ()
void setAgent (const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true)
 Pilot's setAgent is called by its RunMotionModel behavior. Or user can call directly.
virtual void setup ()

Static Public Member Functions

static PilotVerbosity_t getVerbosity ()
static void setVerbosity (PilotVerbosity_t v)
static std::vector< ShapeRootcalculateVisibleLandmarks (const DualCoding::Point &currentLocation, AngTwoPi currentOrientation, AngTwoPi maxTurn, const std::vector< DualCoding::ShapeRoot > &possibleLandmarks)
 Find the landmarks that should be visible from current location.
static bool isLandmarkViewable (const DualCoding::ShapeRoot &selectedLandmark, const DualCoding::Point &currentLocation, AngTwoPi currentOrientation, AngTwoPi maxTurn)
static bool defaultLandmarkExitTest ()
 Returns true if we have enough landmarks to localize.

Static Public Attributes

static const PilotVerbosity_t PVstart = 1<<0
static const PilotVerbosity_t PVevents = 1<<1
static const PilotVerbosity_t PVexecute = 1<<2
static const PilotVerbosity_t PVsuccess = 1<<3
static const PilotVerbosity_t PVfailure = 1<<4
static const PilotVerbosity_t PVcomplete = 1<<5
static const PilotVerbosity_t PVabort = 1<<6
static const PilotVerbosity_t PVpop = 1<<7
static const PilotVerbosity_t PVqueued = 1<<8
static const PilotVerbosity_t PVcollision = 1<<9
static const PilotVerbosity_t PVnavStep = 1<<10
static const PilotVerbosity_t PVshowPath = 1<<11

Private Member Functions

 Pilot (const Pilot &)
 copy constructor; do not call
Pilotoperator= (const Pilot &)
 assignment operator; do not call

Private Attributes

std::queue< PilotRequest * > requests
 Queue of Pilot requests.
PilotRequestcurReq
unsigned int idCounter
 Pilot request counter for assigning a unique id.
MotionManager::MC_ID walk_id
MotionManager::MC_ID waypointwalk_id
Dispatchdispatch_
std::vector< ShapeRootdefaultLandmarks
MapBuilderRequestdefaultLandmarkExtractor
Point initPos
AngTwoPi initHead
PilotRequest planReq
NavigationPlan plan
AngTwoPi maxTurn
bool pushingObj

Static Private Attributes

static PilotVerbosity_t verbosity

Member Typedef Documentation

typedef unsigned int PilotVerbosity_t

Definition at line 92 of file Pilot.h.


Constructor & Destructor Documentation

Pilot ( const std::string &  nodename = "Pilot"  ) 

Definition at line 90 of file Pilot.h.

Pilot ( const Pilot  )  [private]

copy constructor; do not call


Member Function Documentation

static std::vector<ShapeRoot> calculateVisibleLandmarks ( const DualCoding::Point currentLocation,
AngTwoPi  currentOrientation,
AngTwoPi  maxTurn,
const std::vector< DualCoding::ShapeRoot > &  possibleLandmarks 
) [static]

Find the landmarks that should be visible from current location.

void cancelVelocity (  ) 

forces completion of a setVelocity operation

void changeVelocity ( float  vx,
float  vy = 0,
float  va = 0 
)

Change velocity for walk or setVelocity requests.

void computeParticleBounds ( float  widthIncr = 0,
float  heightIncr = 0 
)

Compute bounds for random particle distribution based on world shapes.

static bool defaultLandmarkExitTest (  )  [static]

Returns true if we have enough landmarks to localize.

This could be much more sophisticated: taking landmark geometry into account so we don't settle for two adjacent landmarks if a more widely dispersed set is available.

virtual void doEvent (  )  [virtual]

Reimplemented from BehaviorBase.

virtual void doStart (  )  [virtual]

Reimplemented from BehaviorBase.

virtual void doStop (  )  [virtual]

Reimplemented from BehaviorBase.

void executeRequest (  ) 
unsigned int executeRequest ( BehaviorBase requestingBehavior,
const PilotRequest req 
)
MapBuilderRequest* getDefaultLandmarkExtractor (  )  const

Definition at line 143 of file Pilot.h.

const std::vector<ShapeRoot>& getDefaultLandmarks (  ) 

Definition at line 140 of file Pilot.h.

static PilotVerbosity_t getVerbosity (  )  [static]

Definition at line 106 of file Pilot.h.

MotionManager::MC_ID getWaypointwalk_id (  )  const
static bool isLandmarkViewable ( const DualCoding::ShapeRoot selectedLandmark,
const DualCoding::Point currentLocation,
AngTwoPi  currentOrientation,
AngTwoPi  maxTurn 
) [static]
Pilot& operator= ( const Pilot  )  [private]

assignment operator; do not call

void pilotAbort (  ) 
void pilotPop (  ) 
void randomizeParticles ( float  widthIncr = 1000,
float  heightIncr = 1000 
)

Randomize the particles before localization.

void requestComplete ( const PilotEvent e  ) 
void requestComplete ( PilotTypes::ErrorType_t  errorType  ) 

Referenced by Check::doStart().

void setAgent ( const Point loc,
AngTwoPi  heading,
bool  quiet = false,
bool  updateWaypoint = true 
)

Pilot's setAgent is called by its RunMotionModel behavior. Or user can call directly.

void setDefaultLandmarkExtractor ( const MapBuilderRequest mapreq  ) 
void setDefaultLandmarks ( const std::vector< ShapeRoot > &  landmarks  ) 
virtual void setup (  )  [virtual]

Reimplemented from StateNode.

Definition at line 2271 of file Pilot.h.

void setupLandmarkExtractor (  ) 
static void setVerbosity ( PilotVerbosity_t  v  )  [static]

Definition at line 107 of file Pilot.h.

void setWorldBounds ( const Shape< PolygonData > &  bounds  ) 

Set world bounds to constrain the path planner.

void setWorldBounds ( float  minX,
float  width,
float  minY,
float  height 
)
void unwindForNextRequest (  ) 

Member Data Documentation

Definition at line 2308 of file Pilot.h.

Referenced by Pilot::getDefaultLandmarkExtractor().

std::vector<ShapeRoot> defaultLandmarks [private]

Definition at line 2307 of file Pilot.h.

Referenced by Pilot::getDefaultLandmarks().

Dispatch* dispatch_ [private]

Definition at line 2303 of file Pilot.h.

unsigned int idCounter [private]

Pilot request counter for assigning a unique id.

Definition at line 2300 of file Pilot.h.

AngTwoPi maxTurn [private]
const PilotVerbosity_t PVabort = 1<<6 [static]

Definition at line 99 of file Pilot.h.

const PilotVerbosity_t PVcollision = 1<<9 [static]

Definition at line 102 of file Pilot.h.

const PilotVerbosity_t PVcomplete = 1<<5 [static]

Definition at line 98 of file Pilot.h.

const PilotVerbosity_t PVevents = 1<<1 [static]

Definition at line 94 of file Pilot.h.

const PilotVerbosity_t PVexecute = 1<<2 [static]

Definition at line 95 of file Pilot.h.

const PilotVerbosity_t PVfailure = 1<<4 [static]

Definition at line 97 of file Pilot.h.

const PilotVerbosity_t PVnavStep = 1<<10 [static]

Definition at line 103 of file Pilot.h.

const PilotVerbosity_t PVpop = 1<<7 [static]

Definition at line 100 of file Pilot.h.

const PilotVerbosity_t PVqueued = 1<<8 [static]

Definition at line 101 of file Pilot.h.

const PilotVerbosity_t PVshowPath = 1<<11 [static]

Definition at line 104 of file Pilot.h.

const PilotVerbosity_t PVstart = 1<<0 [static]

Definition at line 93 of file Pilot.h.

const PilotVerbosity_t PVsuccess = 1<<3 [static]

Definition at line 96 of file Pilot.h.

std::queue<PilotRequest*> requests [private]

Queue of Pilot requests.

Definition at line 2298 of file Pilot.h.

PilotVerbosity_t verbosity [static, private]

Definition at line 2296 of file Pilot.h.

Referenced by Pilot::getVerbosity(), and Pilot::setVerbosity().


The documentation for this class was generated from the following file:

DualCoding 5.1CVS
Generated Mon Jan 25 19:31:45 2016 by Doxygen 1.6.3