提交 c3bec4f9 编写于 作者: D Dong Li 提交者: Jiangtao Hu

planning: add protected mode when have priority in junction

上级 2c11c0cf
...@@ -122,13 +122,19 @@ ADCTrajectory::RightOfWayStatus ReferenceLineInfo::GetRightOfWayStatus() const { ...@@ -122,13 +122,19 @@ ADCTrajectory::RightOfWayStatus ReferenceLineInfo::GetRightOfWayStatus() const {
} else if (WithinOverlap(overlap, adc_sl_boundary_.end_s())) { } else if (WithinOverlap(overlap, adc_sl_boundary_.end_s())) {
auto* is_protected = auto* is_protected =
junction_store->Get(junction_dropbox_id(overlap.object_id)); junction_store->Get(junction_dropbox_id(overlap.object_id));
if (!is_protected) { if (is_protected != nullptr && *is_protected) {
continue;
}
if (*is_protected) {
return ADCTrajectory::PROTECTED; return ADCTrajectory::PROTECTED;
} else { } else {
return ADCTrajectory::UNPROTECTED; double junction_s = (overlap.end_s + overlap.start_s) / 2.0;
auto ref_point = reference_line_.GetReferencePoint(junction_s);
if (ref_point.lane_waypoints().empty()) {
return ADCTrajectory::PROTECTED;
}
for (const auto& waypoint : ref_point.lane_waypoints()) {
if (waypoint.lane->lane().turn() == hdmap::Lane::NO_TURN) {
return ADCTrajectory::PROTECTED;
}
}
} }
} }
} }
...@@ -574,13 +580,13 @@ void ReferenceLineInfo::ExportEngageAdvice(EngageAdvice* engage_advice) const { ...@@ -574,13 +580,13 @@ void ReferenceLineInfo::ExportEngageAdvice(EngageAdvice* engage_advice) const {
} }
engage_advice->set_reason("Vehicle heading is not aligned"); engage_advice->set_reason("Vehicle heading is not aligned");
} else { } else {
if (vehicle_state_.driving_mode() != if (vehicle_state_.driving_mode() !=
Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) { Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE); engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE);
} else { } else {
engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED); engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);
}
} }
}
} }
*prev_advice = *engage_advice; *prev_advice = *engage_advice;
} }
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册