apollo_logo
工作台
4
0

理解Planing 2.0插件机制-新增Scenario插件

Planing运行机制回顾

Apollo采用的是双状态机的框架，场景（Scenario）是规划模块的状态机，由场景管理器来切换调度，阶段（Stage）是场景的状态机，由场景来切换调度，任务（Task）是具体执行的规划运算，由阶段来编排调度。任务可以从任务库中复用我们现有的任务，也可以开发新的任务加入到任务库中，供各个场景阶段复用。

场景开发实践——新增一个左转待转场景

场景需求

这里以apollo_virtual_map为地图，开发一个左转待转场景。左转待转场景可以分为两个区域，直行区和左转待转区。

主车在直行区：

如果左转红灯、直行绿灯，则车辆可以进入左转待转区

如果左转绿灯，无论直行灯是红灯还是绿灯都可以通过路口

如果左转红灯、直行红灯，则车辆在直行区停止线等待

主车在左转区：

如果左转为红灯，直行为绿灯，则车辆在左转区停止线前等待

如果左转为绿灯，则车辆可以通行路口

p.s.在待转区如果左转灯由绿变成红，则主车可以不需要在待转区等待，通行路口

额外要求：每个区域巡航速度不同，在直行区巡航速度为5m/s，左转区巡航速度3m/s，通过路口巡航速度8m/s

在base_map.txt的地图文件可以观察下左转待转区的元素：

左转待转场景，左转灯在左转车道上存在两条停止线，分别在直行区和左转区，左转灯的subsignal的类型为ARROW_LEFT；而直行灯在左转的车道上只有一条停止线，位于直行区，且直行灯的subsignal为CIRCLE类型。可以依此来判断是否进入左转待转场景

阶段的划分：

我们可以根据行驶过程分为：

  • 在直行区停车等待的阶段TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_APPROACH
  • 在待转区行停车等待的阶段TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP
  • 通过路口的阶段TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE

实践步骤

  1. 在包管理环境下创建插件模板，通过以下命令可以创建一个左转待转插件的模板
buildtool create --template plugin --namespaces planning --dependencies planning:binary:planning --base_class_name apollo::planning::Scenario --class_name TrafficLightLeftTurnWaitingZone     modules/planning/scenarios/traffic_light_left_turn_waiting_zone
  1. 在包管理模版基础上实现左转待转的Scneario子类
    1. 在IsTransferable()函数中添加场景切入条件：当发现当前参考线上有两个id一样的信号灯overlap，而且是左转信号灯，且其中一个overlap在最近的信号灯组内，则切入该场景
    2. 在Enter()函数设置进入场景前的参数初始化，将直行信号灯，左转信号灯id保存到context_变量中
    3. 在Exit()函数设置场景切出时对成员变量和全局变量的重置

文件目录：

planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.cc
#include <memory>
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
namespace apollo {
namespace planning {
using apollo::hdmap::HDMapUtil;
bool TrafficLightLeftTurnWaitingZone::Init(std::shared_ptr<DependencyInjector> injector, const std::string& name) {
    if (!Scenario::Init(injector, name)) {
        AERROR << "failed to init scenario" << Name();
        return false;
    }
    if (!Scenario::LoadConfig<TrafficLightLeftTurnWaitingZoneConfig>(&context_.scenario_config)) {
        AERROR << "fail to get specific config of scenario " << Name();
        return false;
    }
    return true;
}
bool TrafficLightLeftTurnWaitingZone::IsTransferable(const Scenario* const other_scenario, const Frame& frame) {
    if (other_scenario == nullptr || frame.reference_line_info().empty()) {
        return false;
    }
    const auto& reference_line_info = frame.reference_line_info().front();
    const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    if (first_encountered_overlaps.empty()) {
        return false;
    }
    hdmap::PathOverlap* traffic_sign_overlap = nullptr;
    for (const auto& overlap : first_encountered_overlaps) {
        if (overlap.first == ReferenceLineInfo::STOP_SIGN || overlap.first == ReferenceLineInfo::YIELD_SIGN) {
            return false;
        } else if (overlap.first == ReferenceLineInfo::SIGNAL) {
            traffic_sign_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);
            break;
        }
    }
    if (traffic_sign_overlap == nullptr) {
        return false;
    }
    const double start_check_distance = context_.scenario_config.start_traffic_light_scenario_distance();
    const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
    if (traffic_sign_overlap->start_s - adc_front_edge_s > start_check_distance) {
        return false;
    }
    const auto& turn_type = reference_line_info.GetPathTurnType(traffic_sign_overlap->start_s);
    if (turn_type != hdmap::Lane::LEFT_TURN) {
        return false;
    }
    auto* base_map = HDMapUtil::BaseMapPtr();
    static constexpr double kTrafficLightGroupingMaxDist = 2.0;  // unit: m
    for (const auto& overlap : traffic_light_overlaps) {
        const double dist = overlap.start_s - traffic_sign_overlap->start_s;
        if (fabs(dist) <= kTrafficLightGroupingMaxDist) {
            const auto& signal = base_map->GetSignalById(hdmap::MakeMapId(overlap.object_id))->signal();
            if (signal.subsignal_size() > 0 && signal.subsignal(0).type() == apollo::hdmap::Subsignal::ARROW_LEFT
                && signal.stop_line_size() > 1) {
                return true;
            }
        }
    }
    return false;
}
bool TrafficLightLeftTurnWaitingZone::Enter(Frame* frame) {
    const auto& reference_line_info = frame->reference_line_info().front();
    const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    hdmap::PathOverlap* nearest_traffic_light_overlap = nullptr;
    for (const auto& overlap : first_encountered_overlaps) {
        if (overlap.first == ReferenceLineInfo::SIGNAL) {
            nearest_traffic_light_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);
            break;
        }
    }
    std::vector<hdmap::PathOverlap> next_traffic_lights;
    auto* base_map = HDMapUtil::BaseMapPtr();
    std::string left_turn_signal_id;
    static constexpr double kTrafficLightGroupingMaxDist = 2.0;  // unit: m
    for (const auto& overlap : traffic_light_overlaps) {
        const double dist = overlap.start_s - nearest_traffic_light_overlap->start_s;
        if (fabs(dist) <= kTrafficLightGroupingMaxDist) {
            const auto signal = base_map->GetSignalById(hdmap::MakeMapId(overlap.object_id));
            AINFO << signal->id().id() << "," << overlap.object_id;
            if (signal->signal().subsignal(0).type() == apollo::hdmap::Subsignal::ARROW_LEFT) {
                context_.left_turn_traffic_light_id = overlap.object_id;
                AINFO << "left_turn_signal_id" << context_.left_turn_traffic_light_id;
            } else {
                context_.forward_traffic_light_id = overlap.object_id;
                AINFO << "forward signal id" << context_.forward_traffic_light_id;
            }
        }
    }
    return true;
}
bool TrafficLightLeftTurnWaitingZone::Exit(Frame* frame) {
    context_.left_turn_traffic_light_id.clear();
    context_.forward_traffic_light_id.clear();
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    return true;
}
}  // namespace planning
}  // namespace apollo

添加头文件modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h：

#pragma once
#include <memory>
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/planning/planning_base/scenario_base/scenario.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/proto/traffic_light_left_turn_waiting_zone.pb.h"
namespace apollo {
namespace planning {
struct TrafficLightLeftTurnWaitingZoneContext : public ScenarioContext {
    TrafficLightLeftTurnWaitingZoneConfig scenario_config;
    std::string left_turn_traffic_light_id;
    std::string forward_traffic_light_id;
};
class TrafficLightLeftTurnWaitingZone : public apollo::planning::Scenario {
public:
    bool Init(std::shared_ptr<DependencyInjector> injector, const std::string& name);
    TrafficLightLeftTurnWaitingZoneContext* GetContext() override {
        return &context_;
    }
    bool IsTransferable(const Scenario* other_scenario, const Frame& frame);
    bool Exit(Frame* frame);
    bool Enter(Frame* frame);
private:
    TrafficLightLeftTurnWaitingZoneContext context_;
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::TrafficLightLeftTurnWaitingZone, apollo::planning::Scenario)
}  // namespace planning
}  // namespace apollo
  1. 开发左转待转的第一个阶段，行驶到直行区停止线的阶段TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_APPROACH，实现Stage的Process()函数：
    1. 在reference_line_info中设置该阶段的限速
    2. 如果左转灯是绿灯，且通过了停止线则切到TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE阶段，同时将直行灯加入到add_done_traffic_light_overlap_id的全局变量中
    3. 如果左转灯是红灯，直行灯是绿灯，则切到TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP阶段，同时将左转灯加入到add_done_traffic_light_overlap_id的全局变量中
    4. 如果已经通过了停止线，则切入TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE阶段

文件路径：

planning/scenarios/traffic_light_left_turn_waiting_zone/stage_approach.cc
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_approach.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/planning_base/common/planning_context.h"
namespace apollo {
namespace planning {
using apollo::common::TrajectoryPoint;
using apollo::hdmap::PathOverlap;
using apollo::perception::TrafficLight;
StageResult TrafficLightLeftTurnWaitingZoneStageApproach::Process(
        const TrajectoryPoint& planning_init_point,
        Frame* frame) {
    ADEBUG << "stage: Approach";
    CHECK_NOTNULL(frame);
    CHECK_NOTNULL(context_);
    auto* context = GetContextAs<TrafficLightLeftTurnWaitingZoneContext>();
    const auto& scenario_config = context->scenario_config;
    auto& reference_line_info = frame->mutable_reference_line_info()->front();
    reference_line_info.LimitCruiseSpeed(scenario_config.approach_speed());
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    if (result.HasError()) {
        AERROR << "TrafficLightLeftTurnWaitingZoneStageApproach planning error";
    }
    if (context->left_turn_traffic_light_id.empty() || context->forward_traffic_light_id.empty()) {
        return FinishScenario();
    }
    hdmap::PathOverlap forward_traffic_light_overlap;
    hdmap::PathOverlap left_turn_traffic_light_overlap_first;
    hdmap::PathOverlap left_turn_traffic_light_overlap_second;
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    for (const auto& overlap : traffic_light_overlaps) {
        if (overlap.object_id == context->forward_traffic_light_id) {
            forward_traffic_light_overlap = overlap;
        } else if (overlap.object_id == context->left_turn_traffic_light_id) {
            if (left_turn_traffic_light_overlap_second.start_s < overlap.start_s) {
                std ::swap(left_turn_traffic_light_overlap_first, left_turn_traffic_light_overlap_second);
                left_turn_traffic_light_overlap_second = overlap;
            } else {
                left_turn_traffic_light_overlap_first = overlap;
            }
        }
    }
    const double adc_back_edge_s = reference_line_info.AdcSlBoundary().end_s();
    bool is_passed_forward_stop_line = adc_back_edge_s > forward_traffic_light_overlap.end_s;
    auto forward_signal_color = frame->GetSignal(context->forward_traffic_light_id).color();
    auto left_signal_color = frame->GetSignal(context->left_turn_traffic_light_id).color();
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    if (left_signal_color == TrafficLight::GREEN) {
        if (is_passed_forward_stop_line) {
            return FinishStage("TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE");
        }
        done_traffic_light->add_done_traffic_light_overlap_id(forward_traffic_light_overlap.object_id);
    } else if (forward_signal_color == TrafficLight::GREEN) {
        done_traffic_light->add_done_traffic_light_overlap_id(left_turn_traffic_light_overlap_first.object_id);
    }
    if (is_passed_forward_stop_line) {
        return FinishStage("TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP");
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}
StageResult TrafficLightLeftTurnWaitingZoneStageApproach::FinishStage(std::string next_stage) {
    injector_->planning_context()
            ->mutable_planning_status()
            ->mutable_traffic_light()
            ->mutable_done_traffic_light_overlap_id()
            ->Clear();
    next_stage_ = next_stage;
    return StageResult(StageStatusType::FINISHED);
}
}  // namespace planning
}  // namespace apollo

添加头文件modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_approach.h：

#pragma once
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/planning/planning_base/scenario_base/stage.h"
namespace apollo {
namespace planning {
class TrafficLightLeftTurnWaitingZoneStageApproach : public Stage {
public:
    StageResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override;
private:
    StageResult FinishStage(std::string next_stage);
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(
        apollo::planning::TrafficLightLeftTurnWaitingZoneStageApproach,
        apollo::planning::Stage)
}  // namespace planning
}  // namespace apollo
  1. 开发第二个阶段，待转区行驶的阶段TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP，实现待转区阶段的Process()函数:
    1. 在reference_line_info中设置该阶段的限速
    2. 如果通过了左转停止线，或者左转灯是绿灯则切入TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE阶段

文件路径：

planning/scenarios/traffic_light_left_turn_waiting_zone/stage_creep.cc
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_creep.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
#include <string>
#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/planning_base/common/planning_context.h"
namespace apollo {
namespace planning {
using apollo::common::TrajectoryPoint;
using apollo::hdmap::PathOverlap;
using apollo::perception::TrafficLight;
bool TrafficLightLeftTurnWaitingZoneStageCreep::Init(
        const StagePipeline& config,
        const std::shared_ptr<DependencyInjector>& injector,
        const std::string& config_dir,
        void* context) {
    CHECK_NOTNULL(context);
    bool ret = Stage::Init(config, injector, config_dir, context);
    if (!ret) {
        AERROR << Name() << "init failed!";
        return false;
    }
    return ret;
}
StageResult TrafficLightLeftTurnWaitingZoneStageCreep::Process(
        const TrajectoryPoint& planning_init_point,
        Frame* frame) {
    CHECK_NOTNULL(frame);
    CHECK_NOTNULL(context_);
    auto* context = GetContextAs<TrafficLightLeftTurnWaitingZoneContext>();
    const auto& scenario_config = context->scenario_config;
    auto& reference_line_info = frame->mutable_reference_line_info()->front();
    reference_line_info.LimitCruiseSpeed(scenario_config.creep_speed());
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    if (result.HasError()) {
        AERROR << "TrafficLightLeftTurnWaitingZoneStageCreep planning error";
    }
    hdmap::PathOverlap forward_traffic_light_overlap;
    hdmap::PathOverlap left_turn_traffic_light_overlap_first;
    hdmap::PathOverlap left_turn_traffic_light_overlap_second;
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    for (const auto& overlap : traffic_light_overlaps) {
        if (overlap.object_id == context->forward_traffic_light_id) {
            forward_traffic_light_overlap = overlap;
        } else if (overlap.object_id == context->left_turn_traffic_light_id) {
            if (left_turn_traffic_light_overlap_second.start_s < overlap.start_s) {
                std ::swap(left_turn_traffic_light_overlap_first, left_turn_traffic_light_overlap_second);
                left_turn_traffic_light_overlap_second = overlap;
            } else {
                left_turn_traffic_light_overlap_first = overlap;
            }
        }
    }
    const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
    bool is_passed_left_turn_stop_line = adc_front_edge_s > left_turn_traffic_light_overlap_second.end_s;
    auto left_signal_color = frame->GetSignal(context->left_turn_traffic_light_id).color();
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    if (is_passed_left_turn_stop_line || left_signal_color == TrafficLight::GREEN) {
        reference_line_info.LimitCruiseSpeed(scenario_config.cruise_speed());
        return FinishStage();
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}
StageResult TrafficLightLeftTurnWaitingZoneStageCreep::FinishStage() {
    next_stage_ = "TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE";
    return StageResult(StageStatusType::FINISHED);
}
}  // namespace planning
}  // namespace apollo

添加头文件modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_creep.h：

#pragma once
#include <memory>
#include <string>
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/planning/planning_base/scenario_base/stage.h"
namespace apollo {
namespace planning {
class TrafficLightLeftTurnWaitingZoneStageCreep : public Stage {
public:
    bool Init(
            const StagePipeline& config,
            const std::shared_ptr<DependencyInjector>& injector,
            const std::string& config_dir,
            void* context) override;
    StageResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override;
private:
    StageResult FinishStage();
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::TrafficLightLeftTurnWaitingZoneStageCreep, Stage)
}  // namespace planning
}  // namespace apollo
  1. 开发第三个阶段通过交叉路口TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE：
    1. 该阶段可以继承当前已有的交叉路口阶段基类BaseStageTrafficLightCruise
    2. 设置当前阶段限速
    3. 这一阶段将无视红绿灯信息，因此将直行灯和左转灯都加入到add_done_traffic_light_overlap_id

文件路径：

planning/scenarios/traffic_light_left_turn_waiting_zone/stage_intersection_cruise.cc
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_intersection_cruise.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
#include "cyber/common/log.h"
namespace apollo {
namespace planning {
StageResult TrafficLightLeftTurnWaitingZoneStageIntersectionCruise::Process(
        const common::TrajectoryPoint& planning_init_point,
        Frame* frame) {
    const auto* context = GetContextAs<TrafficLightLeftTurnWaitingZoneContext>();
    const auto& scenario_config = context->scenario_config;
    auto& reference_line_info = frame->mutable_reference_line_info()->front();
    reference_line_info.LimitCruiseSpeed(scenario_config.cruise_speed());
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    done_traffic_light->add_done_traffic_light_overlap_id(context->forward_traffic_light_id);
    done_traffic_light->add_done_traffic_light_overlap_id(context->left_turn_traffic_light_id);
    if (result.HasError()) {
        AERROR << "TrafficLightLeftTurnWaitingZoneStageIntersectionCruise "
               << "plan error";
    }
    bool stage_done = CheckDone(*frame, injector_->planning_context(), true);
    if (stage_done) {
        return FinishStage();
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}
StageResult TrafficLightLeftTurnWaitingZoneStageIntersectionCruise::FinishStage() {
    return FinishScenario();
}
}  // namespace planning
}  // namespace apollo

添加头文件modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_intersection_cruise.h：

#pragma once
#include <string>
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/planning/planning_base/scenario_base/traffic_light_base/base_stage_traffic_light_cruise.h"
namespace apollo {
namespace planning {
class TrafficLightLeftTurnWaitingZoneStageIntersectionCruise : public BaseStageTrafficLightCruise {
public:
    StageResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override;
private:
    StageResult FinishStage();
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::TrafficLightLeftTurnWaitingZoneStageIntersectionCruise, Stage)
}  // namespace planning
}  // namespace apollo
  1. 完成场景的流水线配置conf/pipeline.pb.txt，配置需要启动的阶段，和每个阶段用到的任务

文件路径

planning/scenarios/traffic_light_left_turn_waiting_zone/conf/pipeline.pb.txt
stage: {
  name: "TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_APPROACH"
  type: "TrafficLightLeftTurnWaitingZoneStageApproach"
  enabled: true
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  task {
    name: "FALLBACK_PATH"
    type: "FallbackPath"
  }
  task {
    name: "PATH_DECIDER"
    type: "PathDecider"
  }
  task {
    name: "RULE_BASED_STOP_DECIDER"
    type: "RuleBasedStopDecider"
  }
  task {
    name: "SPEED_BOUNDS_PRIORI_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "SPEED_HEURISTIC_OPTIMIZER"
    type: "PathTimeHeuristicOptimizer"
  }
  task {
    name: "SPEED_DECIDER"
    type: "SpeedDecider"
  }
  task {
    name: "SPEED_BOUNDS_FINAL_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "PIECEWISE_JERK_SPEED"
    type: "PiecewiseJerkSpeedOptimizer"
  }
}
stage: {
  name: "TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CREEP"
  type: "TrafficLightLeftTurnWaitingZoneStageCreep"
  enabled: true
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  task {
    name: "FALLBACK_PATH"
    type: "FallbackPath"
  }
  task {
    name: "PATH_DECIDER"
    type: "PathDecider"
  }
  task {
    name: "RULE_BASED_STOP_DECIDER"
    type: "RuleBasedStopDecider"
  }
  task {
    name: "SPEED_BOUNDS_PRIORI_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "SPEED_HEURISTIC_OPTIMIZER"
    type: "PathTimeHeuristicOptimizer"
  }
  task {
    name: "SPEED_DECIDER"
    type: "SpeedDecider"
  }
  task {
    name: "SPEED_BOUNDS_FINAL_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "PIECEWISE_JERK_SPEED"
    type: "PiecewiseJerkSpeedOptimizer"
  }
}
stage: {
  name: "TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE"
  type: "TrafficLightLeftTurnWaitingZoneStageIntersectionCruise"
  enabled: true
    task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  task {
    name: "FALLBACK_PATH"
    type: "FallbackPath"
  }
  task {
    name: "PATH_DECIDER"
    type: "PathDecider"
  }
  task {
    name: "RULE_BASED_STOP_DECIDER"
    type: "RuleBasedStopDecider"
  }
  task {
    name: "ST_BOUNDS_DECIDER"
    type: "STBoundsDecider"
  }
  task {
    name: "SPEED_BOUNDS_PRIORI_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "SPEED_HEURISTIC_OPTIMIZER"
    type: "PathTimeHeuristicOptimizer"
  }
  task {
    name: "SPEED_DECIDER"
    type: "SpeedDecider"
  }
  task {
    name: "SPEED_BOUNDS_FINAL_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "PIECEWISE_JERK_SPEED"
    type: "PiecewiseJerkSpeedOptimizer"
  }
}
  1. 添加场景的参数文件
  • 将场景用到的参数配置到一个proto文件

文件路径：

planning/scenarios/traffic_light_left_turn_waiting_zone/proto/traffic_light_left_turn_waiting_zone.proto
syntax = "proto2";
package apollo.planning;
message TrafficLightLeftTurnWaitingZoneConfig {
  optional double start_traffic_light_scenario_distance = 1;  // meter
  optional double approach_speed = 2;  
  optional double creep_speed = 3;
  optional double cruise_speed = 4;
}
  • 修改配置的编译文件BUILD，将其编译为一个

文件路径：

proto_library(planning/scenarios/traffic_light_left_turn_waiting_zone/proto/BUILD
load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library")
load("//tools:apollo_package.bzl", "apollo_cc_library", "apollo_package", "apollo_plugin")
load("//tools/proto:proto.bzl", "proto_library")
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
proto_library(
    name = "traffic_light_left_turn_waiting_zone_proto",
    srcs = ["traffic_light_left_turn_waiting_zone.proto"],
)
apollo_package()
cpplint()
  • 将场景的参数配置添加到planning/scenarios/traffic_light_left_turn_waiting_zone/conf/scenario_conf.pb.txt中
start_traffic_light_scenario_distance: 30.0
approach_speed:5.0
creep_speed: 3.0
cruise_speed:8.0
  1. 将添加的scenario和stage插件注册到文件中

modules/planning/scenarios/traffic_light_left_turn_waiting_zone/plugin_traffic_light_left_turn_waiting_zone_description.xml

<library path="modules/planning/scenarios/traffic_light_left_turn_waiting_zone/libtraffic_light_left_turn_waiting_zone.so">
    <class type="apollo::planning::TrafficLightLeftTurnWaitingZone" base_class="apollo::planning::Scenario"></class>
    <class type="apollo::planning::TrafficLightLeftTurnWaitingZoneStageApproach" base_class="apollo::planning::Stage"></class>
    <class type="apollo::planning::TrafficLightLeftTurnWaitingZoneStageCreep" base_class="apollo::planning::Stage"></class>
    <class type="apollo::planning::TrafficLightLeftTurnWaitingZoneStageIntersectionCruise" base_class="apollo::planning::Stage"></class>
</library>
  1. 将左转待转场景加入到planning的场景管理流水线中

文件路径：

profiles/current/modules/planning/planning_base/conf/planning_config.pb.txt
topic_config {
  chassis_topic: "/apollo/canbus/chassis"
  hmi_status_topic: "/apollo/hmi/status"
  localization_topic: "/apollo/localization/pose"
  planning_pad_topic: "/apollo/planning/pad"
  planning_trajectory_topic: "/apollo/planning"
  prediction_topic: "/apollo/prediction"
  relative_map_topic: "/apollo/relative_map"
  routing_request_topic: "/apollo/external_command/lane_follow"
  routing_response_topic: "/apollo/routing_response"
  planning_command_topic: "/apollo/planning/command"
  story_telling_topic: "/apollo/storytelling"
  traffic_light_detection_topic: "/apollo/perception/traffic_light"
  planning_learning_data_topic: "/apollo/planning/learning_data"
}
# NO_LEARNING / E2E / HYBRID / RL_TEST / E2E_TEST / HYBRID_TEST
learning_mode: NO_LEARNING
reference_line_config {
  pnc_map_class: "apollo::planning::LaneFollowMap"
}
standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_public_road_config {
    scenario {
      name: "EMERGENCY_PULL_OVER"
      type: "EmergencyPullOverScenario"
    }
    scenario {
      name: "EMERGENCY_STOP"
      type: "EmergencyStopScenario"
    }
    scenario {
      name: "VALET_PARKING"
      type: "ValetParkingScenario"
    }
    scenario {
      name: "BARE_INTERSECTION_UNPROTECTED"
      type: "BareIntersectionUnprotectedScenario"
    }
    scenario {
      name: "STOP_SIGN_UNPROTECTED"
      type: "StopSignUnprotectedScenario"
    }
    scenario {
      name: "YIELD_SIGN"
      type: "YieldSignScenario"
    }
    scenario {
      name: "TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE"
      type: "TrafficLightLeftTurnWaitingZone"
    }
    scenario {
      name: "TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN"
      type: "TrafficLightUnprotectedLeftTurnScenario"
    }
    scenario {
      name: "TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN"
      type: "TrafficLightUnprotectedRightTurnScenario"
    }
    scenario {
      name: "TRAFFIC_LIGHT_PROTECTED"
      type: "TrafficLightProtectedScenario"
    }
    scenario {
      name: "PULL_OVER"
      type: "PullOverScenario"
    }
    scenario {
      name: "PARK_AND_GO"
      type: "ParkAndGoScenario"
    }
    scenario {
      name: "LANE_FOLLOW"
      type: "LaneFollowScenario"
    }
  }
}
  1. 开发一个仿真测试的脚本，通过键盘输入来控制信号灯的变换。

文件路径：

modules/planning/scenarios/traffic_light_left_turn_waiting_zone/tools/manual_traffic_light.py
from cyber.python.cyber_py3 import cyber
from cyber.python.cyber_py3 import cyber_time
import modules.common_msgs.perception_msgs.traffic_light_detection_pb2 as traffic_light_detection_pb2
import threading
import time
def add_left_light(color, traffic_light_pb):
    light = traffic_light_pb.traffic_light.add()
    light.color = color
    light.id = "451089192"
    light.tracking_time = 10.0
def add_forward_light(color, traffic_light_pb):
    light = traffic_light_pb.traffic_light.add()
    light.color = color
    light.id = "451089193"
    light.tracking_time = 10.0
seq_num = 0
def add_header(msg):
    global seq_num
    msg.header.sequence_num = seq_num
    msg.header.timestamp_sec = cyber_time.Time.now().to_sec()
    msg.header.module_name = "manual_traffic_light"
    seq_num = seq_num + 1
def pub_func(writer):
    while not cyber.is_shutdown():
        global traffic_light_msg
        add_header(traffic_light_msg)
        writer.write(traffic_light_msg)
        time.sleep(0.1)
traffic_light_msg = None
if __name__ == '__main__':
    traffic_light_msg = traffic_light_detection_pb2.TrafficLightDetection()
    cyber.init()
    node = cyber.Node("traffic_light_command")
    writer = node.create_writer(
        "/apollo/perception/traffic_light", traffic_light_detection_pb2.TrafficLightDetection)
    thread = threading.Thread(target=pub_func, args=(writer,))
    thread.start()
    while not cyber.is_shutdown():
        m = input(
            "1: all red 2: forward green  3: left green 4:all green\n")
        traffic_light_msg.ClearField('traffic_light')
        print(m)
        if m == '1':
            add_left_light(
                traffic_light_detection_pb2.TrafficLight.RED, traffic_light_msg)
            add_forward_light(
                traffic_light_detection_pb2.TrafficLight.RED, traffic_light_msg)
        elif m == "2":
            add_left_light(
                traffic_light_detection_pb2.TrafficLight.RED, traffic_light_msg)
            add_forward_light(
                traffic_light_detection_pb2.TrafficLight.GREEN, traffic_light_msg)
        elif m == "3":
            add_left_light(
                traffic_light_detection_pb2.TrafficLight.GREEN, traffic_light_msg)
            add_forward_light(
                traffic_light_detection_pb2.TrafficLight.RED, traffic_light_msg)
        elif m == "4":
            add_left_light(
                traffic_light_detection_pb2.TrafficLight.GREEN, traffic_light_msg)
            add_forward_light(
                traffic_light_detection_pb2.TrafficLight.GREEN, traffic_light_msg)
    cyber.shutdown()
  1. 修改BUILD文件，配置编译依赖。
  • 将包内所有代码编译为一个apollo的插件，添加代码中所有对基础库的依赖
load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library")
load("//tools:apollo.bzl", "cyber_plugin_description")
load("//tools:apollo_package.bzl", "apollo_cc_library", "apollo_package", "apollo_plugin")
load("//tools/proto:proto.bzl", "proto_library")
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
filegroup(
    name = "left_turn_waiting_zone_files",
    srcs = glob([
        "conf/**",
    ]),
)
apollo_plugin(
    name = "libtraffic_light_left_turn_waiting_zone.so",
    srcs = [
        "stage_approach.cc",
        "stage_creep.cc",
        "stage_intersection_cruise.cc",
        "traffic_light_left_turn_waiting_zone.cc",
    ],
    hdrs = [
        "stage_approach.h",
        "stage_creep.h",
        "stage_intersection_cruise.h",
        "traffic_light_left_turn_waiting_zone.h",
    ],
    copts = ["-DMODULE_NAME=\\\"planning\\\""],
    description = ":plugin_traffic_light_left_turn_waiting_zone_description.xml",
    deps = [
        "//cyber",
        "//modules/common/util:common_util",
        "//modules/common/util:util_tool",
        "//modules/common_msgs/planning_msgs:planning_cc_proto",
        "//modules/map:apollo_map",
        "//modules/planning/planning_base:apollo_planning_planning_base",
        "//modules/planning/scenarios/traffic_light_left_turn_waiting_zone/proto:traffic_light_left_turn_waiting_zone_cc_proto",
    ],
)
apollo_package()
cpplint()
  1. 编译开发好的包
buildtool build -p modules/planning/scenarios/traffic_light_left_turn_waiting_zone
  1. 在Dreamview仿真验证
    1. 选择planning2.0场景集，左转待转场景

    1. 将车放置在地图左下角的路口，左转车道上
    1. 打开planning模块
    2. 在终端启动红绿灯脚本，使直行灯和左转灯都为红色
#需要提前安装tools工具，需要在容器中输入该指令
sudo apt install apollo-neo-tools
python3 modules/planning/scenarios/traffic_light_left_turn_waiting_zone/tools/manual_traffic_light.py
发送目标车道的路由请求
    1. 发送目标车道的路由请求
    1. 车辆行驶到停止线后，修改为直行绿灯左转红灯

在终端输入：2

现象：

    1. 车辆行驶到左转区停止线后，修改为左转绿灯

在终端输入：3

现象：

    1. 重新仿真测试，当直行区左转灯是绿灯时测试效果
    2. 重新仿真测试，当左转区由绿灯变成红灯测试效果

云实验链接：

该实践内容已经上线Apollo云实验室，开快速启动进行实践体验: https://apollo.chjunkong.com/community/course/25

原创声明，本文由作者授权发布于Apollo开发者社区，未经许可，不得转载。
发表评论已发表 0 条评论
登录后可评论，请前往 登录
暂无评论～快去发表自己的独特见解吧！
个人头像
宇新
作者其他文章
·Apollo感知模型部署
·进阶课程审核通过名单
·aem profile功能使用
·9.0源码下如何安装Dreamview插件
目录
Planing运行机制回顾
场景开发实践——新增一个左转待转场景
云实验链接：