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 ORCA_PATHMAINTAINER_H
00011 #define ORCA_PATHMAINTAINER_H
00012 
00013 #include <orca/pathfollower2d.h>
00014 #include <orcaice/context.h>
00015 #include <hydronavutil/pose.h>
00016 #include "goal.h"
00017 
00018 namespace localnav {
00019 
00020 class PathFollowerInterface;
00021 class Clock;
00022 
00023 //
00024 // @author Alex Brooks
00025 //
00026 // This thing is responsible for keeping track of where we are in the path.
00027 // It doesn't need any mutexes, because all accesses are made by the MainLoop's thread.
00028 // (hence it is _not_ thread-safe).
00029 //
00030 class PathMaintainer
00031 {
00032 
00033 public: 
00034 
00035     PathMaintainer( PathFollowerInterface     &pathFollowerInterface,
00036                     const Clock               &clock,
00037                     const orcaice::Context    &context );
00038 
00039     //
00040     // Functions for the LocalNavManager
00041     //
00042 
00043     // Gets a list of up to maxNumGoals goals, the first
00044     // of which being the one currently sought, given that the robot
00045     // is at 'pose'.
00046     //
00047     // Goals are in robot's local coordinate system.
00048     // 
00049     // returns FALSE if there are no active goals, and TRUE otherwise
00050     bool getActiveGoals( std::vector<orcalocalnav::Goal> &goals,
00051                          int maxNumGoals,
00052                          const hydronavutil::Pose &pose );
00053 
00054     //
00055     // Functions called by MainLoop, to trigger comms with outside world
00056     //
00057 
00058     void checkForNewPath();
00059     void checkForWpIndexChange();
00060 
00061 private: 
00062 
00063     bool waypointReached( const orca::Waypoint2d &wp,
00064                           const hydronavutil::Pose &pose,
00065                           double timeNow );
00066     void  incrementWpIndex();
00067 
00068     // How long since path activation
00069     double secSinceActivation() const;
00070     
00071     // Keep a local copy of the path, so we don't have to mess with buffers every 
00072     // time the nav manager wants to look at it.
00073     orca::PathFollower2dData path_;
00074     
00075     // The index of the next waypoint being seeked.
00076     int wpIndex_;
00077 
00078     // If the index changes, we need to tell the world.
00079     bool wpIndexChanged_;
00080 
00081     // The timing of the entire path is relative to this
00082     orca::Time pathStartTime_;
00083 
00084     PathFollowerInterface &pathFollowerInterface_;
00085 
00086     // The global time
00087     const Clock &clock_;
00088 
00089     orcaice::Context context_;
00090 };
00091 
00092 }
00093 
00094 #endif
pathmaintainer.h
