diff --git a/README.md b/README.md index 5018fa4b..3cf0f7e3 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ * 编译规则使用cmake； * 程序运行不再依赖docker，而是可以直接在ubuntu 18/20运行； * 适配c++17； -* 增加快速碰撞检测系统，gjk/epa/bvh，由张玉博士、我在2019-2020年开发，可以参考(https://arxiv.org/abs/2011.09117); +* 增加快速碰撞检测系统，gjk/epa/bvh，由张玉博士、我在2019-2020年开发，可以参考(https://arxiv.org/abs/2011.09117)，可以将系统耗时2秒降低为20毫秒； * 增加2d的交互界面，用来实时调试系统状态； * 增加仿真底盘，用来测试决策规划控制; * 增加path decision调试工具; @@ -15,6 +15,14 @@ * 增加record解析脚本； * 增加log文件解析脚本； * 通过cyber rt c++/python版本，实时订阅cyber 消息，并且完成解析，可以实时分析问题或者单步调试问题； +* hmi可以自主添加障碍物，并且可以控制障碍物的运动状态； +* hmi可以自主控制红绿灯，并且可以发布红绿灯； +* 增加路由点的配置文件，自动化读取； +* 跟车不使用单一的跟车距离，引入了st point of interest,来生成可变的可行驶区域; +* 限速优化，限速构成的边界考虑到车辆速度，不然速度优化经常无解； +* 修复routing lane change的bug； + + # 软件依赖 基本库： diff --git a/modules/planning/tasks/deciders/speed_decider/speed_decider.cc b/modules/planning/tasks/deciders/speed_decider/speed_decider.cc index c6adb504..efb72597 100644 --- a/modules/planning/tasks/deciders/speed_decider/speed_decider.cc +++ b/modules/planning/tasks/deciders/speed_decider/speed_decider.cc @@ -91,118 +91,6 @@ common::Status SpeedDecider::Execute(Frame* frame, record_constraints(reference_line_info); - // 识别是否在car follow scenario - - bool has_follow_decision = false; - bool is_only_following_decision = true; - double following_min_s = 200.0; - std::string following_obs_id; - - double stopping_min_s = 200.0; - std::string stop_obs_id; - - for (const auto* obstacle : - reference_line_info->path_decision()->obstacles().Items()) - { -#if debug_speed_decider - AINFO << obstacle->DebugString(); - -#endif - - if (obstacle->IsVirtual()) - { - continue; - } - - if (obstacle->LongitudinalDecision().has_stop()) - { - is_only_following_decision = false; - - if (obstacle->path_st_boundary().min_s() < stopping_min_s) - { - stopping_min_s = obstacle->path_st_boundary().min_s(); - stop_obs_id = obstacle->Id(); - } - } - - if (obstacle->LongitudinalDecision().has_overtake() || - obstacle->LongitudinalDecision().has_yield()) - { - is_only_following_decision = false; - } - - if (obstacle->LongitudinalDecision().has_follow()) - { - has_follow_decision = true; - - if (obstacle->path_st_boundary().min_s() < following_min_s) - { - following_min_s = obstacle->path_st_boundary().min_s(); - following_obs_id = obstacle->Id(); - } - } - } - -#if debug_speed_decider - AINFO << "is car follow scenario " << has_follow_decision; - -#endif - - SpeedDecision speed_decision; - speed_decision.Clear(); - - if (has_follow_decision && is_only_following_decision) - { - speed_decision.set_type(INTERACTION_SECENERIO_FOLLOW); - - reference_line_info->set_close_following_obstacle(following_obs_id); - - // update time headway - - double time_headway; - - double adc_speed = reference_line_info->get_veh_linear_velocity(); - if (adc_speed < 0.1) - { - time_headway = 1000.0; - } - else - { - time_headway = following_min_s / adc_speed; - } - - speed_decision.set_time_headway(time_headway); - - Obstacle* close_following_obs = - reference_line_info->get_mutable_close_following_obstacle(); - - if (close_following_obs != nullptr) - { - - close_following_obs->set_time_of_headway(time_headway); - - double ttc = 1000.0; - - if (close_following_obs->speed() < adc_speed && adc_speed > 0.1) - { - ttc = following_min_s / - (adc_speed - close_following_obs->speed()); - } - - close_following_obs->set_time_to_collision(ttc); - speed_decision.set_ttc(ttc); - } - - reference_line_info->set_speed_decision(speed_decision); - - AINFO << "print_adc_time_headway:" - << "(" << time_headway << "," - << ")"; - } - else - { - reference_line_info->clear_following_obstacle(); - } return Status::OK(); } @@ -635,9 +523,6 @@ Status SpeedDecider::MakeObjectDecision(const SpeedData& speed_profile, << ", obs id: " << obstacle->Id() << ", st curve is below"; - // will be retired if bev perception fix reverse obs - continue; - ObjectDecisionType stop_decision; if (create_stop_decision_for_reverse_obs(*mutable_obstacle, @@ -710,8 +595,6 @@ Status SpeedDecider::MakeObjectDecision(const SpeedData& speed_profile, << ", obs id: " << obstacle->Id() << ", st curve is cross"; - // will be retired if bev perception fix reverse obs - continue; ObjectDecisionType stop_decision; diff --git a/modules/planning/tasks/deciders/speed_decider/yield_decider.cc b/modules/planning/tasks/deciders/speed_decider/yield_decider.cc index e138a1ce..07281398 100644 --- a/modules/planning/tasks/deciders/speed_decider/yield_decider.cc +++ b/modules/planning/tasks/deciders/speed_decider/yield_decider.cc @@ -19,113 +19,14 @@ int estimate_yield_poi_by_two_phase_brake_curve(ObjectYield* yield_decision, const double emergency_brake_dist) { - YieldAccMode ego_acc_mode; - yield_interaction_info hard_brake_interaction_info; - yield_interaction_info soft_brake_interaction_info; - - double ego_yield_min_gap = 100.0; - - // double min_gap; - double emergency_brake_curve_min_gap; - double soft_brake_curve_min_gap; - double end_poi_gap; - - // 计算point of interset, 包含起点，end point, min gap point - // init yield_decision->set_distance_s(-FLAGS_yield_distance); - yield_decision->mutable_yield_poi()->set_has_interaction(false); - - // update first phase - estimate_st_gap_poi_by_st(&hard_brake_interaction_info, obstacle, - hard_brake_speed_data); - - if (hard_brake_interaction_info.has_has_interaction() && - hard_brake_interaction_info.min_gap_point().has_s_gap()) - { - emergency_brake_curve_min_gap = - hard_brake_interaction_info.min_gap_point().s_gap(); - } - else - { - AERROR << "no st collision info"; - - return 0; - } - - if (emergency_brake_curve_min_gap <= 0.0) - { - ego_yield_min_gap = 1.0; - ego_acc_mode = YIELD_ACC_MODE_HARD_BRAKE_AND_WILL_COLLISION; - } - else - { - estimate_st_gap_poi_by_st(&soft_brake_interaction_info, obstacle, - soft_brake_speed_data); - - if (soft_brake_interaction_info.has_has_interaction() && - soft_brake_interaction_info.min_gap_point().has_s_gap()) - { - soft_brake_curve_min_gap = - soft_brake_interaction_info.min_gap_point().s_gap(); - } - else - { - AERROR << "no st collision info"; - - return 0; - } - - if (soft_brake_curve_min_gap <= 0.0) - { - ego_yield_min_gap = std::min(FLAGS_yield_distance, - emergency_brake_curve_min_gap); - - ego_acc_mode = YIELD_ACC_MODE_HARD_BRAKE; - } - else if (soft_brake_curve_min_gap > emergency_brake_dist) - { - ego_yield_min_gap = - std::min(FLAGS_yield_distance, soft_brake_curve_min_gap); - - ego_acc_mode = YIELD_ACC_MODE_NOT_SURE; - } - else - { - ego_yield_min_gap = - std::min(FLAGS_yield_distance, soft_brake_curve_min_gap); - - ego_acc_mode = YIELD_ACC_MODE_SOFT_BRAKE; - - end_poi_gap = soft_brake_interaction_info.end_point().s_gap(); - - end_poi_gap = std::min(end_poi_gap, FLAGS_yield_distance); - - yield_interaction_info* yield_poi = - yield_decision->mutable_yield_poi(); - - yield_poi->set_has_interaction(true); - - yield_poi->mutable_end_point()->set_s_gap(end_poi_gap); - - yield_poi->mutable_end_point()->set_time( - soft_brake_interaction_info.end_point().time()); - } - } - - yield_decision->set_yield_acc_mode(ego_acc_mode); - yield_decision->set_distance_s(-ego_yield_min_gap); - - #if debug_yield_decider const STBoundary& st_bound = obstacle.path_st_boundary(); - AINFO << "obs id: " << obstacle.Id() - << ", yield acc mode: " << YieldAccMode_Name(ego_acc_mode) - << ", yield min gap: " << ego_yield_min_gap - << ", emergency_brake_dist " << emergency_brake_dist; + AINFO << "obs id: " << obstacle.Id(); AINFO << "start collision, t,s: " << st_bound.min_t() << " , " << st_bound.min_s(); @@ -217,9 +118,6 @@ int estimate_proper_yield_poi(ObjectYield* yield_decision, } } - // ego speed is not small - // https://qwybs7wggx.feishu.cn/docx/NVBMdiKtJo3Qu4xJtnacrFran7b - estimate_yield_poi_by_two_phase_brake_curve( yield_decision, obstacle, adc_speed, adc_acc, path_length, hard_brake_speed_data, soft_brake_speed_data, stop_dist); @@ -227,118 +125,6 @@ int estimate_proper_yield_poi(ObjectYield* yield_decision, return 0; } -int estimate_st_gap_poi_by_st(yield_interaction_info* interaction_info, - const Obstacle& obstacle, - const SpeedData* adc_speed_data) -{ - interaction_info->set_has_interaction(false); - - if (adc_speed_data == nullptr) - { - AERROR <<"speed data is null"; - - return 0; - } - - const STBoundary& st_bound = obstacle.path_st_boundary(); - - if (adc_speed_data->size() < 1 || st_bound.IsEmpty()) - { - AERROR << "speed data is invalid or no collision with obstacle"; - - return 0; - } - - double time; - double unit_time = 0.1; - - double obs_s_upper; - double obs_s_lower; - - double ego_s; - common::SpeedPoint ego_speed_point; - - bool has_interaction; - bool has_speed_point; - - double cur_gap; - - // init gap point - - double min_gap_point_t; - double min_gap_s; - - double end_point_t; - double end_point_s_gap; - - min_gap_point_t = 0.0; - min_gap_s = 1000.0; - - bool has_first_interaction_point = false; - - // init - time = -0.1; - - for (size_t i = 0; i < 70; i++) - { - time += unit_time; - - has_interaction = - st_bound.GetBoundarySRange(time, &obs_s_upper, &obs_s_lower); - - if (!has_interaction) - { - continue; - } - - has_speed_point = adc_speed_data->EvaluateByTime(time, &ego_speed_point); - if (!has_speed_point) - { - continue; - } - - cur_gap = obs_s_lower - ego_speed_point.s(); - - // update first point - if (!has_first_interaction_point) - { - has_first_interaction_point = true; - - interaction_info->mutable_start_point()->set_time(time); - interaction_info->mutable_start_point()->set_s_gap(cur_gap); - } - - // update minimum gap point - if (cur_gap < min_gap_s) - { - min_gap_s = cur_gap; - min_gap_point_t = time; - } - - // update end point - end_point_t = time; - end_point_s_gap = cur_gap; - } - - interaction_info->set_has_interaction(has_first_interaction_point); - - interaction_info->mutable_min_gap_point()->set_time(min_gap_point_t); - interaction_info->mutable_min_gap_point()->set_s_gap(min_gap_s); - - interaction_info->mutable_end_point()->set_time(end_point_t); - interaction_info->mutable_end_point()->set_s_gap(end_point_s_gap); - -#if debug_yield_decider - - - AINFO << "obs id: " << obstacle.Id(); - AINFO << interaction_info->DebugString(); - -#endif - - return 0; -} - } } // namespace apollo diff --git a/modules/planning/tasks/deciders/speed_decider/yield_decider.h b/modules/planning/tasks/deciders/speed_decider/yield_decider.h index 63a5fee2..cd115850 100644 --- a/modules/planning/tasks/deciders/speed_decider/yield_decider.h +++ b/modules/planning/tasks/deciders/speed_decider/yield_decider.h @@ -12,7 +12,6 @@ namespace apollo namespace planning { -// update yield时的关键点 int estimate_proper_yield_poi(ObjectYield* yield_decision, const Obstacle& obstacle, const double adc_speed, const double adc_acc, @@ -21,10 +20,6 @@ int estimate_proper_yield_poi(ObjectYield* yield_decision, const SpeedData* soft_brake_speed_data, const SLBoundary& adc_sl_boundary); -int estimate_st_gap_poi_by_st(yield_interaction_info* interaction_info, - const Obstacle& obstacle, - const SpeedData* adc_speed_data); - int estimate_yield_poi_by_two_phase_brake_curve( ObjectYield* yield_decision, const Obstacle& obstacle, const double adc_speed, const double adc_acc, const double path_length,