提交 010cb6da 编写于 作者: C Calvin Miao 提交者: Liangliang Zhang

Prediction: added timestamp track

上级 f9a590bf
syntax = "proto2";
package apollo.data.data_recorder;
message RecorderInfo {
......
......@@ -14,6 +14,7 @@ cc_library(
"//modules/common/adapters:adapter_manager",
"//modules/common/adapters/proto:adapter_config_proto",
"//modules/common/util",
"//modules/common/time",
"//modules/common/proto:pnc_point_proto",
"//modules/localization/proto:localization_proto",
"//modules/perception/proto:perception_proto",
......
......@@ -19,6 +19,7 @@
#include <cmath>
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/util/file.h"
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/container/container_manager.h"
......@@ -36,6 +37,7 @@ using ::apollo::common::Status;
using ::apollo::common::TrajectoryPoint;
using ::apollo::common::adapter::AdapterConfig;
using ::apollo::common::adapter::AdapterManager;
using ::apollo::common::time::Clock;
using ::apollo::localization::LocalizationEstimate;
using ::apollo::perception::PerceptionObstacle;
using ::apollo::perception::PerceptionObstacles;
......@@ -110,6 +112,7 @@ void Prediction::OnLocalization(const LocalizationEstimate& localization) {
}
void Prediction::OnPerception(const PerceptionObstacles& perception_obstacles) {
double start_timestamp = Clock::NowInSecond();
ObstaclesContainer* obstacles_container = dynamic_cast<ObstaclesContainer*>(
ContainerManager::instance()->GetContainer(
AdapterConfig::PERCEPTION_OBSTACLES));
......@@ -121,15 +124,10 @@ void Prediction::OnPerception(const PerceptionObstacles& perception_obstacles) {
auto prediction_obstacles =
PredictorManager::instance()->prediction_obstacles();
AdapterManager::FillPredictionHeader(Name(), &prediction_obstacles);
prediction_obstacles.set_start_timestamp(start_timestamp);
prediction_obstacles.set_end_timestamp(Clock::NowInSecond());
AdapterManager::PublishPrediction(prediction_obstacles);
for (auto const& prediction_obstacle :
prediction_obstacles.prediction_obstacle()) {
for (auto const& trajectory : prediction_obstacle.trajectory()) {
for (auto const& trajectory_point : trajectory.trajectory_point()) {
CHECK(IsValidTrajectoryPoint(trajectory_point));
}
}
}
ADEBUG << "Received a perception message ["
<< perception_obstacles.ShortDebugString() << "].";
}
......
......@@ -22,9 +22,18 @@ message PredictionObstacle {
}
message PredictionObstacles {
optional apollo.common.Header header = 1; // timestamp is included in header
// 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;
}
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册