fixed reference_line param in st boundary mapper (#14)

modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc

@@ -24,10 +24,10 @@
 #include <limits>
 #include <vector>
 
+#include "modules/common/math/vec2d.h"
 #include "modules/common/proto/path_point.pb.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/math/double.h"
-#include "modules/common/math/vec2d.h"
 
 namespace apollo {
 namespace planning {
@@ -44,7 +44,8 @@ DpStBoundaryMapper::DpStBoundaryMapper(
 Status DpStBoundaryMapper::get_graph_boundary(
     const common::TrajectoryPoint& initial_planning_point,
     const DecisionData& decision_data, const PathData& path_data,
-    const double planning_distance, const double planning_time,
+    const ReferenceLine& reference_line, const double planning_distance,
+    const double planning_time,
     std::vector<StGraphBoundary>* const obs_boundary) const {
   if (planning_time < 0.0) {
     const std::string msg = "Fail to get params since planning_time < 0.";
@@ -54,7 +55,7 @@ Status DpStBoundaryMapper::get_graph_boundary(
 
   if (path_data.path().num_of_points() < 2) {
     AERROR << "Fail to get params since path has "
-        <<  path_data.path().num_of_points() << " points.";
+           << path_data.path().num_of_points() << " points.";
     return Status(ErrorCode::PLANNING_ERROR, "Fail to get params");
   }
 
@@ -70,11 +71,12 @@ Status DpStBoundaryMapper::get_graph_boundary(
     if (static_obs_vec[i] == nullptr) {
       continue;
     }
-    if (!map_obstacle_without_trajectory(
-        *static_obs_vec[i], path_data, planning_distance, planning_time,
-        obs_boundary).ok()) {
+    if (!map_obstacle_without_trajectory(*static_obs_vec[i], path_data,
+                                         planning_distance, planning_time,
+                                         obs_boundary)
+             .ok()) {
       AERROR << "Fail to map static obstacle with id "
-          << static_obs_vec[i]->Id();
+             << static_obs_vec[i]->Id();
       return Status(ErrorCode::PLANNING_ERROR,
                     "Fail to map static obstacle with id");
     }
@@ -84,11 +86,12 @@ Status DpStBoundaryMapper::get_graph_boundary(
     if (dynamic_obs_vec[i] == nullptr) {
       continue;
     }
-    if (!map_obstacle_with_trajectory(
-        *dynamic_obs_vec[i], path_data, planning_distance,
-        planning_time, obs_boundary).ok()) {
+    if (!map_obstacle_with_trajectory(*dynamic_obs_vec[i], path_data,
+                                      planning_distance, planning_time,
+                                      obs_boundary)
+             .ok()) {
       AERROR << "Fail to map dynamic obstacle with id "
-          << dynamic_obs_vec[i]->Id();
+             << dynamic_obs_vec[i]->Id();
       return Status(ErrorCode::PLANNING_ERROR,
                     "Fail to map dynamic obstacle with id");
     }
@@ -105,16 +108,17 @@ Status DpStBoundaryMapper::map_obstacle_with_trajectory(
   std::vector<STPoint> lower_points;
   std::vector<STPoint> upper_points;
   const std::vector<PathPoint>& veh_path = path_data.path().path_points();
-  for (std::uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) {
+  for (std::uint32_t i = 0; i < obstacle.prediction_trajectories().size();
+       ++i) {
     PredictionTrajectory pred_traj = obstacle.prediction_trajectories()[i];
     bool skip = true;
     for (std::uint32_t j = 0; j < pred_traj.num_of_points(); ++j) {
       TrajectoryPoint cur_obs_point = pred_traj.trajectory_point_at(j);
       // construct bounding box
-      ::apollo::common::math::Box2d obs_box({cur_obs_point.path_point().x(),
-          cur_obs_point.path_point().y()},
-          cur_obs_point.path_point().theta(),
-          obstacle.Length(), obstacle.Width());
+      ::apollo::common::math::Box2d obs_box(
+          {cur_obs_point.path_point().x(), cur_obs_point.path_point().y()},
+          cur_obs_point.path_point().theta(), obstacle.Length(),
+          obstacle.Width());
 
       // loop path data to find the lower and upper bound
       const double buffer = st_boundary_config().boundary_buffer();

modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.h

@@ -39,7 +39,8 @@ class DpStBoundaryMapper : public StBoundaryMapper {
   apollo::common::Status get_graph_boundary(
       const common::TrajectoryPoint& initial_planning_point,
       const DecisionData& decision_data, const PathData& path_data,
-      const double planning_distance, const double planning_time,
+      const ReferenceLine& reference_line, const double planning_distance,
+      const double planning_time,
       std::vector<StGraphBoundary>* const boundary) const override;
 
  private:

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -59,6 +59,7 @@ bool DpStSpeedOptimizer::Init() {
 
 Status DpStSpeedOptimizer::Process(const PathData& path_data,
                                    const TrajectoryPoint& init_point,
+                                   const ReferenceLine& reference_line,
                                    DecisionData* const decision_data,
                                    SpeedData* const speed_data) {
   const auto& veh_param = VehicleConfigHelper::GetConfig().vehicle_param();
@@ -74,7 +75,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
                                                        ->init_planning_point();
   if (!st_mapper
            .get_graph_boundary(initial_planning_point, *decision_data,
-                               path_data,
+                               path_data, reference_line,
                                dp_st_speed_config_.total_path_length(),
                                dp_st_speed_config_.total_time(), &boundaries)
            .ok()) {

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.h

@@ -40,6 +40,7 @@ class DpStSpeedOptimizer : public SpeedOptimizer {
  private:
   virtual apollo::common::Status Process(const PathData& path_data,
                                          const TrajectoryPoint& init_point,
+                                         const ReferenceLine& reference_line,
                                          DecisionData* const decision_data,
                                          SpeedData* const speed_data) override;
   StBoundaryConfig st_boundary_config_;

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc

@@ -36,12 +36,12 @@
 namespace apollo {
 namespace planning {
 
-using apollo::common::ErrorCode;
-using apollo::common::PathPoint;
-using apollo::common::Status;
-using apollo::common::config::VehicleParam;
-using apollo::common::math::Box2d;
-using apollo::common::math::Vec2d;
+using ErrorCode = apollo::common::ErrorCode;
+using Status = apollo::common::Status;
+using PathPoint = apollo::common::PathPoint;
+using VehicleParam = apollo::common::config::VehicleParam;
+using Box2d = apollo::common::math::Box2d;
+using Vec2d = apollo::common::math::Vec2d;
 
 QpSplineStBoundaryMapper::QpSplineStBoundaryMapper(
     const StBoundaryConfig& st_boundary_config, const VehicleParam& veh_param)
@@ -50,7 +50,8 @@ QpSplineStBoundaryMapper::QpSplineStBoundaryMapper(
 Status QpSplineStBoundaryMapper::get_graph_boundary(
     const common::TrajectoryPoint& initial_planning_point,
     const DecisionData& decision_data, const PathData& path_data,
-    const double planning_distance, const double planning_time,
+    const ReferenceLine& reference_line, const double planning_distance,
+    const double planning_time,
     std::vector<StGraphBoundary>* const obs_boundary) const {
   if (obs_boundary) {
     const std::string msg = "obs_boundary is NULL.";
@@ -73,55 +74,20 @@ Status QpSplineStBoundaryMapper::get_graph_boundary(
   }
 
   obs_boundary->clear();
+  Status ret = Status::OK();
 
-  Status ret;
-  /*
-  const auto& planning_data = processed_data.planning_data();
-  const auto& main_decision = planning_data.main_decision();
-  QUIT_IF(!planning_data.has_main_decision(), ErrorCode::PLANNING_SKIP,
-          Level::X_ERROR, "Skip mapping because no decision. \n%s",
-          planning_data.DebugString().c_str());
-
-  ErrorCode ret = ErrorCode::PLANNING_OK;
-
+  const auto& main_decision = decision_data.main_decision();
   if (main_decision.has_stop()) {
-    ret = map_main_decision_stop(
-        main_decision.stop(), processed_data_proxy.target_reference_line_raw(),
-        planning_distance, planning_time, obs_boundary);
-    QUIT_IF(ret != ErrorCode::PLANNING_OK && ret != ErrorCode::PLANNING_SKIP,
-            ret, Level::X_ERROR,
-            "Failed to get_params since map_main_decision_stop error.");
-    obs_boundary->back().set_id("main decision stop");
-  } else if (main_decision.has_change_lane()) {
-    if (main_decision.change_lane().has_default_lane_stop()) {
-      ret = map_main_decision_stop(
-          main_decision.change_lane().default_lane_stop(),
-          processed_data_proxy.default_reference_line_raw(), planning_distance,
-          planning_time, obs_boundary);
-      QUIT_IF(ret != ErrorCode::PLANNING_OK && ret != ErrorCode::PLANNING_SKIP,
-              ret, Level::X_ERROR,
-              "Fail to get_params due to map default lane stop error.");
-      obs_boundary->back().set_id("default_lane stop");
-    }
-    if (main_decision.change_lane().has_target_lane_stop()) {
-      ret = map_main_decision_stop(
-          main_decision.change_lane().target_lane_stop(),
-          processed_data_proxy.target_reference_line_raw(), planning_distance,
-          planning_time, obs_boundary);
-      QUIT_IF(ret != ErrorCode::PLANNING_OK && ret != ErrorCode::PLANNING_SKIP,
-              ret, Level::X_ERROR,
-              "Fail to get_params due to map target lane stop error.");
-      obs_boundary->back().set_id("target_lane stop");
+    ret =
+        map_main_decision_stop(main_decision.stop(), reference_line,
+                               planning_distance, planning_time, obs_boundary);
+    if (ret != Status::OK() && ret != Status(ErrorCode::PLANNING_SKIP, "")) {
+      return ret;
     }
   } else if (main_decision.has_mission_complete()) {
-    ret = map_main_decision_mission_complete(
-        processed_data_proxy.target_reference_line_raw(), planning_distance,
-        planning_time, obs_boundary);
-    QUIT_IF(ret != ErrorCode::PLANNING_OK && ret != ErrorCode::PLANNING_SKIP,
-            ret, Level::X_ERROR,
-            "Failed to get_params since map_mission_complete error.");
+    // TODO: fill in this function.
+    // ret = map_main_decision_mission_complete();
   }
-  */
 
   const auto& static_obs_vec = decision_data.StaticObstacles();
   for (const auto* obs : static_obs_vec) {
@@ -168,6 +134,14 @@ Status QpSplineStBoundaryMapper::get_graph_boundary(
   return Status::OK();
 }
 
+Status QpSplineStBoundaryMapper::map_main_decision_stop(
+    const MainStop& main_stop, const ReferenceLine& reference_line,
+    const double planning_distance, const double planning_time,
+    std::vector<StGraphBoundary>* const boundary) const {
+  // TODO: complete this function.
+  return Status::OK();
+}
+
 Status QpSplineStBoundaryMapper::map_obstacle_with_planning(
     const common::TrajectoryPoint& initial_planning_point,
     const Obstacle& obstacle, const PathData& path_data,

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.h

@@ -42,10 +42,16 @@ class QpSplineStBoundaryMapper : public StBoundaryMapper {
   apollo::common::Status get_graph_boundary(
       const common::TrajectoryPoint& initial_planning_point,
       const DecisionData& decision_data, const PathData& path_data,
-      const double planning_distance, const double planning_time,
+      const ReferenceLine& reference_line, const double planning_distance,
+      const double planning_time,
       std::vector<StGraphBoundary>* const boundary) const override;
 
  private:
+  apollo::common::Status map_main_decision_stop(
+      const MainStop& main_stop, const ReferenceLine& reference_line,
+      const double planning_distance, const double planning_time,
+      std::vector<StGraphBoundary>* const boundary) const;
+
   apollo::common::Status map_obstacle_with_planning(
       const common::TrajectoryPoint& initial_planning_point,
       const Obstacle& obstacle, const PathData& path_data,

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -60,6 +60,7 @@ bool QpSplineStSpeedOptimizer::Init() {
 
 Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
                                          const TrajectoryPoint& init_point,
+                                         const ReferenceLine& reference_line,
                                          DecisionData* const decision_data,
                                          SpeedData* const speed_data) {
   if (!is_init_) {
@@ -75,7 +76,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   QpSplineStBoundaryMapper st_mapper(st_boundary_config_, veh_param);
   std::vector<StGraphBoundary> boundaries;
   if (st_mapper.get_graph_boundary(
-          init_point, *decision_data, path_data,
+          init_point, *decision_data, path_data, reference_line,
           qp_spline_st_speed_config_.total_path_length(),
           qp_spline_st_speed_config_.total_time(),
           &boundaries) != Status::OK()) {
@@ -84,8 +85,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   }
 
   SpeedLimit speed_limits;
-  const auto& pose =
-      apollo::common::VehicleState::instance()->pose();
+  const auto& pose = apollo::common::VehicleState::instance()->pose();
   const auto& hdmap = apollo::planning::DataCenter::instance()->map();
   if (st_mapper.get_speed_limits(pose, hdmap, path_data, total_length,
                                  qp_spline_st_speed_config_.total_time(),

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.h

@@ -42,7 +42,8 @@ class QpSplineStSpeedOptimizer : public SpeedOptimizer {
   virtual common::Status Process(
       const PathData& path_data,
       const apollo::common::TrajectoryPoint& init_point,
-      DecisionData* const decision_data, SpeedData* const speed_data) override;
+      const ReferenceLine& reference_line, DecisionData* const decision_data,
+      SpeedData* const speed_data) override;
   StBoundaryConfig st_boundary_config_;
   QpSplineStSpeedConfig qp_spline_st_speed_config_;
 };

modules/planning/optimizer/speed_optimizer.cc

@@ -28,10 +28,10 @@ namespace planning {
 SpeedOptimizer::SpeedOptimizer(const std::string& name) : Optimizer(name) {}
 
 apollo::common::Status SpeedOptimizer::Optimize(PlanningData* planning_data) {
-  return Process(planning_data->path_data(),
-                 planning_data->init_planning_point(),
-                 planning_data->mutable_decision_data(),
-                 planning_data->mutable_speed_data());
+  return Process(
+      planning_data->path_data(), planning_data->init_planning_point(),
+      planning_data->reference_line(), planning_data->mutable_decision_data(),
+      planning_data->mutable_speed_data());
 }
 
 }  // namespace planning

modules/planning/optimizer/speed_optimizer.h

@@ -40,6 +40,7 @@ class SpeedOptimizer : public Optimizer {
  protected:
   virtual apollo::common::Status Process(const PathData& path_data,
                                          const TrajectoryPoint& init_point,
+                                         const ReferenceLine& reference_line,
                                          DecisionData* const decision_data,
                                          SpeedData* const speed_data) = 0;
 };

modules/planning/optimizer/st_graph/st_boundary_mapper.h

@@ -24,8 +24,8 @@
 #include <vector>
 
 #include "modules/common/configs/proto/vehicle_config.pb.h"
-#include "modules/common/status/status.h"
 #include "modules/common/proto/path_point.pb.h"
+#include "modules/common/status/status.h"
 #include "modules/localization/proto/pose.pb.h"
 #include "modules/planning/proto/st_boundary_config.pb.h"
 
@@ -50,7 +50,8 @@ class StBoundaryMapper {
   virtual apollo::common::Status get_graph_boundary(
       const common::TrajectoryPoint& initial_planning_point,
       const DecisionData& decision_data, const PathData& path_data,
-      const double planning_distance, const double planning_time,
+      const ReferenceLine& reference_line, const double planning_distance,
+      const double planning_time,
       std::vector<StGraphBoundary>* const boundary) const = 0;
 
   virtual apollo::common::Status get_speed_limits(