$search
00001 /* -*- mode: C++ -*- */ 00002 /* 00003 * Description: global ART type definitions 00004 * 00005 * Copyright (C) 2009 Austin Robot Technology 00006 * 00007 * License: Modified BSD Software License Agreement 00008 * 00009 * $Id: types.h 614 2010-09-24 15:08:46Z jack.oquin $ 00010 */ 00011 00012 #ifndef _TYPES_HH_ 00013 #define _TYPES_HH_ 00014 00020 #include <stdio.h> 00021 #include <art/epsilon.h> 00022 #include <art_map/coordinates.h> 00023 #include <art_msgs/MapID.h> 00024 #include <art_msgs/WayPoint.h> 00025 00028 #define WAYPT_NAME_SIZE 20 00029 typedef struct {char str[WAYPT_NAME_SIZE];} waypt_name_t; 00030 00031 // some fixed-size types passed in messages 00032 typedef int32_t poly_id_t; //< polygon ID 00033 typedef int16_t segment_id_t; //< RNDF segment ID 00034 typedef int16_t lane_id_t; //< RNDF lane ID 00035 typedef int16_t point_id_t; //< RNDF way-point ID 00036 typedef uint16_t waypt_index_t; //< parser way-point index 00037 00038 enum Lane_marking 00039 {DOUBLE_YELLOW, SOLID_YELLOW, SOLID_WHITE, BROKEN_WHITE, UNDEFINED}; 00040 00042 class ElementID 00043 { 00044 public: 00045 segment_id_t seg; 00046 lane_id_t lane; 00047 point_id_t pt; 00048 00049 // constructors 00050 ElementID() 00051 { 00052 seg = lane = pt = -1; 00053 }; 00054 ElementID(segment_id_t _seg, lane_id_t _lane, point_id_t _pt) 00055 { 00056 seg = _seg; 00057 lane = _lane; 00058 pt = _pt; 00059 }; 00060 ElementID(art_msgs::MapID mid) 00061 { 00062 seg = mid.seg; 00063 lane = mid.lane; 00064 pt = mid.pt; 00065 }; 00066 00068 art_msgs::MapID toMapID() 00069 { 00070 art_msgs::MapID mid; 00071 mid.seg = this->seg; 00072 mid.lane = this->lane; 00073 mid.pt = this->pt; 00074 return mid; 00075 }; 00076 00077 waypt_name_t lane_name(void) const 00078 { 00079 waypt_name_t lanename; 00080 snprintf(lanename.str, sizeof(lanename.str), "%d.%d", seg, lane); 00081 return lanename; 00082 }; 00083 00084 waypt_name_t name(void) const 00085 { 00086 waypt_name_t wayname; 00087 snprintf(wayname.str, sizeof(wayname.str), "%d.%d.%d", seg, lane, pt); 00088 return wayname; 00089 }; 00090 00091 bool operator==(const ElementID &that) const 00092 { 00093 return (this->seg == that.seg 00094 && this->lane == that.lane 00095 && this->pt == that.pt); 00096 } 00097 00098 bool operator!=(const ElementID &that) const 00099 { 00100 return (this->seg != that.seg 00101 || this->lane != that.lane 00102 || this->pt != that.pt); 00103 } 00104 00105 bool operator<(const ElementID &that) const 00106 { 00107 if (this->seg != that.seg) 00108 return this->seg < that.seg; 00109 else if (this->lane != that.lane) 00110 return this->lane < that.lane; 00111 else return this->pt < that.pt; 00112 } 00113 00114 bool operator>(const ElementID &that) const 00115 { 00116 if (this->seg != that.seg) 00117 return this->seg > that.seg; 00118 else if (this->lane != that.lane) 00119 return this->lane > that.lane; 00120 else return this->pt > that.pt; 00121 } 00122 bool valid() const 00123 { 00124 return (seg >= 0 && lane >= 0 && pt >= 0); 00125 } 00126 bool same_lane(segment_id_t segid, lane_id_t laneid) const 00127 { 00128 return (seg == segid && lane == laneid); 00129 }; 00130 00131 bool same_lane(ElementID wayid) const 00132 { 00133 return (seg == wayid.seg && lane == wayid.lane); 00134 }; 00135 00136 waypt_name_t seg_name(void) const 00137 { 00138 waypt_name_t segname; 00139 snprintf(segname.str, sizeof(segname.str), "%d", seg); 00140 return segname; 00141 }; 00142 }; 00143 00144 typedef ElementID LaneID; 00145 00146 class WayPointNode //< way-point graph node 00147 { 00148 public: 00149 LatLong ll; //< latitude and longitude 00150 MapXY map; //< MapXY position 00151 ElementID id; //< way-point ID 00152 waypt_index_t index; //< parser index of waypoint 00153 00154 // way-point flags 00155 bool is_entry; //< lane or zone exit point 00156 bool is_exit; //< lane or zone entry point 00157 bool is_goal; //< this is a goal checkpoint 00158 bool is_lane_change; //< change lanes after here 00159 bool is_spot; //< parking spot 00160 bool is_stop; //< stop line here 00161 bool is_perimeter; //< zone perimeter point 00162 int checkpoint_id; //< checkpoint ID or zero 00163 float lane_width; 00164 00165 // constructors 00166 WayPointNode(){ clear();}; 00167 WayPointNode(const MapXY &point) : map(point) { clear();}; 00168 WayPointNode(const art_msgs::WayPoint &wp) 00169 { 00170 ll.latitude = wp.latitude; 00171 ll.longitude = wp.longitude; 00172 map = MapXY(wp.mapxy); 00173 id = ElementID(wp.id); 00174 index = wp.index; 00175 is_entry = is_entry; 00176 is_exit = is_exit; 00177 is_goal = is_goal; 00178 is_lane_change = is_lane_change; 00179 is_spot = is_spot; 00180 is_stop = is_stop; 00181 is_perimeter = is_perimeter; 00182 checkpoint_id = checkpoint_id; 00183 lane_width = lane_width; 00184 }; 00185 00186 // public methods 00187 void clear() 00188 { 00189 is_entry = is_exit = is_goal = is_spot = is_stop = false; 00190 is_perimeter = is_lane_change = false; 00191 checkpoint_id = index = 0; 00192 lane_width=0; 00193 id = ElementID(); 00194 }; 00195 00196 bool operator<(const WayPointNode &that) 00197 { 00198 return (this->id < that.id); 00199 }; 00200 00201 bool operator==(const WayPointNode &that) 00202 { 00203 return (this->ll == that.ll && 00204 this->map == that.map && 00205 this->id == that.id && 00206 this->index == that.index && 00207 this->lane_width == that.lane_width && 00208 this->is_entry == that.is_entry && 00209 this->is_exit == that.is_exit && 00210 this->is_goal == that.is_goal && 00211 this->is_spot == that.is_spot && 00212 this->is_perimeter == that.is_perimeter && 00213 this->checkpoint_id == that.checkpoint_id); 00214 }; 00215 00217 art_msgs::WayPoint toWayPoint(void) 00218 { 00219 art_msgs::WayPoint wp; 00220 wp.latitude = this->ll.latitude; 00221 wp.longitude = this->ll.longitude; 00222 wp.mapxy.x = this->map.x; 00223 wp.mapxy.y = this->map.y; 00224 wp.id = this->id.toMapID(); 00225 wp.index = this->index; 00226 wp.lane_width = this->lane_width; 00227 wp.is_entry = this->is_entry; 00228 wp.is_exit = this->is_exit; 00229 wp.is_goal = this->is_goal; 00230 wp.is_spot = this->is_spot; 00231 wp.is_perimeter = this->is_perimeter; 00232 wp.checkpoint_id = this->checkpoint_id; 00233 wp.lane_width = this->lane_width; 00234 return wp; 00235 }; 00236 00237 00238 }; 00239 00240 class WayPointEdge //< graph edge 00241 { 00242 public: 00243 waypt_index_t startnode_index; 00244 waypt_index_t endnode_index; 00245 float distance; 00246 float speed_max; 00247 float speed_min; 00248 00249 bool is_exit; 00250 bool blocked; 00251 bool is_implicit; 00252 00253 Lane_marking left_boundary; 00254 Lane_marking right_boundary; 00255 00256 //constructor 00257 WayPointEdge() { clear(); }; 00258 WayPointEdge(WayPointNode& node1, WayPointNode& node2, 00259 Lane_marking lb, Lane_marking rb, bool _is_exit) 00260 { 00261 startnode_index = node1.index; 00262 endnode_index = node2.index; 00263 left_boundary = lb; 00264 right_boundary = rb; 00265 is_exit = _is_exit; 00266 distance = -1.0; 00267 blocked = false; 00268 speed_max = speed_min = 0; 00269 is_implicit=false; 00270 }; 00271 bool operator==(const WayPointEdge &that) 00272 { 00273 return (this->startnode_index == that.startnode_index && 00274 this->endnode_index == that.endnode_index && 00275 this->distance == that.distance && 00276 Epsilon::equal(this->speed_max,that.speed_max) && 00277 Epsilon::equal(this->speed_min,that.speed_min) && 00278 this->is_exit == that.is_exit && 00279 this->left_boundary == that.left_boundary && 00280 this->right_boundary == that.right_boundary 00281 ); 00282 }; 00283 void clear(){ 00284 blocked = false; 00285 startnode_index = 0; 00286 endnode_index = 0; 00287 distance = -1.0; 00288 speed_max = speed_min = 0; 00289 is_exit = false; 00290 left_boundary = right_boundary = UNDEFINED; 00291 is_implicit=false; 00292 }; 00293 00294 WayPointEdge reverse() { 00295 WayPointEdge reverse(*this); 00296 00297 reverse.startnode_index = endnode_index; 00298 reverse.endnode_index = startnode_index; 00299 reverse.left_boundary = right_boundary; 00300 reverse.right_boundary = left_boundary; 00301 00302 return reverse; 00303 }; 00304 }; 00305 00306 #endif // _TYPES_HH_ //