Planning module
是自动驾驶的决策模块,会收集其他模块的所有信息进行综合决策,Apollo
开源系统的架构如下:
Planning
会订阅感知,预测,地图,导航,定位,底盘,HMI(Pad)
,Storytelling
模块发布的消息.
channel | message |
module |
content |
---|---|---|---|
"/apollo/prediction" |
PredictionObstacles |
Predicition |
障碍物信息 |
"/apollo/perception/traffic_light" |
TrafficLightDetection |
Perception |
交通灯信息 |
"/apollo/relative_map" |
MapMsg |
Map * |
地图信息 |
"/apollo/routing_response" |
RoutingResponse |
Routing |
导航信息 |
"/apollo/localization/pose" |
LocalizationEstimate |
Localization |
定位信息 |
"/apollo/canbus/chassis" |
Chassis |
Canbus |
底盘信息 |
"/apollo/planning/pad" |
PadMessage |
Pad * |
Pad 信息 |
"/apollo/storytelling" |
Stories |
Storytelling * |
-- |
注: 地图信息分为
HD Map
(UTM
坐标系)和Relative Map
(相对坐标系),两者地图数据格式相同.目前
Apollo
并未对Storytelling
信息做响应或者处理.
Planning
模块会向控制模块广播ADCTrajectory
,向导航模块广播重规划消息,并广播用于planning
模块机器学习所需的数据.
channel | message |
module |
content |
---|---|---|---|
"/apollo/planning" |
ADCTrajectory |
Control |
规划信息 |
"/apollo/routing_request" |
RoutingRequest |
Routing |
导航更新信息 |
"/apollo/planning/learning_data" |
PlanningLearningData |
Planning |
Planning 机器学习数据 |
1.1.1 PredictionObstacles
Perception Module
检测到的障碍物PerceptionObstacles
,经由Predicition Module
进行封装成PredictionObstacles
.
1.1.1.1 proto目录
apollo\modules\perception\proto\perception_obstacle.proto
apollo\modules\prediction\proto\prediction_obstacle.proto
1.1.1.2 proto定义
syntax = "proto2"; // protobuf version
package apollo.perception; // namespace apollo::perception
// 依赖的其他 proto 定义
import "modules/common/proto/error_code.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/header.proto";
import "modules/common/proto/pnc_point.proto";
import "modules/map/proto/map_lane.proto";
// 2D框
message BBox2D {
optional double xmin = 1; // in pixels. // 像素级
optional double ymin = 2; // in pixels.
optional double xmax = 3; // in pixels.
optional double ymax = 4; // in pixels.
}
// 检测到车辆的灯光状态
message LightStatus {
optional double brake_visible = 1; // 刹车灯可见
optional double brake_switch_on = 2; // 刹车灯打开
optional double left_turn_visible = 3; // 左转向灯可见
optional double left_turn_switch_on = 4; // 左转向灯打开
optional double right_turn_visible = 5; // 右转向灯可见
optional double right_turn_switch_on = 6; // 右转向灯打开
}
message SensorMeasurement {
optional string sensor_id = 1; // 传感器id
optional int32 id = 2; // 障碍物ID?
optional apollo.common.Point3D position = 3; // 障碍物坐标
optional double theta = 4;
optional double length = 5;
optional double width = 6;
optional double height = 7;
optional apollo.common.Point3D velocity = 8;
optional PerceptionObstacle.Type type = 9;
optional PerceptionObstacle.SubType sub_type = 10;
optional double timestamp = 11;
optional BBox2D box = 12; // only for camera measurements
}
message Trajectory {
optional double probability = 1; // probability of this trajectory
repeated apollo.common.TrajectoryPoint trajectory_point = 2; // 可能由多条轨迹
}
message DebugMessage {
// can have multiple trajectories per obstacle
repeated Trajectory trajectory = 1;
}
message PerceptionObstacle {
optional int32 id = 1; // obstacle ID.
// obstacle position in the world coordinate system.(x, y, z)
optional apollo.common.Point3D position = 2;
optional double theta = 3; // heading in the world coordinate system.
optional apollo.common.Point3D velocity = 4; // obstacle velocity.(x, y, z)
// Size of obstacle bounding box.
optional double length = 5; // obstacle length.
optional double width = 6; // obstacle width.
optional double height = 7; // obstacle height.
repeated apollo.common.Point3D polygon_point = 8; // obstacle corner points.
// duration of an obstacle since detection in s.
optional double tracking_time = 9;
enum Type {
UNKNOWN = 0; // 不能确定的类型
UNKNOWN_MOVABLE = 1; // 移动的不确定类型
UNKNOWN_UNMOVABLE = 2; // 静止的不确定类型
PEDESTRIAN = 3; // Pedestrian, usually determined by moving behavior.
BICYCLE = 4; // bike, motor bike
VEHICLE = 5; // Passenger car or truck.
};
optional Type type = 10; // obstacle type
optional double timestamp = 11; // GPS time in seconds.
// Just for offline debugging, will not fill this field on board.
// Format: [x0, y0, z0, x1, y1, z1...]
repeated double point_cloud = 12 [packed = true];
optional double confidence = 13 [deprecated = true]; // 置信度??
enum ConfidenceType {
CONFIDENCE_UNKNOWN = 0;
CONFIDENCE_CNN = 1;
CONFIDENCE_RADAR = 2;
};
optional ConfidenceType confidence_type = 14 [deprecated = true];
// trajectory of object.
repeated apollo.common.Point3D drops = 15 [deprecated = true]; // 障碍物的历史轨迹
// The following fields are new added in Apollo 4.0
optional apollo.common.Point3D acceleration = 16; // obstacle acceleration
// a stable obstacle point in the world coordinate system
// position defined above is the obstacle bounding box ground center
optional apollo.common.Point3D anchor_point = 17; // 矩形的中心点??
optional BBox2D bbox2d = 18; // 将障碍物的多边形形状近似(放大)成矩形
enum SubType {
ST_UNKNOWN = 0;
ST_UNKNOWN_MOVABLE = 1;
ST_UNKNOWN_UNMOVABLE = 2;
ST_CAR = 3;
ST_VAN = 4;
ST_TRUCK = 5;
ST_BUS = 6;
ST_CYCLIST = 7;
ST_MOTORCYCLIST = 8;
ST_TRICYCLIST = 9;
ST_PEDESTRIAN = 10;
ST_TRAFFICCONE = 11;
};
optional SubType sub_type = 19; // obstacle sub_type
repeated SensorMeasurement measurements = 20; // sensor measurements 来自传感器的信息
// orthogonal distance between obstacle lowest point and ground plane
optional double height_above_ground = 21 [default = nan]; // 高度
// position covariance which is a row-majored 3x3 matrix
repeated double position_covariance = 22 [packed = true]; // 坐标的协方差
// velocity covariance which is a row-majored 3x3 matrix
repeated double velocity_covariance = 23 [packed = true]; // 速度的协方差
// acceleration covariance which is a row-majored 3x3 matrix
repeated double acceleration_covariance = 24 [packed = true]; // 加速度的协方差
// lights of vehicles
optional LightStatus light_status = 25; // 障碍物如果是车辆,车辆的灯光信息
// Debug Message
optional DebugMessage msg = 26;
}
message LaneMarker {
optional apollo.hdmap.LaneBoundaryType.Type lane_type = 1; // 车道线类型(双黄线,实白线等)
optional double quality = 2; // range = [0,1]; 1 = the best quality // 车道线识别的质量
optional int32 model_degree = 3; // 车道线多项式的阶次
// equation X = c3 * Z^3 + c2 * Z^2 + c1 * Z + c0
optional double c0_position = 4; // 常数项系数
optional double c1_heading_angle = 5; // 一次项系数
optional double c2_curvature = 6; // 二次项系数
optional double c3_curvature_derivative = 7; // 三次项系数
optional double view_range = 8; // 感知的可视范围?
optional double longitude_start = 9; // z的最小值
optional double longitude_end = 10; // z的最大值
}
// 车道线
message LaneMarkers {
optional LaneMarker left_lane_marker = 1; // adc车道的左侧车道线
optional LaneMarker right_lane_marker = 2; // adc车道的右侧车道线
repeated LaneMarker next_left_lane_marker = 3; // adc左车道的左侧车道线
repeated LaneMarker next_right_lane_marker = 4; // adc右车道的左侧车道线
}
// 车道上距ADC最近的车辆
message CIPVInfo {
optional int32 cipv_id = 1; // 最近车辆的ID
repeated int32 potential_cipv_id = 2; // 可能是最近车辆的ID(多个)
}
message PerceptionObstacles {
repeated PerceptionObstacle perception_obstacle = 1; // An array of obstacles
optional apollo.common.Header header = 2; // Header
optional apollo.common.ErrorCode error_code = 3 [default = OK]; // 错误码
optional LaneMarkers lane_marker = 4; // 本车道和左右相邻车道的标识(车道线多项式和车道边界类型)
optional CIPVInfo cipv_info = 5; // Closest In Path Vehicle (CIPV)
}
syntax = "proto2";
package apollo.prediction;
import "modules/common/proto/error_code.proto";
import "modules/common/proto/header.proto";
import "modules/prediction/proto/scenario.proto";
import "modules/perception/proto/perception_obstacle.proto";
import "modules/prediction/proto/feature.proto";
// estimated obstacle intent 预测障碍物的行为/意图
message ObstacleIntent {
enum Type {
UNKNOWN = 0;
STOP = 1;
STATIONARY = 2;
MOVING = 3;
CHANGE_LANE = 4;
LOW_ACCELERATION = 5;
HIGH_ACCELERATION = 6;
LOW_DECELERATION = 7;
HIGH_DECELERATION = 8;
}
optional Type type = 1 [default = UNKNOWN];
}
// self driving car intent 自车的驾驶意图
message Intent {
enum Type {
UNKNOWN = 0;
STOP = 1;
CRUISE = 2;
CHANGE_LANE = 3;
}
optional Type type = 1 [default = UNKNOWN];
}
message PredictionObstacle {
optional apollo.perception.PerceptionObstacle perception_obstacle = 1; // 障碍物信息
optional double timestamp = 2; // GPS time in seconds 时间戳
// the length of the time for this prediction (e.g. 10s)
optional double predicted_period = 3; // 预测周期
// can have multiple trajectories per obstacle
repeated Trajectory trajectory = 4; // 障碍物的预测轨迹,一个障碍物可能有多条预测轨迹,每条轨迹有概率值
// estimated obstacle intent
optional ObstacleIntent intent = 5; // 障碍物的意图
optional ObstaclePriority priority = 6; // 预测给出的障碍物的处理的等级: caution/normal/ignore
optional bool is_static = 7 [default = false]; // 是否是静态障碍物
// Feature history latest -> earliest sequence
repeated Feature feature = 8; // 用于做预测的特征信息, PNC模块不用关心,是prediction使用的
}
message PredictionObstacles {
// timestamp is included in header
optional apollo.common.Header header = 1;
// make prediction for multiple obstacles
repeated PredictionObstacle prediction_obstacle = 2;
// perception error code
optional apollo.common.ErrorCode perception_error_code = 3;
// start timestamp
optional double start_timestamp = 4;
// end timestamp
optional double end_timestamp = 5;
// self driving car intent
optional Intent intent = 6;
// Scenario
optional Scenario scenario = 7; // 场景信息, prediction模块使用的,PNC不用关心, Planning有自己的scenario,且二者定义不同
}
1.1.2 TrafficLightDetection
Perception Module
检测到交通信号灯.
1.1.2.1 proto目录
apollo\modules\perception\proto\traffic_light_detection.proto
1.1.2.2 proto定义
syntax = "proto2";
package apollo.perception;
import "modules/common/proto/header.proto";
message TrafficLightBox {
optional int32 x = 1;
optional int32 y = 2;
optional int32 width = 3;
optional int32 height = 4;
optional TrafficLight.Color color = 5;
optional bool selected = 6;
optional string camera_name = 7;
}
message TrafficLightDebug {
optional TrafficLightBox cropbox = 1;
repeated TrafficLightBox box = 2;
optional int32 signal_num = 3;
optional int32 valid_pos = 4;
optional double ts_diff_pos = 5;
optional double ts_diff_sys = 6;
optional int32 project_error = 7;
optional double distance_to_stop_line = 8;
optional int32 camera_id = 9 [deprecated = true];
repeated TrafficLightBox crop_roi = 10;
repeated TrafficLightBox projected_roi = 11;
repeated TrafficLightBox rectified_roi = 12;
repeated TrafficLightBox debug_roi = 13;
}
message TrafficLight {
enum Color {
UNKNOWN = 0;
RED = 1;
YELLOW = 2;
GREEN = 3;
BLACK = 4;
};
optional Color color = 1; // 信号灯颜色
// Traffic light string-ID in the map data.
optional string id = 2;
// How confidence about the detected results, between 0 and 1.
optional double confidence = 3 [default = 1.0]; // 置信度
// Duration of the traffic light since detected.
optional double tracking_time = 4;
// Is traffic blinking
optional bool blink = 5; // 交通灯是否闪烁
// v2x traffic light remaining time.
optional double remaining_time = 6; // 持续时间,用来估算变灯时间
}
message TrafficLightDetection {
optional apollo.common.Header header = 2;
repeated TrafficLight traffic_light = 1; // 交通灯,多个
optional TrafficLightDebug traffic_light_debug = 3; // debug信息,perception使用
optional bool contain_lights = 4; // ??
enum CameraID {
CAMERA_FRONT_LONG = 0;
CAMERA_FRONT_NARROW = 1;
CAMERA_FRONT_SHORT = 2;
CAMERA_FRONT_WIDE = 3;
};
optional CameraID camera_id = 5; // 哪个传感器检测到的?
}
apollo介绍之map模块
1.2.1 proto 目录
apollo\modules\map\proto\map.proto
1.2.2 proto定义
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_clear_area.proto";
import "modules/map/proto/map_crosswalk.proto";
import "modules/map/proto/map_junction.proto";
import "modules/map/proto/map_lane.proto";
import "modules/map/proto/map_overlap.proto";
import "modules/map/proto/map_signal.proto";
import "modules/map/proto/map_speed_bump.proto";
import "modules/map/proto/map_stop_sign.proto";
import "modules/map/proto/map_yield_sign.proto";
import "modules/map/proto/map_road.proto";
import "modules/map/proto/map_parking_space.proto";
import "modules/map/proto/map_pnc_junction.proto";
import "modules/map/proto/map_rsu.proto";
// This message defines how we project the ellipsoidal Earth surface to a plane.
message Projection {
// PROJ.4 setting:
// "+proj=tmerc +lat_0={origin.lat} +lon_0={origin.lon} +k={scale_factor}
// +ellps=WGS84 +no_defs"
optional string proj = 1;
}
message Header {
optional bytes version = 1;
optional bytes date = 2;
optional Projection projection = 3;
optional bytes district = 4;
optional bytes generation = 5;
optional bytes rev_major = 6;
optional bytes rev_minor = 7;
optional double left = 8;
optional double top = 9;
optional double right = 10;
optional double bottom = 11;
optional bytes vendor = 12;
}
message Map {
optional Header header = 1;
repeated Crosswalk crosswalk = 2; // 人行横道
repeated Junction junction = 3; // 路口
repeated Lane lane = 4; // lane
repeated StopSign stop_sign = 5; // 停车标识
repeated Signal signal = 6; // 交通标志
repeated YieldSign yield = 7; // 让行标识(国内没有,美国有)
repeated Overlap overlap = 8; // 任何一对在地图上重合的东西,包括(车道,路口,人行横道)
repeated ClearArea clear_area = 9; // 禁停区域
repeated SpeedBump speed_bump = 10; // 减速带
repeated Road road = 11; // road
repeated ParkingSpace parking_space = 12; // 停车场
repeated PNCJunction pnc_junction = 13; // PNC 路口
repeated RSU rsu = 14;
}
syntax = "proto2";
import "modules/common/proto/geometry.proto";
package apollo.hdmap;
// Polygon, not necessary convex.
message Polygon {
repeated apollo.common.PointENU point = 1;
}
// Straight line segment.
message LineSegment {
repeated apollo.common.PointENU point = 1;
}
// Generalization of a line.
message CurveSegment {
oneof curve_type {
LineSegment line_segment = 1;
}
optional double s = 6; // start position (s-coordinate)
optional apollo.common.PointENU start_position = 7;
optional double heading = 8; // start orientation
optional double length = 9;
}
// An object similar to a line but that need not be straight.
message Curve {
repeated CurveSegment segment = 1;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
// 车道线类型
message LaneBoundaryType {
enum Type {
UNKNOWN = 0;
DOTTED_YELLOW = 1;
DOTTED_WHITE = 2;
SOLID_YELLOW = 3;
SOLID_WHITE = 4;
DOUBLE_YELLOW = 5;
CURB = 6;
};
// Offset relative to the starting point of boundary
optional double s = 1;
// support multiple types
repeated Type types = 2;
}
// 车道线
message LaneBoundary {
optional Curve curve = 1; // 车道线的坐标点
optional double length = 2; // 长度
// indicate whether the lane boundary exists in real world
optional bool virtual = 3;
// in ascending order of s
repeated LaneBoundaryType boundary_type = 4; // 类型
}
// Association between central point to closest boundary.
// lane在每个采样点s处的车道宽度
message LaneSampleAssociation {
optional double s = 1;
optional double width = 2;
}
// road是由lane构成的
// A lane is part of a roadway, that is designated for use by a single line of
// vehicles.
// Most public roads (include highways) have more than two lanes.
message Lane {
optional Id id = 1;
// Central lane as reference trajectory, not necessary to be the geometry
// central.
// 车道中心线,不一定是车道的几何中心,在planning作为参考线.
// 是由一个个的lane segment(由两个坐标点构成的线段)组成的
optional Curve central_curve = 2;
// Lane boundary curve.
optional LaneBoundary left_boundary = 3; // 左侧车道线
optional LaneBoundary right_boundary = 4; // 右侧车道线
// in meters.
optional double length = 5; // 长度
// Speed limit of the lane, in meters per second.
optional double speed_limit = 6; // 限速
repeated Id overlap_id = 7;
// All lanes can be driving into (or from).
repeated Id predecessor_id = 8; // 先序,如果有多个,则本车道是汇入车道
repeated Id successor_id = 9; // 后序,如果有多个,则本车道是汇出车道
// Neighbor lanes on the same direction.
repeated Id left_neighbor_forward_lane_id = 10; // 同向的左侧相邻车道
repeated Id right_neighbor_forward_lane_id = 11; // 同向的右侧相邻车道
enum LaneType {
NONE = 1;
CITY_DRIVING = 2;
BIKING = 3;
SIDEWALK = 4;
PARKING = 5;
SHOULDER = 6;
};
optional LaneType type = 12; // 车道类型: 城市道路/自行车道路/人行道/停车区域/路肩
enum LaneTurn {
NO_TURN = 1;
LEFT_TURN = 2;
RIGHT_TURN = 3;
U_TURN = 4;
};
optional LaneTurn turn = 13; // 转弯类型
repeated Id left_neighbor_reverse_lane_id = 14; // 反向的左侧相邻车道
repeated Id right_neighbor_reverse_lane_id = 15; // 反向的右侧相邻车道
optional Id junction_id = 16; // 路口id
// Association between central point to closest boundary.
repeated LaneSampleAssociation left_sample = 17; //车道中心线到左侧车道线的宽度
repeated LaneSampleAssociation right_sample = 18; //车道中心线到左侧车道线的宽度
enum LaneDirection {
FORWARD = 1;
BACKWARD = 2;
BIDIRECTION = 3;
}
optional LaneDirection direction = 19; // 车道方向
// Association between central point to closest road boundary.
repeated LaneSampleAssociation left_road_sample = 20; //车道中心线到左侧road boudary的宽度
repeated LaneSampleAssociation right_road_sample = 21; //车道中心线到右侧road boudary的宽度
repeated Id self_reverse_lane_id = 22; // ??
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
message BoundaryEdge {
optional Curve curve = 1;
enum Type {
UNKNOWN = 0;
NORMAL = 1;
LEFT_BOUNDARY = 2;
RIGHT_BOUNDARY = 3;
};
optional Type type = 2;
}
message BoundaryPolygon {
repeated BoundaryEdge edge = 1;
}
// boundary with holes
message RoadBoundary {
optional BoundaryPolygon outer_polygon = 1;
// if boundary without hole, hole is null
repeated BoundaryPolygon hole = 2; // ???
}
message RoadROIBoundary {
optional Id id = 1;
repeated RoadBoundary road_boundaries = 2;
}
// road section defines a road cross-section, At least one section must be
// defined in order to
// use a road, If multiple road sections are defined, they must be listed in
// order along the road
message RoadSection {
optional Id id = 1;
// lanes contained in this section
repeated Id lane_id = 2; // section由多个lane构成
// boundary of section
optional RoadBoundary boundary = 3; // 道路边界
}
// The road is a collection of traffic elements, such as lanes, road boundary
// etc.
// It provides general information about the road.
message Road {
optional Id id = 1;
repeated RoadSection section = 2; // 将road沿纵向分成一个个的片段(section),每个section在横向上由一个个lane组成
// if lane road not in the junction, junction id is null.
optional Id junction_id = 3; // 路口id
enum Type {
UNKNOWN = 0;
HIGHWAY = 1;
CITY_ROAD = 2;
PARK = 3;
};
optional Type type = 4; // 道路类型, lane的类型会更多,更细致
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
// Crosswalk is a place designated for pedestrians to cross a road.
message Crosswalk {
optional Id id = 1;
optional Polygon polygon = 2; // 人行横道区域,是一个多边形
repeated Id overlap_id = 3; // 相互重叠区域的id
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
// 两条或者两条以上道路的交叉口
// A junction is the junction at-grade of two or more roads crossing.
message Junction {
optional Id id = 1;
optional Polygon polygon = 2;
repeated Id overlap_id = 3; // 相互重叠区域的id
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
// A stop sign is a traffic sign to notify drivers that they must stop before
// proceeding.
message StopSign {
optional Id id = 1;
repeated Curve stop_line = 2; // 停止线(由一系列的点构成)
repeated Id overlap_id = 3;
enum StopType {
UNKNOWN = 0;
ONE_WAY = 1;
TWO_WAY = 2;
THREE_WAY = 3;
FOUR_WAY = 4;
ALL_WAY = 5;
};
optional StopType type = 4;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/common/proto/geometry.proto";
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
message Subsignal {
enum Type {
UNKNOWN = 1;
CIRCLE = 2; // 环岛?
ARROW_LEFT = 3; // 左转
ARROW_FORWARD = 4; // 直行
ARROW_RIGHT = 5; // 右转
ARROW_LEFT_AND_FORWARD = 6; // 左转和直行
ARROW_RIGHT_AND_FORWARD = 7; // 右转和直行
ARROW_U_TURN = 8; // U turn
};
optional Id id = 1;
optional Type type = 2;
// Location of the center of the bulb. now no data support.
optional apollo.common.PointENU location = 3;
}
message SignInfo {
enum Type {
None = 0;
NO_RIGHT_TURN_ON_RED = 1; // 红灯时禁止右转
};
optional Type type = 1;
}
message Signal {
enum Type {
UNKNOWN = 1;
MIX_2_HORIZONTAL = 2;
MIX_2_VERTICAL = 3;
MIX_3_HORIZONTAL = 4;
MIX_3_VERTICAL = 5;
SINGLE = 6;
};
optional Id id = 1;
optional Polygon boundary = 2;
repeated Subsignal subsignal = 3;
// TODO: add orientation. now no data support.
repeated Id overlap_id = 4;
optional Type type = 5; // 交通标志的形状和数量
// stop line
repeated Curve stop_line = 6; // ??
repeated SignInfo sign_info = 7;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
// A yield indicates that each driver must prepare to stop if necessary to let a
// driver on another approach proceed.
// A driver who stops or slows down to let another vehicle through has yielded
// the right of way to that vehicle.
message YieldSign {
optional Id id = 1;
repeated Curve stop_line = 2;
repeated Id overlap_id = 3;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
message LaneOverlapInfo {
optional double start_s = 1; // position (s-coordinate)
optional double end_s = 2; // position (s-coordinate)
optional bool is_merge = 3; // 是否是汇入车道???
optional Id region_overlap_id = 4;
}
message SignalOverlapInfo {}
message StopSignOverlapInfo {}
message CrosswalkOverlapInfo {
optional Id region_overlap_id = 1;
}
message JunctionOverlapInfo {}
message YieldOverlapInfo {}
message ClearAreaOverlapInfo {}
message SpeedBumpOverlapInfo {}
message ParkingSpaceOverlapInfo {}
message PNCJunctionOverlapInfo {}
message RSUOverlapInfo {}
message RegionOverlapInfo {
optional Id id = 1;
repeated Polygon polygon = 2;
}
// Information about one object in the overlap.
message ObjectOverlapInfo {
optional Id id = 1;
oneof overlap_info {
LaneOverlapInfo lane_overlap_info = 3;
SignalOverlapInfo signal_overlap_info = 4;
StopSignOverlapInfo stop_sign_overlap_info = 5;
CrosswalkOverlapInfo crosswalk_overlap_info = 6;
JunctionOverlapInfo junction_overlap_info = 7;
YieldOverlapInfo yield_sign_overlap_info = 8;
ClearAreaOverlapInfo clear_area_overlap_info = 9;
SpeedBumpOverlapInfo speed_bump_overlap_info = 10;
ParkingSpaceOverlapInfo parking_space_overlap_info = 11;
PNCJunctionOverlapInfo pnc_junction_overlap_info = 12;
RSUOverlapInfo rsu_overlap_info = 13;
}
}
// Here, the "overlap" includes any pair of objects on the map
// (e.g. lanes, junctions, and crosswalks).
message Overlap {
optional Id id = 1;
// Information about one overlap, include all overlapped objects.
repeated ObjectOverlapInfo object = 2;
repeated RegionOverlapInfo region_overlap = 3;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
// A clear area means in which stopping car is prohibited
message ClearArea {
optional Id id = 1;
repeated Id overlap_id = 2;
optional Polygon polygon = 3;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
message SpeedBump {
optional Id id = 1;
repeated Id overlap_id = 2;
repeated Curve position = 3;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
// ParkingSpace is a place designated to park a car.
message ParkingSpace {
optional Id id = 1;
optional Polygon polygon = 2;
repeated Id overlap_id = 3;
optional double heading = 4;
}
// ParkingLot is a place for parking cars.
message ParkingLot {
optional Id id = 1;
optional Polygon polygon = 2;
repeated Id overlap_id = 3;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
import "modules/map/proto/map_geometry.proto";
message Passage {
optional Id id = 1;
repeated Id signal_id = 2;
repeated Id yield_id = 3;
repeated Id stop_sign_id = 4;
repeated Id lane_id = 5;
enum Type {
UNKNOWN = 0;
ENTRANCE = 1;
EXIT = 2;
};
optional Type type = 6;
};
message PassageGroup {
optional Id id = 1;
repeated Passage passage = 2;
};
message PNCJunction {
optional Id id = 1;
optional Polygon polygon = 2;
repeated Id overlap_id = 3;
repeated PassageGroup passage_group = 4;
}
syntax = "proto2";
package apollo.hdmap;
import "modules/map/proto/map_id.proto";
message RSU {
optional Id id = 1;
optional Id junction_id = 2;
repeated Id overlap_id = 3;
};
解析百度Apollo之Routing模块
1.3.1 proto 目录
apollo\modules\routing\proto\routing.proto
1.3.2 proto定义
syntax = "proto2";
package apollo.routing;
import "modules/common/proto/header.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/error_code.proto";
import "modules/map/proto/map_parking_space.proto";
message LaneWaypoint {
optional string id = 1;
optional double s = 2;
optional apollo.common.PointENU pose = 3;
}
message LaneSegment {
optional string id = 1;
optional double start_s = 2; // lanesegment 在lane上的起始位置
optional double end_s = 3; // lanesegment 在lane上的终止位置
}
message ParkingInfo {
optional string parking_space_id = 1;
optional apollo.common.PointENU parking_point = 2;
}
message RoutingRequest {
optional apollo.common.Header header = 1;
// at least two points. The first is start point, the end is final point.
// The routing must go through each point in waypoint.
repeated LaneWaypoint waypoint = 2; // 导航路径的查询点
repeated LaneSegment blacklisted_lane = 3; // ??
repeated string blacklisted_road = 4; // ??
optional bool broadcast = 5 [default = true]; // ??
optional apollo.hdmap.ParkingSpace parking_space = 6 [deprecated = true];
optional ParkingInfo parking_info = 7;
}
message Measurement {
optional double distance = 1;
}
enum ChangeLaneType {
FORWARD = 0;
LEFT = 1;
RIGHT = 2;
};
// passage是不是应该加上priority
message Passage {
repeated LaneSegment segment = 1; // passage的一段,包含了id和起止点信息
optional bool can_exit = 2; // 当前passage是否可以接续到Routing结果的另外一个passage上
optional ChangeLaneType change_lane_type = 3 [default = FORWARD]; //道路的类型,有FORWARD,LEFT,RIGHT三种取值。
}
message RoadSegment {
optional string id = 1;
repeated Passage passage = 2; // 车道,把lane分段,由lane segment组成
}
message RoutingResponse {
optional apollo.common.Header header = 1; // header
repeated RoadSegment road = 2; // road,由纵向分割的一个个section构成
optional Measurement measurement = 3; // 应该是到终点的距离
optional RoutingRequest routing_request = 4; // 全局规划(导航)的查询点(必经点)
// the map version which is used to build road graph
optional bytes map_version = 5;
optional apollo.common.StatusPb status = 6;
}
1.4.1 proto 目录
apollo\modules\localization\proto\localization.proto
1.4.2 proto定义
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
syntax = "proto2";
package apollo.localization;
import "modules/common/proto/header.proto";
import "modules/localization/proto/pose.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/pnc_point.proto";
import "modules/localization/proto/localization_status.proto";
message Uncertainty {
// Standard deviation of position, east/north/up in meters.
optional apollo.common.Point3D position_std_dev = 1;
// Standard deviation of quaternion qx/qy/qz, unitless.
optional apollo.common.Point3D orientation_std_dev = 2;
// Standard deviation of linear velocity, east/north/up in meters per second.
optional apollo.common.Point3D linear_velocity_std_dev = 3;
// Standard deviation of linear acceleration, right/forward/up in meters per
// square second.
optional apollo.common.Point3D linear_acceleration_std_dev = 4;
// Standard deviation of angular velocity, right/forward/up in radians per
// second.
optional apollo.common.Point3D angular_velocity_std_dev = 5;
// TODO: Define covariance items when needed.
}
message LocalizationEstimate {
optional apollo.common.Header header = 1;
optional apollo.localization.Pose pose = 2; // adc的位置,速度,加速度,航向角
optional Uncertainty uncertainty = 3; // 定位信息的标准差
// The time of pose measurement, seconds since 1970-1-1 (UNIX time).
optional double measurement_time = 4; // In seconds.
// Future trajectory actually driven by the drivers
repeated apollo.common.TrajectoryPoint trajectory_point = 5;
// msf status
optional MsfStatus msf_status = 6;
// msf quality
optional MsfSensorMsgStatus sensor_status = 7;
}
enum MeasureState {
OK = 0;
WARNNING = 1;
ERROR = 2;
CRITICAL_ERROR = 3;
FATAL_ERROR = 4;
}
message LocalizationStatus {
optional apollo.common.Header header = 1;
optional MeasureState fusion_status = 2;
optional MeasureState gnss_status = 3 [deprecated = true];
optional MeasureState lidar_status = 4 [deprecated = true];
// The time of pose measurement, seconds since 1970-1-1 (UNIX time).
optional double measurement_time = 5; // In seconds.
optional string state_message = 6;
}
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
syntax = "proto2";
package apollo.localization;
import "modules/common/proto/geometry.proto";
message Pose {
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
optional apollo.common.PointENU position = 1; // 车辆后轴中心的坐标(x, y, z),和地图是一样的坐标系
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
optional apollo.common.Quaternion orientation = 2;
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
optional apollo.common.Point3D linear_velocity = 3; // 地图所用坐标系
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per square second.
optional apollo.common.Point3D linear_acceleration = 4; // 地图所用坐标系
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
optional apollo.common.Point3D angular_velocity = 5; // 地图所用坐标系
// Heading
// The heading is zero when the car is facing East and positive when facing
// North.
optional double heading = 6;
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
optional apollo.common.Point3D linear_acceleration_vrf = 7; // 车体坐标系下的加速度
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
optional apollo.common.Point3D angular_velocity_vrf = 8; // 车体坐标系下的角速度
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
optional apollo.common.Point3D euler_angles = 9; // 欧拉角
}
1.5.1 proto 目录
apollo\modules\canbus\proto\chassis.proto
apollo\modules\common\proto\vehicle_signal.proto
1.5.2 proto定义
syntax = "proto2";
package apollo.canbus;
import "modules/common/proto/header.proto";
import "modules/common/proto/vehicle_signal.proto";
import "modules/common/proto/drive_state.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/configs/proto/vehicle_config.proto";
// next id :31
message Chassis
// 驾驶模式
enum DrivingMode {
COMPLETE_MANUAL = 0; // human drive
COMPLETE_AUTO_DRIVE = 1; // 自动驾驶模式
AUTO_STEER_ONLY = 2; // only steer
AUTO_SPEED_ONLY = 3; // include throttle and brake
// security mode when manual intervention happens, only response status
EMERGENCY_MODE = 4;
}
enum ErrorCode {
NO_ERROR = 0;
CMD_NOT_IN_PERIOD = 1; // control cmd not in period, Control cmd超时
// car chassis report error, like steer, brake, throttle, gear fault
CHASSIS_ERROR = 2;
// classify the types of the car chassis errors
CHASSIS_ERROR_ON_STEER = 6;
CHASSIS_ERROR_ON_BRAKE = 7;
CHASSIS_ERROR_ON_THROTTLE = 8;
CHASSIS_ERROR_ON_GEAR = 9;
MANUAL_INTERVENTION = 3; // human manual intervention
// receive car chassis can frame not in period
CHASSIS_CAN_NOT_IN_PERIOD = 4; // 底盘反馈消息超时
UNKNOWN_ERROR = 5;
}
// 档位信息
enum GearPosition {
GEAR_NEUTRAL = 0;
GEAR_DRIVE = 1;
GEAR_REVERSE = 2;
GEAR_PARKING = 3;
GEAR_LOW = 4;
GEAR_INVALID = 5;
GEAR_NONE = 6;
}
optional bool engine_started = 3; // 发动机点火标志位
// Engine speed in RPM.
optional float engine_rpm = 4 [default = nan];
// Vehicle Speed in meters per second.
optional float speed_mps = 5 [default = nan];
// Vehicle odometer in meters.
optional float odometer_m = 6 [default = nan]; // 里程计
// Fuel range in meters.
optional int32 fuel_range_m = 7;
// Real throttle location in [%], ranging from 0 to 100.
optional float throttle_percentage = 8 [default = nan];
// Real brake location in [%], ranging from 0 to 100.
optional float brake_percentage = 9 [default = nan];
// Real steering location in [%], ranging from -100 to 100.
// steering_angle / max_steering_angle
// Clockwise: negative
// CountClockwise: positive
optional float steering_percentage = 11 [default = nan];
// Applied steering torque in [Nm].
optional float steering_torque_nm = 12 [default = nan]; // 驾驶员施加在方向盘上的转矩??
// Parking brake status.
optional bool parking_brake = 13; // 驻车制动,手刹
// Light signals.
optional bool high_beam_signal = 14 [deprecated = true]; // 远光灯
optional bool low_beam_signal = 15 [deprecated = true]; // 近光灯
optional bool left_turn_signal = 16 [deprecated = true]; // 左转灯
optional bool right_turn_signal = 17 [deprecated = true]; // 右转灯
optional bool horn = 18 [deprecated = true]; // 喇叭
optional bool wiper = 19; // 雨刮器
optional bool disengage_status = 20 [deprecated = true]; // 驾驶员是否脱离方向盘???
optional DrivingMode driving_mode = 21 [default = COMPLETE_MANUAL]; // 驾驶模式
optional ErrorCode error_code = 22 [default = NO_ERROR]; // 错误码
optional GearPosition gear_location = 23; // 档位
// timestamp for steering module
optional double steering_timestamp = 24; // In seconds, with 1e-6 accuracy
// chassis also needs it own sending timestamp
optional apollo.common.Header header = 25;
optional int32 chassis_error_mask = 26 [default = 0]; // 错误码掩码??
optional apollo.common.VehicleSignal signal = 27; // 驾驶员输入信号: 转向\灯光\喇叭
// Only available for Lincoln now
optional ChassisGPS chassis_gps = 28;
optional apollo.common.EngageAdvice engage_advice = 29;
optional WheelSpeed wheel_speed = 30; // 轮速信息
optional Surround surround = 31;
// Vehicle registration information
optional License license = 32 [deprecated = true];
// Real gear location.
// optional int32 gear_location = 10 [deprecated = true]; deprecated use enum
// replace this [id 23]
optional apollo.common.VehicleID vehicle_id = 33;
optional int32 battery_soc_percentage = 34 [default = -1];
}
message ChassisGPS {
optional double latitude = 1;
optional double longitude = 2;
optional bool gps_valid = 3;
optional int32 year = 4;
optional int32 month = 5;
optional int32 day = 6;
optional int32 hours = 7;
optional int32 minutes = 8;
optional int32 seconds = 9;
optional double compass_direction = 10;
optional double pdop = 11;
optional bool is_gps_fault = 12;
optional bool is_inferred = 13;
optional double altitude = 14;
optional double heading = 15;
optional double hdop = 16;
optional double vdop = 17;
optional GpsQuality quality = 18;
optional int32 num_satellites = 19;
optional double gps_speed = 20;
}
enum GpsQuality {
FIX_NO = 0;
FIX_2D = 1;
FIX_3D = 2;
FIX_INVALID = 3;
}
message WheelSpeed {
enum WheelSpeedType {
FORWARD = 0;
BACKWARD = 1;
STANDSTILL = 2;
INVALID = 3;
}
optional bool is_wheel_spd_rr_valid = 1 [default = false];
optional WheelSpeedType wheel_direction_rr = 2 [default = INVALID];
optional double wheel_spd_rr = 3 [default = 0.0];
optional bool is_wheel_spd_rl_valid = 4 [default = false];
optional WheelSpeedType wheel_direction_rl = 5 [default = INVALID];
optional double wheel_spd_rl = 6 [default = 0.0];
optional bool is_wheel_spd_fr_valid = 7 [default = false];
optional WheelSpeedType wheel_direction_fr = 8 [default = INVALID];
optional double wheel_spd_fr = 9 [default = 0.0];
optional bool is_wheel_spd_fl_valid = 10 [default = false];
optional WheelSpeedType wheel_direction_fl = 11 [default = INVALID];
optional double wheel_spd_fl = 12 [default = 0.0];
}
// 雷达
message Sonar {
optional double range = 1; // Meter, 距离
optional apollo.common.Point3D translation = 2; // Meter
optional apollo.common.Quaternion rotation = 3;
}
message Surround {
optional bool cross_traffic_alert_left = 1; // 左侧车辆报警
optional bool cross_traffic_alert_left_enabled = 2; // 左侧车辆报警是否使能
optional bool blind_spot_left_alert = 3; // 左侧盲区报警
optional bool blind_spot_left_alert_enabled = 4;
optional bool cross_traffic_alert_right = 5; // 右侧车辆报警
optional bool cross_traffic_alert_right_enabled = 6;
optional bool blind_spot_right_alert = 7; // 右侧盲区报警
optional bool blind_spot_right_alert_enabled = 8;
optional double sonar00 = 9; // 毫米波雷达测距
optional double sonar01 = 10;
optional double sonar02 = 11;
optional double sonar03 = 12;
optional double sonar04 = 13;
optional double sonar05 = 14;
optional double sonar06 = 15;
optional double sonar07 = 16;
optional double sonar08 = 17;
optional double sonar09 = 18;
optional double sonar10 = 19;
optional double sonar11 = 20;
optional bool sonar_enabled = 21; // 雷达是否使用
optional bool sonar_fault = 22; // 雷达是否故障
repeated double sonar_range = 23; // 雷达测距范围
repeated Sonar sonar = 24;
}
message License {
optional string vin = 1 [deprecated = true];
}
syntax = "proto2";
package apollo.common;
// 驾驶员输入信号
message VehicleSignal {
enum TurnSignal {
TURN_NONE = 0;
TURN_LEFT = 1;
TURN_RIGHT = 2;
};
optional TurnSignal turn_signal = 1; // 转向信号
// lights enable command
optional bool high_beam = 2; // 远光灯是否打开
optional bool low_beam = 3; // 近光灯是否打开
optional bool horn = 4; // 喇叭是否打开
optional bool emergency_light = 5; // 双闪是否打开
}
在L2
或者L3
智能驾驶系统中,没有Pad
输入,此功能是Robotaix
中乘客的指令信息.
1.6.1 proto 目录
apollo\modules\planning\proto\pad.proto
1.6.2 proto定义
syntax = "proto2";
package apollo.planning;
import "modules/common/proto/header.proto";
enum DrivingAction {
NONE = 100;
FOLLOW = 0; // acc
CHANGE_LEFT = 1; // 向左换道
CHANGE_RIGHT = 2; // 向右换道
PULL_OVER = 3; // 靠边停车
STOP = 4; // 停车
RESUME_CRUISE = 5; // 启动
};
message PadMessage {
optional apollo.common.Header header = 1;
// driving action
optional DrivingAction action = 2;
}
Storytelling Module
发布的信息和Planning
中的scenarios
相似,Planning
并未对Storytelling
信息做响应或者处理.
1.7.1 proto 目录
apollo\modules\storytelling\proto\story.proto
1.7.2 proto定义
syntax = "proto2";
package apollo.storytelling;
import "modules/common/proto/header.proto";
message CloseToCrosswalk {
optional string id = 1;
optional double distance = 2 [default = nan];
}
message CloseToClearArea {
optional string id = 1;
optional double distance = 2 [default = nan];
}
message CloseToJunction {
enum JunctionType {
PNC_JUNCTION = 1;
JUNCTION = 2;
};
optional string id = 1;
optional JunctionType type = 2;
optional double distance = 3 [default = nan];
}
message CloseToSignal {
optional string id = 1;
optional double distance = 2 [default = nan];
}
message CloseToStopSign {
optional string id = 1;
optional double distance = 2 [default = nan];
}
message CloseToYieldSign {
optional string id = 1;
optional double distance = 2 [default = nan];
}
// Usage guide for action modules:
// 1. Call `stories.has_XXX()` to check if a story you are interested is in
// charge.
// 2. Access the story details if necessary, and take action accordingly.
message Stories {
optional apollo.common.Header header = 1;
optional CloseToClearArea close_to_clear_area = 2;
optional CloseToCrosswalk close_to_crosswalk = 3;
optional CloseToJunction close_to_junction = 4;
optional CloseToSignal close_to_signal = 5;
optional CloseToStopSign close_to_stop_sign = 6;
optional CloseToYieldSign close_to_yield_sign = 7;
}
2.1.1 proto 目录
apollo\modules\planning\proto\planning.proto
2.1.2 proto定义
syntax = "proto2";
package apollo.planning;
import "modules/canbus/proto/chassis.proto";
import "modules/common/proto/drive_state.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/header.proto";
import "modules/common/proto/pnc_point.proto";
import "modules/map/proto/map_id.proto";
import "modules/planning/proto/decision.proto";
import "modules/planning/proto/planning_internal.proto";
message EStop {
// is_estop == true when emergency stop is required
optional bool is_estop = 1; // 是否紧急刹车
optional string reason = 2; // 紧急刹车原因
}
message TaskStats {
optional string name = 1;
optional double time_ms = 2;
}
message LatencyStats {
optional double total_time_ms = 1;
repeated TaskStats task_stats = 2;
optional double init_frame_time_ms = 3;
}
message RSSInfo {
optional bool is_rss_safe = 1;
optional double cur_dist_lon = 2;
optional double rss_safe_dist_lon = 3;
optional double acc_lon_range_minimum = 4;
optional double acc_lon_range_maximum = 5;
optional double acc_lat_left_range_minimum = 6;
optional double acc_lat_left_range_maximum = 7;
optional double acc_lat_right_range_minimum = 8;
optional double acc_lat_right_range_maximum = 9;
}
// next id: 24
message ADCTrajectory {
optional apollo.common.Header header = 1;
optional double total_path_length = 2; // in meters, 规划轨迹的长度
optional double total_path_time = 3; // in seconds, 规划轨迹的时间
// path data + speed data
repeated apollo.common.TrajectoryPoint trajectory_point = 12; // 轨迹
optional EStop estop = 6; // 紧急刹车
// path point without speed info
repeated apollo.common.PathPoint path_point = 13; // 规划的路径信息(没有速度信息)
// is_replan == true mean replan triggered
optional bool is_replan = 9 [default = false]; // 是否是重规划的轨迹
optional string replan_reason = 22; // 重规划的原因
// Specify trajectory gear
optional apollo.canbus.Chassis.GearPosition gear = 10; // 档位信息
optional apollo.planning.DecisionResult decision = 14; //决策信息:任务决策(停车换道等),障碍物决策,灯光信息(转向信号等)
optional LatencyStats latency_stats = 15; // Planning的运算时间造成的延迟
// the routing used for current planning result
optional apollo.common.Header routing_header = 16; // 导航信息的时间戳
optional apollo.planning_internal.Debug debug = 8; // planning的debug信息
enum RightOfWayStatus { // adc右侧道路的状态:有保护,无保护
UNPROTECTED = 0;
PROTECTED = 1;
}
optional RightOfWayStatus right_of_way_status = 17;
// lane id along current reference line
repeated apollo.hdmap.Id lane_id = 18; // 当前车道id
// set the engage advice for based on current planning result.
optional apollo.common.EngageAdvice engage_advice = 19;
// the region where planning cares most
message CriticalRegion {
repeated apollo.common.Polygon region = 1;
}
// critical region will be empty when planning is NOT sure which region is
// critical
// critical regions may or may not overlap
optional CriticalRegion critical_region = 20; //
enum TrajectoryType {
UNKNOWN = 0;
NORMAL = 1; // 规划求解成功
PATH_FALLBACK = 2; // 路径规划求解失败,fallback
SPEED_FALLBACK = 3; // 速度规划求解失败,fallback
PATH_REUSED = 4; // 复用上一帧路路径
}
optional TrajectoryType trajectory_type = 21 [default = UNKNOWN];
// lane id along target reference line
repeated apollo.hdmap.Id target_lane_id = 23; // 目标车道ID
// output related to RSS
optional RSSInfo rss_info = 100;
}
2.2.1 proto 目录
apollo\modules\routing\proto\routing.proto
2.2.2 proto定义
见Routing Input
2.3.1 proto 目录
apollo\modules\planning\proto\leaning_data.proto
2.3.2 proto定义
syntax = "proto2";
package apollo.planning;
import "modules/canbus/proto/chassis.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/header.proto";
import "modules/common/proto/pnc_point.proto";
import "modules/map/proto/map_lane.proto";
import "modules/perception/proto/perception_obstacle.proto";
import "modules/prediction/proto/feature.proto";
import "modules/prediction/proto/prediction_obstacle.proto";
import "modules/perception/proto/traffic_light_detection.proto";
import "modules/routing/proto/routing.proto";
message OverlapFeature {
optional string id = 1;
optional double distance = 2;
}
message PlanningTag {
optional apollo.hdmap.Lane.LaneTurn lane_turn = 1;
optional OverlapFeature clear_area = 2;
optional OverlapFeature crosswalk = 3;
optional OverlapFeature pnc_junction = 4;
optional OverlapFeature signal = 5;
optional OverlapFeature stop_sign = 6;
optional OverlapFeature yield_sign = 7;
}
message ChassisFeature {
optional double message_timestamp_sec = 1;
// Features from chassis
// Vehicle Speed in meters per second.
optional float speed_mps = 2;
// Real throttle location in [%], ranging from 0 to 100.
optional float throttle_percentage = 3;
// Real brake location in [%], ranging from 0 to 100.
optional float brake_percentage = 4;
// Real steering location in [%], ranging from -100 to 100.
// steering_angle / max_steering_angle
// Clockwise: negative
// CountClockwise: positive
optional float steering_percentage = 5;
optional apollo.canbus.Chassis.GearPosition gear_location = 6;
}
message LocalizationFeature {
optional double message_timestamp_sec = 1;
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
optional apollo.common.PointENU position = 2;
// Heading
// The heading is zero when the car is facing East and positive when facing
// North.
optional double heading = 3;
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
optional apollo.common.Point3D linear_velocity = 4;
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
optional apollo.common.Point3D linear_acceleration = 5;
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
optional apollo.common.Point3D angular_velocity = 6;
}
// based on apollo.common.PathPoint
message CommonPathPointFeature {
// coordinates
optional double x = 1;
optional double y = 2;
optional double z = 3;
// direction on the x-y plane
optional double theta = 4;
// accumulated distance from beginning of the path
optional double s = 5;
// The lane ID where the path point is on
optional string lane_id = 6;
}
// based on apollo.commom.TrajectoryPoint
message CommonTrajectoryPointFeature {
// path point
optional CommonPathPointFeature path_point = 1;
// linear velocity
optional double v = 2; // in [m/s]
// linear acceleration
optional double a = 3;
// relative time from beginning of the trajectory
optional double relative_time = 4;
// Gaussian probability information
optional apollo.common.GaussianInfo gaussian_info = 5;
}
message TrajectoryPointFeature {
optional double timestamp_sec = 1;
optional CommonTrajectoryPointFeature trajectory_point = 2;
}
// based on apollo.perception.PerceptionObstacle
message PerceptionObstacleFeature {
optional double timestamp_sec = 1; // GPS time in seconds
// obstacle position in the OBJECT coordinate system
optional apollo.common.Point3D position = 2;
// heading in RELATIVE coordinate to ADC
optional double theta = 3;
// obstacle velocity in RELATIVE coordinate to ADC
optional apollo.common.Point3D velocity = 4; // obstacle velocity
// obstacle acceleration in RELATIVE coordinate to ADC
optional apollo.common.Point3D acceleration = 5;
// obstacle corner points in RELATIVE coordinate to ADC
repeated apollo.common.Point3D polygon_point = 6;
}
// obstacle的序列化信息
message ObstacleTrajectoryFeature {
repeated PerceptionObstacleFeature perception_obstacle_history = 1;
repeated TrajectoryPointFeature evaluated_trajectory_point = 2;
}
// based on apollo.prediction.Trajectory
message PredictionTrajectoryFeature {
optional double probability = 1; // probability of this trajectory
repeated TrajectoryPointFeature trajectory_point = 2;
}
// based on apollo.prediction.PredictionObstacle
message PredictionObstacleFeature {
optional double timestamp_sec = 1; // GPS time in seconds
optional double predicted_period = 2;
optional apollo.prediction.ObstacleIntent intent = 3;
optional apollo.prediction.ObstaclePriority priority = 4;
optional bool is_static = 5 [default = false];
// can have multiple trajectories per obstacle
repeated PredictionTrajectoryFeature trajectory = 6;
}
message ObstacleFeature {
optional int32 id = 1; // obstacle ID.
optional double length = 2; // obstacle length.
optional double width = 3; // obstacle width.
optional double height = 4; // obstacle height.
optional apollo.perception.PerceptionObstacle.Type type = 5; // obstacle type
optional ObstacleTrajectoryFeature obstacle_trajectory = 6;
optional PredictionObstacleFeature obstacle_prediction = 7;
}
// based on apollo.routing.RoutingResponse
message RoutingResponseFeature {
repeated apollo.routing.RoadSegment road = 1;
optional apollo.routing.Measurement measurement = 2;
}
message RoutingFeature {
optional RoutingResponseFeature routing_response = 1;
repeated string local_routing_lane_id = 2; // local routing close to ADC
optional RoutingResponseFeature local_routing = 3;
}
// based on apollo.perception.TrafficLight
message TrafficLightFeature {
optional apollo.perception.TrafficLight.Color color = 1;
optional string id = 2;
optional double confidence = 3 [default = 1.0];
optional double tracking_time = 4;
optional bool blink = 5;
optional double remaining_time = 6;
}
message TrafficLightDetectionFeature {
optional double message_timestamp_sec = 1;
repeated TrafficLightFeature traffic_light = 2;
}
message ADCTrajectoryPoint {
optional double timestamp_sec = 1; // localization measurement_time
optional PlanningTag planning_tag = 2;
optional CommonTrajectoryPointFeature trajectory_point = 3;
}
message LearningOutput {
repeated TrajectoryPointFeature adc_future_trajectory_point = 1;
}
message LearningDataFrame {
// localization message publishing time in seconds.
optional double message_timestamp_sec = 1;
optional uint32 frame_num = 2;
optional string map_name = 3;
optional PlanningTag planning_tag = 4;
// Features from chassis
optional ChassisFeature chassis = 5;
// Features from localization
optional LocalizationFeature localization = 6;
// Features from perception obstacles
repeated ObstacleFeature obstacle = 7;
// Features from routing
optional RoutingFeature routing = 8;
// Features from traffic_light_detection
optional TrafficLightDetectionFeature traffic_light_detection = 9;
// ADC past trajectory
repeated ADCTrajectoryPoint adc_trajectory_point = 10;
// Learning ouputs
optional LearningOutput output = 11;
}
message LearningData {
repeated LearningDataFrame learning_data_frame = 1;
}
message PlanningLearningData {
optional apollo.common.Header header = 1;
optional LearningDataFrame learning_data_frame = 2;
}