goal.h

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2006 Alex Brooks, Alexei Makarenko, Tobias Kaupp
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef GOAL_H
00011 #define GOAL_H
00012 
00013 #include <cmath>
00014 #include <iostream>
00015 
00016 namespace orcalocalnav {
00017 
00018 //
00019 // Goal location in robot-centric Coord System
00020 //
00021 // @author Alex Brooks
00022 //
00023 class Goal 
00024 {
00025 public:
00026 
00027     // Use this for a particular goal
00028     void set( double x,
00029               double y,
00030               double theta,
00031               double distanceTolerance,
00032               double headingTolerance,
00033               double secRemaining,
00034               double intendedSpeed,
00035               double maxSpeed,
00036               double maxTurnrate );
00037 
00038     // Use this when no goal is active.
00039     void setNoGoal();
00040 
00041     double x, y, theta;
00042     double distanceTolerance;
00043     double headingTolerance;
00044 
00045     // Time remaining before we're supposed to be at this goal
00046     double timeRemaining;
00047 
00048     double intendedSpeed;
00049 
00050     // Constraints
00051     double maxSpeed;
00052     double maxTurnrate;
00053 };
00054 
00055 // Non-members    
00056 inline double distanceToGoal( const Goal &goal )
00057 { return hypot( goal.y, goal.x ); }
00058 inline double directionToGoal( const Goal &goal )
00059 { return atan2( goal.y, goal.x ); }
00060 inline bool translationalGoalPosReached( const Goal &goal )
00061 { return ( distanceToGoal(goal) < goal.distanceTolerance ); }
00062 inline bool rotationalGoalPosReached( const Goal &goal )
00063 { return fabs(goal.theta)<goal.headingTolerance; }
00064 inline bool goalPosReached( const Goal &goal )
00065 { return translationalGoalPosReached(goal) && rotationalGoalPosReached(goal); }
00066 inline bool goalTimeReached( const Goal &goal )
00067 { return goal.timeRemaining <= 0; }
00068 
00069 std::ostream &operator<<( std::ostream &s, const Goal &g );
00070 
00071 }
00072 
00073 #endif