added get speed limit functions on reference line.

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc

@@ -80,15 +80,15 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
 
   const auto& main_decision = decision_data.Decision().main_decision();
   if (main_decision.has_stop()) {
-    ret = map_main_decision_stop(main_decision.stop(), reference_line,
-                                 planning_distance, planning_time,
-                                 st_graph_boundaries);
+    ret = MapMainDecisionStop(main_decision.stop(), reference_line,
+                              planning_distance, planning_time,
+                              st_graph_boundaries);
     if (!ret.ok() && ret.code() != ErrorCode::PLANNING_SKIP) {
       return Status(ErrorCode::PLANNING_ERROR);
     }
   } else if (main_decision.has_mission_complete()) {
-    ret = map_mission_complete(reference_line, planning_distance, planning_time,
-                               st_graph_boundaries);
+    ret = MapMissionComplete(reference_line, planning_distance, planning_time,
+                             st_graph_boundaries);
     if (!ret.ok() && ret.code() != ErrorCode::PLANNING_SKIP) {
       return Status(ErrorCode::PLANNING_ERROR);
     }
@@ -101,7 +101,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
     if (obs == nullptr) {
       continue;
     }
-    ret = map_obstacle_without_prediction_trajectory(
+    ret = MapObstacleWithoutPredictionTrajectory(
         initial_planning_point, *obs, path_data, planning_distance,
         planning_time, st_graph_boundaries);
     if (!ret.ok()) {
@@ -118,7 +118,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
     }
     for (auto& obj_decision : obs->Decisions()) {
       if (obj_decision.has_follow()) {
-        ret = map_obstacle_without_prediction_trajectory(
+        ret = MapObstacleWithoutPredictionTrajectory(
             initial_planning_point, *obs, obj_decision, path_data,
             reference_line, planning_distance, planning_time,
             st_graph_boundaries);
@@ -129,7 +129,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
                         "Fail to map follow dynamic obstacle");
         }
       } else if (obj_decision.has_overtake() || obj_decision.has_yield()) {
-        ret = map_obstacle_with_prediction_trajectory(
+        ret = MapObstacleWithPredictionTrajectory(
             initial_planning_point, *obs, obj_decision, path_data,
             planning_distance, planning_time, st_graph_boundaries);
         if (!ret.ok()) {
@@ -143,7 +143,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
   return Status::OK();
 }
 
-Status StBoundaryMapperImpl::map_main_decision_stop(
+Status StBoundaryMapperImpl::MapMainDecisionStop(
     const MainStop& main_stop, const ReferenceLine& reference_line,
     const double planning_distance, const double planning_time,
     std::vector<StGraphBoundary>* const boundary) const {
@@ -155,7 +155,7 @@ Status StBoundaryMapperImpl::map_main_decision_stop(
   SLPoint sl_point;
   if (!reference_line.get_point_in_frenet_frame(
           Vec2d(map_point.x(), map_point.y()), &sl_point)) {
-    AERROR << "Fail to map_main_decision_stop since get_point_in_frenet_frame "
+    AERROR << "Fail to MapMainDecisionStop since get_point_in_frenet_frame "
               "failed.";
     return Status(ErrorCode::PLANNING_ERROR);
   }
@@ -171,7 +171,7 @@ Status StBoundaryMapperImpl::map_main_decision_stop(
     if (stop_rear_center_s >=
         reference_line.length() - FLAGS_backward_routing_distance) {
       AWARN << common::util::StrCat(
-          "Skip to map_main_decision_stop since stop_rear_center_s[",
+          "Skip to MapMainDecisionStop since stop_rear_center_s[",
           stop_rear_center_s, "] > path length[", reference_line.length(),
           "].");
       return Status(ErrorCode::PLANNING_SKIP);
@@ -199,7 +199,7 @@ Status StBoundaryMapperImpl::map_main_decision_stop(
   return Status::OK();
 }
 
-Status StBoundaryMapperImpl::map_mission_complete(
+Status StBoundaryMapperImpl::MapMissionComplete(
     const ReferenceLine& reference_line, const double planning_distance,
     const double planning_time,
     std::vector<StGraphBoundary>* const boundary) const {
@@ -226,7 +226,7 @@ Status StBoundaryMapperImpl::map_mission_complete(
   return Status::OK();
 }
 
-Status StBoundaryMapperImpl::map_obstacle_with_prediction_trajectory(
+Status StBoundaryMapperImpl::MapObstacleWithPredictionTrajectory(
     const common::TrajectoryPoint& initial_planning_point,
     const Obstacle& obstacle, const ObjectDecisionType obj_decision,
     const PathData& path_data, const double planning_distance,
@@ -366,7 +366,7 @@ Status StBoundaryMapperImpl::map_obstacle_with_prediction_trajectory(
               : Status::OK();
 }
 
-Status StBoundaryMapperImpl::map_obstacle_without_prediction_trajectory(
+Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory(
     const common::TrajectoryPoint& initial_planning_point,
     const Obstacle& obstacle, const ObjectDecisionType obj_decision,
     const PathData& path_data, const ReferenceLine& reference_line,

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.h

@@ -43,24 +43,24 @@ class StBoundaryMapperImpl : public StBoundaryMapper {
       std::vector<StGraphBoundary>* const boundary) const override;
 
  private:
-  apollo::common::Status map_main_decision_stop(
+  apollo::common::Status MapMainDecisionStop(
       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_mission_complete(
+  apollo::common::Status MapMissionComplete(
       const ReferenceLine& reference_line, const double planning_distance,
       const double planning_time,
       std::vector<StGraphBoundary>* const boundary) const;
 
-  apollo::common::Status map_obstacle_with_prediction_trajectory(
+  apollo::common::Status MapObstacleWithPredictionTrajectory(
       const common::TrajectoryPoint& initial_planning_point,
       const Obstacle& obstacle, const ObjectDecisionType obj_decision,
       const PathData& path_data, const double planning_distance,
       const double planning_time,
       std::vector<StGraphBoundary>* const boundary) const;
 
-  apollo::common::Status map_obstacle_without_prediction_trajectory(
+  apollo::common::Status MapObstacleWithoutPredictionTrajectory(
       const common::TrajectoryPoint& initial_planning_point,
       const Obstacle& obstacle, const ObjectDecisionType obj_decision,
       const PathData& path_data, const ReferenceLine& reference_line,

modules/planning/reference_line/reference_line.cc

@@ -38,6 +38,7 @@ namespace apollo {
 namespace planning {
 
 using MapPath = hdmap::Path;
+using SLPoint = apollo::common::SLPoint;
 
 ReferenceLine::ReferenceLine(
     const std::vector<ReferencePoint>& reference_points)
@@ -50,10 +51,9 @@ ReferenceLine::ReferenceLine(
     const std::vector<hdmap::LaneSegment>& lane_segments,
     const double max_approximation_error)
     : reference_points_(reference_points),
-      map_path_(MapPath(
-          std::vector<hdmap::MapPathPoint>(reference_points.begin(),
-                                           reference_points.end()),
-          lane_segments, max_approximation_error)) {}
+      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
+                            reference_points.begin(), reference_points.end()),
+                        lane_segments, max_approximation_error)) {}
 
 ReferencePoint ReferenceLine::get_reference_point(const double s) const {
   const auto& accumulated_s = map_path_.accumulated_s();
@@ -61,8 +61,7 @@ ReferencePoint ReferenceLine::get_reference_point(const double s) const {
     AWARN << "The requested s is nearer than the start point of the reference "
              "line; reference line starts at "
           << accumulated_s.back() << ", requested " << s << ".";
-    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0,
-                             0.0, 0.0);
+    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
     if (ref_point.lane_waypoints().empty()) {
       ref_point.add_lane_waypoints(reference_points_.front().lane_waypoints());
     }
@@ -72,8 +71,7 @@ ReferencePoint ReferenceLine::get_reference_point(const double s) const {
     AWARN << "The requested s exceeds the reference line; reference line "
              "ends at "
           << accumulated_s.back() << "requested " << s << " .";
-    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0,
-                             0.0, 0.0);
+    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
     if (ref_point.lane_waypoints().empty()) {
       ref_point.add_lane_waypoints(reference_points_.back().lane_waypoints());
     }
@@ -183,8 +181,8 @@ bool ReferenceLine::get_point_in_frenet_frame(
 
   if (s > map_path_.accumulated_s().back()) {
     AERROR << "The s of point is bigger than the length of current path. s: "
-           << s << ", curr path length: "
-           << map_path_.accumulated_s().back() << ".";
+           << s << ", curr path length: " << map_path_.accumulated_s().back()
+           << ".";
     return false;
   }
   sl_point->set_s(s);
@@ -211,9 +209,7 @@ const std::vector<ReferencePoint>& ReferenceLine::reference_points() const {
   return reference_points_;
 }
 
-const MapPath& ReferenceLine::reference_map_line() const {
-  return map_path_;
-}
+const MapPath& ReferenceLine::reference_map_line() const { return map_path_; }
 
 double ReferenceLine::get_lane_width(const double s) const {
   // TODO(startcode) : need implement;
@@ -273,5 +269,28 @@ void ReferenceLine::get_s_range_from_box2d(
   }
 }
 
+double ReferenceLine::GetSpeedLimitFromS(const double s) const {
+  const auto& map_path_point = get_reference_point(s);
+  double speed_limit = std::numeric_limits<double>::max();
+  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
+    if (lane_waypoint.lane == nullptr) {
+      AWARN << "lane_waypoint.lane is nullptr";
+      continue;
+    }
+    speed_limit =
+        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
+  }
+  return speed_limit;
+}
+
+double ReferenceLine::GetSpeedLimitFromPoint(
+    const common::math::Vec2d& point) const {
+  SLPoint sl;
+  if (!get_point_in_frenet_frame(point, &sl)) {
+    return false;
+  }
+  return GetSpeedLimitFromS(sl.s());
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/reference_line/reference_line.h

@@ -66,10 +66,8 @@ class ReferenceLine {
   void get_s_range_from_box2d(const ::apollo::common::math::Box2d& box2d,
                               double* max_s, double* min_s) const;
 
-  double GetSpeedLimitFromS(const double s) const { return 10.0; };
-  double GetSpeedLimitFromPoint(const common::math::Vec2d& point) const {
-    return 10.0;
-  };
+  double GetSpeedLimitFromS(const double s) const;
+  double GetSpeedLimitFromPoint(const common::math::Vec2d& point) const;
 
  private:
   static ReferencePoint interpolate(const ReferencePoint& p0, const double s0,