首先需要理解Planning模块是基于Scenario、Stage、Task这样的层次来进行的,即:场景->步骤->具体的决策方法。Apollo可以应对自动驾驶所面临的不同道路场景,都是通过Scenario统一注册与管理。Scenario通过一个有限状态机来判断选择当前行车场景,每个Scenario下又有多个Stage,指当前场景下需要执行的粗略步骤。
Planning模块根据routing(导航模块),prediction(感知模块)感知的周围环境信息,以及地图定位导航信息为自动驾驶车辆规划出一条运动轨迹(包含坐标,速度,加速度,jerk加加速度,时间等信息),然后将这些信息传递给控制模块。
1、stage_approach
代码路径:modules/planning/scenarios/traffic_light/unprotected_left_turn/stage_approach.cc
本部分实现的功能实现直行红绿灯的判断,等待直行绿灯后进入待转区。
/**
* @file
**/
#include "modules/planning/scenarios/traffic_light/unprotected_left_turn/stage_approach.h"
#include <iterator>
#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/speed_profile_generator.h"
#include "modules/planning/scenarios/util/util.h"
#include "modules/planning/tasks/deciders/creep_decider/creep_decider.h"
namespace apollo {
namespace planning {
namespace scenario {
namespace traffic_light {
using apollo::common::TrajectoryPoint;
using apollo::cyber::Clock;
using apollo::hdmap::PathOverlap;
using apollo::perception::TrafficLight;
/*sam_start**********************/
using apollo::hdmap::HDMapUtil;
/*sam_end***********************/
Stage::StageStatus TrafficLightUnprotectedLeftTurnStageApproach::Process(
const TrajectoryPoint& planning_init_point, Frame* frame) {
ADEBUG << "stage: Approach";
CHECK_NOTNULL(frame);
scenario_config_.CopyFrom(GetContext()->scenario_config);
if (!config_.enabled()) {
return FinishStage(frame);
}
// set cruise_speed to slow down
frame->mutable_reference_line_info()->front().SetCruiseSpeed(
scenario_config_.approach_cruise_speed());
bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
if (!plan_ok) {
AERROR << "TrafficLightUnprotectedLeftTurnStageApproach planning error";
}
if (GetContext()->current_traffic_light_overlap_ids.empty()) {
return FinishScenario();
}
const auto& reference_line_info = frame->reference_line_info().front();
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
/*sam_start**********************/
const auto hdmap_ptr = HDMapUtil::BaseMapPtr();
/*sam_end***********************/
PathOverlap* traffic_light = nullptr;
bool traffic_light_all_done = true;
for (const auto& traffic_light_overlap_id :
GetContext()->current_traffic_light_overlap_ids) {
// get overlap along reference line
PathOverlap* current_traffic_light_overlap =
scenario::util::GetOverlapOnReferenceLine(reference_line_info,
traffic_light_overlap_id,
ReferenceLineInfo::SIGNAL);
if (!current_traffic_light_overlap) {
continue;
}
/*ss_start**********************/
//获取左转红绿灯的状态
apollo::hdmap::Id signal_id;
signal_id.set_id(current_traffic_light_overlap->object_id);
auto signal = hdmap_ptr->GetSignalById(signal_id)->signal();
TrafficLight::Color signal_color = TrafficLight::UNKNOWN;
for (auto subsignal : signal.subsignal()) {
if (subsignal.type() == apollo::hdmap::Subsignal::CIRCLE) {
signal_color = frame->GetSignal(subsignal.id().id()).color();
break;
}
}
/*ss_end***********************/
// set right_of_way_status
reference_line_info.SetJunctionRightOfWay(
current_traffic_light_overlap->start_s, false);
const double distance_adc_to_stop_line =
std::min(signal.stop_line().cbegin()->segment().cbegin()->s(),signal.stop_line().cend()->segment().begin()->s()) - adc_front_edge_s;
/*ss_comment_start**********************/
//auto signal_color = frame->GetSignal(traffic_light_overlap_id).color();
/*ss_comment_end***********************/
ADEBUG << "traffic_light_overlap_id[" << traffic_light_overlap_id
<< "] start_s[" << current_traffic_light_overlap->start_s
<< "] distance_adc_to_stop_line[" << distance_adc_to_stop_line
<< "] color[" << signal_color << "]";
if (distance_adc_to_stop_line < 0)
return FinishStage(frame);
// check on traffic light color and distance to stop line
if (signal_color != TrafficLight::GREEN ||
distance_adc_to_stop_line >=
scenario_config_.max_valid_stop_distance()) {
traffic_light_all_done = false;
break;
}
}
if (traffic_light == nullptr) {
return FinishScenario();
}
if (traffic_light_all_done) {
return FinishStage(frame);
}
return Stage::RUNNING;
}
Stage::StageStatus TrafficLightUnprotectedLeftTurnStageApproach::FinishStage(
Frame* frame) {
// check speed at stop_stage
const double adc_speed = injector_->vehicle_state()->linear_velocity();
if (adc_speed > scenario_config_.max_adc_speed_before_creep()) {
// skip creep
next_stage_ = ScenarioConfig ::
TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN_INTERSECTION_CRUISE;
} else {
// creep
// update PlanningContext
injector_->planning_context()
->mutable_planning_status()
->mutable_traffic_light()
->mutable_done_traffic_light_overlap_id()
->Clear();
for (const auto& traffic_light_overlap_id :
GetContext()->current_traffic_light_overlap_ids) {
injector_->planning_context()
->mutable_planning_status()
->mutable_traffic_light()
->add_done_traffic_light_overlap_id(traffic_light_overlap_id);
}
GetContext()->creep_start_time = Clock::NowInSeconds();
next_stage_ = ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN_CREEP;
}
// reset cruise_speed
auto& reference_line_info = frame->mutable_reference_line_info()->front();
reference_line_info.SetCruiseSpeed(FLAGS_default_cruise_speed);
return Stage::FINISHED;
}
} // namespace traffic_light
} // namespace scenario
} // namespace planning
} // namespace apollo
2、stage_creep
代码路径:modules/planning/scenarios/traffic_light/unprotected_left_turn/stage_creep.cc
本部代码需要复制上面stage_approach代码,需要将圆灯改成左转灯,等待左转灯绿灯后进入下一个stage。