Planning: deleted unused functions.

modules/canbus/can_client/can_client_tool.cc

@@ -73,13 +73,9 @@ class CanAgent {
  public:
   explicit CanAgent(TestCanParam *param_ptr) : param_ptr_(param_ptr) {}
 
-  TestCanParam *param_ptr() {
-    return param_ptr_;
-  }
+  TestCanParam *param_ptr() { return param_ptr_; }
 
-  CanAgent *other_agent() {
-    return other_agent_;
-  }
+  CanAgent *other_agent() { return other_agent_; }
 
   bool Start() {
     thread_recv_.reset(new std::thread([this] { RecvThreadFunc(); }));
@@ -170,25 +166,15 @@ class CanAgent {
     return;
   }
 
-  void AddOtherAgent(CanAgent *agent) {
-    other_agent_ = agent;
-  }
+  void AddOtherAgent(CanAgent *agent) { other_agent_ = agent; }
 
-  bool is_receiving() {
-    return is_receiving_;
-  }
+  bool is_receiving() const { return is_receiving_; }
 
-  void is_receiving(bool val) {
-    is_receiving_ = val;
-  }
+  void is_receiving(bool val) { is_receiving_ = val; }
 
-  bool is_sending_finish() {
-    return is_sending_finish_;
-  }
+  bool is_sending_finish() const { return is_sending_finish_; }
 
-  void is_sending_finish(bool val) {
-    is_sending_finish_ = val;
-  }
+  void is_sending_finish(bool val) { is_sending_finish_ = val; }
 
   void RecvThreadFunc() {
     using ::apollo::common::time::Clock;

modules/planning/common/frame.cc

@@ -255,11 +255,6 @@ bool Frame::CheckCollision() {
 
 uint32_t Frame::SequenceNum() const { return sequence_num_; }
 
-const Obstacle *Frame::FindObstacle(const std::string &obstacle_id) {
-  std::lock_guard<std::mutex> lock(obstacles_mutex_);
-  return obstacles_.Find(obstacle_id);
-}
-
 std::vector<ReferenceLine> Frame::CreateReferenceLineFromRouting(
     const common::PointENU &position, const routing::RoutingResponse &routing) {
   std::vector<ReferenceLine> reference_lines;

modules/planning/common/frame.h

@@ -75,7 +75,6 @@ class Frame {
   std::vector<ReferenceLineInfo> &reference_line_info();
 
   bool AddObstacle(std::unique_ptr<Obstacle> obstacle);
-  const Obstacle *FindObstacle(const std::string &obstacle_id);
 
   const ReferenceLineInfo *FindDriveReferenceLineInfo();
   const ReferenceLineInfo *DriveReferenceLinfInfo() const;

modules/planning/common/path/discretized_path.cc

@@ -69,30 +69,7 @@ common::PathPoint DiscretizedPath::EvaluateUsingLinearApproximation(
     return path_points_.back();
   }
   return util::InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower,
-                                                path_s);
-}
-
-int DiscretizedPath::QueryClosestPoint(const double path_s) const {
-  if (path_points_.empty()) {
-    return -1;
-  }
-  auto it_lower = QueryLowerBound(path_s);
-
-  if (it_lower == path_points_.begin()) {
-    return 0;
-  }
-  if (it_lower == path_points_.end()) {
-    return path_points_.size() - 1;
-  }
-
-  double d0 = path_s - (it_lower - 1)->s();
-  double d1 = it_lower->s() - path_s;
-
-  if (d0 < d1) {
-    return static_cast<int>(it_lower - path_points_.begin()) - 1;
-  } else {
-    return static_cast<int>(it_lower - path_points_.begin());
-  }
+                                                   path_s);
 }
 
 const std::vector<common::PathPoint> &DiscretizedPath::path_points() const {

modules/planning/common/path/discretized_path.h

@@ -48,8 +48,6 @@ class DiscretizedPath {
 
   common::PathPoint EvaluateUsingLinearApproximation(const double path_s) const;
 
-  int QueryClosestPoint(const double path_s) const;
-
   const std::vector<common::PathPoint>& path_points() const;
 
   std::uint32_t NumOfPoints() const;

modules/planning/common/path_decision.cc

@@ -30,17 +30,6 @@ namespace planning {
 
 using IndexedPathObstacles = IndexedList<std::string, PathObstacle>;
 
-bool PathDecision::AddPathObstacles(
-    const std::vector<const PathObstacle *> &path_obstacles) {
-  for (const auto *path_obstacle : path_obstacles) {
-    if (!path_obstacles_.Add(path_obstacle->Id(), *path_obstacle)) {
-      AERROR << "Failed to add obstacle: " << path_obstacle->Id();
-      return false;
-    }
-  }
-  return true;
-}
-
 bool PathDecision::AddPathObstacle(
     std::unique_ptr<PathObstacle> &&path_obstacle) {
   auto id = path_obstacle->Id();

modules/planning/common/path_decision.h

@@ -39,9 +39,6 @@ class PathDecision {
  public:
   PathDecision() = default;
 
-  bool AddPathObstacles(
-      const std::vector<const PathObstacle *> &path_obstacles);
-
   bool AddPathObstacle(std::unique_ptr<PathObstacle> &&path_obstacle);
 
   const IndexedList<std::string, PathObstacle> &path_obstacles() const;

modules/planning/common/speed/st_boundary.cc

@@ -29,8 +29,8 @@
 namespace apollo {
 namespace planning {
 
-using Vec2d = common::math::Vec2d;
-using LineSegment2d = common::math::LineSegment2d;
+using common::math::Vec2d;
+using common::math::LineSegment2d;
 
 StBoundary::StBoundary(
     const std::vector<std::pair<STPoint, STPoint>>& point_pairs) {
@@ -174,16 +174,6 @@ STPoint StBoundary::BottomRightPoint() const {
   return lower_points_.back();
 }
 
-STPoint StBoundary::TopRightPoint() const {
-  DCHECK(!upper_points_.empty()) << "StBoundary has zero points.";
-  return upper_points_.back();
-}
-
-STPoint StBoundary::TopLeftPoint() const {
-  DCHECK(!upper_points_.empty()) << "StBoundary has zero points.";
-  return upper_points_.front();
-}
-
 StBoundary StBoundary::ExpandByS(const double s) const {
   if (lower_points_.empty()) {
     AERROR << "The current st_boundary has NO points.";
@@ -336,10 +326,10 @@ bool StBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
 }
 
 double StBoundary::DistanceS(const STPoint& st_point) const {
-  constexpr double kMaxDistance = 1.0e10;
   double s_upper;
   double s_lower;
   if (GetBoundarySRange(st_point.t(), &s_upper, &s_lower)) {
+    constexpr double kMaxDistance = 1.0e10;
     return kMaxDistance;
   }
   if (st_point.s() < s_lower) {

modules/planning/common/speed/st_boundary.h

@@ -66,8 +66,6 @@ class StBoundary : public common::math::Polygon2d {
 
   STPoint BottomLeftPoint() const;
   STPoint BottomRightPoint() const;
-  STPoint TopRightPoint() const;
-  STPoint TopLeftPoint() const;
 
   StBoundary ExpandByS(const double s) const;
   StBoundary ExpandByT(const double t) const;

modules/planning/common/trajectory/discretized_trajectory.cc

@@ -39,14 +39,6 @@ DiscretizedTrajectory::DiscretizedTrajectory(
   trajectory_points_ = std::move(trajectory_points);
 }
 
-double DiscretizedTrajectory::TimeLength() const {
-  if (trajectory_points_.empty()) {
-    return 0.0;
-  }
-  return trajectory_points_.back().relative_time() -
-         trajectory_points_.front().relative_time();
-}
-
 TrajectoryPoint DiscretizedTrajectory::Evaluate(
     const double relative_time) const {
   CHECK(!trajectory_points_.empty());
@@ -82,7 +74,7 @@ TrajectoryPoint DiscretizedTrajectory::EvaluateUsingLinearApproximation(
     return trajectory_points_.front();
   }
   return util::InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower,
-                                                relative_time);
+                                                   relative_time);
 }
 
 std::uint32_t DiscretizedTrajectory::QueryNearestPoint(
@@ -139,11 +131,6 @@ TrajectoryPoint DiscretizedTrajectory::StartPoint() const {
   return trajectory_points_.front();
 }
 
-TrajectoryPoint DiscretizedTrajectory::EndPoint() const {
-  CHECK(!trajectory_points_.empty());
-  return trajectory_points_.back();
-}
-
 std::uint32_t DiscretizedTrajectory::NumOfPoints() const {
   return trajectory_points_.size();
 }
@@ -153,27 +140,6 @@ const std::vector<TrajectoryPoint>& DiscretizedTrajectory::trajectory_points()
   return trajectory_points_;
 }
 
-void DiscretizedTrajectory::set_trajectory_points(
-    std::vector<TrajectoryPoint> points) {
-  trajectory_points_ = std::move(points);
-  CHECK(Valid() && "The input trajectory points have wrong relative time!");
-}
-
-bool DiscretizedTrajectory::Valid() const {
-  std::size_t size = trajectory_points_.size();
-  if (size == 0 || size == 1) {
-    return true;
-  }
-
-  for (std::size_t i = 1; i < size; ++i) {
-    if (!(trajectory_points_[i - 1].relative_time() <
-          trajectory_points_[i].relative_time())) {
-      return false;
-    }
-  }
-  return true;
-}
-
 void DiscretizedTrajectory::Clear() { trajectory_points_.clear(); }
 
 }  // namespace planning

modules/planning/common/trajectory/discretized_trajectory.h

@@ -23,9 +23,9 @@
 
 #include <vector>
 
+#include "glog/logging.h"
 #include "modules/common/math/vec2d.h"
 #include "modules/planning/common/trajectory/trajectory.h"
-#include "glog/logging.h"
 
 namespace apollo {
 namespace planning {
@@ -34,27 +34,20 @@ class DiscretizedTrajectory : public Trajectory {
  public:
   DiscretizedTrajectory() = default;
 
-  DiscretizedTrajectory(
-      std::vector<common::TrajectoryPoint> trajectory_points);
+  DiscretizedTrajectory(std::vector<common::TrajectoryPoint> trajectory_points);
 
   virtual ~DiscretizedTrajectory() = default;
 
-  double TimeLength() const override;
-
-  common::TrajectoryPoint Evaluate(
-      const double relative_time) const override;
+  common::TrajectoryPoint Evaluate(const double relative_time) const override;
 
   common::TrajectoryPoint StartPoint() const override;
 
-  common::TrajectoryPoint EndPoint() const override;
-
   virtual common::TrajectoryPoint EvaluateUsingLinearApproximation(
       const double relative_time) const;
 
   virtual uint32_t QueryNearestPoint(const double relative_time) const;
 
-  virtual uint32_t QueryNearestPoint(
-      const common::math::Vec2d& position) const;
+  virtual uint32_t QueryNearestPoint(const common::math::Vec2d& position) const;
 
   virtual void AppendTrajectoryPoint(
       const common::TrajectoryPoint& trajectory_point);
@@ -62,8 +55,8 @@ class DiscretizedTrajectory : public Trajectory {
   template <typename Iter>
   void PrependTrajectoryPoints(Iter begin, Iter end) {
     if (!trajectory_points_.empty() && begin != end) {
-      CHECK((end - 1)->relative_time()
-                < trajectory_points_.front().relative_time());
+      CHECK((end - 1)->relative_time() <
+            trajectory_points_.front().relative_time());
     }
     trajectory_points_.insert(trajectory_points_.begin(), begin, end);
   }
@@ -73,12 +66,8 @@ class DiscretizedTrajectory : public Trajectory {
 
   uint32_t NumOfPoints() const;
 
-  void set_trajectory_points(std::vector<common::TrajectoryPoint> points);
-
   const std::vector<common::TrajectoryPoint>& trajectory_points() const;
 
-  virtual bool Valid() const;
-
   virtual void Clear();
 
  protected:

modules/planning/common/trajectory/trajectory.h

@@ -32,14 +32,10 @@ class Trajectory {
 
   virtual ~Trajectory() = default;
 
-  virtual double TimeLength() const = 0;
-
   virtual common::TrajectoryPoint Evaluate(
       const double relative_time) const = 0;
 
   virtual common::TrajectoryPoint StartPoint() const = 0;
-
-  virtual common::TrajectoryPoint EndPoint() const = 0;
 };
 
 }  // namespace planning

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -44,7 +44,7 @@ using apollo::common::VehicleParam;
 
 namespace {
 
-bool CheckOverlapOnDpStGraph(const std::vector<StBoundary> boundaries,
+bool CheckOverlapOnDpStGraph(const std::vector<StBoundary>& boundaries,
                              const StGraphPoint& p1, const StGraphPoint& p2) {
   for (const auto& boundary : boundaries) {
     common::math::LineSegment2d seg(p1.point(), p2.point());

modules/planning/tasks/qp_spline_path/qp_frenet_frame.cc

@@ -115,14 +115,6 @@ bool QpFrenetFrame::Init(const uint32_t num_points,
   return true;
 }
 
-const ReferenceLine& QpFrenetFrame::GetReferenceLine() const {
-  return reference_line_;
-}
-
-double QpFrenetFrame::feasible_longitudinal_upper_bound() const {
-  return feasible_longitudinal_upper_bound_;
-}
-
 bool QpFrenetFrame::GetMapBound(const double s,
                                 std::pair<double, double>* const bound) const {
   return GetBound(s, hdmap_bound_, bound);

modules/planning/tasks/qp_spline_path/qp_frenet_frame.h

@@ -33,8 +33,8 @@
 
 #include "modules/planning/common/path_obstacle.h"
 #include "modules/planning/common/speed/speed_data.h"
-#include "modules/planning/reference_line/reference_line.h"
 #include "modules/planning/proto/planning_internal.pb.h"
+#include "modules/planning/reference_line/reference_line.h"
 
 namespace apollo {
 namespace planning {
@@ -52,10 +52,6 @@ class QpFrenetFrame {
   bool Init(const uint32_t num_points,
             apollo::planning_internal::Debug* planning_debug);
 
-  const ReferenceLine& GetReferenceLine() const;
-
-  double feasible_longitudinal_upper_bound() const;
-
   bool GetMapBound(const double s,
                    std::pair<double, double>* const bound) const;
 

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -425,83 +425,6 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
   return Status::OK();
 }
 
-Status StBoundaryMapper::MapFollowDecision(
-    const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
-    StBoundary* const boundary) const {
-  DCHECK_NOTNULL(boundary);
-  DCHECK(obj_decision.has_follow())
-      << "Map obstacle without prediction trajectory is ONLY supported "
-         "when "
-         "the object decision is follow. The current object decision is: "
-      << obj_decision.DebugString();
-
-  const auto* obstacle = path_obstacle.obstacle();
-  SLPoint obstacle_sl_point;
-  reference_line_.XYToSL({obstacle->Perception().position().x(),
-                          obstacle->Perception().position().y()},
-                         &obstacle_sl_point);
-
-  const auto& ref_point =
-      reference_line_.GetReferencePoint(obstacle->Perception().position().x(),
-                                        obstacle->Perception().position().y());
-
-  const double speed_coeff =
-      std::cos(obstacle->Perception().theta() - ref_point.heading());
-  AERROR_IF(speed_coeff < 0.0)
-      << "Obstacle is moving opposite to the "
-         "reference line. ref_point: "
-      << ref_point.DebugString() << ", path obstacle:\n"
-      << path_obstacle.obstacle()->Perception().DebugString();
-
-  const auto& start_point = path_data_.discretized_path().StartPoint();
-  SLPoint start_sl_point;
-  if (!reference_line_.XYToSL({start_point.x(), start_point.y()},
-                              &start_sl_point)) {
-    std::string msg = "Fail to get s and l of start point.";
-    AERROR << msg;
-    return common::Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-
-  const double distance_to_obstacle =
-      obstacle_sl_point.s() -
-      obstacle->Perception().length() / 2.0 *
-          st_boundary_config_.expanding_coeff() -
-      start_sl_point.s() - vehicle_param_.front_edge_to_center() -
-      st_boundary_config_.follow_buffer();
-
-  if (distance_to_obstacle > planning_distance_) {
-    std::string msg = "obstacle is out of range.";
-    ADEBUG << msg;
-    return Status::OK();
-  }
-
-  const auto& velocity = obstacle->Perception().velocity();
-  const double speed = std::hypot(velocity.x(), velocity.y()) * speed_coeff;
-
-  const double s_min_lower = distance_to_obstacle;
-  const double s_min_upper =
-      std::max(distance_to_obstacle + 1.0, planning_distance_);
-  const double s_max_lower = s_min_lower + planning_time_ * speed;
-  const double s_max_upper = std::max(s_max_lower, planning_distance_);
-
-  std::vector<std::pair<STPoint, STPoint>> point_pairs;
-  point_pairs.emplace_back(STPoint(s_min_lower, 0.0),
-                           STPoint(s_min_upper, 0.0));
-  point_pairs.emplace_back(STPoint(s_max_lower, planning_time_),
-                           STPoint(s_max_upper, planning_time_));
-  *boundary = StBoundary(point_pairs);
-
-  const double characteristic_length =
-      std::fabs(obj_decision.follow().distance_s()) +
-      st_boundary_config_.follow_buffer();
-
-  boundary->SetCharacteristicLength(characteristic_length);
-  boundary->SetId(obstacle->Id());
-  boundary->SetBoundaryType(StBoundary::BoundaryType::FOLLOW);
-
-  return Status::OK();
-}
-
 bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point,
                                     const Box2d& obs_box,
                                     const double buffer) const {

modules/planning/tasks/st_graph/st_boundary_mapper.h

@@ -75,10 +75,6 @@ class StBoundaryMapper {
       const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
       StBoundary* const boundary) const;
 
-  apollo::common::Status MapFollowDecision(
-      const PathObstacle& obstacle, const ObjectDecisionType& obj_decision,
-      StBoundary* const boundary) const;
-
   void AppendBoundary(const StBoundary& boundary,
                       std::vector<StBoundary>* st_boundaries) const;