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__