remove remove PlanningObject and refactor Obstacle class

modules/planning/common/BUILD

@@ -39,9 +39,11 @@ cc_library(
     ],
     deps = [
         "//modules/common/math:box2d",
+        "//modules/common/math:polygon2d",
         "//modules/perception/proto:perception_proto",
-        "//modules/planning/common:planning_object",
         "//modules/planning/common:planning_util",
+        "//modules/planning/proto:planning_proto",
+        "//modules/prediction/proto:prediction_proto",
     ],
 )
 
@@ -58,20 +60,6 @@ cc_library(
     ],
 )
 
-cc_library(
-    name = "planning_object",
-    srcs = [
-        "planning_object.cc",
-    ],
-    hdrs = [
-        "planning_object.h",
-    ],
-    deps = [
-        "//modules/common/math:polygon2d",
-        "//modules/planning/proto:planning_proto",
-    ],
-)
-
 cc_library(
     name = "planning_data",
     srcs = [

modules/planning/common/decision_data.cc

@@ -27,7 +27,7 @@ const DecisionResult &DecisionData::Decision() const { return decision_; }
 
 DecisionResult *DecisionData::MutableDecision() { return &decision_; }
 
-const std::list<Obstacle> &DecisionData::Obstacles() const {
+const std::vector<Obstacle *> &DecisionData::Obstacles() const {
   return obstacles_;
 }
 
@@ -47,12 +47,13 @@ std::vector<Obstacle *> *DecisionData::MutableDynamicObstacles() {
   return &dynamic_obstacles_;
 }
 
-void DecisionData::AddObstacle(const Obstacle &obstacle) {
+void DecisionData::AddObstacle(Obstacle *obstacle) {
   obstacles_.push_back(obstacle);
-  if (obstacle.Type() == perception::PerceptionObstacle::UNKNOWN_UNMOVABLE) {
-    static_obstacles_.push_back(&(obstacles_.back()));
+  if (obstacle->Perception().type() ==
+      perception::PerceptionObstacle::UNKNOWN_UNMOVABLE) {
+    static_obstacles_.push_back(obstacle);
   } else {
-    dynamic_obstacles_.push_back(&(obstacles_.back()));
+    dynamic_obstacles_.push_back(obstacle);
   }
 }
 

modules/planning/common/decision_data.h

@@ -36,16 +36,16 @@ class DecisionData {
   const DecisionResult &Decision() const;
   DecisionResult *MutableDecision();
 
-  const std::list<Obstacle> &Obstacles() const;
+  const std::vector<Obstacle *> &Obstacles() const;
   const std::vector<Obstacle *> &StaticObstacles() const;
   const std::vector<Obstacle *> &DynamicObstacles() const;
   std::vector<Obstacle *> *MutableStaticObstacles();
   std::vector<Obstacle *> *MutableDynamicObstacles();
-  void AddObstacle(const Obstacle &obstacle);
+  void AddObstacle(Obstacle *obstacle);
 
  private:
   DecisionResult decision_;
-  std::list<Obstacle> obstacles_;
+  std::vector<Obstacle *> obstacles_;
   std::vector<Obstacle *> static_obstacles_;
   std::vector<Obstacle *> dynamic_obstacles_;
 };

modules/planning/common/frame.cc

@@ -60,35 +60,19 @@ const common::TrajectoryPoint &Frame::PlanningStartPoint() const {
   return planning_start_point_;
 }
 
-void Frame::SetDecisionDataFromPrediction(
-    const prediction::PredictionObstacles &prediction_obstacles) {
-  for (const auto &prediction_obstacle :
-       prediction_obstacles.prediction_obstacle()) {
-    Obstacle obstacle;
-    auto &perception_obstacle = prediction_obstacle.perception_obstacle();
-    obstacle.SetId(std::to_string(perception_obstacle.id()));
-    obstacle.SetType(perception_obstacle.type());
-    obstacle.SetHeight(perception_obstacle.height());
-    obstacle.SetWidth(perception_obstacle.width());
-    obstacle.SetLength(perception_obstacle.length());
-    double theta = perception_obstacle.theta();
-    obstacle.SetHeading(theta);
-    double speed = perception_obstacle.velocity().x() * cos(theta) +
-                   perception_obstacle.velocity().y() * sin(theta);
-    obstacle.SetSpeed(speed);
-
-    for (const auto &trajectory : prediction_obstacle.trajectory()) {
-      obstacle.add_prediction_trajectory(trajectory);
-    }
-
-    mutable_planning_data()->mutable_decision_data()->AddObstacle(obstacle);
-  }
-}
-
 void Frame::SetPrediction(const prediction::PredictionObstacles &prediction) {
   prediction_ = prediction;
 }
 
+void Frame::CreateObstacles(const prediction::PredictionObstacles &prediction) {
+  std::list<std::unique_ptr<Obstacle> > obstacles;
+  Obstacle::CreateObstacles(prediction, &obstacles);
+  for (auto &ptr : obstacles) {
+    mutable_planning_data()->mutable_decision_data()->AddObstacle(ptr.get());
+    obstacles_.Add(ptr->Id(), std::move(ptr));
+  }
+}
+
 bool Frame::Init() {
   if (!pnc_map_) {
     AERROR << "map is null, call SetMap() first";
@@ -109,7 +93,7 @@ bool Frame::Init() {
     return false;
   }
   if (FLAGS_enable_prediction) {
-    SetDecisionDataFromPrediction(prediction_);
+    CreateObstacles(prediction_);
   }
   return true;
 }
@@ -172,8 +156,8 @@ bool Frame::SmoothReferenceLine() {
   return true;
 }
 
-const ObstacleTable &Frame::GetObstacleTable() const { return obstacle_table_; }
-ObstacleTable *Frame::MutableObstacleTable() { return &obstacle_table_; }
+const Obstacles &Frame::GetObstacles() const { return obstacles_; }
+Obstacles *Frame::MutableObstacles() { return &obstacles_; }
 
 std::string Frame::DebugString() const {
   return "Frame: " + std::to_string(sequence_num_);

modules/planning/common/frame.h

@@ -39,7 +39,7 @@
 namespace apollo {
 namespace planning {
 
-using ObstacleTable = IndexedList<std::string, Obstacle>;
+using Obstacles = IndexedList<std::string, Obstacle>;
 
 class Frame {
  public:
@@ -61,8 +61,8 @@ class Frame {
   PlanningData *mutable_planning_data();
   std::string DebugString() const;
 
-  const ObstacleTable &GetObstacleTable() const;
-  ObstacleTable *MutableObstacleTable();
+  const Obstacles &GetObstacles() const;
+  Obstacles *MutableObstacles();
 
   void set_computed_trajectory(const PublishableTrajectory &trajectory);
   const PublishableTrajectory &computed_trajectory() const;
@@ -70,15 +70,14 @@ class Frame {
  private:
   bool CreateReferenceLineFromRouting();
   bool SmoothReferenceLine();
-  void SetDecisionDataFromPrediction(
-      const prediction::PredictionObstacles &prediction_obstacles);
+  void CreateObstacles(const prediction::PredictionObstacles &prediction);
 
  private:
   common::TrajectoryPoint planning_start_point_;
 
   hdmap::RoutingResponse routing_result_;
   prediction::PredictionObstacles prediction_;
-  ObstacleTable obstacle_table_;
+  Obstacles obstacles_;
   uint32_t sequence_num_ = 0;
   hdmap::Path hdmap_path_;
   localization::Pose init_pose_;

modules/planning/common/obstacle.cc

@@ -22,6 +22,7 @@
 
 #include <algorithm>
 
+#include "modules/common/log.h"
 #include "modules/planning/common/planning_util.h"
 
 namespace apollo {
@@ -29,48 +30,25 @@ namespace planning {
 
 const std::string& Obstacle::Id() const { return id_; }
 
-void Obstacle::SetId(const std::string& id) { id_ = id; }
-
-double Obstacle::Height() const { return height_; }
-
-void Obstacle::SetHeight(const double height) { height_ = height; }
-
-double Obstacle::Width() const { return width_; }
-
-void Obstacle::SetWidth(const double width) { width_ = width; }
-
-double Obstacle::Length() const { return length_; }
-
-void Obstacle::SetLength(const double length) { length_ = length; }
-
-double Obstacle::Heading() const { return heading_; }
-
-void Obstacle::SetHeading(const double heading) { heading_ = heading; }
-
-common::math::Box2d Obstacle::BoundingBox() const {
-  return ::apollo::common::math::Box2d(center_, heading_, length_, width_);
-}
-
-const perception::PerceptionObstacle::Type& Obstacle::Type() const {
-  return type_;
-}
-
-void Obstacle::SetType(const perception::PerceptionObstacle::Type& type) {
-  type_ = type;
-}
-
-double Obstacle::Speed() const { return speed_; }
-
-void Obstacle::SetSpeed(const double speed) { speed_ = speed; }
-
-common::TrajectoryPoint Obstacle::get_point_at(
-    const prediction::Trajectory& trajectory,
+Obstacle::Obstacle(const std::string& id,
+                   const perception::PerceptionObstacle& perception_obstacle,
+                   const prediction::Trajectory& trajectory)
+    : id_(id),
+      trajectory_(trajectory),
+      perception_obstacle_(perception_obstacle),
+      perception_bounding_box_({perception_obstacle_.position().x(),
+                                perception_obstacle_.position().y()},
+                               perception_obstacle_.theta(),
+                               perception_obstacle_.length(),
+                               perception_obstacle_.width()) {}
+
+common::TrajectoryPoint Obstacle::GetPointAtTime(
     const double relative_time) const {
   auto comp = [](const common::TrajectoryPoint p, const double relative_time) {
     return p.relative_time() < relative_time;
   };
 
-  const auto& points = trajectory.trajectory_point();
+  const auto& points = trajectory_.trajectory_point();
   auto it_lower =
       std::lower_bound(points.begin(), points.end(), relative_time, comp);
 
@@ -80,5 +58,44 @@ common::TrajectoryPoint Obstacle::get_point_at(
   return util::interpolate(*(it_lower - 1), *it_lower, relative_time);
 }
 
+common::math::Box2d Obstacle::GetBoundingBox(
+    const common::TrajectoryPoint& point) const {
+  return common::math::Box2d({point.path_point().x(), point.path_point().y()},
+                             point.path_point().theta(),
+                             perception_obstacle_.length(),
+                             perception_obstacle_.width());
+}
+
+const common::math::Box2d& Obstacle::PerceptionBoundingBox() const {
+  return perception_bounding_box_;
+}
+
+const prediction::Trajectory& Obstacle::Trajectory() const {
+  return trajectory_;
+}
+
+const perception::PerceptionObstacle& Obstacle::Perception() const {
+  return perception_obstacle_;
+}
+
+void Obstacle::CreateObstacles(
+    const prediction::PredictionObstacles& predictions,
+    std::list<std::unique_ptr<Obstacle>>* obstacles) {
+  if (!obstacles) {
+    AERROR << "the provided obstacles is empty";
+    return;
+  }
+  for (const auto& prediction_obstacle : predictions.prediction_obstacle()) {
+    const auto perception_id = prediction_obstacle.perception_obstacle().id();
+    int trajectory_index = 0;
+    for (const auto& trajectory : prediction_obstacle.trajectory()) {
+      std::string obstacle_id = std::to_string(perception_id) + "_" +
+                                std::to_string(trajectory_index);
+      obstacles->emplace_back(std::unique_ptr<Obstacle>(new Obstacle(
+          obstacle_id, prediction_obstacle.perception_obstacle(), trajectory)));
+    }
+  }
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/obstacle.h

@@ -21,72 +21,65 @@
 #ifndef MODULES_PLANNING_COMMON_OBSTACLE_H_
 #define MODULES_PLANNING_COMMON_OBSTACLE_H_
 
-#include <string>
+#include <list>
 #include <vector>
 
-#include "modules/common/math/box2d.h"
 #include "modules/perception/proto/perception_obstacle.pb.h"
-#include "modules/planning/common/planning_object.h"
+#include "modules/planning/proto/decision.pb.h"
+#include "modules/prediction/proto/prediction_obstacle.pb.h"
+
+#include "modules/common/math/box2d.h"
+#include "modules/common/math/vec2d.h"
 
 namespace apollo {
 namespace planning {
 
-class Obstacle : public PlanningObject {
+class Obstacle {
  public:
   Obstacle() = default;
 
-  const std::string &Id() const;
-
-  void SetId(const std::string &id);
-
-  const perception::PerceptionObstacle::Type &Type() const;
-  void SetType(const perception::PerceptionObstacle::Type &type);
-
-  double Height() const;
-  void SetHeight(const double height);
+  Obstacle(const std::string &id,
+           const perception::PerceptionObstacle &perception,
+           const prediction::Trajectory &trajectory);
 
-  double Width() const;
-  void SetWidth(const double width);
-
-  double Length() const;
-  void SetLength(const double length);
-
-  double Heading() const;
-  void SetHeading(const double heading);
+  const std::string &Id() const;
 
-  double Speed() const;
-  void SetSpeed(const double speed);
+  common::TrajectoryPoint GetPointAtTime(const double time) const;
+  common::math::Box2d GetBoundingBox(
+      const common::TrajectoryPoint &point) const;
+  /**
+   * @brief get the perception bounding box
+   */
+  const common::math::Box2d &PerceptionBoundingBox() const;
 
-  apollo::common::math::Vec2d Center() const { return center_; };
+  const prediction::Trajectory &Trajectory() const;
 
-  apollo::common::math::Box2d BoundingBox() const;
+  const perception::PerceptionObstacle &Perception() const;
 
-  const std::vector<prediction::Trajectory> &prediction_trajectories() const {
-    return trajectories_;
+  // FIXME fake functions, will be migrate out soon.
+  const std::vector<ObjectDecisionType> &Decisions() const {
+    return decisions_;
   }
-  void add_prediction_trajectory(
-      const prediction::Trajectory &prediction_trajectory) {
-    trajectories_.push_back(prediction_trajectory);
-  }
-
-  common::TrajectoryPoint get_point_at(const prediction::Trajectory &trajectory,
-                                       const double time) const;
+  std::vector<ObjectDecisionType> *MutableDecisions() { return &decisions_; }
+
+  /**
+   * @brief This is a helper function that can create obstacles from prediction
+   * data.  The original prediction may have multiple trajectories for each
+   * obstacle. But this function will create one obstacle for each trajectory.
+   * @param predictions The prediction results
+   * @param obstacles The output obstacles saved in a list of unique_ptr.
+   */
+  static void CreateObstacles(
+      const prediction::PredictionObstacles &predictions,
+      std::list<std::unique_ptr<Obstacle>> *obstacles);
 
  private:
   std::string id_ = 0;
-
-  // TODO: (Liangliang) set those parameters.
-  double height_ = 0.0;
-  double width_ = 0.0;
-  double length_ = 0.0;
-  double heading_ = 0.0;
-  // NOTICE: check is speed_ is set before usage.
-  double speed_ = 0.0;
-
-  std::vector<prediction::Trajectory> trajectories_;
-  ::apollo::common::math::Vec2d center_;
-  perception::PerceptionObstacle::Type type_ =
-      perception::PerceptionObstacle::VEHICLE;
+  prediction::Trajectory trajectory_;
+  perception::PerceptionObstacle perception_obstacle_;
+  // FIXME move out later
+  std::vector<ObjectDecisionType> decisions_;
+  common::math::Box2d perception_bounding_box_;
 };
 
 }  // namespace planning

modules/planning/common/planning_data.h

@@ -30,7 +30,6 @@
 #include "modules/planning/common/decision_data.h"
 #include "modules/planning/common/path/path_data.h"
 #include "modules/planning/common/planning_data.h"
-#include "modules/planning/common/planning_object.h"
 #include "modules/planning/common/speed/speed_data.h"
 #include "modules/planning/common/trajectory/publishable_trajectory.h"
 #include "modules/planning/reference_line/reference_line.h"

modules/planning/common/planning_object.cc

-/******************************************************************************
- * 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.
- *****************************************************************************/
-
-/**
- * @file: planning_object.cc
- *
- **/
-
-#include "modules/planning/common/planning_object.h"
-
-namespace apollo {
-namespace planning {
-
-PlanningObject::PlanningObjectType PlanningObject::ObjectType() const {
-  return object_type_;
-}
-
-PlanningObject::PlanningObjectType* PlanningObject::MutableObjectType() {
-  return &object_type_;
-}
-
-const std::vector<ObjectDecisionType>& PlanningObject::Decisions() const {
-  return decisions_;
-}
-
-std::vector<ObjectDecisionType>* PlanningObject::MutableDecisions() {
-  return &decisions_;
-}
-
-const apollo::common::math::Polygon2d& PlanningObject::Polygon() const {
-  return polygon_;
-}
-
-apollo::common::math::Polygon2d* PlanningObject::MutablePolygon() {
-  return &polygon_;
-}
-
-}  // namespace planning
-}  // namespace apollo

modules/planning/common/planning_object.h

-/******************************************************************************
- * 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.
- *****************************************************************************/
-
-/**
- * @file: planning_object.h
- *
- **/
-
-#ifndef MODULES_PLANNING_COMMON_PLANNING_OBJECT_H_
-#define MODULES_PLANNING_COMMON_PLANNING_OBJECT_H_
-
-#include <vector>
-
-#include "modules/common/math/polygon2d.h"
-#include "modules/planning/proto/decision.pb.h"
-
-namespace apollo {
-namespace planning {
-
-class PlanningObject {
- public:
-  enum class PlanningObjectType {
-    OBSTACLE = 0,
-    MAP_OBJECT = 1,
-  };
-
- public:
-  PlanningObject() = default;
-  virtual PlanningObjectType ObjectType() const;
-  virtual PlanningObjectType* MutableObjectType();
-  virtual const apollo::common::math::Polygon2d& Polygon() const;
-  virtual apollo::common::math::Polygon2d* MutablePolygon();
-  virtual const std::vector<ObjectDecisionType>& Decisions() const;
-  virtual std::vector<ObjectDecisionType>* MutableDecisions();
-
- private:
-  PlanningObjectType object_type_;
-  apollo::common::math::Polygon2d polygon_;
-  std::vector<ObjectDecisionType> decisions_;
-};
-
-}  // namespace planning
-}  // namespace apollo
-
-#endif  // MODULES_PLANNING_COMMON_PLANNING_OBJECT_H_

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -264,7 +264,8 @@ bool DpRoadGraph::compute_object_decision_from_path(
 
   for (std::size_t i = 0; i < static_obstacles->size(); ++i) {
     bool ignore = true;
-    const auto &static_obstacle_box = (*static_obstacles)[i]->BoundingBox();
+    const auto &static_obstacle_box =
+        (*static_obstacles)[i]->PerceptionBoundingBox();
     common::SLPoint obs_sl;
 
     if (!reference_line.get_point_in_frenet_frame(
@@ -330,8 +331,7 @@ bool DpRoadGraph::compute_object_decision_from_path(
   size_t evaluate_times = static_cast<size_t>(
       std::floor(total_time / config_.eval_time_interval()));
   for (size_t i = 0; i < dynamic_obstacles->size(); ++i) {
-    const auto &trajectories =
-        (*dynamic_obstacles)[i]->prediction_trajectories();
+    auto *obstacle = (*dynamic_obstacles)[i];
 
     // list of Box2d for ego car given heuristic speed profile
     std::vector<::apollo::common::math::Box2d> ego_by_time;
@@ -340,40 +340,31 @@ bool DpRoadGraph::compute_object_decision_from_path(
       AERROR << "fill_ego_by_time error";
     }
 
-    for (size_t j = 0; j < trajectories.size(); ++j) {
-      const auto &trajectory = trajectories[j];
-      std::vector<::apollo::common::math::Box2d> obstacle_by_time;
-      for (size_t time = 0; time <= evaluate_times; ++time) {
-        auto traj_point = (*dynamic_obstacles)[i]->get_point_at(
-            trajectory, time * config_.eval_time_interval());
-        ::apollo::common::math::Vec2d center_point = {
-            traj_point.path_point().x(), traj_point.path_point().y()};
-        ::apollo::common::math::Box2d obstacle_box = {
-            center_point, traj_point.path_point().theta(),
-            (*dynamic_obstacles)[i]->BoundingBox().length(),
-            (*dynamic_obstacles)[i]->BoundingBox().width()};
-        obstacle_by_time.push_back(obstacle_box);
-      }
+    std::vector<::apollo::common::math::Box2d> obstacle_by_time;
+    for (size_t time = 0; time <= evaluate_times; ++time) {
+      auto traj_point =
+          obstacle->GetPointAtTime(time * config_.eval_time_interval());
+      obstacle_by_time.push_back(obstacle->GetBoundingBox(traj_point));
+    }
 
-      // if two lists of boxes collide
-      if (obstacle_by_time.size() != ego_by_time.size()) {
-        AINFO << "dynamic_obstacle_by_time size[" << obstacle_by_time.size()
-              << "] != ego_by_time[" << ego_by_time.size()
-              << "] from heuristic_speed_data";
-        continue;
-      }
+    // if two lists of boxes collide
+    if (obstacle_by_time.size() != ego_by_time.size()) {
+      AINFO << "dynamic_obstacle_by_time size[" << obstacle_by_time.size()
+            << "] != ego_by_time[" << ego_by_time.size()
+            << "] from heuristic_speed_data";
+      continue;
+    }
 
-      // judge for collision
-      for (size_t k = 0; k < obstacle_by_time.size(); ++k) {
-        if (ego_by_time[k].DistanceTo(obstacle_by_time[k]) <
-            FLAGS_dynamic_decision_follow_range) {
-          // Follow
-          ObjectDecisionType object_follow;
-          ObjectFollow *object_follow_ptr = object_follow.mutable_follow();
-          object_follow_ptr->set_distance_s(FLAGS_dp_path_decision_buffer);
-          (*dynamic_obstacles)[i]->MutableDecisions()->push_back(object_follow);
-          break;
-        }
+    // judge for collision
+    for (size_t k = 0; k < obstacle_by_time.size(); ++k) {
+      if (ego_by_time[k].DistanceTo(obstacle_by_time[k]) <
+          FLAGS_dynamic_decision_follow_range) {
+        // Follow
+        ObjectDecisionType object_follow;
+        ObjectFollow *object_follow_ptr = object_follow.mutable_follow();
+        object_follow_ptr->set_distance_s(FLAGS_dp_path_decision_buffer);
+        (*dynamic_obstacles)[i]->MutableDecisions()->push_back(object_follow);
+        break;
       }
     }
   }

modules/planning/optimizer/dp_poly_path/trajectory_cost.cc

@@ -50,31 +50,22 @@ TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
 
   // Mapping Static obstacle
   for (const auto ptr_static_obstacle : decision_data.StaticObstacles()) {
-    Vec2d center_point = ptr_static_obstacle->Center();
-    Box2d obstacle_box = {center_point, ptr_static_obstacle->Heading(),
-                          ptr_static_obstacle->BoundingBox().length(),
-                          ptr_static_obstacle->BoundingBox().width()};
-    static_obstacle_boxes_.push_back(std::move(obstacle_box));
+    static_obstacle_boxes_.push_back(
+        ptr_static_obstacle->PerceptionBoundingBox());
   }
 
   // Mapping dynamic obstacle
   for (const auto ptr_dynamic_obstacle : decision_data.DynamicObstacles()) {
-    for (const auto trajectory :
-         ptr_dynamic_obstacle->prediction_trajectories()) {
-      std::vector<Box2d> obstacle_by_time;
-      for (std::size_t time = 0; time < evaluate_times_ + 1; ++time) {
-        TrajectoryPoint traj_point = ptr_dynamic_obstacle->get_point_at(
-            trajectory, time * config.eval_time_interval());
-        Vec2d center_point = {traj_point.path_point().x(),
-                              traj_point.path_point().y()};
-        Box2d obstacle_box = {center_point, traj_point.path_point().theta(),
-                              ptr_dynamic_obstacle->BoundingBox().length(),
-                              ptr_dynamic_obstacle->BoundingBox().width()};
-        obstacle_by_time.push_back(std::move(obstacle_box));
-      }
-      dynamic_obstacle_trajectory_.push_back(std::move(obstacle_by_time));
-      dynamic_obstacle_probability_.push_back(trajectory.probability());
+    const auto &trajectory = ptr_dynamic_obstacle->Trajectory();
+    std::vector<Box2d> obstacle_by_time;
+    for (std::size_t time = 0; time < evaluate_times_ + 1; ++time) {
+      TrajectoryPoint traj_point = ptr_dynamic_obstacle->GetPointAtTime(
+          time * config.eval_time_interval());
+      obstacle_by_time.push_back(
+          ptr_dynamic_obstacle->GetBoundingBox(traj_point));
     }
+    dynamic_obstacle_trajectory_.push_back(std::move(obstacle_by_time));
+    dynamic_obstacle_probability_.push_back(trajectory.probability());
   }
   CHECK(dynamic_obstacle_trajectory_.size() ==
         dynamic_obstacle_probability_.size());

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -40,7 +40,7 @@ DpStGraph::DpStGraph(const DpStSpeedConfig& dp_config)
 
 Status DpStGraph::Search(const StGraphData& st_graph_data,
                          DecisionData* const decision_data,
-                         SpeedData* const speed_data, ObstacleTable* table) {
+                         SpeedData* const speed_data, Obstacles* table) {
   init_point_ = st_graph_data.init_point();
 
   if (st_graph_data.path_data_length() <
@@ -283,7 +283,7 @@ Status DpStGraph::retrieve_speed_profile(SpeedData* const speed_data) const {
 
 Status DpStGraph::get_object_decision(const StGraphData& st_graph_data,
                                       const SpeedData& speed_profile,
-                                      ObstacleTable* obstacles) const {
+                                      Obstacles* obstacles) const {
   if (speed_profile.speed_vector().size() < 2) {
     const std::string msg = "dp_st_graph failed to get speed profile.";
     AERROR << msg;

modules/planning/optimizer/dp_st_speed/dp_st_graph.h

@@ -45,7 +45,7 @@ class DpStGraph {
   apollo::common::Status Search(const StGraphData& st_graph_data,
                                 DecisionData* const decision_data,
                                 SpeedData* const speed_data,
-                                ObstacleTable* table);
+                                Obstacles* table);
 
  private:
   apollo::common::Status InitCostTable();
@@ -57,7 +57,7 @@ class DpStGraph {
 
   apollo::common::Status get_object_decision(const StGraphData& st_graph_data,
                                              const SpeedData& speed_profile,
-                                             ObstacleTable* obstacles) const;
+                                             Obstacles* obstacles) const;
 
   apollo::common::Status CalculateTotalCost(const StGraphData& st_graph_data);
   void CalculateCostAt(const StGraphData& st_graph_data, const uint32_t r,

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -96,7 +96,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
   DpStGraph st_graph(dp_st_speed_config_);
   if (!st_graph
            .Search(st_graph_data, decision_data, speed_data,
-                   frame_->MutableObstacleTable())
+                   frame_->MutableObstacles())
            .ok()) {
     const std::string msg = "Failed to search graph with dynamic programming.";
     AERROR << msg;

modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc

@@ -185,14 +185,7 @@ bool QpFrenetFrame::mapping_dynamic_obstacle_with_decision(
     const Obstacle& obstacle) {
   const std::vector<ObjectDecisionType>& decision = obstacle.Decisions();
 
-  if (decision.size() != obstacle.prediction_trajectories().size()) {
-    AERROR << "prediction_trajectories size["
-           << obstacle.prediction_trajectories().size()
-           << " does NOT match decision size[" << decision.size() << "].";
-    return true;
-  }
-
-  for (uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) {
+  for (uint32_t i = 0; i < decision.size(); ++i) {
     if (!decision[i].has_nudge()) {
       continue;
     }
@@ -200,18 +193,13 @@ bool QpFrenetFrame::mapping_dynamic_obstacle_with_decision(
     double buffer = std::fabs(nudge.distance_l());
 
     int nudge_side = nudge.type() == ObjectNudge::RIGHT_NUDGE ? 1 : -1;
-    const prediction::Trajectory& predicted_trajectory =
-        obstacle.prediction_trajectories()[i];
 
     for (const SpeedPoint& veh_point : _discretized_veh_loc) {
       double time = veh_point.t();
-      common::TrajectoryPoint trajectory_point =
-          obstacle.get_point_at(predicted_trajectory, time);
+      common::TrajectoryPoint trajectory_point = obstacle.GetPointAtTime(time);
       common::math::Vec2d xy_point(trajectory_point.path_point().x(),
                                    trajectory_point.path_point().y());
-      common::math::Box2d obs_box(xy_point,
-                                  trajectory_point.path_point().theta(),
-                                  obstacle.Length(), obstacle.Width());
+      common::math::Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
       // project obs_box on reference line
       std::vector<common::math::Vec2d> corners;
       obs_box.GetAllCorners(&corners);
@@ -282,7 +270,7 @@ bool QpFrenetFrame::mapping_static_obstacle_with_decision(
     const auto& nudge = decision.nudge();
     const double buffer = std::fabs(nudge.distance_l());
     if (nudge.type() == ObjectNudge::RIGHT_NUDGE) {
-      common::math::Box2d box = obstacle.BoundingBox();
+      const auto& box = obstacle.PerceptionBoundingBox();
       std::vector<common::math::Vec2d> corners;
       box.GetAllCorners(&corners);
       if (!mapping_polygon(corners, buffer, -1, &_static_obstacle_bound)) {
@@ -291,7 +279,7 @@ bool QpFrenetFrame::mapping_static_obstacle_with_decision(
         return false;
       }
     } else {
-      common::math::Box2d box = obstacle.BoundingBox();
+      const auto& box = obstacle.PerceptionBoundingBox();
       std::vector<common::math::Vec2d> corners;
       box.GetAllCorners(&corners);
       if (!mapping_polygon(corners, buffer, 1, &_static_obstacle_bound)) {

modules/planning/optimizer/st_graph/BUILD

@@ -67,7 +67,6 @@ cc_library(
         "//modules/planning/common:frame",
         "//modules/planning/common:obstacle",
         "//modules/planning/common:planning_gflags",
-        "//modules/planning/common:planning_object",
         "//modules/planning/common:speed_limit",
         "//modules/planning/common/path:discretized_path",
         "//modules/planning/common/path:frenet_frame_path",

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc

@@ -235,11 +235,12 @@ Status StBoundaryMapperImpl::MapObstacleWithPredictionTrajectory(
   std::vector<STPoint> lower_points;
   std::vector<STPoint> upper_points;
 
-  const double speed = obstacle.Speed();
+  const auto& speed = obstacle.Perception().velocity();
+  const double scalar_speed = std::hypot(speed.x(), speed.y());
   const double minimal_follow_time = st_boundary_config().minimal_follow_time();
   double follow_distance = -1.0;
   if (obj_decision.has_follow()) {
-    follow_distance = std::fmax(speed * minimal_follow_time,
+    follow_distance = std::fmax(scalar_speed * minimal_follow_time,
                                 std::fabs(obj_decision.follow().distance_s())) +
                       vehicle_param().front_edge_to_center();
   }
@@ -247,118 +248,111 @@ Status StBoundaryMapperImpl::MapObstacleWithPredictionTrajectory(
   bool skip = true;
   std::vector<STPoint> boundary_points;
   const auto& adc_path_points = path_data.discretized_path().points();
-  if (obstacle.prediction_trajectories().size() == 0) {
+  const auto& trajectory = obstacle.Trajectory();
+  if (trajectory.trajectory_point_size() == 0) {
     AWARN << "Obstacle (id = " << obstacle.Id()
           << ") has NO prediction trajectory.";
   }
 
-  for (uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) {
-    const auto& trajectory = obstacle.prediction_trajectories()[i];
-    for (int j = 0; j < trajectory.trajectory_point_size(); ++j) {
-      const auto& trajectory_point = trajectory.trajectory_point(j);
-      // TODO(all): fix trajectory point relative time issue.
-      double trajectory_point_time = trajectory_point.relative_time();
-      const Box2d obs_box(
-          Vec2d(trajectory_point.path_point().x(),
-                trajectory_point.path_point().y()),
-          trajectory_point.path_point().theta(),
-          obstacle.Length() * st_boundary_config().expending_coeff(),
-          obstacle.Width() * st_boundary_config().expending_coeff());
-      int64_t low = 0;
-      int64_t high = adc_path_points.size() - 1;
-      bool find_low = false;
-      bool find_high = false;
-      while (low < high) {
-        if (find_low && find_high) {
-          break;
-        }
-        if (!find_low) {
-          if (!CheckOverlap(adc_path_points[low], vehicle_param(), obs_box,
-                            st_boundary_config().boundary_buffer())) {
-            ++low;
-          } else {
-            find_low = true;
-          }
-        }
-        if (!find_high) {
-          if (!CheckOverlap(adc_path_points[high], vehicle_param(), obs_box,
-                            st_boundary_config().boundary_buffer())) {
-            --high;
-          } else {
-            find_high = true;
-          }
+  for (int j = 0; j < trajectory.trajectory_point_size(); ++j) {
+    const auto& trajectory_point = trajectory.trajectory_point(j);
+    // TODO(all): fix trajectory point relative time issue.
+    double trajectory_point_time = trajectory_point.relative_time();
+    const Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
+    int64_t low = 0;
+    int64_t high = adc_path_points.size() - 1;
+    bool find_low = false;
+    bool find_high = false;
+    while (low < high) {
+      if (find_low && find_high) {
+        break;
+      }
+      if (!find_low) {
+        if (!CheckOverlap(adc_path_points[low], vehicle_param(), obs_box,
+                          st_boundary_config().boundary_buffer())) {
+          ++low;
+        } else {
+          find_low = true;
         }
       }
-      if (find_high && find_low) {
-        lower_points.emplace_back(
-            adc_path_points[low].s() - st_boundary_config().point_extension(),
-            trajectory_point_time);
-        upper_points.emplace_back(
-            adc_path_points[high].s() + st_boundary_config().point_extension(),
-            trajectory_point_time);
-      } else {
-        if (obj_decision.has_yield() || obj_decision.has_overtake()) {
-          AINFO << "Point[" << j << "] cannot find low or high index.";
+      if (!find_high) {
+        if (!CheckOverlap(adc_path_points[high], vehicle_param(), obs_box,
+                          st_boundary_config().boundary_buffer())) {
+          --high;
+        } else {
+          find_high = true;
         }
       }
+    }
+    if (find_high && find_low) {
+      lower_points.emplace_back(
+          adc_path_points[low].s() - st_boundary_config().point_extension(),
+          trajectory_point_time);
+      upper_points.emplace_back(
+          adc_path_points[high].s() + st_boundary_config().point_extension(),
+          trajectory_point_time);
+    } else {
+      if (obj_decision.has_yield() || obj_decision.has_overtake()) {
+        AINFO << "Point[" << j << "] cannot find low or high index.";
+      }
+    }
 
-      if (lower_points.size() > 0) {
-        boundary_points.clear();
-        const double buffer = st_boundary_config().follow_buffer();
-        boundary_points.emplace_back(lower_points.at(0).s() - buffer,
-                                     lower_points.at(0).t());
-        boundary_points.emplace_back(lower_points.back().s() - buffer,
-                                     lower_points.back().t());
-        boundary_points.emplace_back(upper_points.back().s() + buffer +
-                                         st_boundary_config().boundary_buffer(),
-                                     upper_points.back().t());
-        boundary_points.emplace_back(upper_points.at(0).s() + buffer,
-                                     upper_points.at(0).t());
-        if (lower_points.at(0).t() > lower_points.back().t() ||
-            upper_points.at(0).t() > upper_points.back().t()) {
-          AWARN << "lower/upper points are reversed.";
-        }
+    if (lower_points.size() > 0) {
+      boundary_points.clear();
+      const double buffer = st_boundary_config().follow_buffer();
+      boundary_points.emplace_back(lower_points.at(0).s() - buffer,
+                                   lower_points.at(0).t());
+      boundary_points.emplace_back(lower_points.back().s() - buffer,
+                                   lower_points.back().t());
+      boundary_points.emplace_back(upper_points.back().s() + buffer +
+                                       st_boundary_config().boundary_buffer(),
+                                   upper_points.back().t());
+      boundary_points.emplace_back(upper_points.at(0).s() + buffer,
+                                   upper_points.at(0).t());
+      if (lower_points.at(0).t() > lower_points.back().t() ||
+          upper_points.at(0).t() > upper_points.back().t()) {
+        AWARN << "lower/upper points are reversed.";
+      }
 
-        // change boundary according to obj_decision.
-        StGraphBoundary::BoundaryType b_type =
-            StGraphBoundary::BoundaryType::UNKNOWN;
-        if (obj_decision.has_follow()) {
-          boundary_points.at(0).set_s(boundary_points.at(0).s() -
-                                      follow_distance);
-          boundary_points.at(1).set_s(boundary_points.at(1).s() -
-                                      follow_distance);
-          boundary_points.at(3).set_t(-1.0);
-          b_type = StGraphBoundary::BoundaryType::FOLLOW;
-        } else if (obj_decision.has_yield()) {
-          const double dis = std::fabs(obj_decision.yield().distance_s());
-          // TODO(all): remove the arbitrary numbers in this part.
-          if (boundary_points.at(0).s() - dis < 0.0) {
-            boundary_points.at(0).set_s(
-                std::fmax(boundary_points.at(0).s() - 2.0, 0.0));
-          } else {
-            boundary_points.at(0).set_s(
-                std::fmax(boundary_points.at(0).s() - dis, 0.0));
-          }
-          if (boundary_points.at(1).s() - dis < 0.0) {
-            boundary_points.at(1).set_s(
-                std::fmax(boundary_points.at(0).s() - 4.0, 0.0));
-          } else {
-            boundary_points.at(1).set_s(
-                std::fmax(boundary_points.at(0).s() - dis, 0.0));
-          }
-          b_type = StGraphBoundary::BoundaryType::YIELD;
-        } else if (obj_decision.has_overtake()) {
-          const double dis = std::fabs(obj_decision.overtake().distance_s());
-          boundary_points.at(2).set_s(boundary_points.at(2).s() + dis);
-          boundary_points.at(3).set_s(boundary_points.at(3).s() + dis);
+      // change boundary according to obj_decision.
+      StGraphBoundary::BoundaryType b_type =
+          StGraphBoundary::BoundaryType::UNKNOWN;
+      if (obj_decision.has_follow()) {
+        boundary_points.at(0).set_s(boundary_points.at(0).s() -
+                                    follow_distance);
+        boundary_points.at(1).set_s(boundary_points.at(1).s() -
+                                    follow_distance);
+        boundary_points.at(3).set_t(-1.0);
+        b_type = StGraphBoundary::BoundaryType::FOLLOW;
+      } else if (obj_decision.has_yield()) {
+        const double dis = std::fabs(obj_decision.yield().distance_s());
+        // TODO(all): remove the arbitrary numbers in this part.
+        if (boundary_points.at(0).s() - dis < 0.0) {
+          boundary_points.at(0).set_s(
+              std::fmax(boundary_points.at(0).s() - 2.0, 0.0));
+        } else {
+          boundary_points.at(0).set_s(
+              std::fmax(boundary_points.at(0).s() - dis, 0.0));
         }
-
-        const double area = GetArea(boundary_points);
-        if (Double::compare(area, 0.0) > 0) {
-          boundary->emplace_back(boundary_points);
-          boundary->back().set_boundary_type(b_type);
-          skip = false;
+        if (boundary_points.at(1).s() - dis < 0.0) {
+          boundary_points.at(1).set_s(
+              std::fmax(boundary_points.at(0).s() - 4.0, 0.0));
+        } else {
+          boundary_points.at(1).set_s(
+              std::fmax(boundary_points.at(0).s() - dis, 0.0));
         }
+        b_type = StGraphBoundary::BoundaryType::YIELD;
+      } else if (obj_decision.has_overtake()) {
+        const double dis = std::fabs(obj_decision.overtake().distance_s());
+        boundary_points.at(2).set_s(boundary_points.at(2).s() + dis);
+        boundary_points.at(3).set_s(boundary_points.at(3).s() + dis);
+      }
+
+      const double area = GetArea(boundary_points);
+      if (Double::compare(area, 0.0) > 0) {
+        boundary->emplace_back(boundary_points);
+        boundary->back().set_boundary_type(b_type);
+        skip = false;
       }
     }
   }
@@ -381,22 +375,18 @@ Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory(
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  const double speed = obstacle.Speed();
-  const double heading = obstacle.Heading();
-  const Vec2d center = obstacle.Center();
-  const Box2d box(center, heading,
-                  obstacle.Length() * st_boundary_config().expending_coeff(),
-                  obstacle.Width() * st_boundary_config().expending_coeff());
+  const auto& speed = obstacle.Perception().velocity();
+  const double scalar_speed = std::hypot(speed.x(), speed.y());
 
-  const PathPoint ref_point =
-      reference_line.get_reference_point(center.x(), center.y());
-  const double speed_coeff = std::cos(heading - ref_point.theta());
+  const auto& perception = obstacle.Perception();
+  const PathPoint ref_point = reference_line.get_reference_point(
+      perception.position().x(), perception.position().y());
+  const double speed_coeff = std::cos(perception.theta() - ref_point.theta());
   if (speed_coeff < 0.0) {
-    std::string msg = common::util::StrCat(
-        "Obstacle is moving opposite to the reference line. Obstacle heading: ",
-        heading, "; ref_point.theta(): ", ref_point.theta());
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
+    AERROR << "Obstacle is moving opposite to the reference line. Obstacle: "
+           << perception.DebugString();
+    return common::Status(ErrorCode::PLANNING_ERROR,
+                          "obstacle is moving opposite the reference line");
   }
 
   const auto& point = path_data.discretized_path().points()[0];
@@ -414,10 +404,10 @@ Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory(
   }
 
   double follow_speed = 0.0;
-  if (speed > st_boundary_config().follow_speed_threshold()) {
+  if (scalar_speed > st_boundary_config().follow_speed_threshold()) {
     follow_speed = st_boundary_config().follow_speed_threshold() * speed_coeff;
   } else {
-    follow_speed = speed * speed_coeff *
+    follow_speed = scalar_speed * speed_coeff *
                    st_boundary_config().follow_speed_damping_factor();
   }
 
@@ -443,9 +433,9 @@ Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory(
   boundary->emplace_back(boundary_points);
 
   const double characteristic_length =
-      std::fmax(
-          speed * speed_coeff * st_boundary_config().minimal_follow_time(),
-          std::fabs(obj_decision.follow().distance_s())) +
+      std::fmax(scalar_speed * speed_coeff *
+                    st_boundary_config().minimal_follow_time(),
+                std::fabs(obj_decision.follow().distance_s())) +
       VehicleConfigHelper::GetConfig().vehicle_param().front_edge_to_center() +
       st_boundary_config().follow_buffer();