00001
00002
00003
00004 #ifndef CAJUN_PLANNER_UTIL_H
00005 #define CAJUN_PLANNER_UTIL_H
00006
00007 #include "rndf.H"
00008 #include "data_type.H"
00009
00010 #include "world_state.H"
00011
00012 #include "planner_defs.H"
00013 #include "task_planner.H"
00014 #include "rndf_point.H"
00015
00016 #define MIN_ATRACK 0.0
00017
00018
00019 namespace cajun
00020 {
00021 void append_path (path_t &target_path_, const path_t &source_path_);
00022
00023 bool waypt_reached (rndf_t const *rndf_, double x_, double y_,
00024 rndf_t::waypoint_t const &w_,
00025 double width_multiplier_, double parking_radius_,
00026 double min_atrack_ = MIN_ATRACK);
00027 bool zone_spot_waypt_reached (rndf_t const *rndf_, double x_,
00028 double y_, rndf_t::waypoint_t const &w_,
00029 double parking_radius_);
00030
00031 bool point_reached (rndf_t const *rndf_, double px_, double py_,
00032 double x_, double y_, rndf_t::waypoint_t const &w_,
00033 double width_multiplier_);
00034
00035 bool covered_by_path (path_t const &path_, double p_width_, double x_,
00036 double y_);
00037
00038 void linear_interpolation (std::vector <path_wp_t> &inter_points_,
00039 double interp_dis_,
00040 double start_x_, double start_y_,
00041 double end_x_, double end_y_);
00042
00043 bool point_on_path (const path_t &path_, double x_, double y_, unsigned &index_before_, unsigned &index_after_,
00044 double dis_before_path_,
00045 double dis_after_path_, double dis_off_path_,
00046 bool debug_ = false);
00047
00048 bool compute_end_point (double start_x_, double start_y_,
00049 double &end_x_, double &end_y_,
00050 relative_side_t &side_,
00051 rndf_lane_data_t &next_lb_,
00052 double width_multiplier_,
00053 double look_ahead_ratio_, bool same_dir_);
00054
00055 void trace_back_path (const path_t *path_, unsigned ref_i,
00056 unsigned &trace_back_i, double &trace_dis_);
00057
00058 void copy_path (path_t &source_p_, path_data_t &des_p_);
00059
00060 void copy_path (path_t &source_p_, path_t &des_p_);
00061
00062 void copy_path (double x_, double y_, path_t const &src_path_,
00063 path_t &des_path_, double dis_before_,
00064 double dis_after_, double dis_off_path_);
00065
00066
00067 bool copy_path (std::vector <path_wp_t> &inter_points_,
00068 path_t &path_, double &p_len_left_,
00069 unsigned start_i_, unsigned end_i_,
00070 double min_speed_,double max_speed_);
00071
00072 bool copy_path (rndf_lane_data_t &lb_, path_t &path_, double &p_len_left_,
00073 unsigned start_i, unsigned end_i, double min_speed_,
00074 double max_speed_);
00075
00076 path_status_t add_rndf_lane_to_path (rndf_lane_data_t &lb_,
00077 path_t &path_, double &p_len_left_,
00078 double end_x_,
00079 double end_y_, double min_speed_,
00080 double max_speed_,
00081 bool reverse_,
00082 double width_multiplier_,
00083 double dis_before_ = 1,
00084 double dis_after_ = 1);
00085
00086 path_status_t add_path_to_switch_lane (path_t &path_,
00087 double &p_len_left_,
00088 relative_side_t &side_,
00089 rndf_lane_data_t &lb_,
00090 bool same_dir_,
00091 double min_speed_,
00092 double max_speed_,
00093 double m_width_multiplier,
00094 double m_look_ahead_ratio);
00095
00096 void set_last_speed_zero (path_t &path_);
00097
00098 void reset_zero_speed (path_t &path_);
00099
00100 void filter_path (path_t &p_);
00101
00102 void filter_path (path_data_t &p_);
00103
00104 void extend_path (path_t &path_, double dis_, double inter_dis_);
00105 void extend_path (path_t &path_, double dis_, double inter_dis_,
00106 double orient_);
00107
00108
00109 bool passing_lane_id (rndf_t const *rndf_, unsigned sid_,
00110 unsigned lid, unsigned &pasing_lid_);
00111
00112 bool before_intersection_within_dis (world_state_t *ws_, rndf_t const *rndf_,
00113 unsigned sid_, unsigned lid_,
00114 double x_, double y_,
00115 double for_dis_,
00116 bool stop_intersection_);
00117
00118 bool after_intersection_within_dis (rndf_t const *rndf_,
00119 unsigned sid_, unsigned lid_,
00120 double x_, double y_,
00121 double for_dis_);
00122
00123 bool lane_blocked_for_dis (world_state_t *ws_, unsigned sid_,
00124 unsigned lid_, double x_, double y_,
00125 double for_dis_, bool forward_,
00126 bool for_static_);
00127
00128 bool task_planner_for_mission (rndf_t const *rndf_,
00129 rndf_point_t *wp_id_1_,
00130 rndf_point_t *wp_id_2_,
00131 std::vector<tp_package_t> &tp_seq_);
00132
00133
00134 bool zone_task_planner_for_mission (
00135 rndf_t const *rndf_, rndf_point_t *wp_id_1_,
00136 rndf_point_t *wp_id_2_,
00137 std::vector<tp_package_t> &tp_seq_);
00138
00139 bool have_to_stop_at_intersection (rndf_t const *rndf_,
00140 rndf_point_t *rp_1_,
00141 rndf_point_t *rp_2_);
00142
00143 double intersection_turn_angle (rndf_t const *rndf_,
00144 rndf_point_t *exit_point_,
00145 rndf_point_t *entry_point_);
00146
00147 bool is_exit_entry_pair (rndf_t const *rndf_,
00148 rndf_t::waypoint_id_t const &exit_wp_id,
00149 rndf_t::waypoint_id_t const &entry_wp_id);
00150
00151 bool is_exit_entry_pair (rndf_t const *rndf_,
00152 rndf_point_t const *exit_rp,
00153 rndf_point_t const *entry_rp);
00154
00155 double lane_width (rndf_t const *rndf_, unsigned sid_, unsigned lid_);
00156
00157 bool danger_entry_point (rndf_t const *rndf_,
00158 rndf_t::waypoint_id_t const &exit_wp_,
00159 rndf_t::waypoint_id_t const &entry_wp_,
00160 std::vector<rndf_t::waypoint_id_t> &danger_points_);
00161
00162 bool intersecting_intersection_legs (
00163 rndf_t const *rndf_,
00164 rndf_t::waypoint_id_t const &exit_wp_id_,
00165 rndf_t::waypoint_id_t const &entry_wp_id_,
00166 std::vector<rndf_t::lane_seg_t> &lane_list_);
00167
00168 bool comes_before (world_state_t *ws_,
00169 double x1_, double y1_, double x2_, double y2_,
00170 unsigned sid_, unsigned lid_, unsigned wid_,
00171 double width_multiplier_);
00172
00173 bool blk_on_lane_segment ( world_state_t *ws_,
00174 rndf_t::waypoint_id_t const &start_wp_id_,
00175 rndf_t::waypoint_id_t const &end_wp_id_,
00176 double static_, double valid_blk_since_);
00177
00178 bool point_on_lane (
00179 world_state_t *ws_,
00180 rndf_t const *rndf_,
00181 unsigned sid_, unsigned passing_lid_,
00182 double x_, double y_,
00183 double &new_x_, double &new_y_, unsigned &wid_before,
00184 double width_multiplier_,
00185 double first_lane_dis_before_,
00186 double last_lane_dis_after_);
00187
00188 void set_speed_limit (path_t const &base_path_, path_t &path_,
00189 double min_speed_, double max_speed_);
00190
00191 double path_end_orient (path_t const &path_);
00192
00193 bool find_a_entry_to_zone (rndf_t const *rndf_, unsigned zone_id,
00194 unsigned &entry_wid_);
00195
00198 bool consecutive_rndf_points (rndf_point_t *rp1_,
00199 rndf_point_t *rp2_);
00200
00201 bool last_tp_planned (task_planner_t *tp_);
00202
00203 void set_path_speed (path_t &path_, double min_speed_,
00204 double max_speed_);
00205 void print_path (path_t const &path_);
00206
00207 bool point_on_confined_path (const path_t &path_, double x_, double y_,
00208 unsigned starti_, unsigned endi_,
00209 unsigned &index_before_, unsigned &index_after_,
00210 double dis_before_path_,
00211 double dis_after_path_,
00212 double dis_off_path_, bool debug_ = false);
00213
00214 };
00215
00216 #endif
