added planning/common/path

modules/common/proto/path_point.proto

@@ -2,6 +2,18 @@ syntax = "proto2";
 
 package apollo.common;
 
+message SLPoint {
+    optional double s = 1;
+    optional double l = 2;
+}
+
+message FrenetFramePoint {
+    optional double s = 1;
+    optional double l = 2;
+    optional double dl = 3;
+    optional double ddl = 4;
+}
+
 message PathPoint {
   // coordinates
   optional double x = 1;

modules/map/hdmap/hdmap_common.cc

@@ -13,11 +13,11 @@ See the License for the specific language governing permissions and
 limitations under the License.
 =========================================================================*/
 
-#include <iostream>
 #include <algorithm>
+#include <iostream>
 
-#include "modules/map/hdmap/hdmap_common.h"
 #include "glog/logging.h"
+#include "modules/map/hdmap/hdmap_common.h"
 
 namespace {
 
@@ -28,57 +28,58 @@ const double kSegmentationEpsilon = 0.2;
 const double kDuplicatedPointsEpsilon = 1e-7;
 
 void remove_duplicates(std::vector<apollo::common::math::Vec2d> *points) {
-    CHECK_NOTNULL(points);
-
-    int count = 0;
-    const double limit = kDuplicatedPointsEpsilon * kDuplicatedPointsEpsilon;
-    for (size_t i = 0; i < points->size(); ++i) {
-        if (count == 0 ||
-            (*points)[i].DistanceSquareTo((*points)[count - 1]) > limit) {
-        (*points)[count++] = (*points)[i];
-        }
+  CHECK_NOTNULL(points);
+
+  int count = 0;
+  const double limit = kDuplicatedPointsEpsilon * kDuplicatedPointsEpsilon;
+  for (size_t i = 0; i < points->size(); ++i) {
+    if (count == 0 ||
+        (*points)[i].DistanceSquareTo((*points)[count - 1]) > limit) {
+      (*points)[count++] = (*points)[i];
     }
-    points->resize(count);
+  }
+  points->resize(count);
 }
 
 void points_from_curve(const apollo::hdmap::Curve &input_curve,
-                        std::vector<apollo::common::math::Vec2d> *points) {
-    CHECK_NOTNULL(points)->clear();
+                       std::vector<apollo::common::math::Vec2d> *points) {
+  CHECK_NOTNULL(points)->clear();
 
-    for (const auto &curve : input_curve.segment()) {
+  for (const auto &curve : input_curve.segment()) {
     if (curve.has_line_segment()) {
-        for (const auto &point : curve.line_segment().point()) {
-            points->emplace_back(point.x(), point.y());
-        }
+      for (const auto &point : curve.line_segment().point()) {
+        points->emplace_back(point.x(), point.y());
+      }
     } else {
-        LOG(FATAL) << "Can not handle curve type.";
-    }
+      LOG(FATAL) << "Can not handle curve type.";
     }
-    remove_duplicates(points);
+  }
+  remove_duplicates(points);
 }
 
 apollo::common::math::Polygon2d convert_to_polygon2d(
-                            const apollo::hdmap::Polygon &polygon) {
-    std::vector<apollo::common::math::Vec2d> points;
-    points.reserve(polygon.point_size());
-    for (const auto &point : polygon.point()) {
-        points.emplace_back(point.x(), point.y());
-    }
-    remove_duplicates(&points);
-    while (points.size() >= 2 &&
-        points[0].DistanceTo(points.back())
-                            <= apollo::common::math::kMathEpsilon) {
-        points.pop_back();
-    }
-    return apollo::common::math::Polygon2d(points);
+    const apollo::hdmap::Polygon &polygon) {
+  std::vector<apollo::common::math::Vec2d> points;
+  points.reserve(polygon.point_size());
+  for (const auto &point : polygon.point()) {
+    points.emplace_back(point.x(), point.y());
+  }
+  remove_duplicates(&points);
+  while (points.size() >= 2 &&
+         points[0].DistanceTo(points.back()) <=
+             apollo::common::math::kMathEpsilon) {
+    points.pop_back();
+  }
+  return apollo::common::math::Polygon2d(points);
 }
 
-void segments_from_curve(const apollo::hdmap::Curve &curve,
-                    std::vector<apollo::common::math::LineSegment2d> *segments) {
-    std::vector<apollo::common::math::Vec2d> points;
-    points_from_curve(curve, &points);
-    for (size_t i = 0; i + 1 < points.size(); ++i) {
-        segments->emplace_back(points[i], points[i + 1]);
+void segments_from_curve(
+    const apollo::hdmap::Curve &curve,
+    std::vector<apollo::common::math::LineSegment2d> *segments) {
+  std::vector<apollo::common::math::Vec2d> points;
+  points_from_curve(curve, &points);
+  for (size_t i = 0; i + 1 < points.size(); ++i) {
+    segments->emplace_back(points[i], points[i + 1]);
   }
 }
 }  // namespace
@@ -89,165 +90,164 @@ namespace hdmap {
 LaneInfo::LaneInfo(const apollo::hdmap::Lane &lane) : _lane(lane) { init(); }
 
 void LaneInfo::init() {
-    points_from_curve(_lane.central_curve(), &_points);
-    CHECK_GE(_points.size(), 2);
-    _segments.clear();
-    _accumulated_s.clear();
-    _unit_directions.clear();
-    _headings.clear();
-
-    double s = 0;
-    for (size_t i = 0; i + 1 < _points.size(); ++i) {
-        _segments.emplace_back(_points[i], _points[i + 1]);
-        _accumulated_s.push_back(s);
-        _unit_directions.push_back(_segments.back().unit_direction());
-        s += _segments.back().length();
-    }
+  points_from_curve(_lane.central_curve(), &_points);
+  CHECK_GE(_points.size(), 2);
+  _segments.clear();
+  _accumulated_s.clear();
+  _unit_directions.clear();
+  _headings.clear();
+
+  double s = 0;
+  for (size_t i = 0; i + 1 < _points.size(); ++i) {
+    _segments.emplace_back(_points[i], _points[i + 1]);
     _accumulated_s.push_back(s);
-    _total_length = s;
-    CHECK(!_unit_directions.empty());
-    _unit_directions.push_back(_unit_directions.back());
-    for (const auto &direction : _unit_directions) {
-        _headings.push_back(direction.Angle());
-    }
-    CHECK(!_segments.empty());
+    _unit_directions.push_back(_segments.back().unit_direction());
+    s += _segments.back().length();
+  }
+  _accumulated_s.push_back(s);
+  _total_length = s;
+  CHECK(!_unit_directions.empty());
+  _unit_directions.push_back(_unit_directions.back());
+  for (const auto &direction : _unit_directions) {
+    _headings.push_back(direction.Angle());
+  }
+  CHECK(!_segments.empty());
 
-    _sampled_left_width.clear();
-    _sampled_right_width.clear();
-    for (const auto &sample : _lane.left_sample()) {
-        _sampled_left_width.emplace_back(sample.s(), sample.width());
-    }
-    for (const auto &sample : _lane.right_sample()) {
-        _sampled_right_width.emplace_back(sample.s(), sample.width());
-    }
+  _sampled_left_width.clear();
+  _sampled_right_width.clear();
+  for (const auto &sample : _lane.left_sample()) {
+    _sampled_left_width.emplace_back(sample.s(), sample.width());
+  }
+  for (const auto &sample : _lane.right_sample()) {
+    _sampled_right_width.emplace_back(sample.s(), sample.width());
+  }
 }
 
 void LaneInfo::get_width(const double s, double *left_width,
                          double *right_width) const {
-    if (left_width != nullptr) {
-        *left_width = get_width_from_sample(_sampled_left_width, s);
-    }
-    if (right_width != nullptr) {
-        *right_width = get_width_from_sample(_sampled_right_width, s);
-    }
+  if (left_width != nullptr) {
+    *left_width = get_width_from_sample(_sampled_left_width, s);
+  }
+  if (right_width != nullptr) {
+    *right_width = get_width_from_sample(_sampled_right_width, s);
+  }
 }
 
 double LaneInfo::get_width(const double s) const {
-    double left_width = 0.0;
-    double right_width = 0.0;
-    get_width(s, &left_width, &right_width);
-    return left_width + right_width;
+  double left_width = 0.0;
+  double right_width = 0.0;
+  get_width(s, &left_width, &right_width);
+  return left_width + right_width;
 }
 
 double LaneInfo::get_effective_width(const double s) const {
-    double left_width = 0.0;
-    double right_width = 0.0;
-    get_width(s, &left_width, &right_width);
-    return 2 * std::min(left_width, right_width);
+  double left_width = 0.0;
+  double right_width = 0.0;
+  get_width(s, &left_width, &right_width);
+  return 2 * std::min(left_width, right_width);
 }
 
 double LaneInfo::get_width_from_sample(
-                        const std::vector<LaneInfo::SampledWidth>& samples,
-                        const double s) const {
-    if (samples.empty()) {
-        return 0.0;
-    }
-    if (s <= samples[0].first) {
-        return samples[0].second;
-    }
-    if (s >= samples.back().first) {
-        return samples.back().second;
-    }
-    int low = 0;
-    int high = static_cast<int>(samples.size());
-    while (low + 1 < high) {
-        const int mid = (low + high) / 2;
-        if (samples[mid].first <= s) {
-            low = mid;
-        } else {
-            high = mid;
-        }
+    const std::vector<LaneInfo::SampledWidth> &samples, const double s) const {
+  if (samples.empty()) {
+    return 0.0;
+  }
+  if (s <= samples[0].first) {
+    return samples[0].second;
+  }
+  if (s >= samples.back().first) {
+    return samples.back().second;
+  }
+  int low = 0;
+  int high = static_cast<int>(samples.size());
+  while (low + 1 < high) {
+    const int mid = (low + high) / 2;
+    if (samples[mid].first <= s) {
+      low = mid;
+    } else {
+      high = mid;
     }
-    const LaneInfo::SampledWidth& sample1 = samples[low];
-    const LaneInfo::SampledWidth& sample2 = samples[high];
-    const double ratio = (sample2.first - s) / (sample2.first - sample1.first);
-    return sample1.second * ratio + sample2.second * (1.0 - ratio);
+  }
+  const LaneInfo::SampledWidth &sample1 = samples[low];
+  const LaneInfo::SampledWidth &sample2 = samples[high];
+  const double ratio = (sample2.first - s) / (sample2.first - sample1.first);
+  return sample1.second * ratio + sample2.second * (1.0 - ratio);
 }
 
 JunctionInfo::JunctionInfo(const apollo::hdmap::Junction &junction)
     : _junction(junction) {
-    init();
+  init();
 }
 
 void JunctionInfo::init() {
-    _polygon = convert_to_polygon2d(_junction.polygon());
-    CHECK_GT(_polygon.num_points(), 2);
+  _polygon = convert_to_polygon2d(_junction.polygon());
+  CHECK_GT(_polygon.num_points(), 2);
 }
 
 SignalInfo::SignalInfo(const apollo::hdmap::Signal &signal) : _signal(signal) {
-    init();
+  init();
 }
 
 void SignalInfo::init() {
-    for (const auto &stop_line : _signal.stop_line()) {
-        segments_from_curve(stop_line, &_segments);
-    }
-    CHECK(!_segments.empty());
-    std::vector<apollo::common::math::Vec2d> points;
-    for (const auto& segment : _segments) {
-        points.emplace_back(segment.start());
-        points.emplace_back(segment.end());
-    }
-    CHECK_GT(points.size(), 0);
+  for (const auto &stop_line : _signal.stop_line()) {
+    segments_from_curve(stop_line, &_segments);
+  }
+  CHECK(!_segments.empty());
+  std::vector<apollo::common::math::Vec2d> points;
+  for (const auto &segment : _segments) {
+    points.emplace_back(segment.start());
+    points.emplace_back(segment.end());
+  }
+  CHECK_GT(points.size(), 0);
 }
 
 CrosswalkInfo::CrosswalkInfo(const apollo::hdmap::Crosswalk &crosswalk)
     : _crosswalk(crosswalk) {
-    init();
+  init();
 }
 
 void CrosswalkInfo::init() {
-    _polygon = convert_to_polygon2d(_crosswalk.polygon());
-    CHECK_GT(_polygon.num_points(), 2);
+  _polygon = convert_to_polygon2d(_crosswalk.polygon());
+  CHECK_GT(_polygon.num_points(), 2);
 }
 
 StopSignInfo::StopSignInfo(const apollo::hdmap::StopSign &stop_sign)
     : _stop_sign(stop_sign) {
-    init();
+  init();
 }
 
 void StopSignInfo::init() {
-    // for (const auto &stop_line : _stop_sign.stop_line()) {
-    //     segments_from_curve(stop_line, &_segments);
-    // }
-    segments_from_curve(_stop_sign.stop_line(), &_segments);
-    CHECK(!_segments.empty());
+  // for (const auto &stop_line : _stop_sign.stop_line()) {
+  //     segments_from_curve(stop_line, &_segments);
+  // }
+  segments_from_curve(_stop_sign.stop_line(), &_segments);
+  CHECK(!_segments.empty());
 }
 
 YieldSignInfo::YieldSignInfo(const apollo::hdmap::YieldSign &yield_sign)
     : _yield_sign(yield_sign) {
-    init();
+  init();
 }
 
 void YieldSignInfo::init() {
-    // for (const auto &stop_line : _yield_sign.stop_line()) {
-    //     segments_from_curve(stop_line, &_segments);
-    // }
-    segments_from_curve(_yield_sign.stop_line(), &_segments);
-    CHECK(!_segments.empty());
+  // for (const auto &stop_line : _yield_sign.stop_line()) {
+  //     segments_from_curve(stop_line, &_segments);
+  // }
+  segments_from_curve(_yield_sign.stop_line(), &_segments);
+  CHECK(!_segments.empty());
 }
 
 OverlapInfo::OverlapInfo(const apollo::hdmap::Overlap &overlap)
     : _overlap(overlap) {}
 
-const ObjectOverlapInfo * OverlapInfo::get_object_overlap_info(
-                            const apollo::hdmap::Id &id) const {
-    for (const auto &object : _overlap.object()) {
-        if (object.id().id() == id.id()) {
-        return &object;
-        }
+const ObjectOverlapInfo *OverlapInfo::get_object_overlap_info(
+    const apollo::hdmap::Id &id) const {
+  for (const auto &object : _overlap.object()) {
+    if (object.id().id() == id.id()) {
+      return &object;
     }
-    return nullptr;
+  }
+  return nullptr;
 }
 
 }  // namespace hdmap

modules/planning/common/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
 package(default_visibility = ["//visibility:public"])
 
 cc_library(
@@ -36,3 +38,5 @@ cc_library(
         "//modules/localization/proto:localization_proto",
     ],
 )
+
+cpplint()

modules/planning/common/logic_point.cc

@@ -28,43 +28,27 @@
 namespace apollo {
 namespace planning {
 
-LogicPoint::LogicPoint(const double x,
-                       const double y,
-                       const double s,
-                       const double heading,
-                       const double kappa,
+LogicPoint::LogicPoint(const double x, const double y, const double s,
+                       const double heading, const double kappa,
                        const double dkappa)
     : ::apollo::common::math::Vec2d(x, y),
       _s(s),
       _heading(heading),
       _kappa(kappa),
       _dkappa(dkappa),
-      _lane_id("") {
-}
+      _lane_id("") {}
 
-double LogicPoint::s() const {
-  return _s;
-}
+double LogicPoint::s() const { return _s; }
 
-double LogicPoint::heading() const {
-  return _heading;
-}
+double LogicPoint::heading() const { return _heading; }
 
-double LogicPoint::kappa() const {
-  return _kappa;
-}
+double LogicPoint::kappa() const { return _kappa; }
 
-double LogicPoint::dkappa() const {
-  return _dkappa;
-}
+double LogicPoint::dkappa() const { return _dkappa; }
 
-void LogicPoint::set_lane_id(const std::string &lane_id) {
-  _lane_id = lane_id;
-}
+void LogicPoint::set_lane_id(const std::string &lane_id) { _lane_id = lane_id; }
 
-const std::string &LogicPoint::lane_id() const {
-  return _lane_id;
-}
+const std::string &LogicPoint::lane_id() const { return _lane_id; }
 
-} // namespace planning
-} // namespace apollo
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/logic_point.h

@@ -33,11 +33,8 @@ namespace planning {
 
 class LogicPoint : public ::apollo::common::math::Vec2d {
  public:
-  explicit LogicPoint(const double x,
-                      const double y,
-                      const double s,
-                      const double heading,
-                      const double kappa,
+  explicit LogicPoint(const double x, const double y, const double s,
+                      const double heading, const double kappa,
                       const double dkappa);
   double s() const;
   double heading() const;
@@ -45,6 +42,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
   double dkappa() const;
   const std::string &lane_id() const;
   void set_lane_id(const std::string &lane_id);
+
  private:
   double _s;
   double _heading;
@@ -53,7 +51,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
   std::string _lane_id;
 };
 
-} // namespace planning
-} // namespace apollo
+}  // namespace planning
+}  // namespace apollo
 
-#endif // MODULES_PLANNING_COMMON_LOGIC_POINT_H
+#endif  // MODULES_PLANNING_COMMON_LOGIC_POINT_H

modules/planning/common/path/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "path_point_util",
+    srcs = [
+        "path_point_util.cc",
+        "sl_point_util.cc",
+    ],
+    hdrs = [
+        "path_point_util.h",
+        "sl_point_util.h",
+    ],
+    deps = [
+        "//modules/common/proto:path_point_proto",
+        "//modules/planning/math:hermite_spline",
+        "//modules/planning/math:integration",
+        "@eigen//:eigen",
+    ],
+)
+
+cc_library(
+    name = "path",
+    hdrs = [
+        "path.h",
+    ],
+)
+
+cc_library(
+    name = "discretized_path",
+    srcs = [
+        "discretized_path.cc",
+    ],
+    hdrs = [
+        "discretized_path.h",
+    ],
+    deps = [
+        ":path",
+        ":path_point_util",
+        "//modules/common/proto:path_point_proto",
+    ],
+)
+
+cc_library(
+    name = "frenet_frame_path",
+    srcs = [
+        "frenet_frame_path.cc",
+    ],
+    hdrs = [
+        "frenet_frame_path.h",
+    ],
+    deps = [
+        ":path",
+        ":path_point_util",
+        "//modules/common/proto:path_point_proto",
+    ],
+)
+
+cc_library(
+    name = "path_data",
+    srcs = [
+        "path_data.cc",
+    ],
+    hdrs = [
+        "path_data.h",
+    ],
+    deps = [
+        ":discretized_path",
+        ":frenet_frame_path",
+        "//modules/planning/math:double",
+    ],
+)
+
+cpplint()

modules/planning/common/path/discretized_path.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 path.cc
+ **/
+
+#include "modules/planning/common/path/discretized_path.h"
+
+#include <utility>
+#include "glog/logging.h"
+
+#include "modules/planning/common/path/path_point_util.h"
+
+namespace apollo {
+namespace planning {
+
+DiscretizedPath::DiscretizedPath(std::vector<common::PathPoint> path_points) {
+  path_points_ = std::move(path_points);
+}
+
+common::PathPoint DiscretizedPath::evaluate(const double param) const {
+  CHECK_GT(path_points_.size(), 1);
+  CHECK(path_points_.front().s() <= param && path_points_.back().s() <= param);
+
+  auto it_lower = query_lower_bound(param);
+  if (it_lower == path_points_.begin()) {
+    return path_points_.front();
+  }
+
+  return util::interpolate(*(it_lower - 1), *it_lower, param);
+}
+
+double DiscretizedPath::param_length() const {
+  if (path_points_.empty()) {
+    return 0.0;
+  }
+  return path_points_.back().s() - path_points_.front().s();
+}
+
+int DiscretizedPath::query_closest_point(const double param) const {
+  if (path_points_.empty()) {
+    return -1;
+  }
+  auto it_lower = query_lower_bound(param);
+
+  if (it_lower == path_points_.begin()) {
+    return 0;
+  }
+  if (it_lower == path_points_.end()) {
+    return path_points_.size() - 1;
+  }
+
+  double d0 = param - (it_lower - 1)->s();
+  double d1 = it_lower->s() - param;
+
+  if (d0 < d1) {
+    return static_cast<int>(it_lower - path_points_.begin()) - 1;
+  } else {
+    return static_cast<int>(it_lower - path_points_.begin());
+  }
+}
+
+std::vector<common::PathPoint>* DiscretizedPath::mutable_path_points() {
+  return &path_points_;
+}
+
+const std::vector<common::PathPoint>& DiscretizedPath::path_points() const {
+  return path_points_;
+}
+
+std::size_t DiscretizedPath::num_of_points() const {
+  return path_points_.size();
+}
+
+const common::PathPoint& DiscretizedPath::path_point_at(
+    const std::size_t index) const {
+  CHECK(index < path_points_.size());
+  return path_points_[index];
+}
+
+common::PathPoint DiscretizedPath::start_point() const {
+  CHECK(!path_points_.empty());
+  return path_points_.front();
+}
+
+common::PathPoint DiscretizedPath::end_point() const {
+  CHECK(!path_points_.empty());
+  return path_points_.back();
+}
+
+common::PathPoint& DiscretizedPath::path_point_at(const std::size_t index) {
+  CHECK(index < path_points_.size());
+  return path_points_[index];
+}
+
+std::vector<common::PathPoint>::const_iterator
+DiscretizedPath::query_lower_bound(const double param) const {
+  auto func = [](const common::PathPoint& tp, const double param) {
+    return tp.s() < param;
+  };
+  return std::lower_bound(path_points_.begin(), path_points_.end(), param,
+                          func);
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/path/discretized_path.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 discretized_path.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_PATH_DISCRETIZED_PATH_H_
+#define MODULES_PLANNING_COMMON_PATH_DISCRETIZED_PATH_H_
+
+#include <vector>
+
+#include "modules/planning/common/path/path.h"
+#include "modules/planning/common/path/path_point_util.h"
+
+namespace apollo {
+namespace planning {
+
+class DiscretizedPath : public Path {
+ public:
+  DiscretizedPath() = default;
+
+  explicit DiscretizedPath(std::vector<common::PathPoint> path_points);
+
+  virtual ~DiscretizedPath() = default;
+
+  common::PathPoint evaluate(const double param) const override;
+
+  double param_length() const override;
+
+  common::PathPoint start_point() const override;
+
+  common::PathPoint end_point() const override;
+
+  int query_closest_point(const double param) const;
+
+  std::vector<common::PathPoint>* mutable_path_points();
+
+  const std::vector<common::PathPoint>& path_points() const;
+
+  std::size_t num_of_points() const;
+
+  const common::PathPoint& path_point_at(const std::size_t index) const;
+
+  common::PathPoint& path_point_at(const std::size_t index);
+
+ private:
+  std::vector<common::PathPoint>::const_iterator query_lower_bound(
+      const double param) const;
+
+  std::vector<common::PathPoint> path_points_;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif /* MODULES_PLANNING_COMMON_PATH_PATH_H_ */

modules/planning/common/path/frenet_frame_path.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 frenet_frame_path.cc
+ **/
+#include "modules/planning/common/path/frenet_frame_path.h"
+
+#include <utility>
+#include "glog/logging.h"
+
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+
+FrenetFramePath::FrenetFramePath(
+    std::vector<common::FrenetFramePoint> sl_points) {
+  points_ = std::move(sl_points);
+}
+
+void FrenetFramePath::set_frenet_points(
+    const std::vector<common::FrenetFramePoint>& points) {
+  points_ = points;
+}
+
+std::vector<common::FrenetFramePoint>* FrenetFramePath::mutable_points() {
+  return &points_;
+}
+
+const std::vector<common::FrenetFramePoint>& FrenetFramePath::points() const {
+  return points_;
+}
+
+std::size_t FrenetFramePath::num_points() const { return points_.size(); }
+
+const common::FrenetFramePoint& FrenetFramePath::point_at(
+    const std::size_t index) const {
+  CHECK(index < points_.size());
+  return points_[index];
+}
+
+common::FrenetFramePoint& FrenetFramePath::point_at(const std::size_t index) {
+  CHECK(index < points_.size());
+  return points_[index];
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/path/frenet_frame_path.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 frenet_frame_path.h
+ **/
+
+#ifndef BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_
+#define BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_
+
+#include <vector>
+
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+
+class FrenetFramePath {
+ public:
+  FrenetFramePath() = default;
+
+  explicit FrenetFramePath(std::vector<common::FrenetFramePoint> sl_points);
+
+  virtual ~FrenetFramePath() = default;
+
+  void set_frenet_points(const std::vector<common::FrenetFramePoint>& points);
+
+  std::vector<common::FrenetFramePoint>* mutable_points();
+
+  const std::vector<common::FrenetFramePoint>& points() const;
+
+  std::size_t num_points() const;
+
+  const common::FrenetFramePoint& point_at(const std::size_t index) const;
+
+  common::FrenetFramePoint& point_at(const std::size_t index);
+
+ private:
+  std::vector<common::FrenetFramePoint> points_;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif /* BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_ */

modules/planning/common/path/path.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 path.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_PATH_PATH_H_
+#define MODULES_PLANNING_COMMON_PATH_PATH_H_
+
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+
+class Path {
+ public:
+  Path() = default;
+
+  virtual ~Path() = default;
+
+  virtual common::PathPoint evaluate(const double param) const = 0;
+
+  virtual double param_length() const = 0;
+
+  virtual common::PathPoint start_point() const = 0;
+
+  virtual common::PathPoint end_point() const = 0;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif /* MODULES_PLANNING_COMMON_PATH_PATH_H_ */

modules/planning/common/path/path_data.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 path_data.cc
+ **/
+
+#include "modules/planning/common/path/path_data.h"
+
+#include <sstream>
+#include "glog/logging.h"
+
+#include "modules/planning/math/double.h"
+
+namespace apollo {
+namespace planning {
+
+void PathData::set_path(const DiscretizedPath& path) { path_ = path; }
+
+void PathData::set_frenet_path(const FrenetFramePath& frenet_path) {
+  frenet_path_ = frenet_path;
+}
+
+DiscretizedPath* PathData::mutable_path() { return &path_; }
+
+const DiscretizedPath& PathData::path() const { return path_; }
+
+FrenetFramePath* PathData::mutable_frenet_frame_path() { return &frenet_path_; }
+
+const FrenetFramePath& PathData::frenet_frame_path() const {
+  return frenet_path_;
+}
+
+bool PathData::get_path_point_with_path_s(
+    const double s, common::PathPoint* const path_point) const {
+  const auto& path_points = path_.path_points();
+
+  if (path_points.size() < 2 || s < path_points.front().s() ||
+      path_points.back().s() < s) {
+    return false;
+  }
+
+  auto comp = [](const common::PathPoint& path_point, const double s) {
+    return path_point.s() < s;
+  };
+
+  auto it_lower =
+      std::lower_bound(path_points.begin(), path_points.end(), s, comp);
+  if (it_lower == path_points.begin()) {
+    *path_point = *it_lower;
+  } else {
+    double s0 = (it_lower - 1)->s();
+    double s1 = it_lower->s();
+    CHECK(s0 < s1);
+    *path_point =
+        util::interpolate_linear_approximation(*(it_lower - 1), *it_lower, s);
+  }
+  return true;
+}
+
+bool PathData::get_path_point_with_ref_s(
+    const double ref_s, common::PathPoint* const path_point) const {
+  const auto& frenet_points = frenet_path_.points();
+  if (frenet_points.size() < 2 || ref_s < frenet_points.front().s() ||
+      frenet_points.back().s() < ref_s) {
+    return false;
+  }
+
+  auto comp = [](const common::FrenetFramePoint& frenet_point,
+                 const double ref_s) { return frenet_point.s() < ref_s; };
+
+  auto it_lower =
+      std::lower_bound(frenet_points.begin(), frenet_points.end(), ref_s, comp);
+  if (it_lower == frenet_points.begin()) {
+    *path_point = path_.path_points().front();
+  } else {
+    //        std::size_t index_lower = (std::size_t)(it_lower -
+    //        frenet_points.begin());
+    //
+    //        double ref_s0 = (it_lower - 1)->s();
+    //        double ref_s1 = it_lower->s();
+    //
+    //        CHECK(ref_s0 < ref_s1);
+    //        double weight = (ref_s - ref_s0) / (ref_s1 - ref_s0);
+    //        *path_point =
+    //        common::PathPoint::interpolate_linear_approximation(path_.path_point_at(index_lower
+    //        - 1),
+    //                path_.path_point_at(index_lower), weight);
+  }
+  return true;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/path/path_data.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 path_data.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_
+#define MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_
+
+#include <vector>
+
+#include "modules/planning/common/path/discretized_path.h"
+#include "modules/planning/common/path/frenet_frame_path.h"
+
+namespace apollo {
+namespace planning {
+
+class PathData {
+ public:
+  PathData() = default;
+
+  void set_path(const DiscretizedPath& path);
+
+  void set_frenet_path(const FrenetFramePath& frenet_path);
+
+  DiscretizedPath* mutable_path();
+
+  const DiscretizedPath& path() const;
+
+  FrenetFramePath* mutable_frenet_frame_path();
+
+  const FrenetFramePath& frenet_frame_path() const;
+
+  bool get_path_point_with_path_s(const double s,
+                                  common::PathPoint* const path_point) const;
+
+  bool get_path_point_with_ref_s(const double ref_s,
+                                 common::PathPoint* const path_point) const;
+
+  // TODO(fanhaoyang) add check if the path data is valid
+ private:
+  DiscretizedPath path_;
+
+  FrenetFramePath frenet_path_;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_

modules/planning/common/path/path_point_util.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 path_point_util.cc
+ **/
+
+#include "modules/planning/common/path/path_point_util.h"
+
+#include <utility>
+
+#include "modules/planning/math/hermite_spline.h"
+#include "modules/planning/math/integration.h"
+
+namespace apollo {
+namespace planning {
+namespace util {
+
+common::PathPoint interpolate(const common::PathPoint& p0,
+                              const common::PathPoint& p1, const double s) {
+  double s0 = p0.s();
+  double s1 = p1.s();
+  CHECK(s0 <= s && s <= s1);
+
+  std::array<double, 2> gx0{{p0.theta(), p0.kappa()}};
+  std::array<double, 2> gx1{{p1.theta(), p1.kappa()}};
+  HermiteSpline<double, 3> geometry_spline(gx0, gx1, s0, s1);
+  auto func_cos_theta = [&geometry_spline](const double s) {
+    auto theta = geometry_spline.evaluate(0, s);
+    return std::cos(theta);
+  };
+  auto func_sin_theta = [&geometry_spline](const double s) {
+    auto theta = geometry_spline.evaluate(0, s);
+    return std::sin(theta);
+  };
+
+  double x = p0.x() + Integration::gauss_legendre(func_cos_theta, s0, s);
+  double y = p0.y() + Integration::gauss_legendre(func_sin_theta, s0, s);
+  double theta = geometry_spline.evaluate(0, s);
+  double kappa = geometry_spline.evaluate(1, s);
+  double dkappa = geometry_spline.evaluate(2, s);
+  double d2kappa = geometry_spline.evaluate(3, s);
+
+  common::PathPoint p;
+  p.set_x(x);
+  p.set_y(y);
+  p.set_theta(theta);
+  p.set_kappa(kappa);
+  p.set_dkappa(dkappa);
+  p.set_ddkappa(d2kappa);
+  p.set_s(s);
+  return std::move(p);
+}
+
+common::PathPoint interpolate_linear_approximation(
+    const common::PathPoint& left, const common::PathPoint& right,
+    const double s) {
+  double s0 = left.s();
+  double s1 = right.s();
+  CHECK(s0 < s1);
+
+  common::PathPoint path_point;
+  double weight = (s - s0) / (s1 - s0);
+  double x = (1 - weight) * left.x() + weight * right.x();
+  double y = (1 - weight) * left.y() + weight * right.y();
+  double cos_heading =
+      (1 - weight) * std::cos(left.theta()) + weight * std::cos(right.theta());
+  double sin_heading =
+      (1 - weight) * std::sin(left.theta()) + weight * std::sin(right.theta());
+  double theta = std::atan2(sin_heading, cos_heading);
+  double kappa = (1 - weight) * left.kappa() + weight * right.kappa();
+  double dkappa = (1 - weight) * left.dkappa() + weight * right.dkappa();
+  double ddkappa = (1 - weight) * left.ddkappa() + weight * right.ddkappa();
+  path_point.set_x(x);
+  path_point.set_y(y);
+  path_point.set_theta(theta);
+  path_point.set_kappa(kappa);
+  path_point.set_dkappa(dkappa);
+  path_point.set_ddkappa(ddkappa);
+  return path_point;
+}
+
+}  // namespace util
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/path/path_point_util.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 path_point_util.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_
+#define MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_
+
+#include "Eigen/Dense"
+
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+namespace util {
+
+common::PathPoint interpolate(const common::PathPoint& p0,
+                              const common::PathPoint& p1, const double s);
+
+// @ weight shall between 1 and 0
+common::PathPoint interpolate_linear_approximation(
+    const common::PathPoint& left, const common::PathPoint& right,
+    const double s);
+
+}  // namespace util
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_

modules/planning/common/path/sl_point_util.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 sl_point_util.cc
+ **/
+
+#include "modules/planning/common/path/sl_point_util.h"
+
+namespace apollo {
+namespace planning {
+namespace util {
+
+common::SLPoint interpolate(const common::SLPoint& start,
+                            const common::SLPoint& end, const double weight) {
+  common::SLPoint point;
+  double s = start.s() * (1 - weight) + end.s() * weight;
+  double l = start.l() * (1 - weight) + end.l() * weight;
+  point.set_s(s);
+  point.set_l(l);
+  return point;
+}
+
+}  // namespace util
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/path/sl_point_util.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 sl_point_util.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_PATH_SL_POINT_H_
+#define MODULES_PLANNING_COMMON_PATH_SL_POINT_H_
+
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+namespace util {
+
+static common::SLPoint interpolate(const common::SLPoint& start,
+                                   const common::SLPoint& end,
+                                   const double weight);
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_COMMON_PATH_SL_POINT_H_

modules/planning/common/speed/speed_data.cc

@@ -29,75 +29,80 @@ namespace apollo {
 namespace planning {
 
 namespace {
-    bool speed_time_comp(const double t, const SpeedPoint& speed_point) {
-        return t < speed_point.t();
-    }
+bool speed_time_comp(const double t, const SpeedPoint& speed_point) {
+  return t < speed_point.t();
+}
 }
 
 SpeedData::SpeedData(std::vector<SpeedPoint> speed_points) {
-    speed_vector_ = std::move(speed_points);
+  speed_vector_ = std::move(speed_points);
 }
 
 std::vector<SpeedPoint>* SpeedData::mutable_speed_vector() {
-    return &speed_vector_;
+  return &speed_vector_;
 }
 
 const std::vector<SpeedPoint>& SpeedData::speed_vector() const {
-    return speed_vector_;
+  return speed_vector_;
 }
 
 void SpeedData::set_speed_vector(const std::vector<SpeedPoint>& speed_points) {
-    speed_vector_ = std::move(speed_points);
+  speed_vector_ = std::move(speed_points);
 }
 
-bool SpeedData::get_speed_point_with_time(const double t, SpeedPoint* const speed_point) const {
-    if (speed_vector_.size() < 2) {
-        return false;
-    }
-    std::size_t index = find_index(t);
-    if (Double::compare(t, speed_vector_[index].t()) < 0 || index + 1 >= speed_vector_.size()) {
-        return false;
-    }
-
-    // index index + 1
-    double weight = 0.0;
-    if (Double::compare(speed_vector_[index + 1].t(), speed_vector_[index].t()) > 0) {
-        weight = (t - speed_vector_[index].t()) /
-            (speed_vector_[index].t() - speed_vector_[index + 1].t());
-    }
-
-    *speed_point = SpeedPoint::interpolate(speed_vector_[index], speed_vector_[index + 1], weight);
-    return true;    
+bool SpeedData::get_speed_point_with_time(const double t,
+                                          SpeedPoint* const speed_point) const {
+  if (speed_vector_.size() < 2) {
+    return false;
+  }
+  std::size_t index = find_index(t);
+  if (Double::compare(t, speed_vector_[index].t()) < 0 ||
+      index + 1 >= speed_vector_.size()) {
+    return false;
+  }
+
+  // index index + 1
+  double weight = 0.0;
+  if (Double::compare(speed_vector_[index + 1].t(), speed_vector_[index].t()) >
+      0) {
+    weight = (t - speed_vector_[index].t()) /
+             (speed_vector_[index].t() - speed_vector_[index + 1].t());
+  }
+
+  *speed_point = SpeedPoint::interpolate(speed_vector_[index],
+                                         speed_vector_[index + 1], weight);
+  return true;
 }
 
 double SpeedData::total_time() const {
-    if (speed_vector_.empty()) {
-        return 0.0;
-    }
-    return speed_vector_.back().t() - speed_vector_.front().t();
+  if (speed_vector_.empty()) {
+    return 0.0;
+  }
+  return speed_vector_.back().t() - speed_vector_.front().t();
 }
 
 std::string SpeedData::DebugString() const {
-    std::ostringstream sout;
-    sout << "[" << std::endl;
-    for (std::size_t i = 0; i < speed_vector_.size(); ++i) {
-        if (i > 0) {
-            sout << "," << std::endl;
-        }
-        sout << speed_vector_[i].DebugString();
+  std::ostringstream sout;
+  sout << "[" << std::endl;
+  for (std::size_t i = 0; i < speed_vector_.size(); ++i) {
+    if (i > 0) {
+      sout << "," << std::endl;
     }
-    sout << "]" << std::endl;
-    sout.flush();
-    return sout.str();
+    sout << speed_vector_[i].DebugString();
+  }
+  sout << "]" << std::endl;
+  sout.flush();
+  return sout.str();
 }
 
 std::size_t SpeedData::find_index(const double t) const {
-    auto upper_bound = std::upper_bound(speed_vector_.begin() + 1, speed_vector_.end(),
-        t, speed_time_comp);
-    return std::min(speed_vector_.size() - 1,
-        static_cast<std::size_t>(upper_bound - speed_vector_.begin())) - 1;
+  auto upper_bound = std::upper_bound(speed_vector_.begin() + 1,
+                                      speed_vector_.end(), t, speed_time_comp);
+  return std::min(
+             speed_vector_.size() - 1,
+             static_cast<std::size_t>(upper_bound - speed_vector_.begin())) -
+         1;
 }
 
-} // namespace planning
-} // namespace apollo
-
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/speed/speed_data.h

@@ -28,31 +28,31 @@ namespace apollo {
 namespace planning {
 
 class SpeedData {
-public:
-    SpeedData() = default;
+ public:
+  SpeedData() = default;
 
-    SpeedData(std::vector<SpeedPoint> speed_points);
+  SpeedData(std::vector<SpeedPoint> speed_points);
 
-    std::vector<SpeedPoint>* mutable_speed_vector();
+  std::vector<SpeedPoint>* mutable_speed_vector();
 
-    const std::vector<SpeedPoint>& speed_vector() const;
+  const std::vector<SpeedPoint>& speed_vector() const;
 
-    void set_speed_vector(const std::vector<SpeedPoint>& speed_points);
+  void set_speed_vector(const std::vector<SpeedPoint>& speed_points);
 
-    virtual std::string DebugString() const;
+  virtual std::string DebugString() const;
 
-    bool get_speed_point_with_time(const double time, SpeedPoint* const speed_point) const;
+  bool get_speed_point_with_time(const double time,
+                                 SpeedPoint* const speed_point) const;
 
-    double total_time() const;
-private:
-    std::size_t find_index(const double s) const;
+  double total_time() const;
 
-    std::vector<SpeedPoint> speed_vector_;
-};
-
-} // namespace planning
-} // namespace apollo
+ private:
+  std::size_t find_index(const double s) const;
 
-#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_DATA_H
+  std::vector<SpeedPoint> speed_vector_;
+};
 
+}  // namespace planning
+}  // namespace apollo
 
+#endif  // MODULES_PLANNING_COMMON_SPEED_SPEED_DATA_H

modules/planning/common/speed/speed_point.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.
- *****************************************************************************/
+/******************************************************************************
+* 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
@@ -25,56 +25,38 @@
 namespace apollo {
 namespace planning {
 
-SpeedPoint::SpeedPoint(const STPoint& st_point, const double v, const double a, const double j) :
-    STPoint(st_point), v_(v), a_(a), j_(j) {
-}
+SpeedPoint::SpeedPoint(const STPoint& st_point, const double v, const double a,
+                       const double j)
+    : STPoint(st_point), v_(v), a_(a), j_(j) {}
 
-SpeedPoint::SpeedPoint(
-    const double s,
-    const double t,
-    const double v,
-    const double a,
-    const double j) : STPoint(s, t), v_(v), a_(a), j_(j) {
-}
+SpeedPoint::SpeedPoint(const double s, const double t, const double v,
+                       const double a, const double j)
+    : STPoint(s, t), v_(v), a_(a), j_(j) {}
 
-void SpeedPoint::set_v(const double v) {
-    v_ = v;
-}
-
-void SpeedPoint::set_a(const double a) {
-    a_ = a;
-}
+void SpeedPoint::set_v(const double v) { v_ = v; }
 
-void SpeedPoint::set_j(const double j) {
-    j_ = j;
-}
+void SpeedPoint::set_a(const double a) { a_ = a; }
 
-double SpeedPoint::v() const {
-    return v_;
-}
+void SpeedPoint::set_j(const double j) { j_ = j; }
 
-double SpeedPoint::a() const {
-    return a_;
-}
+double SpeedPoint::v() const { return v_; }
 
-double SpeedPoint::j() const {
-    return j_;
-}
+double SpeedPoint::a() const { return a_; }
 
-SpeedPoint SpeedPoint::interpolate(const SpeedPoint& left, const SpeedPoint& right,
-        const double weight) {
-    double s = (1 - weight) * left.s() + weight * right.s();
-    double t = (1 - weight) * left.t() + weight * right.t();
-    double v = (1 - weight) * left.v() + weight * right.v();
-    double a = (1 - weight) * left.a() + weight * right.a();
-    double j = (1 - weight) * left.j() + weight * right.j();
-    return SpeedPoint(s, t, v, a, j);
-}
+double SpeedPoint::j() const { return j_; }
 
-std::string SpeedPoint::DebugString() const {
-    return "";
+SpeedPoint SpeedPoint::interpolate(const SpeedPoint& left,
+                                   const SpeedPoint& right,
+                                   const double weight) {
+  double s = (1 - weight) * left.s() + weight * right.s();
+  double t = (1 - weight) * left.t() + weight * right.t();
+  double v = (1 - weight) * left.v() + weight * right.v();
+  double a = (1 - weight) * left.a() + weight * right.a();
+  double j = (1 - weight) * left.j() + weight * right.j();
+  return SpeedPoint(s, t, v, a, j);
 }
 
-} // namespace planning
-} // namespace apollo
+std::string SpeedPoint::DebugString() const { return ""; }
 
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/speed/speed_point.h

@@ -27,33 +27,31 @@ namespace apollo {
 namespace planning {
 
 class SpeedPoint : public STPoint {
-public:
-    SpeedPoint() = default;
-    SpeedPoint(const double s,
-        const double t,
-        const double v,
-        const double a,
-        const double j);
-    SpeedPoint(const STPoint& st_point, const double v, const double a, const double j);
-    void set_v(const double v);
-    void set_a(const double a);
-    void set_j(const double j);
-
-    double v() const;
-    double a() const;
-    double j() const;
-
-    std::string DebugString() const;
-    static SpeedPoint interpolate(const SpeedPoint& left, const SpeedPoint& right,
-        const double weight);
-private:
-    double v_ = 0.0;
-    double a_ = 0.0;
-    double j_ = 0.0;
+ public:
+  SpeedPoint() = default;
+  SpeedPoint(const double s, const double t, const double v, const double a,
+             const double j);
+  SpeedPoint(const STPoint& st_point, const double v, const double a,
+             const double j);
+  void set_v(const double v);
+  void set_a(const double a);
+  void set_j(const double j);
+
+  double v() const;
+  double a() const;
+  double j() const;
+
+  std::string DebugString() const;
+  static SpeedPoint interpolate(const SpeedPoint& left, const SpeedPoint& right,
+                                const double weight);
+
+ private:
+  double v_ = 0.0;
+  double a_ = 0.0;
+  double j_ = 0.0;
 };
 
-} // namespace planning
-} // namespace apollo
-
-#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H
+}  // namespace planning
+}  // namespace apollo
 
+#endif  // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H

modules/planning/common/speed/st_point.cc

@@ -26,33 +26,23 @@
 namespace apollo {
 namespace planning {
 
-STPoint::STPoint(const double s, const double t) : Vec2d(t, s) {
-};
+STPoint::STPoint(const double s, const double t) : Vec2d(t, s){};
 
-double STPoint::s() const {
-    return y_;
-}
+double STPoint::s() const { return y_; }
 
-double STPoint::t() const {
-    return x_;
-}
+double STPoint::t() const { return x_; }
 
-void STPoint::set_s(const double s) {
-    return set_y(s);
-}
+void STPoint::set_s(const double s) { return set_y(s); }
 
-void STPoint::set_t(const double t) {
-    return set_x(t);
-}
+void STPoint::set_t(const double t) { return set_x(t); }
 
 std::string STPoint::DebugString() const {
-    std::ostringstream sout;
-    sout << "{ \"s\" : " << std::setprecision(6) << s()
-            << ", \"t\" : " << std::setprecision(6) << t() << " }";
-    sout.flush();
-    return sout.str();
+  std::ostringstream sout;
+  sout << "{ \"s\" : " << std::setprecision(6) << s()
+       << ", \"t\" : " << std::setprecision(6) << t() << " }";
+  sout.flush();
+  return sout.str();
 }
 
-} // namespace planning
-} // namespace apollo
-
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/speed/st_point.h

@@ -27,18 +27,17 @@ namespace apollo {
 namespace planning {
 
 class STPoint : public ::apollo::common::math::Vec2d {
-public:
-    STPoint() = default;
-    STPoint(const double s, const double t);
-    double s() const;
-    double t() const;
-    void set_s(const double s);
-    void set_t(const double t);
-    std::string DebugString() const;
+ public:
+  STPoint() = default;
+  STPoint(const double s, const double t);
+  double s() const;
+  double t() const;
+  void set_s(const double s);
+  void set_t(const double t);
+  std::string DebugString() const;
 };
 
-} // namespace planning
-} // namespace apollo
-
-#endif // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H
+}  // namespace planning
+}  // namespace apollo
 
+#endif  // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H

modules/planning/math/spiral_curve/quintic_spiral_curve.cc

@@ -33,7 +33,8 @@ namespace planning {
 using ::apollo::common::ErrorCode;
 using ::apollo::common::PathPoint;
 
-QuinticSpiralCurve::QuinticSpiralCurve(const PathPoint& s, const PathPoint& e)
+QuinticSpiralCurve::QuinticSpiralCurve(const common::PathPoint& s,
+                                       const common::PathPoint& e)
     : SpiralCurve(s, e, 5) {
   // generate an order 5 spiral path with four parameters
 }
@@ -191,7 +192,7 @@ bool QuinticSpiralCurve::calculate_path() {
 }
 
 ErrorCode QuinticSpiralCurve::get_path_vec(
-    const std::size_t n, std::vector<PathPoint>* path_points) const {
+    const std::size_t n, std::vector<common::PathPoint>* path_points) const {
   CHECK_NOTNULL(path_points);
 
   if (n <= 1 || error() > spiral_config().newton_raphson_tol()) {
@@ -199,7 +200,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
   }
 
   path_points->resize(n);
-  std::vector<PathPoint>& result = *path_points;
+  std::vector<common::PathPoint>& result = *path_points;
 
   result[0].set_x(start_point().x());
   result[0].set_y(start_point().y());
@@ -247,7 +248,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
 
 ErrorCode QuinticSpiralCurve::get_path_vec_with_s(
     const std::vector<double>& vec_s,
-    std::vector<PathPoint>* path_points) const {
+    std::vector<common::PathPoint>* path_points) const {
   CHECK_NOTNULL(path_points);
 
   if (error() > spiral_config().newton_raphson_tol()) {
@@ -260,10 +261,10 @@ ErrorCode QuinticSpiralCurve::get_path_vec_with_s(
 
   const std::size_t n = vec_s.size();
 
-  PathPoint ref_point = start_point();
+  common::PathPoint ref_point = start_point();
 
   path_points->resize(n);
-  std::vector<PathPoint>& result = *path_points;
+  std::vector<common::PathPoint>& result = *path_points;
   std::array<double, 6> p_value;
   std::copy(p_params().begin(), p_params().end(), p_value.begin());
   std::array<double, 6> a_params = SpiralFormula::p_to_a_k5(sg(), p_value);

modules/planning/math/spiral_curve/spiral_curve.cc

@@ -27,7 +27,7 @@ namespace planning {
 
 using apollo::common::PathPoint;
 
-SpiralCurve::SpiralCurve(const PathPoint& s, const PathPoint& e,
+SpiralCurve::SpiralCurve(const common::PathPoint& s, const common::PathPoint& e,
                          const std::size_t order)
     : p_params_(order + 1, 0.0),
       sg_(0.0),
@@ -42,9 +42,11 @@ void SpiralCurve::set_spiral_config(const SpiralCurveConfig& spiral_config) {
 }
 
 // output params
-const PathPoint& SpiralCurve::start_point() const { return *start_point_; }
+const common::PathPoint& SpiralCurve::start_point() const {
+  return *start_point_;
+}
 
-const PathPoint& SpiralCurve::end_point() const { return *end_point_; }
+const common::PathPoint& SpiralCurve::end_point() const { return *end_point_; }
 
 double SpiralCurve::sg() const { return sg_; }
 

modules/planning/planner/em_planner.cc

@@ -28,13 +28,10 @@ namespace planning {
 using apollo::common::TrajectoryPoint;
 using apollo::common::vehicle_state::VehicleState;
 
-EMPlanner::EMPlanner() {
-}
-
-bool EMPlanner::Plan(
-    const TrajectoryPoint& start_point,
-    std::vector<TrajectoryPoint>* discretized_trajectory) {
+EMPlanner::EMPlanner() {}
 
+bool EMPlanner::Plan(const TrajectoryPoint& start_point,
+                     std::vector<TrajectoryPoint>* discretized_trajectory) {
   return true;
 }
 

modules/planning/planner/em_planner.h

@@ -52,9 +52,8 @@ class EMPlanner : public Planner {
    * @param discretized_trajectory The computed trajectory
    * @return true if planning succeeds; false otherwise.
    */
-  bool Plan(
-      const apollo::common::TrajectoryPoint& start_point,
-      std::vector<apollo::common::TrajectoryPoint>* trajectory) override;
+  bool Plan(const apollo::common::TrajectoryPoint& start_point,
+            std::vector<apollo::common::TrajectoryPoint>* trajectory) override;
 
  private:
 };

modules/planning/planning.cc

@@ -13,6 +13,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *****************************************************************************/
+#include "modules/planning/planning.h"
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/time/time.h"
 #include "modules/planning/common/planning_gflags.h"

modules/planning/planning.h

@@ -102,9 +102,7 @@ class Planning : public apollo::common::ApolloApp {
       const double header_time,
       const std::vector<common::TrajectoryPoint>& discretized_trajectory);
 
-
-  apollo::common::util::Factory<PlanningConfig::PlannerType,
-                                Planner>
+  apollo::common::util::Factory<PlanningConfig::PlannerType, Planner>
       planner_factory_;
   PlanningConfig config_;
 

modules/planning/planning_test.cc

@@ -14,9 +14,9 @@
  * limitations under the License.
  *****************************************************************************/
 
+#include "modules/planning/planning.h"
 #include "modules/common/adapters/adapter_gflags.h"
 #include "modules/planning/common/planning_gflags.h"
-#include "modules/planning/planning.h"
 #include "modules/planning/proto/planning.pb.h"
 
 #include "gmock/gmock.h"