Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
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
00032 typedef int32_t poly_id_t;
00033 typedef int16_t segment_id_t;
00034 typedef int16_t lane_id_t;
00035 typedef int16_t point_id_t;
00036 typedef uint16_t waypt_index_t;
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
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
00147 {
00148 public:
00149 LatLong ll;
00150 MapXY map;
00151 ElementID id;
00152 waypt_index_t index;
00153
00154
00155 bool is_entry;
00156 bool is_exit;
00157 bool is_goal;
00158 bool is_lane_change;
00159 bool is_spot;
00160 bool is_stop;
00161 bool is_perimeter;
00162 int checkpoint_id;
00163 float lane_width;
00164
00165
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
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
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
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_ //
art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda,
Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34