概述
MessageProcess类是apollo planning模块下modules\planning\common\message_process.cc/.h实现
这个类主要也是为了基于学习的决策规划的数据处理,暂且略过,不详细解读。
在modules\planning\planning_component.cc的这段代码可以看出,只有在学习模式下才会调用该类的成员函数。
从类名来看,消息处理?
从代码来看MessageProcess类主要是实现:
主要也是为了基于学习的决策规划的数据处理
message_process.h
#pragma once
#include <chrono>
#include <fstream>
#include <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include "modules/canbus/proto/chassis.pb.h"
#include "modules/dreamview/proto/hmi_status.pb.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/planning/common/dependency_injector.h"
#include "modules/planning/proto/learning_data.pb.h"
#include "modules/planning/proto/planning_config.pb.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
#include "modules/routing/proto/routing.pb.h"
#include "modules/storytelling/proto/story.pb.h"
namespace apollo {
namespace planning {
class MessageProcess {
public:
bool Init(const PlanningConfig& planning_config);
bool Init(const PlanningConfig& planning_config,
const std::shared_ptr<DependencyInjector>& injector);
void Close();
void OnChassis(const apollo::canbus::Chassis& chassis);
void OnHMIStatus(apollo::dreamview::HMIStatus hmi_status);
void OnLocalization(const apollo::localization::LocalizationEstimate& le);
void OnPrediction(
const apollo::prediction::PredictionObstacles& prediction_obstacles);
void OnRoutingResponse(
const apollo::routing::RoutingResponse& routing_response);
void OnStoryTelling(const apollo::storytelling::Stories& stories);
void OnTrafficLightDetection(
const apollo::perception::TrafficLightDetection& traffic_light_detection);
void ProcessOfflineData(const std::string& record_file);
private:
struct ADCCurrentInfo {
std::pair<double, double> adc_cur_position_;
std::pair<double, double> adc_cur_velocity_;
std::pair<double, double> adc_cur_acc_;
double adc_cur_heading_;
};
apollo::hdmap::LaneInfoConstPtr GetCurrentLane(
const apollo::common::PointENU& position);
bool GetADCCurrentRoutingIndex(int* adc_road_index, int* adc_passage_index,
double* adc_passage_s);
int GetADCCurrentInfo(ADCCurrentInfo* adc_curr_info);
void GenerateObstacleTrajectory(const int frame_num, const int obstacle_id,
const ADCCurrentInfo& adc_curr_info,
ObstacleFeature* obstacle_feature);
void GenerateObstaclePrediction(
const int frame_num,
const apollo::prediction::PredictionObstacle& prediction_obstacle,
const ADCCurrentInfo& adc_curr_info, ObstacleFeature* obstacle_feature);
void GenerateObstacleFeature(LearningDataFrame* learning_data_frame);
bool GenerateLocalRouting(
const int frame_num,
RoutingResponseFeature* local_routing,
std::vector<std::string>* local_routing_lane_ids);
void GenerateRoutingFeature(
const RoutingResponseFeature& local_routing,
const std::vector<std::string>& local_routing_lane_ids,
LearningDataFrame* learning_data_frame);
void GenerateTrafficLightDetectionFeature(
LearningDataFrame* learning_data_frame);
void GenerateADCTrajectoryPoints(
const std::list<apollo::localization::LocalizationEstimate>&
localizations,
LearningDataFrame* learning_data_frame);
void GeneratePlanningTag(LearningDataFrame* learning_data_frame);
bool GenerateLearningDataFrame(LearningDataFrame* learning_data_frame);
private:
std::shared_ptr<DependencyInjector> injector_;
PlanningConfig planning_config_;
std::chrono::time_point<std::chrono::system_clock> start_time_;
std::ofstream log_file_;
std::string record_file_;
std::unordered_map<std::string, std::string> map_m_;
LearningData learning_data_;
int learning_data_file_index_ = 0;
std::list<apollo::localization::LocalizationEstimate> localizations_;
std::unordered_map<int, apollo::prediction::PredictionObstacle>
prediction_obstacles_map_;
std::unordered_map<int, std::list<PerceptionObstacleFeature>>
obstacle_history_map_;
ChassisFeature chassis_feature_;
std::string map_name_;
PlanningTag planning_tag_;
apollo::routing::RoutingResponse routing_response_;
double traffic_light_detection_message_timestamp_;
std::vector<TrafficLightFeature> traffic_lights_;
int total_learning_data_frame_num_ = 0;
double last_localization_message_timestamp_sec_ = 0.0;
};
} // namespace planning
} // namespace apollo
message_process.cc
#include "modules/planning/common/message_process.h"
#include <algorithm>
#include <cmath>
#include <memory>
#include <string>
#include "cyber/common/file.h"
#include "cyber/record/record_reader.h"
#include "cyber/time/clock.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/util/point_factory.h"
#include "modules/common/util/util.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/proto/map_lane.pb.h"
#include "modules/planning/common/feature_output.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/util/math_util.h"
namespace apollo {
namespace planning {
using apollo::canbus::Chassis;
using apollo::cyber::Clock;
using apollo::cyber::record::RecordMessage;
using apollo::cyber::record::RecordReader;
using apollo::dreamview::HMIStatus;
using apollo::hdmap::ClearAreaInfoConstPtr;
using apollo::hdmap::CrosswalkInfoConstPtr;
using apollo::hdmap::HDMapUtil;
using apollo::hdmap::JunctionInfoConstPtr;
using apollo::hdmap::LaneInfoConstPtr;
using apollo::hdmap::PNCJunctionInfoConstPtr;
using apollo::hdmap::SignalInfoConstPtr;
using apollo::hdmap::StopSignInfoConstPtr;
using apollo::hdmap::YieldSignInfoConstPtr;
using apollo::localization::LocalizationEstimate;
using apollo::perception::TrafficLightDetection;
using apollo::prediction::PredictionObstacle;
using apollo::prediction::PredictionObstacles;
using apollo::routing::RoutingResponse;
using apollo::storytelling::CloseToJunction;
using apollo::storytelling::Stories;
//消息处理类的Init初始化函数 输入参数是规划配置类对象planning_config
//PlanningConfig类,就是定义规划配置文件数据结构的一个类,由protobuf中的message生成
//modules\planning\proto\planning_config.proto
//有了这个规划配置类的定义,可以直接将配置文件
//modules\planning\conf\planning_config.pb.txt中的配置数据直接读入到该类对象中
//更改配置只要更改配置文件中的参数,而无需再次编译
//规划配置文件里或者这个规划配置类数据结构包括信息:学习模式类型,与其他模块cyber通信的
//topic,rtk/standard/导航模式的规划配置,以standard模式规划配置为例,其又
//包含standard模式采用的规划器类型:RTK/PUBLIC_ROAD/NAVI/LATTICE
//这个函数其实就是把输入参数规划配置对象读到自己的数据成员里,然后把所有地图名和地图文
//件名的映射map建立起来,最后获取地图路径+文件名字符串,清除数据成员历史障碍物特征数据?
bool MessageProcess::Init(const PlanningConfig& planning_config) {
planning_config_.CopyFrom(planning_config); //直接用输入参数初始化自身数据成员
//planning_config_数据成员规划配置
//建立地图名-地图文件名的映射关系key-value 映射map
//地图文件路径位于modules\map\data\,貌似下面这几张地图被apollo做成隐藏文件了
//所以看不到?路径都是指向这里
map_m_["Sunnyvale"] = "sunnyvale";
map_m_["Sunnyvale Big Loop"] = "sunnyvale_big_loop";
map_m_["Sunnyvale With Two Offices"] = "sunnyvale_with_two_offices";
map_m_["Gomentum"] = "gomentum";
map_m_["Sunnyvale Loop"] = "sunnyvale_loop";
map_m_["San Mateo"] = "san_mateo";
//这一句没太看懂,总之就是获取地图路径+文件名构成的字符串?
//map_name_,像这类名字后带下划线的变量,在apollo中通常是表示类的内部的数据成员
map_name_ = FLAGS_map_dir.substr(FLAGS_map_dir.find_last_of("/") + 1);
//障碍物历史映射map清空数据,储存序号和预测障碍物特征对象的map
obstacle_history_map_.clear();
//这个if是关于基于学习的决策规划,目前不涉及,暂时略过
if (FLAGS_planning_offline_learning) {
// offline process logging
log_file_.open(FLAGS_planning_data_dir + "/learning_data.log",
std::ios_base::out | std::ios_base::app);
start_time_ = std::chrono::system_clock::now();
std::time_t now = std::time(nullptr);
log_file_ << "UTC date and time: " << std::asctime(std::gmtime(&now))
<< "Local date and time: " << std::asctime(std::localtime(&now));
}
return true;
}
//重载Init函数,多了一个参数injector
//DependencyInjector类也是把规划依赖的信息打包成了一个类,包含:规划状态,自车状态,
//规划的历史数据等
bool MessageProcess::Init(const PlanningConfig& planning_config,
const std::shared_ptr<DependencyInjector>& injector) {
//除了将输入参数injector复制给数据成员injector_ ,然后剩下的又是调用上面的Init函数
injector_ = injector;
return Init(planning_config);
}
void MessageProcess::Close() {
FeatureOutput::Clear();
if (FLAGS_planning_offline_learning) {
// offline process logging
const std::string msg = absl::StrCat(
"Total learning_data_frame number: ", total_learning_data_frame_num_);
AINFO << msg;
log_file_ << msg << std::endl;
auto end_time = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end_time - start_time_;
log_file_ << "Time elapsed(sec): " << elapsed_seconds.count() << std::endl
<< std::endl;
log_file_.close();
}
}
void MessageProcess::OnChassis(const apollo::canbus::Chassis& chassis) {
chassis_feature_.set_message_timestamp_sec(chassis.header().timestamp_sec());
chassis_feature_.set_speed_mps(chassis.speed_mps());
chassis_feature_.set_throttle_percentage(chassis.throttle_percentage());
chassis_feature_.set_brake_percentage(chassis.brake_percentage());
chassis_feature_.set_steering_percentage(chassis.steering_percentage());
chassis_feature_.set_gear_location(chassis.gear_location());
}
void MessageProcess::OnHMIStatus(apollo::dreamview::HMIStatus hmi_status) {
const std::string& current_map = hmi_status.current_map();
if (map_m_.count(current_map) > 0) {
map_name_ = map_m_[current_map];
const std::string& map_base_folder = "/apollo/modules/map/data/";
FLAGS_map_dir = map_base_folder + map_name_;
}
}
void MessageProcess::OnLocalization(const LocalizationEstimate& le) {
if (last_localization_message_timestamp_sec_ == 0.0) {
last_localization_message_timestamp_sec_ = le.header().timestamp_sec();
}
const double time_diff =
le.header().timestamp_sec() - last_localization_message_timestamp_sec_;
if (time_diff < 1.0 / FLAGS_planning_loop_rate) {
// for RL_TEST, E2E_TEST or HYBRID_TEST skip this check so that first
// frame can proceed
if (!(planning_config_.learning_mode() == PlanningConfig::RL_TEST ||
planning_config_.learning_mode() == PlanningConfig::E2E_TEST ||
planning_config_.learning_mode() == PlanningConfig::HYBRID_TEST)) {
return;
}
}
if (time_diff >= (1.0 * 2 / FLAGS_planning_loop_rate)) {
const std::string msg = absl::StrCat(
"missing localization too long: time_stamp[",
le.header().timestamp_sec(), "] time_diff[", time_diff, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
}
last_localization_message_timestamp_sec_ = le.header().timestamp_sec();
localizations_.push_back(le);
while (!localizations_.empty()) {
if (localizations_.back().header().timestamp_sec() -
localizations_.front().header().timestamp_sec() <=
FLAGS_trajectory_time_length) {
break;
}
localizations_.pop_front();
}
ADEBUG << "OnLocalization: size[" << localizations_.size() << "] time_diff["
<< localizations_.back().header().timestamp_sec() -
localizations_.front().header().timestamp_sec()
<< "]";
// generate one frame data
LearningDataFrame learning_data_frame;
if (GenerateLearningDataFrame(&learning_data_frame)) {
// output
if (FLAGS_planning_offline_learning) {
// offline
FeatureOutput::InsertLearningDataFrame(record_file_, learning_data_frame);
} else {
// online
injector_->learning_based_data()->InsertLearningDataFrame(
learning_data_frame);
}
}
}
void MessageProcess::OnPrediction(
const PredictionObstacles& prediction_obstacles) {
prediction_obstacles_map_.clear();
for (int i = 0; i < prediction_obstacles.prediction_obstacle_size(); ++i) {
const auto& prediction_obstacle =
prediction_obstacles.prediction_obstacle(i);
const int obstacle_id = prediction_obstacle.perception_obstacle().id();
prediction_obstacles_map_[obstacle_id].CopyFrom(prediction_obstacle);
}
// erase obstacle history if obstacle not exist in current predictio msg
/* comment out to relax this check for now
std::unordered_map<int, std::list<PerceptionObstacleFeature>>::iterator
it = obstacle_history_map_.begin();
while (it != obstacle_history_map_.end()) {
const int obstacle_id = it->first;
if (prediction_obstacles_map_.count(obstacle_id) == 0) {
// not exist in current prediction msg
it = obstacle_history_map_.erase(it);
} else {
++it;
}
}
*/
// debug
// add to obstacle history
// for (const auto& m : obstacle_history_map_) {
// for (const auto& p : m.second) {
// AERROR << "obstacle_history_map_: " << m.first << "; "
// << p.DebugString();
// }
// }
// add to obstacle history
for (const auto& m : prediction_obstacles_map_) {
const auto& perception_obstale = m.second.perception_obstacle();
PerceptionObstacleFeature obstacle_trajectory_point;
obstacle_trajectory_point.set_timestamp_sec(perception_obstale.timestamp());
obstacle_trajectory_point.mutable_position()->CopyFrom(
perception_obstale.position());
obstacle_trajectory_point.set_theta(perception_obstale.theta());
obstacle_trajectory_point.mutable_velocity()->CopyFrom(
perception_obstale.velocity());
for (int j = 0; j < perception_obstale.polygon_point_size(); ++j) {
auto polygon_point = obstacle_trajectory_point.add_polygon_point();
polygon_point->CopyFrom(perception_obstale.polygon_point(j));
}
obstacle_trajectory_point.mutable_acceleration()->CopyFrom(
perception_obstale.acceleration());
obstacle_history_map_[m.first].back().timestamp_sec();
if (obstacle_history_map_[m.first].empty() ||
obstacle_trajectory_point.timestamp_sec() -
obstacle_history_map_[m.first].back().timestamp_sec() >
0) {
obstacle_history_map_[m.first].push_back(obstacle_trajectory_point);
} else {
// abnormal perception data: time_diff <= 0
const double time_diff =
obstacle_trajectory_point.timestamp_sec() -
obstacle_history_map_[m.first].back().timestamp_sec();
const std::string msg = absl::StrCat(
"DISCARD: obstacle_id[", m.first, "] last_timestamp_sec[",
obstacle_history_map_[m.first].back().timestamp_sec(),
"] timestamp_sec[", obstacle_trajectory_point.timestamp_sec(),
"] time_diff[", time_diff, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
}
auto& obstacle_history = obstacle_history_map_[m.first];
while (!obstacle_history.empty()) {
const double time_distance = obstacle_history.back().timestamp_sec() -
obstacle_history.front().timestamp_sec();
if (time_distance < FLAGS_learning_data_obstacle_history_time_sec) {
break;
}
obstacle_history.pop_front();
}
}
}
void MessageProcess::OnRoutingResponse(
const apollo::routing::RoutingResponse& routing_response) {
ADEBUG << "routing_response received at frame["
<< total_learning_data_frame_num_ << "]";
routing_response_.CopyFrom(routing_response);
}
void MessageProcess::OnStoryTelling(
const apollo::storytelling::Stories& stories) {
// clear area
if (stories.has_close_to_clear_area()) {
auto clear_area_tag = planning_tag_.mutable_clear_area();
clear_area_tag->set_id(stories.close_to_clear_area().id());
clear_area_tag->set_distance(stories.close_to_clear_area().distance());
}
// crosswalk
if (stories.has_close_to_crosswalk()) {
auto crosswalk_tag = planning_tag_.mutable_crosswalk();
crosswalk_tag->set_id(stories.close_to_crosswalk().id());
crosswalk_tag->set_distance(stories.close_to_crosswalk().distance());
}
// pnc_junction
if (stories.has_close_to_junction() &&
stories.close_to_junction().type() == CloseToJunction::PNC_JUNCTION) {
auto pnc_junction_tag = planning_tag_.mutable_pnc_junction();
pnc_junction_tag->set_id(stories.close_to_junction().id());
pnc_junction_tag->set_distance(stories.close_to_junction().distance());
}
// traffic_light
if (stories.has_close_to_signal()) {
auto signal_tag = planning_tag_.mutable_signal();
signal_tag->set_id(stories.close_to_signal().id());
signal_tag->set_distance(stories.close_to_signal().distance());
}
// stop_sign
if (stories.has_close_to_stop_sign()) {
auto stop_sign_tag = planning_tag_.mutable_stop_sign();
stop_sign_tag->set_id(stories.close_to_stop_sign().id());
stop_sign_tag->set_distance(stories.close_to_stop_sign().distance());
}
// yield_sign
if (stories.has_close_to_yield_sign()) {
auto yield_sign_tag = planning_tag_.mutable_yield_sign();
yield_sign_tag->set_id(stories.close_to_yield_sign().id());
yield_sign_tag->set_distance(stories.close_to_yield_sign().distance());
}
ADEBUG << planning_tag_.DebugString();
}
void MessageProcess::OnTrafficLightDetection(
const TrafficLightDetection& traffic_light_detection) {
// AINFO << "traffic_light_detection received at frame["
// << total_learning_data_frame_num_ << "]";
traffic_light_detection_message_timestamp_ =
traffic_light_detection.header().timestamp_sec();
traffic_lights_.clear();
for (int i = 0; i < traffic_light_detection.traffic_light_size(); ++i) {
TrafficLightFeature traffic_light;
traffic_light.set_color(traffic_light_detection.traffic_light(i).color());
traffic_light.set_id(traffic_light_detection.traffic_light(i).id());
traffic_light.set_confidence(
traffic_light_detection.traffic_light(i).confidence());
traffic_light.set_tracking_time(
traffic_light_detection.traffic_light(i).tracking_time());
traffic_light.set_remaining_time(
traffic_light_detection.traffic_light(i).remaining_time());
traffic_lights_.push_back(traffic_light);
}
}
void MessageProcess::ProcessOfflineData(const std::string& record_file) {
log_file_ << "Processing: " << record_file << std::endl;
record_file_ = record_file;
RecordReader reader(record_file);
if (!reader.IsValid()) {
AERROR << "Fail to open " << record_file;
return;
}
RecordMessage message;
while (reader.ReadMessage(&message)) {
if (message.channel_name ==
planning_config_.topic_config().chassis_topic()) {
Chassis chassis;
if (chassis.ParseFromString(message.content)) {
OnChassis(chassis);
}
} else if (message.channel_name ==
planning_config_.topic_config().localization_topic()) {
LocalizationEstimate localization;
if (localization.ParseFromString(message.content)) {
OnLocalization(localization);
}
} else if (message.channel_name ==
planning_config_.topic_config().hmi_status_topic()) {
HMIStatus hmi_status;
if (hmi_status.ParseFromString(message.content)) {
OnHMIStatus(hmi_status);
}
} else if (message.channel_name ==
planning_config_.topic_config().prediction_topic()) {
PredictionObstacles prediction_obstacles;
if (prediction_obstacles.ParseFromString(message.content)) {
OnPrediction(prediction_obstacles);
}
} else if (message.channel_name ==
planning_config_.topic_config().routing_response_topic()) {
RoutingResponse routing_response;
if (routing_response.ParseFromString(message.content)) {
OnRoutingResponse(routing_response);
}
} else if (message.channel_name ==
planning_config_.topic_config().story_telling_topic()) {
Stories stories;
if (stories.ParseFromString(message.content)) {
OnStoryTelling(stories);
}
} else if (message.channel_name == planning_config_.topic_config()
.traffic_light_detection_topic()) {
TrafficLightDetection traffic_light_detection;
if (traffic_light_detection.ParseFromString(message.content)) {
OnTrafficLightDetection(traffic_light_detection);
}
}
}
}
bool MessageProcess::GetADCCurrentRoutingIndex(
int* adc_road_index, int* adc_passage_index, double* adc_passage_s) {
if (localizations_.empty()) return false;
static constexpr double kRadius = 4.0;
const auto& pose = localizations_.back().pose();
std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>> lanes;
apollo::hdmap::HDMapUtil::BaseMapPtr()->GetLanes(pose.position(), kRadius,
&lanes);
for (auto& lane : lanes) {
for (int i = 0; i < routing_response_.road_size(); ++i) {
*adc_passage_s = 0;
for (int j = 0; j < routing_response_.road(i).passage_size(); ++j) {
double passage_s = 0;
for (int k = 0; k < routing_response_.road(i).passage(j).segment_size();
++k) {
const auto& segment = routing_response_.road(i).passage(j).segment(k);
passage_s += (segment.end_s() - segment.start_s());
if (lane->id().id() == segment.id()) {
*adc_road_index = i;
*adc_passage_index = j;
*adc_passage_s = passage_s;
return true;
}
}
}
}
}
return false;
}
apollo::hdmap::LaneInfoConstPtr MessageProcess::GetCurrentLane(
const apollo::common::PointENU& position) {
constexpr double kRadiusUnit = 0.1;
std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>> lanes;
for (int i = 1; i <= 10; ++i) {
apollo::hdmap::HDMapUtil::BaseMapPtr()->GetLanes(position, i * kRadiusUnit,
&lanes);
if (lanes.size() > 0) {
break;
}
}
for (auto& lane : lanes) {
for (int i = 0; i < routing_response_.road_size(); ++i) {
for (int j = 0; j < routing_response_.road(i).passage_size(); ++j) {
for (int k = 0; k < routing_response_.road(i).passage(j).segment_size();
++k) {
if (lane->id().id() ==
routing_response_.road(i).passage(j).segment(k).id()) {
return lane;
}
}
}
}
}
return nullptr;
}
int MessageProcess::GetADCCurrentInfo(ADCCurrentInfo* adc_curr_info) {
CHECK_NOTNULL(adc_curr_info);
if (localizations_.empty()) return -1;
// ADC current position / velocity / acc/ heading
const auto& adc_cur_pose = localizations_.back().pose();
adc_curr_info->adc_cur_position_ =
std::make_pair(adc_cur_pose.position().x(), adc_cur_pose.position().y());
adc_curr_info->adc_cur_velocity_ = std::make_pair(
adc_cur_pose.linear_velocity().x(), adc_cur_pose.linear_velocity().y());
adc_curr_info->adc_cur_acc_ =
std::make_pair(adc_cur_pose.linear_acceleration().x(),
adc_cur_pose.linear_acceleration().y());
adc_curr_info->adc_cur_heading_ = adc_cur_pose.heading();
return 1;
}
void MessageProcess::GenerateObstacleTrajectory(
const int frame_num, const int obstacle_id,
const ADCCurrentInfo& adc_curr_info, ObstacleFeature* obstacle_feature) {
auto obstacle_trajectory = obstacle_feature->mutable_obstacle_trajectory();
const auto& obstacle_history = obstacle_history_map_[obstacle_id];
for (const auto& obj_traj_point : obstacle_history) {
auto perception_obstacle_history =
obstacle_trajectory->add_perception_obstacle_history();
perception_obstacle_history->set_timestamp_sec(
obj_traj_point.timestamp_sec());
// convert position to relative coordinate
const auto& relative_posistion = util::WorldCoordToObjCoord(
std::make_pair(obj_traj_point.position().x(),
obj_traj_point.position().y()),
adc_curr_info.adc_cur_position_, adc_curr_info.adc_cur_heading_);
auto position = perception_obstacle_history->mutable_position();
position->set_x(relative_posistion.first);
position->set_y(relative_posistion.second);
// convert theta to relative coordinate
const double relative_theta = util::WorldAngleToObjAngle(
obj_traj_point.theta(), adc_curr_info.adc_cur_heading_);
perception_obstacle_history->set_theta(relative_theta);
// convert velocity to relative coordinate
const auto& relative_velocity = util::WorldCoordToObjCoord(
std::make_pair(obj_traj_point.velocity().x(),
obj_traj_point.velocity().y()),
adc_curr_info.adc_cur_velocity_, adc_curr_info.adc_cur_heading_);
auto velocity = perception_obstacle_history->mutable_velocity();
velocity->set_x(relative_velocity.first);
velocity->set_y(relative_velocity.second);
// convert acceleration to relative coordinate
const auto& relative_acc = util::WorldCoordToObjCoord(
std::make_pair(obj_traj_point.acceleration().x(),
obj_traj_point.acceleration().y()),
adc_curr_info.adc_cur_acc_, adc_curr_info.adc_cur_heading_);
auto acceleration = perception_obstacle_history->mutable_acceleration();
acceleration->set_x(relative_acc.first);
acceleration->set_y(relative_acc.second);
for (int i = 0; i < obj_traj_point.polygon_point_size(); ++i) {
// convert polygon_point(s) to relative coordinate
const auto& relative_point = util::WorldCoordToObjCoord(
std::make_pair(obj_traj_point.polygon_point(i).x(),
obj_traj_point.polygon_point(i).y()),
adc_curr_info.adc_cur_position_, adc_curr_info.adc_cur_heading_);
auto polygon_point = perception_obstacle_history->add_polygon_point();
polygon_point->set_x(relative_point.first);
polygon_point->set_y(relative_point.second);
}
}
}
void MessageProcess::GenerateObstaclePrediction(
const int frame_num,
const PredictionObstacle& prediction_obstacle,
const ADCCurrentInfo& adc_curr_info, ObstacleFeature* obstacle_feature) {
const auto obstacle_id = obstacle_feature->id();
auto obstacle_prediction = obstacle_feature->mutable_obstacle_prediction();
obstacle_prediction->set_timestamp_sec(prediction_obstacle.timestamp());
obstacle_prediction->set_predicted_period(
prediction_obstacle.predicted_period());
obstacle_prediction->mutable_intent()->CopyFrom(prediction_obstacle.intent());
obstacle_prediction->mutable_priority()->CopyFrom(
prediction_obstacle.priority());
obstacle_prediction->set_is_static(prediction_obstacle.is_static());
for (int i = 0; i < prediction_obstacle.trajectory_size(); ++i) {
const auto& obstacle_trajectory = prediction_obstacle.trajectory(i);
auto trajectory = obstacle_prediction->add_trajectory();
trajectory->set_probability(obstacle_trajectory.probability());
// TrajectoryPoint
for (int j = 0; j < obstacle_trajectory.trajectory_point_size(); ++j) {
const auto& obstacle_trajectory_point =
obstacle_trajectory.trajectory_point(j);
if (trajectory->trajectory_point_size() >0) {
const auto last_relative_time =
trajectory->trajectory_point(trajectory->trajectory_point_size()-1)
.trajectory_point().relative_time();
if (obstacle_trajectory_point.relative_time() < last_relative_time) {
const std::string msg = absl::StrCat(
"DISCARD prediction trajectory point: frame_num[", frame_num,
"] obstacle_id[", obstacle_id, "] last_relative_time[",
last_relative_time, "] relative_time[",
obstacle_trajectory_point.relative_time(), "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
continue;
}
}
auto trajectory_point = trajectory->add_trajectory_point();
auto path_point =
trajectory_point->mutable_trajectory_point()->mutable_path_point();
// convert path_point position to relative coordinate
const auto& relative_path_point = util::WorldCoordToObjCoord(
std::make_pair(obstacle_trajectory_point.path_point().x(),
obstacle_trajectory_point.path_point().y()),
adc_curr_info.adc_cur_position_, adc_curr_info.adc_cur_heading_);
path_point->set_x(relative_path_point.first);
path_point->set_y(relative_path_point.second);
// convert path_point theta to relative coordinate
const double relative_theta = util::WorldAngleToObjAngle(
obstacle_trajectory_point.path_point().theta(),
adc_curr_info.adc_cur_heading_);
path_point->set_theta(relative_theta);
path_point->set_s(obstacle_trajectory_point.path_point().s());
path_point->set_lane_id(obstacle_trajectory_point.path_point().lane_id());
const double timestamp_sec = prediction_obstacle.timestamp() +
obstacle_trajectory_point.relative_time();
trajectory_point->set_timestamp_sec(timestamp_sec);
auto tp = trajectory_point->mutable_trajectory_point();
tp->set_v(obstacle_trajectory_point.v());
tp->set_a(obstacle_trajectory_point.a());
tp->set_relative_time(obstacle_trajectory_point.relative_time());
tp->mutable_gaussian_info()->CopyFrom(
obstacle_trajectory_point.gaussian_info());
}
}
}
void MessageProcess::GenerateObstacleFeature(
LearningDataFrame* learning_data_frame) {
ADCCurrentInfo adc_curr_info;
if (GetADCCurrentInfo(&adc_curr_info) == -1) {
const std::string msg = absl::StrCat(
"fail to get ADC current info: frame_num[",
learning_data_frame->frame_num(), "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
return;
}
const int frame_num = learning_data_frame->frame_num();
for (const auto& m : prediction_obstacles_map_) {
auto obstacle_feature = learning_data_frame->add_obstacle();
const auto& perception_obstale = m.second.perception_obstacle();
obstacle_feature->set_id(m.first);
obstacle_feature->set_length(perception_obstale.length());
obstacle_feature->set_width(perception_obstale.width());
obstacle_feature->set_height(perception_obstale.height());
obstacle_feature->set_type(perception_obstale.type());
// obstacle history trajectory points
GenerateObstacleTrajectory(frame_num, m.first, adc_curr_info,
obstacle_feature);
// obstacle prediction
GenerateObstaclePrediction(frame_num, m.second, adc_curr_info,
obstacle_feature);
}
}
bool MessageProcess::GenerateLocalRouting(
const int frame_num,
RoutingResponseFeature* local_routing,
std::vector<std::string>* local_routing_lane_ids) {
local_routing->Clear();
local_routing_lane_ids->clear();
if (routing_response_.road_size() == 0 ||
routing_response_.road(0).passage_size() == 0 ||
routing_response_.road(0).passage(0).segment_size() == 0) {
const std::string msg = absl::StrCat(
"DISCARD: invalid routing_response. frame_num[", frame_num, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
return false;
}
// calculate road_length
std::vector<std::pair<std::string, double>> road_lengths;
for (int i = 0; i < routing_response_.road_size(); ++i) {
ADEBUG << "road_id[" << routing_response_.road(i).id() << "] passage_size["
<< routing_response_.road(i).passage_size() << "]";
double road_length = 0.0;
for (int j = 0; j < routing_response_.road(i).passage_size(); ++j) {
ADEBUG << " passage: segment_size["
<< routing_response_.road(i).passage(j).segment_size() << "]";
double passage_length = 0;
for (int k = 0; k < routing_response_.road(i).passage(j).segment_size();
++k) {
const auto& segment = routing_response_.road(i).passage(j).segment(k);
passage_length += (segment.end_s() - segment.start_s());
}
ADEBUG << " passage_length[" << passage_length << "]";
road_length = std::max(road_length, passage_length);
}
road_lengths.push_back(
std::make_pair(routing_response_.road(i).id(), road_length));
ADEBUG << " road_length[" << road_length << "]";
}
/* debug
for (size_t i = 0; i < road_lengths.size(); ++i) {
AERROR << i << ": " << road_lengths[i].first << "; "
<< road_lengths[i].second;
}
*/
int adc_road_index;
int adc_passage_index;
double adc_passage_s;
if (!GetADCCurrentRoutingIndex(&adc_road_index, &adc_passage_index,
&adc_passage_s) ||
adc_road_index < 0 || adc_passage_index < 0 || adc_passage_s < 0) {
// reset localization history
localizations_.clear();
const std::string msg = absl::StrCat(
"DISCARD: fail to locate ADC on routing. frame_num[",
frame_num, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
return false;
}
ADEBUG << "adc_road_index[" << adc_road_index << "] adc_passage_index["
<< adc_passage_index << "] adc_passage_s[" << adc_passage_s << "]";
constexpr double kLocalRoutingForwardLength = 200.0;
constexpr double kLocalRoutingBackwardLength = 100.0;
// local_routing start point
int local_routing_start_road_index = 0;
double local_routing_start_road_s = 0;
double backward_length = kLocalRoutingBackwardLength;
for (int i = adc_road_index; i >= 0; --i) {
const double road_length =
(i == adc_road_index ? adc_passage_s : road_lengths[i].second);
if (backward_length > road_length) {
backward_length -= road_length;
} else {
local_routing_start_road_index = i;
local_routing_start_road_s = road_length - backward_length;
ADEBUG << "local_routing_start_road_index["
<< local_routing_start_road_index
<< "] local_routing_start_road_s["
<< local_routing_start_road_s << "]";
break;
}
}
// local routing end point
int local_routing_end_road_index = routing_response_.road_size() - 1;
double local_routing_end_road_s =
road_lengths[local_routing_end_road_index].second;
double forwardward_length = kLocalRoutingForwardLength;
for (int i = adc_road_index; i < routing_response_.road_size(); ++i) {
const double road_length =
(i == adc_road_index ? road_lengths[i].second - adc_passage_s
: road_lengths[i].second);
if (forwardward_length > road_length) {
forwardward_length -= road_length;
} else {
local_routing_end_road_index = i;
local_routing_end_road_s =
(i == adc_road_index ? adc_passage_s + forwardward_length
: forwardward_length);
ADEBUG << "local_routing_end_road_index[" << local_routing_end_road_index
<< "] local_routing_end_road_s["
<< local_routing_end_road_s << "]";
break;
}
}
ADEBUG << "local_routing: start_road_index[" << local_routing_start_road_index
<< "] start_road_s[" << local_routing_start_road_s
<< "] end_road_index[" << local_routing_end_road_index
<< "] end_road_s[" << local_routing_end_road_s << "]";
bool local_routing_end = false;
int last_passage_index = adc_passage_index;
for (int i = local_routing_start_road_index;
i <= local_routing_end_road_index; ++i) {
if (local_routing_end) break;
const auto& road = routing_response_.road(i);
auto local_routing_road = local_routing->add_road();
local_routing_road->set_id(road.id());
for (int j = 0; j < road.passage_size(); ++j) {
const auto& passage = road.passage(j);
auto local_routing_passage = local_routing_road->add_passage();
local_routing_passage->set_can_exit(passage.can_exit());
local_routing_passage->set_change_lane_type(passage.change_lane_type());
double road_s = 0;
for (int k = 0; k < passage.segment_size(); ++k) {
const auto& lane_segment = passage.segment(k);
road_s += (lane_segment.end_s() - lane_segment.start_s());
// first road
if (i == local_routing_start_road_index &&
road_s < local_routing_start_road_s) {
continue;
}
local_routing_passage->add_segment()->CopyFrom(lane_segment);
ADEBUG << "ADD road[" << i << "] id[" << road.id() << "] passage[" << j
<< "] id[" << lane_segment.id() << "] length["
<< lane_segment.end_s() - lane_segment.start_s() << "]";
// set local_routing_lane_ids
if (i == adc_road_index) {
// adc_road_index, pick the passage where ADC is
if (j == adc_passage_index) {
local_routing_lane_ids->push_back(lane_segment.id());
ADEBUG << "ADD local_routing_lane_ids: road[" << i
<< "] passage[" << j << "]: " << lane_segment.id();
last_passage_index = j;
}
} else {
if (road.passage_size() == 1) {
// single passage
ADEBUG << "ADD local_routing_lane_ids: road[" << i
<< "] passage[" << j << "]: " << lane_segment.id();
local_routing_lane_ids->push_back(lane_segment.id());
last_passage_index = j;
} else {
// multi passages
if (i < adc_road_index) {
// road behind ADC position
if (j == adc_passage_index ||
(j == road.passage_size() - 1 &&
j < adc_passage_index)) {
ADEBUG << "ADD local_routing_lane_ids: road[" << i
<< "] passage[" << j << "] adc_passage_index["
<< adc_passage_index << "] passage_size["
<< road.passage_size() << "]: " << lane_segment.id();
local_routing_lane_ids->push_back(lane_segment.id());
}
} else {
// road in front of ADC position:
// pick the passage towards change-left
if (j == last_passage_index + 1 ||
(j == road.passage_size() - 1 &&
j < last_passage_index + 1)) {
ADEBUG << "ADD local_routing_lane_ids: road[" << i
<< "] passage[" << j << "] last_passage_index["
<< last_passage_index << "] passage_size["
<< road.passage_size() << "]: " << lane_segment.id();
local_routing_lane_ids->push_back(lane_segment.id());
last_passage_index = j;
}
}
}
}
// last road
if (i == local_routing_end_road_index &&
road_s >= local_routing_end_road_s) {
local_routing_end = true;
break;
}
}
}
}
// check local_routing: to filter out map mismatching frames
if (FLAGS_planning_offline_learning) {
for (size_t i = 0; i < local_routing_lane_ids->size(); ++i) {
const std::string lane_id = local_routing_lane_ids->at(i);
const auto& lane =
hdmap::HDMapUtil::BaseMap().GetLaneById(hdmap::MakeMapId(lane_id));
if (lane == nullptr) {
const std::string msg = absl::StrCat(
"DISCARD: fail to find local_routing_lane on map. frame_num[",
frame_num, "] lane[", lane_id, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
return false;
}
}
}
return true;
}
void MessageProcess::GenerateRoutingFeature(
const RoutingResponseFeature& local_routing,
const std::vector<std::string>& local_routing_lane_ids,
LearningDataFrame* learning_data_frame) {
auto routing = learning_data_frame->mutable_routing();
routing->Clear();
routing->mutable_routing_response()->mutable_measurement()->set_distance(
routing_response_.measurement().distance());
for (int i = 0; i < routing_response_.road_size(); ++i) {
routing->mutable_routing_response()->add_road()->CopyFrom(
routing_response_.road(i));
}
for (const auto& lane_id : local_routing_lane_ids) {
routing->add_local_routing_lane_id(lane_id);
}
routing->mutable_local_routing()->CopyFrom(local_routing);
const int frame_num = learning_data_frame->frame_num();
const int local_routing_lane_id_size = routing->local_routing_lane_id_size();
if (local_routing_lane_id_size == 0) {
const std::string msg = absl::StrCat(
"empty local_routing. frame_num[", frame_num, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
}
if (local_routing_lane_id_size > 100) {
const std::string msg = absl::StrCat(
"LARGE local_routing. frame_num[", frame_num,
"] local_routing_lane_id_size[", local_routing_lane_id_size, "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
}
ADEBUG << "local_routing: frame_num[" << frame_num
<< "] size[" << routing->local_routing_lane_id_size() << "]";
}
void MessageProcess::GenerateTrafficLightDetectionFeature(
LearningDataFrame* learning_data_frame) {
auto traffic_light_detection =
learning_data_frame->mutable_traffic_light_detection();
traffic_light_detection->set_message_timestamp_sec(
traffic_light_detection_message_timestamp_);
traffic_light_detection->clear_traffic_light();
for (const auto& tl : traffic_lights_) {
auto traffic_light = traffic_light_detection->add_traffic_light();
traffic_light->CopyFrom(tl);
}
}
void MessageProcess::GenerateADCTrajectoryPoints(
const std::list<LocalizationEstimate>& localizations,
LearningDataFrame* learning_data_frame) {
std::vector<LocalizationEstimate> localization_samples;
for (const auto& le : localizations) {
localization_samples.insert(localization_samples.begin(), le);
}
constexpr double kSearchRadius = 1.0;
std::string clear_area_id;
double clear_area_distance = 0.0;
std::string crosswalk_id;
double crosswalk_distance = 0.0;
std::string pnc_junction_id;
double pnc_junction_distance = 0.0;
std::string signal_id;
double signal_distance = 0.0;
std::string stop_sign_id;
double stop_sign_distance = 0.0;
std::string yield_sign_id;
double yield_sign_distance = 0.0;
int trajectory_point_index = 0;
std::vector<ADCTrajectoryPoint> adc_trajectory_points;
for (const auto& localization_sample : localization_samples) {
ADCTrajectoryPoint adc_trajectory_point;
adc_trajectory_point.set_timestamp_sec(
localization_sample.measurement_time());
auto trajectory_point = adc_trajectory_point.mutable_trajectory_point();
auto& pose = localization_sample.pose();
trajectory_point->mutable_path_point()->set_x(pose.position().x());
trajectory_point->mutable_path_point()->set_y(pose.position().y());
trajectory_point->mutable_path_point()->set_z(pose.position().z());
trajectory_point->mutable_path_point()->set_theta(pose.heading());
const double v =
std::sqrt(pose.linear_velocity().x() * pose.linear_velocity().x() +
pose.linear_velocity().y() * pose.linear_velocity().y());
trajectory_point->set_v(v);
const double a = std::sqrt(
pose.linear_acceleration().x() * pose.linear_acceleration().x() +
pose.linear_acceleration().y() * pose.linear_acceleration().y());
trajectory_point->set_a(a);
auto planning_tag = adc_trajectory_point.mutable_planning_tag();
// planning_tag: lane_turn
const auto& cur_point = common::util::PointFactory::ToPointENU(
pose.position().x(), pose.position().y(), pose.position().z());
LaneInfoConstPtr lane = GetCurrentLane(cur_point);
// lane_turn
apollo::hdmap::Lane::LaneTurn lane_turn = apollo::hdmap::Lane::NO_TURN;
if (lane != nullptr) {
lane_turn = lane->lane().turn();
}
planning_tag->set_lane_turn(lane_turn);
planning_tag_.set_lane_turn(lane_turn);
if (FLAGS_planning_offline_learning) {
// planning_tag: overlap tags
double point_distance = 0.0;
if (trajectory_point_index > 0) {
auto& next_point = adc_trajectory_points[trajectory_point_index - 1]
.trajectory_point()
.path_point();
point_distance = common::util::DistanceXY(next_point, cur_point);
}
common::PointENU hdmap_point;
hdmap_point.set_x(cur_point.x());
hdmap_point.set_y(cur_point.y());
// clear area
planning_tag->clear_clear_area();
std::vector<ClearAreaInfoConstPtr> clear_areas;
if (HDMapUtil::BaseMap().GetClearAreas(hdmap_point, kSearchRadius,
&clear_areas) == 0 &&
clear_areas.size() > 0) {
clear_area_id = clear_areas.front()->id().id();
clear_area_distance = 0.0;
} else {
if (!clear_area_id.empty()) {
clear_area_distance += point_distance;
}
}
if (!clear_area_id.empty()) {
planning_tag->mutable_clear_area()->set_id(clear_area_id);
planning_tag->mutable_clear_area()->set_distance(clear_area_distance);
}
// crosswalk
planning_tag->clear_crosswalk();
std::vector<CrosswalkInfoConstPtr> crosswalks;
if (HDMapUtil::BaseMap().GetCrosswalks(hdmap_point, kSearchRadius,
&crosswalks) == 0 &&
crosswalks.size() > 0) {
crosswalk_id = crosswalks.front()->id().id();
crosswalk_distance = 0.0;
} else {
if (!crosswalk_id.empty()) {
crosswalk_distance += point_distance;
}
}
if (!crosswalk_id.empty()) {
planning_tag->mutable_crosswalk()->set_id(crosswalk_id);
planning_tag->mutable_crosswalk()->set_distance(crosswalk_distance);
}
// pnc_junction
std::vector<PNCJunctionInfoConstPtr> pnc_junctions;
if (HDMapUtil::BaseMap().GetPNCJunctions(hdmap_point, kSearchRadius,
&pnc_junctions) == 0 &&
pnc_junctions.size() > 0) {
pnc_junction_id = pnc_junctions.front()->id().id();
pnc_junction_distance = 0.0;
} else {
if (!pnc_junction_id.empty()) {
pnc_junction_distance += point_distance;
}
}
if (!pnc_junction_id.empty()) {
planning_tag->mutable_pnc_junction()->set_id(pnc_junction_id);
planning_tag->mutable_pnc_junction()->set_distance(
pnc_junction_distance);
}
// signal
std::vector<SignalInfoConstPtr> signals;
if (HDMapUtil::BaseMap().GetSignals(hdmap_point, kSearchRadius,
&signals) == 0 &&
signals.size() > 0) {
signal_id = signals.front()->id().id();
signal_distance = 0.0;
} else {
if (!signal_id.empty()) {
signal_distance += point_distance;
}
}
if (!signal_id.empty()) {
planning_tag->mutable_signal()->set_id(signal_id);
planning_tag->mutable_signal()->set_distance(signal_distance);
}
// stop sign
std::vector<StopSignInfoConstPtr> stop_signs;
if (HDMapUtil::BaseMap().GetStopSigns(hdmap_point, kSearchRadius,
&stop_signs) == 0 &&
stop_signs.size() > 0) {
stop_sign_id = stop_signs.front()->id().id();
stop_sign_distance = 0.0;
} else {
if (!stop_sign_id.empty()) {
stop_sign_distance += point_distance;
}
}
if (!stop_sign_id.empty()) {
planning_tag->mutable_stop_sign()->set_id(stop_sign_id);
planning_tag->mutable_stop_sign()->set_distance(stop_sign_distance);
}
// yield sign
std::vector<YieldSignInfoConstPtr> yield_signs;
if (HDMapUtil::BaseMap().GetYieldSigns(hdmap_point, kSearchRadius,
&yield_signs) == 0 &&
yield_signs.size() > 0) {
yield_sign_id = yield_signs.front()->id().id();
yield_sign_distance = 0.0;
} else {
if (!yield_sign_id.empty()) {
yield_sign_distance += point_distance;
}
}
if (!yield_sign_id.empty()) {
planning_tag->mutable_yield_sign()->set_id(yield_sign_id);
planning_tag->mutable_yield_sign()->set_distance(yield_sign_distance);
}
}
adc_trajectory_points.push_back(adc_trajectory_point);
++trajectory_point_index;
}
// update learning data
std::reverse(adc_trajectory_points.begin(), adc_trajectory_points.end());
for (const auto& trajectory_point : adc_trajectory_points) {
auto adc_trajectory_point = learning_data_frame->add_adc_trajectory_point();
adc_trajectory_point->CopyFrom(trajectory_point);
}
if (adc_trajectory_points.size() <= 5) {
const std::string msg = absl::StrCat(
"too few adc_trajectory_points: frame_num[",
learning_data_frame->frame_num(), "] size[",
adc_trajectory_points.size(), "]");
AERROR << msg;
if (FLAGS_planning_offline_learning) {
log_file_ << msg << std::endl;
}
}
// AINFO << "number of ADC trajectory points in one frame: "
// << trajectory_point_index;
}
void MessageProcess::GeneratePlanningTag(
LearningDataFrame* learning_data_frame) {
auto planning_tag = learning_data_frame->mutable_planning_tag();
if (FLAGS_planning_offline_learning) {
planning_tag->set_lane_turn(planning_tag_.lane_turn());
} else {
if (planning_config_.learning_mode() != PlanningConfig::NO_LEARNING) {
// online learning
planning_tag->CopyFrom(planning_tag_);
}
}
}
bool MessageProcess::GenerateLearningDataFrame(
LearningDataFrame* learning_data_frame) {
const double start_timestamp = Clock::NowInSeconds();
RoutingResponseFeature local_routing;
std::vector<std::string> local_routing_lane_ids;
if (!GenerateLocalRouting(total_learning_data_frame_num_,
&local_routing, &local_routing_lane_ids)) {
return false;
}
// add timestamp_sec & frame_num
learning_data_frame->set_message_timestamp_sec(
localizations_.back().header().timestamp_sec());
learning_data_frame->set_frame_num(total_learning_data_frame_num_++);
// map_name
learning_data_frame->set_map_name(map_name_);
// planning_tag
GeneratePlanningTag(learning_data_frame);
// add chassis
auto chassis = learning_data_frame->mutable_chassis();
chassis->CopyFrom(chassis_feature_);
// add localization
auto localization = learning_data_frame->mutable_localization();
localization->set_message_timestamp_sec(
localizations_.back().header().timestamp_sec());
const auto& pose = localizations_.back().pose();
localization->mutable_position()->CopyFrom(pose.position());
localization->set_heading(pose.heading());
localization->mutable_linear_velocity()->CopyFrom(pose.linear_velocity());
localization->mutable_linear_acceleration()->CopyFrom(
pose.linear_acceleration());
localization->mutable_angular_velocity()->CopyFrom(pose.angular_velocity());
// add traffic_light
GenerateTrafficLightDetectionFeature(learning_data_frame);
// add routing
GenerateRoutingFeature(local_routing, local_routing_lane_ids,
learning_data_frame);
// add obstacle
GenerateObstacleFeature(learning_data_frame);
// add trajectory_points
GenerateADCTrajectoryPoints(localizations_, learning_data_frame);
const double end_timestamp = Clock::NowInSeconds();
const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
ADEBUG << "MessageProcess: start_timestamp[" << start_timestamp
<< "] end_timestamp[" << end_timestamp << "] time_diff_ms["
<< time_diff_ms << "]";
return true;
}
} // namespace planning
} // namespace apollo