Orca Robotics

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 maxSpeed,
00035               double maxTurnrate );
00036 
00037     // Use this when no goal is active.
00038     void setNoGoal();
00039 
00040     double x, y, theta;
00041     double distanceTolerance;
00042     double headingTolerance;
00043 
00044     // Time remaining before we're supposed to be at this goal
00045     double timeRemaining;
00046 
00047     // Constraints
00048     double maxSpeed;
00049     double maxTurnrate;
00050 };
00051 
00052 // Non-members    
00053 inline double distanceToGoal( const Goal &goal )
00054 { return hypot( goal.y, goal.x ); }
00055 inline double directionToGoal( const Goal &goal )
00056 { return atan2( goal.y, goal.x ); }
00057 inline bool translationalGoalPosReached( const Goal &goal )
00058 { return ( distanceToGoal(goal) < goal.distanceTolerance ); }
00059 inline bool rotationalGoalPosReached( const Goal &goal )
00060 { return fabs(goal.theta)<goal.headingTolerance; }
00061 inline bool goalPosReached( const Goal &goal )
00062 { return translationalGoalPosReached(goal) && rotationalGoalPosReached(goal); }
00063 inline bool goalTimeReached( const Goal &goal )
00064 { return goal.timeRemaining <= 0; }
00065 
00066 std::ostream &operator<<( std::ostream &s, const Goal &g );
00067 
00068 }
00069 
00070 #endif