Path.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Commander route path interface
00004  *
00005  *  Copyright (C) 2007, 2010, Austin Robot Technology
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: Path.h 435 2010-08-20 15:24:22Z jack.oquin $
00010  */
00011 
00012 #ifndef __PATH_H__
00013 #define __PATH_H__
00014 
00015 #include <deque>
00016 #include <iostream>
00017 
00018 #include <art/epsilon.h>
00019 #include <art/conversions.h>
00020 #include <art_map/zones.h>
00021 
00022 class Path
00023 {
00024 public:
00025   Path()
00026   {
00027     path.clear();
00028   };
00029   
00030   ~Path()
00031   {
00032     // invalidate any iterators
00033     path.clear();
00034   };
00035   
00036 
00037   // Return way-point from Path[index] relatively safely,
00038   // use last WayPointEdge if index too big,
00039   // return null WayPointEdge if Path was empty.
00040 
00041   WayPointEdge at(unsigned index) const
00042   {
00043     if (path.size() > index)
00044       return path.at(index);
00045     else if (path.size() > 0)
00046       return last();
00047     else
00048       return WayPointEdge();
00049   }
00050 
00051   void clear(void)
00052   {
00053     path.clear();
00054   }
00055 
00056   // return last WayPointN of Path
00057   WayPointEdge last(void) const
00058   {
00059     if (path.empty())
00060       return WayPointEdge();
00061     return path.at(path.size()-1);
00062   }
00063 
00064   void pop_front(void)
00065   {
00066     if (path.size() > 1)
00067       path.pop_front();
00068   }
00069 
00070   void new_path(waypt_index_t startid,
00071                 waypt_index_t endid,
00072                 const WayPointEdgeList& edges)
00073   {
00074     path.clear();
00075 
00076     append_path(startid, endid, edges);
00077 
00078   }
00079 
00080 
00081   void append_path(waypt_index_t startid,
00082                    waypt_index_t endid,
00083                    const WayPointEdgeList& edges)
00084   {
00085 
00086     if (edges.empty())
00087       {
00088         WayPointEdge new_edge;
00089         new_edge.startnode_index=startid;
00090         new_edge.endnode_index=endid;
00091         new_edge.speed_min=0;
00092         new_edge.distance=0.0;
00093         new_edge.speed_max=DEFAULT_ZONE_SPEED;
00094         path.push_back(new_edge);
00095         return;
00096       }
00097 
00098     /*    
00099     // If A* was run from inside a zone, then first edge touches exit
00100     // of zone, not inner waypoint.
00101     if (edges[0].startnode_index != startid) 
00102       {
00103         WayPointEdge new_edge;
00104         new_edge.startnode_index=startid;
00105         new_edge.distance=0.0;
00106         new_edge.endnode_index=edges[0].startnode_index;
00107         new_edge.speed_min=0;
00108         new_edge.speed_max=DEFAULT_ZONE_SPEED;
00109         path.push_back(new_edge);
00110       }
00111     */
00112     for (uint i=0; i < edges.size(); i++)
00113       {
00114         path.push_back(edges[i]);
00115         
00116       }
00117 
00118     /*
00119     // If A* was run to get into zone, then last edge goes to zone
00120     // entry, not to waypoint inside zone
00121     if (edges[edges.size()-1].endnode_index != endid) 
00122       {
00123         WayPointEdge new_edge;
00124         new_edge.startnode_index=edges[edges.size()-1].endnode_index;
00125         new_edge.distance=0.0;
00126         new_edge.endnode_index=endid;
00127         new_edge.speed_min=0;
00128         new_edge.speed_max=DEFAULT_ZONE_SPEED;
00129         path.push_back(new_edge);
00130       }
00131     */
00132 
00133   }
00134 
00135 
00136 
00137   // return number of way-points in Path
00138   unsigned size(void) const
00139   {
00140     return path.size();
00141   }
00142 
00143   // return whether Path is empty
00144   bool empty(void) const
00145   {
00146     return path.empty();
00147   }
00148 
00149   void print(const Graph& graph)
00150   {
00151     ROS_INFO("===============");
00152     for (unsigned i = 0; i < path.size(); i++)
00153       {
00154         if (i==0)
00155           ROS_INFO_STREAM("route [" << i << "] is "
00156                           << graph.get_node_by_index(path.at(i).startnode_index)->id.name().str);
00157 
00158         ROS_INFO_STREAM("route [" << i+1 << "] is "
00159                         << graph.get_node_by_index(path.at(i).endnode_index)->id.name().str);
00160       }
00161     ROS_INFO("================");
00162   }
00163 
00164 
00165 private:
00166   // hide the deque implementation
00167   std::deque<WayPointEdge> path;
00168 
00169 };
00170 
00171 #endif // __PATH_H__
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11