Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PilotTypes.h

Go to the documentation of this file.
00001 #ifndef DEFINED_PilotTypes_h_
00002 #define DEFINED_PilotTypes_h_
00003 
00004 #include <iostream>
00005 
00006 #include "DualCoding/Point.h"
00007 #include "DualCoding/ShapeRoot.h"
00008 #include "Planners/Navigation/ShapeSpacePlannerXYTheta.h"
00009 
00010 namespace DualCoding{
00011   namespace PilotTypes {
00012 
00013     typedef ShapeSpacePlannerXYTheta::NodeType_t NodeType_t;
00014     typedef NodeType_t::NodeValue_t NodeValue_t;
00015 
00016   //! What we're asking the Pilot to do
00017   enum RequestType_t {
00018     localize, //!< Localize using the available landmarks
00019     walk, //!< Walk the distance specified by @a dx / @a dy / @a da
00020     waypointWalk, //!< Execute the trajectory in @a waypointlist
00021     setVelocity, //!< Set velocity for continuous walking
00022     goToShape, //!< Plan a path to @a targetShape
00023     pushObject, //!< Push @a objectSjape to @a targetShape
00024     visualSearch, //!< Turn until search predicate returns true
00025     setOdometry, //!< Turn head and set the visual odometry settings
00026     noRequest, //!< Dummy case
00027     numRequestTypes
00028   };
00029 
00030   extern const char* RequestTypeNames[numRequestTypes];
00031 
00032   //! What error the Pilot is going to return
00033   enum ErrorType_t {
00034     noError = 0,        //!< Request completed without error
00035     someError,          //!< Generic error will match anything but noError in a PiotTrans
00036     invalidRequest,     //!< Ill-formed PilotRequest, such as mssing parameters
00037     abort,              //!< Request was aborted
00038     cantLocalize,       //!< Localization failed
00039     startCollides,      //!< Start state would be in collision
00040     endCollides,        //!< End state would be in collision
00041     noPath,             //!< Path planning failed
00042     noSpace,    //!< Not enough space to maneuver
00043     collisionDetected,  //!< The robot hit something
00044     searchFailed,       //!< Visual search gave up
00045     numErrorTypes
00046   };
00047 
00048   extern const char* ErrorTypeNames[numErrorTypes];
00049 
00050   //! Options for handling collisions
00051   enum CollisionAction_t {
00052     collisionIgnore = 0,     //!< Don't check for collisions
00053     collisionReport,         //!< Report the collision in a PilotEvent, but continue
00054     collisionStop,           //!< Stop the robot and terminate the request
00055     collisionReplan          //!< Try to recover and proceed
00056   };
00057 
00058   enum NavigationStepType_t {
00059     localizeStep,     //!< localize during path following
00060     travelStep,       //!< travel forward during path following
00061     turnStep,         //!< turn to face next point on path
00062     preTravelStep,    //!< adjust heading before travel forward
00063     headingStep,      //!< turn to specific heading (at end of plan)
00064     turnObjStep,      //!< turn while pushing an object
00065     acquireObjStep    //!< move to acquire contact with object
00066   };
00067 
00068   struct NavigationStep {
00069     NavigationStep(NavigationStepType_t _type, Point _waypoint, AngTwoPi _orientation, AngSignTwoPi _turn): 
00070       type(_type), waypoint(_waypoint), orientation(_orientation), turn(_turn), visibleLandmarks() {};
00071     NavigationStepType_t type;
00072     Point waypoint;
00073     AngTwoPi orientation;
00074     AngSignTwoPi turn;
00075     std::vector<ShapeRoot> visibleLandmarks;
00076     std::string toString() const;
00077   };
00078 
00079   std::ostream& operator<<(std::ostream& os, const NavigationStep &step);
00080 
00081   //! Stores and indexes into a sequence of actions to complete a navigation task
00082   struct NavigationPlan {
00083     NavigationPlan() : path(), walkBackward(false), steps(), currentStep() {};
00084     std::vector<NodeValue_t> path;   //!< Path returned by path planner, from which the navigation plan is generated
00085     bool walkBackward; //!< If true, use heading opposite the direction of travel
00086     std::vector<NavigationStep> steps;     //!< The plan is a sequence of navigation steps
00087     std::vector<NavigationStep>::const_iterator currentStep;   //!< Index into the plan
00088     std::string toString() const;
00089     void addNavigationStep(NavigationStepType_t type, const DualCoding::Point &waypoint,
00090                            AngTwoPi orientation, AngSignTwoPi turn, const std::vector<ShapeRoot> &landmarks);
00091     void addNavigationStep(NavigationStepType_t type, const NodeValue_t &waypoint,
00092                            const std::vector<ShapeRoot> &landmarks);
00093     void clear() {
00094       path.clear();
00095       walkBackward = false;
00096       steps.clear();
00097       currentStep = steps.begin();
00098     }
00099   };
00100 
00101   std::ostream& operator<<(std::ostream& os, const NavigationPlan &plan);
00102 
00103   static const unsigned int invalid_Pilot_ID = -1U;
00104 
00105 } } // end of namespaces
00106 
00107 #endif

DualCoding 5.1CVS
Generated Mon May 9 04:56:27 2016 by Doxygen 1.6.3