00001
00002
00003
00004 #ifndef CAJUN_RNDF_LANE_UTIL_H
00005 #define CAJUN_RNDF_LANE_UTIL_H
00006
00007 #include "data_type.H"
00008 #include "rndf.H"
00009 #include "world_state.H"
00010 #include <string>
00011
00012 #define INVALID_HEADING -10000
00013
00014 namespace cajun
00015 {
00016 unsigned find_boundary_index_after_point (
00017 rndf_lane_data_t const &lb_, unsigned start_ind_,
00018 double x_, double y_, double &lx_, double &ly_,
00019 double &dis_from_lane_, double dis_off_,
00020 double dis_before_=1, double dis_after_=1,
00021 bool debug = false);
00022
00023 bool find_look_ahead_point (rndf_lane_data_t const &lb_,
00024 double x_, double y_,
00025 double &lad_,
00026 double &lx_, double &ly_,
00027 unsigned &wi_,
00028 double width_factor_);
00029
00030 bool find_xtrack_from_lane (rndf_lane_data_t const &lb_,
00031 double x_, double y_,
00032 double width_factor_,
00033 double &xtrack_dis_);
00034
00035 bool find_look_behind_point (rndf_lane_data_t const &lb_,
00036 double x_, double y_,
00037 double &lad_,
00038 double &lx_, double &ly_,
00039 unsigned &wi_,
00040 double width_factor_);
00041
00042 void get_filename_for_wp_pair (unsigned start_sid,
00043 unsigned start_lid,
00044 unsigned start_wid,
00045 unsigned end_sid, unsigned end_lid,
00046 unsigned end_wid,
00047 std::string &filename_);
00048
00049 bool get_linear_rndf_lanes (
00050 std::vector<rndf_lane_data_t> &lane_list_,
00051 rndf_t const *rndf_, double interp_dis_);
00052
00053 void get_lane_from_files ( std::vector<rndf_lane_data_t> &lane_list_,
00054 std::string lane_data_dir_,
00055 rndf_t const *rndf_,
00056 double interp_dis_);
00057
00058 bool get_lanes_around (world_state_t *ws_, double x_, double y_,
00059 std::vector<rndf_lane_data_t> &lane_list,
00060 double rad_);
00061
00062 void print_rndf_lane (rndf_lane_data_t const &lane_);
00063
00064 void print_rndf_lane (std::vector<rndf_lane_data_t> const &lanes_);
00065
00066 void classify_lanes (std::vector<rndf_lane_data_t> const &rndf_lanes_,
00067 std::vector<rndf_lane_data_t> ®ular_lanes_,
00068 std::vector<rndf_lane_data_t> &intersection_lanes_);
00069
00070 bool map_onto_rndf_lane (world_state_t *ws_, rndf_lane_data_t &lane_,
00071 double x_, double y_,
00072 double width_multiplier_,
00073 double bubble_rad_,
00074 double first_lane_seg_dis_before_,
00075 double dis_after_,
00076 double at_heading = INVALID_HEADING);
00077 bool map_onto_classified_lanes (
00078 double x_, double y_,
00079 std::vector<rndf_lane_data_t> const &rndf_lanes_,
00080 rndf_lane_data_t &best_lane_,
00081 std::vector<unsigned> &all_lane_indexes_,
00082 double width_multiplier_,
00083 double dis_before_, double dis_after_,
00084 double at_heading_ = INVALID_HEADING);
00085
00086 bool map_onto_rndf_lane_from_lanes (world_state_t *ws_,
00087 unsigned sid_,
00088 std::vector<unsigned> const &lid_list,
00089 unsigned &best_lid_,
00090 unsigned &wid_,
00091 double x_, double y_,
00092 double width_multiplier_,
00093 double bubble_rad_,
00094 double dis_after_);
00095
00096 bool map_onto_all_rndf_lane (
00097 world_state_t *ws_,
00098 std::vector <rndf_lane_data_t> &mapped_lane_list_,
00099 double x_, double y_, double width_multiplier_,
00100 double bubble_rad_, double dis_after_);
00101
00102 bool map_onto_given_rndf_lanes (
00103 double x_, double y_,
00104 std::vector<rndf_lane_data_t> const &rndf_lanes_,
00105 rndf_lane_data_t &best_lane_,
00106 std::vector<unsigned> &all_lane_indexes_,
00107 double width_multiplier_, double dis_before_,
00108 double dis_after_,
00109 double at_heading = INVALID_HEADING);
00110
00111 bool map_onto_rndf_lane_segment (
00112 world_state_t *ws_, rndf_t const *rndf_,
00113 double x_, double y_, unsigned sid_, unsigned lid_,
00114 unsigned &wid_, double max_dis_off_,
00115 double first_lane_dis_before_,
00116 double last_lane_dis_after_);
00117
00118 double distance_between_points (world_state_t *ws_, unsigned sid_,
00119 unsigned lid_, double x1_,
00120 double y1_, unsigned wid1_,
00121 double x2_, double y2_,
00122 unsigned wid2_);
00123
00124 double dis_along_rndf_lane (rndf_lane_data_t const &lb_,
00125 unsigned si_, unsigned ei_);
00126
00127 double rndf_lane_pre_dis (rndf_lane_data_t const &lb_,
00128 double first_lane_dis_before_);
00129
00130 double rndf_lane_post_dis (rndf_lane_data_t const &lb_,
00131 rndf_t const *rndf_,
00132 double last_lane_dis_after_);
00133
00134 double rndf_lane_heading_at (rndf_lane_data_t const &lb_,
00135 unsigned id_);
00136
00137 double distance_along_lane (rndf_lane_data_t const &lb_,
00138 double x1_, double y1_,
00139 double x2_, double y2_,
00140 double width_factor_);
00141
00142 double rndf_length_between (rndf_lane_data_t const &lb,
00143 unsigned start_i_, unsigned end_i_);
00144
00145
00146
00147 };
00148
00149
00150 #endif
