00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00019 #ifndef __RNDF_h__
00020 #define __RNDF_h__
00021
00022 #include <vector>
00023 #include <string>
00024 #include <iterator>
00025 #include <map>
00026
00027 #include <art_map/Graph.h>
00028
00029 template <class T>
00030 void print_vector (std::vector<T> vec);
00031
00032
00033 class LL_Waypoint {
00034 public:
00035 int waypoint_id;
00036 LatLong ll;
00037
00038
00039
00040
00041 LL_Waypoint(std::string line, int x, int y, int line_number, bool& valid,
00042 bool verbose);
00043 bool isvalid(){return(waypoint_id > 0);};
00044 void clear(){ waypoint_id = -1; ll.latitude = ll.longitude = -1.0;};
00045 void print(){print_without_newline(); printf("\n");};
00046 void print_without_newline(){
00047 printf("Waypoint %d, Latitude: %f, Longitude: %f",
00048 waypoint_id, ll.latitude, ll.longitude);
00049 };
00050 };
00051
00052 typedef LL_Waypoint Perimeter_Point;
00053
00054
00055 class Checkpoint {
00056 public:
00057 int checkpoint_id;
00058 int waypoint_id;
00059
00060
00061 Checkpoint(){};
00062
00063 Checkpoint (std::string line, int x, int y, int line_number, bool& valid,
00064 bool verbose);
00065 bool isvalid(){return (waypoint_id > 0 && checkpoint_id > 0 );};
00066 void clear(){ waypoint_id = checkpoint_id = -1;};
00067 void print(){
00068 printf("Checkpoint Number %d is at Waypoint %d\n",
00069 checkpoint_id, waypoint_id);
00070 };
00071 };
00072
00073 class Unique_id{
00074 public:
00075 int waypoint_id;
00076 int lane_id;
00077 int segment_id;
00078
00079
00080 bool isvalid(){return (waypoint_id > 0 && lane_id >= 0 && segment_id > 0);};
00081 void clear();
00082 void print(){printf("%d.%d.%d", segment_id, lane_id, waypoint_id);};
00083 };
00084
00085 class Exit {
00086 public:
00087 Unique_id start_point;
00088 Unique_id end_point;
00089 Exit(std::string line, int x, int y, int line_number, bool& valid,
00090 bool verbose);
00091 bool isvalid(){ return (start_point.isvalid() && end_point.isvalid());};
00092 void clear(){start_point.waypoint_id = end_point.waypoint_id
00093 = end_point.lane_id = end_point.segment_id = -1;};
00094 void print();
00095 };
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114 class Stop{
00115 public:
00116 int waypoint_id;
00117 bool isvalid(){return (waypoint_id > 0);};
00118 void clear(){waypoint_id = -1;};
00119 void print(){printf("Stop at Waypoint %d\n", waypoint_id);};
00120
00121 Stop (std::string line, int x, int y, int line_number, bool& valid, bool verbose);
00122 };
00123
00124
00125 class Lane{
00126 public:
00127 int lane_id;
00128 int number_of_waypoints;
00129 std::vector<LL_Waypoint> waypoints;
00130
00131 int lane_width;
00132 Lane_marking left_boundary;
00133 Lane_marking right_boundary;
00134 std::vector<Checkpoint> checkpoints;
00135 std::vector<Stop> stops;
00136 std::vector<Exit> exits;
00137
00138
00139 bool isvalid(){return (lane_id > 0 && number_of_waypoints > 0
00140 && lane_width >= 0
00141 && number_of_waypoints == (int)waypoints.size());};
00142 void clear();
00143 void print();
00144 };
00145
00146
00147 class Segment{
00148 public:
00149 int segment_id;
00150 int number_of_lanes;
00151
00152 std::string segment_name;
00153 std::vector<Lane> lanes;
00154
00155
00156 bool isvalid(){return (segment_id > 0 && number_of_lanes > 0
00157 && number_of_lanes == (int)lanes.size());};
00158 void clear();
00159 void print();
00160 };
00161
00162
00163 class Perimeter{
00164 public:
00165 int perimeter_id;
00166
00167 int number_of_perimeterpoints;
00168
00169 std::vector<Exit> exits_from_perimeter;
00170 std::vector<Perimeter_Point> perimeterpoints;
00171
00172
00173 bool isvalid(){return (perimeter_id == 0 && number_of_perimeterpoints > 0
00174 && number_of_perimeterpoints == (int)perimeterpoints.size());};
00175 void clear();
00176 void print();
00177 };
00178
00179
00180 class Spot{
00181 public:
00182 int spot_id;
00183
00184 int spot_width;
00185 Checkpoint checkpoint;
00186 std::vector<LL_Waypoint> waypoints;
00187
00188
00189 bool isvalid(){return (spot_id > 0 && spot_width >= 0);};
00190 void clear();
00191 void print();
00192 };
00193
00194
00195
00196 class Zone{
00197 public:
00198 int zone_id;
00199 int number_of_parking_spots;
00200
00201 std::string zone_name;
00202
00203 Perimeter perimeter;
00204 std::vector<Spot> spots;
00205
00206
00207 bool isvalid(){return(zone_id > 0 && number_of_parking_spots >= 0
00208 && number_of_parking_spots == (int)spots.size());};
00209 void clear();
00210 void print();
00211 };
00212
00213
00214 class Speed_Limit {
00215 public:
00216 int id;
00217 int min_speed;
00218 int max_speed;
00219
00220 bool isvalid(){return(id > 0 && min_speed >= 0 && max_speed >= 0);};
00221 void clear(){id = min_speed = max_speed = -1;};
00222 void print();
00223
00224 Speed_Limit(std::string line, int line_number, bool& valid, bool verbose);
00225 Speed_Limit(){clear();};
00226
00227 bool operator==(const Speed_Limit &that)
00228 {
00229 return (this->id == that.id
00230 && this->min_speed == that.min_speed
00231 && this->max_speed == that.max_speed);
00232 }
00233
00234 };
00235
00236 class RNDF {
00237 public:
00238
00239 std::string filename;
00240 int number_of_segments;
00241 int number_of_zones;
00242
00243
00244 std::string format_version;
00245 std::string creation_date;
00246
00247 std::vector<Segment> segments;
00248 std::vector<Zone> zones;
00249
00250
00251 RNDF(std::string rndfname, bool verbose=false);
00252 ~RNDF() {};
00253
00254 void populate_graph(Graph& graph);
00255 void print();
00256
00257 bool is_valid;
00258
00259 private:
00260 bool isvalid(){return(number_of_segments > 0 && number_of_zones >= 0
00261 && number_of_segments == (int) segments.size()
00262 && number_of_zones == (int) zones.size());};
00263 struct id_comparator {
00264 bool operator()(const ElementID e1, const ElementID e2) const {
00265 if (e1.seg != e2.seg)
00266 return e1.seg < e2.seg;
00267 else if (e1.lane != e2.lane)
00268 return e1.lane < e2.lane;
00269 else return e1.pt < e2.pt;
00270 }
00271 };
00272 typedef std::map<ElementID, WayPointNode, id_comparator> id_to_waypoint_map;
00273
00274 id_to_waypoint_map id_map;
00275
00276 std::vector<WayPointEdge> edges;
00277
00278 void prep_graph();
00279
00280 int line_number;
00281 Lane_marking parse_boundary(std::string line, bool& valid);
00282 };
00283
00284 class MDF {
00285 public:
00286 std::string filename;
00287 std::string RNDF_name;
00288
00289
00290 std::string format_version;
00291 std::string creation_date;
00292
00293
00294 int number_of_checkpoints;
00295 std::vector<int> checkpoint_ids;
00296
00297 int number_of_speedlimits;
00298 std::vector<Speed_Limit> speed_limits;
00299
00300 void print();
00301 bool add_speed_limits(Graph& graph);
00302 MDF(std::string mdfname, bool verbose=false);
00303 ~MDF(){};
00304
00305 bool is_valid;
00306 private:
00307
00308 int line_number;
00309 Speed_Limit parse_speedlimits(std::string line);
00310 bool isvalid(){return (number_of_checkpoints > 0 && number_of_speedlimits > 0
00311 && number_of_checkpoints == (int) checkpoint_ids.size()
00312 && number_of_speedlimits == (int) speed_limits.size());};
00313 };
00314
00315
00316 std::string parse_string(std::string line, std::string token,
00317 int line_number, bool& valid, bool verbose);
00318 int parse_integer(std::string line, std::string token,
00319 int line_number, bool& valid, bool verbose);
00320 int parse_integer(std::string line, int line_number, bool& valid, bool verbose);
00321
00322 void checkpoint_error(int seg, int lane, int way);
00323 void stop_error(int seg, int lane, int way);
00324 void exit_error(Exit& exit);
00325 void print_error_message(int line_number, std::string token);
00326
00327 #endif