apollo::planning::MessageProcess类 参考

#include <message_process.h>

apollo::planning::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)

详细描述

在文件 message_process.h 第 48 行定义.

成员函数说明

Close()

void apollo::planning::MessageProcess::Close

(

)

在文件 message_process.cc 第 94 行定义.

                           {
  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();
  }
}

Init() [1/2]

bool apollo::planning::MessageProcess::Init

(

const PlanningConfig &

planning_config

)

在文件 message_process.cc 第 62 行定义.

                                                               {
  planning_config_.CopyFrom(planning_config);
 
  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_ = FLAGS_map_dir.substr(FLAGS_map_dir.find_last_of("/") + 1);
 
  obstacle_history_map_.clear();
 
  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() [2/2]

bool apollo::planning::MessageProcess::Init	(	const PlanningConfig &	planning_config,
		const std::shared_ptr< DependencyInjector > &	injector
	)

在文件 message_process.cc 第 88 行定义.

                                                                             {
  injector_ = injector;
  return Init(planning_config);
}

OnChassis()

void apollo::planning::MessageProcess::OnChassis

(

const apollo::canbus::Chassis &

chassis

)

在文件 message_process.cc 第 111 行定义.

                                                                 {
  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());
}

OnHMIStatus()

void apollo::planning::MessageProcess::OnHMIStatus

(

apollo::dreamview::HMIStatus

hmi_status

)

在文件 message_process.cc 第 120 行定义.

                                                                    {
  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 = "modules/map/data/";
    FLAGS_map_dir = map_base_folder + map_name_;
  }
}

OnLocalization()

void apollo::planning::MessageProcess::OnLocalization

(

const apollo::localization::LocalizationEstimate &

le

)

在文件 message_process.cc 第 129 行定义.

                                                                  {
  static constexpr double kEpsilon = 1e-12;
  if (std::abs(last_localization_message_timestamp_sec_) < kEpsilon) {
    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);
    }
  }
}

OnPrediction()

void apollo::planning::MessageProcess::OnPrediction

(

const apollo::prediction::PredictionObstacles &

prediction_obstacles

)

在文件 message_process.cc 第 186 行定义.

                                                     {
  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();
    }
  }
}

OnRoutingResponse()

void apollo::planning::MessageProcess::OnRoutingResponse

(

const apollo::routing::RoutingResponse &

routing_response

)

在文件 message_process.cc 第 270 行定义.

                                                          {
  ADEBUG << "routing_response received at frame["
         << total_learning_data_frame_num_ << "]";
  routing_response_.CopyFrom(routing_response);
}

OnStoryTelling()

void apollo::planning::MessageProcess::OnStoryTelling

(

const apollo::storytelling::Stories &

stories

)

在文件 message_process.cc 第 277 行定义.

                                              {
  // 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();
}

OnTrafficLightDetection()

void apollo::planning::MessageProcess::OnTrafficLightDetection

(

const apollo::perception::TrafficLightDetection &

traffic_light_detection

)

在文件 message_process.cc 第 325 行定义.

                                                          {
  // 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);
  }
}

ProcessOfflineData()

void apollo::planning::MessageProcess::ProcessOfflineData

(

const std::string &

record_file

)

在文件 message_process.cc 第 346 行定义.

                                                                    {
  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);
      }
    }
  }
}

---

该类的文档由以下文件生成:

• modules/planning/planning_base/common/message_process.h
• modules/planning/planning_base/common/message_process.cc