merged distributed utility files to planning_util

modules/planning/common/BUILD

@@ -171,4 +171,20 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "planning_util",
+    srcs = [
+        "planning_util.cc",
+    ],
+    hdrs = [
+        "planning_util.h",
+    ],
+    deps = [
+        "//modules/common/math",
+        "//modules/common/proto:path_point_proto",
+        "//modules/planning/math:hermite_spline",
+        "//modules/planning/proto:planning_proto",
+    ],
+)
+
 cpplint()

modules/planning/common/path/BUILD

@@ -2,24 +2,6 @@ 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/common/math:math",
-        "@eigen//:eigen",
-    ],
-)
-
 cc_library(
     name = "path",
     hdrs = [
@@ -37,8 +19,8 @@ cc_library(
     ],
     deps = [
         ":path",
-        ":path_point_util",
         "//modules/common/proto:path_point_proto",
+        "//modules/planning/common:planning_util",
     ],
 )
 
@@ -52,8 +34,8 @@ cc_library(
     ],
     deps = [
         ":path",
-        ":path_point_util",
         "//modules/common/proto:path_point_proto",
+        "//modules/planning/common:planning_util",
     ],
 )
 

modules/planning/common/path/discretized_path.cc

@@ -20,10 +20,11 @@
 
 #include "modules/planning/common/path/discretized_path.h"
 
+#include <algorithm>
 #include <utility>
 
 #include "modules/common/log.h"
-#include "modules/planning/common/path/path_point_util.h"
+#include "modules/planning/common/planning_util.h"
 
 namespace apollo {
 namespace planning {

modules/planning/common/path/discretized_path.h

@@ -24,7 +24,6 @@
 #include <vector>
 
 #include "modules/planning/common/path/path.h"
-#include "modules/planning/common/path/path_point_util.h"
 
 namespace apollo {
 namespace planning {

modules/planning/common/path/path_data.cc

@@ -20,10 +20,12 @@
 
 #include "modules/planning/common/path/path_data.h"
 
+#include <algorithm>
 #include <sstream>
 
 #include "modules/common/log.h"
 #include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/common/planning_util.h"
 #include "modules/planning/math/double.h"
 
 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 {
-
-apollo::common::PathPoint interpolate(
-    const apollo::common::PathPoint &p0,
-    const apollo::common::PathPoint &p1, const double s);
-
-// @ weight shall between 1 and 0
-apollo::common::PathPoint interpolate_linear_approximation(
-    const apollo::common::PathPoint &p0,
-    const apollo::common::PathPoint &p1,
-    const double s);
-
-apollo::common::TrajectoryPoint interpolate(
-    const apollo::common::TrajectoryPoint &tp0,
-    const apollo::common::TrajectoryPoint &tp1,
-    const double t);
-
-apollo::common::TrajectoryPoint interpolate_linear_approximation(
-    const apollo::common::TrajectoryPoint &tp0,
-    const apollo::common::TrajectoryPoint &tp1,
-    const double t);
-
-}  // 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 {
-
-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/path/path_point_util.cc → modules/planning/common/planning_util.cc

@@ -14,12 +14,11 @@
  * limitations under the License.
  *****************************************************************************/
 
-/**
- * @file path_point_util.cc
- **/
-
-#include "modules/planning/common/path/path_point_util.h"
+#include "modules/planning/common/planning_util.h"
 
+#include <array>
+#include <cmath>
+#include <memory>
 #include <utility>
 
 #include "modules/common/math/integral.h"
@@ -30,8 +29,19 @@ namespace apollo {
 namespace planning {
 namespace util {
 
-using apollo::common::PathPoint;
-using apollo::common::TrajectoryPoint;
+using common::PathPoint;
+using common::TrajectoryPoint;
+
+SpeedPoint MakeSpeedPoint(const double s, const double t, double v, double a,
+                          double da) {
+  SpeedPoint point;
+  point.set_s(s);
+  point.set_t(t);
+  point.set_v(v);
+  point.set_a(a);
+  point.set_da(da);
+  return point;
+}
 
 PathPoint interpolate(const PathPoint &p0, const PathPoint &p1,
                       const double s) {
@@ -51,10 +61,10 @@ PathPoint interpolate(const PathPoint &p0, const PathPoint &p1,
     return std::sin(theta);
   };
 
-  double x = p0.x() + apollo::common::math::IntegrateByGaussLegendre(
-      func_cos_theta, s0, s);
-  double y = p0.y() + apollo::common::math::IntegrateByGaussLegendre(
-      func_sin_theta, s0, s);
+  double x =
+      p0.x() + common::math::IntegrateByGaussLegendre(func_cos_theta, s0, s);
+  double y =
+      p0.y() + common::math::IntegrateByGaussLegendre(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);
@@ -68,7 +78,7 @@ PathPoint interpolate(const PathPoint &p0, const PathPoint &p1,
   p.set_dkappa(dkappa);
   p.set_ddkappa(d2kappa);
   p.set_s(s);
-  return std::move(p);
+  return p;
 }
 
 PathPoint interpolate_linear_approximation(const PathPoint &p0,
@@ -114,8 +124,8 @@ TrajectoryPoint interpolate(const TrajectoryPoint &tp0,
   auto func_v = [&dynamic_spline](const double t) {
     return dynamic_spline.evaluate(0, t);
   };
-  double s1 = apollo::common::math::IntegrateByGaussLegendre(func_v, t0, t1);
-  double s = apollo::common::math::IntegrateByGaussLegendre(func_v, t0, t);
+  double s1 = common::math::IntegrateByGaussLegendre(func_v, t0, t1);
+  double s = common::math::IntegrateByGaussLegendre(func_v, t0, t);
 
   double v = dynamic_spline.evaluate(0, t);
   double a = dynamic_spline.evaluate(1, t);
@@ -132,10 +142,10 @@ TrajectoryPoint interpolate(const TrajectoryPoint &tp0,
     return std::sin(theta);
   };
 
-  double x = pp0.x() + apollo::common::math::IntegrateByGaussLegendre(
-      func_cos_theta, s0, s);
-  double y = pp0.y() + apollo::common::math::IntegrateByGaussLegendre(
-      func_sin_theta, s0, s);
+  double x =
+      pp0.x() + common::math::IntegrateByGaussLegendre(func_cos_theta, s0, s);
+  double y =
+      pp0.y() + common::math::IntegrateByGaussLegendre(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);
@@ -167,26 +177,36 @@ TrajectoryPoint interpolate_linear_approximation(const TrajectoryPoint &tp0,
   double t1 = tp1.relative_time();
 
   TrajectoryPoint tp;
-  tp.set_v(apollo::common::math::lerp(tp0.v(), t0, tp1.v(), t1, t));
-  tp.set_a(apollo::common::math::lerp(tp0.a(), t0, tp1.a(), t1, t));
+  tp.set_v(common::math::lerp(tp0.v(), t0, tp1.v(), t1, t));
+  tp.set_a(common::math::lerp(tp0.a(), t0, tp1.a(), t1, t));
   tp.set_relative_time(t);
 
   PathPoint *path_point = tp.mutable_path_point();
-  path_point->set_x(apollo::common::math::lerp(pp0.x(), t0, pp1.x(), t1, t));
-  path_point->set_y(apollo::common::math::lerp(pp0.y(), t0, pp1.y(), t1, t));
+  path_point->set_x(common::math::lerp(pp0.x(), t0, pp1.x(), t1, t));
+  path_point->set_y(common::math::lerp(pp0.y(), t0, pp1.y(), t1, t));
   path_point->set_theta(
-      apollo::common::math::lerp(pp0.theta(), t0, pp1.theta(), t1, t));
+      common::math::lerp(pp0.theta(), t0, pp1.theta(), t1, t));
   path_point->set_kappa(
-      apollo::common::math::lerp(pp0.kappa(), t0, pp1.kappa(), t1, t));
+      common::math::lerp(pp0.kappa(), t0, pp1.kappa(), t1, t));
   path_point->set_dkappa(
-      apollo::common::math::lerp(pp0.dkappa(), t0, pp1.dkappa(), t1, t));
+      common::math::lerp(pp0.dkappa(), t0, pp1.dkappa(), t1, t));
   path_point->set_ddkappa(
-      apollo::common::math::lerp(pp0.ddkappa(), t0, pp1.ddkappa(), t1, t));
-  path_point->set_s(apollo::common::math::lerp(pp0.s(), t0, pp1.s(), t1, t));
+      common::math::lerp(pp0.ddkappa(), t0, pp1.ddkappa(), t1, t));
+  path_point->set_s(common::math::lerp(pp0.s(), t0, pp1.s(), t1, t));
 
   return tp;
 }
 
+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/util/planning_util.h → modules/planning/common/planning_util.h

@@ -37,6 +37,25 @@ namespace util {
 SpeedPoint MakeSpeedPoint(const double s, const double l, double v, double a,
                           double da);
 
+common::SLPoint interpolate(const common::SLPoint &start,
+                            const common::SLPoint &end, const double weight);
+
+apollo::common::PathPoint interpolate(const apollo::common::PathPoint &p0,
+                                      const apollo::common::PathPoint &p1,
+                                      const double s);
+
+// @ weight shall between 1 and 0
+apollo::common::PathPoint interpolate_linear_approximation(
+    const apollo::common::PathPoint &p0, const apollo::common::PathPoint &p1,
+    const double s);
+
+apollo::common::TrajectoryPoint interpolate(
+    const apollo::common::TrajectoryPoint &tp0,
+    const apollo::common::TrajectoryPoint &tp1, const double t);
+
+apollo::common::TrajectoryPoint interpolate_linear_approximation(
+    const apollo::common::TrajectoryPoint &tp0,
+    const apollo::common::TrajectoryPoint &tp1, const double t);
 }  // namespace util
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/speed/BUILD

@@ -31,6 +31,7 @@ cc_library(
         "//modules/common/math",
         "//modules/common/proto:path_point_proto",
         "//modules/planning/common:planning_gflags",
+        "//modules/planning/common:planning_util",
         "//modules/planning/math:double",
         "//modules/planning/proto:planning_proto",
     ],

modules/planning/common/speed/speed_data.cc

@@ -18,8 +18,9 @@
  * @file speed_data.cc
  **/
 
-#include "modules/planning/common/speed/speed_data.h"
 #include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/common/planning_util.h"
+#include "modules/planning/common/speed/speed_data.h"
 
 #include <algorithm>
 #include <sstream>
@@ -35,12 +36,14 @@ bool speed_time_comp(const double t, const SpeedPoint& speed_point) {
 }
 }
 
-SpeedData::SpeedData(std::vector<SpeedPoint> speed_points) {
-  speed_vector_ = std::move(speed_points);
+void SpeedData::add_speed_point(const double s, const double time,
+                                const double v, const double a,
+                                const double da) {
+  speed_vector_.push_back(util::MakeSpeedPoint(s, time, v, a, da));
 }
 
-std::vector<SpeedPoint>* SpeedData::mutable_speed_vector() {
-  return &speed_vector_;
+SpeedData::SpeedData(std::vector<SpeedPoint> speed_points) {
+  speed_vector_ = std::move(speed_points);
 }
 
 const std::vector<SpeedPoint>& SpeedData::speed_vector() const {
@@ -86,7 +89,8 @@ std::string SpeedData::DebugString() const {
   std::ostringstream sout;
   sout << "[" << std::endl;
   for (int i = 0; i < static_cast<int>(speed_vector_.size()) &&
-         i < FLAGS_trajectory_point_num_for_debug; ++i) {
+                  i < FLAGS_trajectory_point_num_for_debug;
+       ++i) {
     if (i > 0) {
       sout << "," << std::endl;
     }

modules/planning/common/speed/speed_data.h

@@ -33,14 +33,15 @@ class SpeedData {
 
   SpeedData(std::vector<SpeedPoint> speed_points);
 
-  std::vector<SpeedPoint>* mutable_speed_vector();
-
   const std::vector<SpeedPoint>& speed_vector() const;
 
   void set_speed_vector(const std::vector<SpeedPoint>& speed_points);
 
   virtual std::string DebugString() const;
 
+  void add_speed_point(const double s, const double time, const double v,
+                       const double a, const double da);
+
   bool get_speed_point_with_time(const double time,
                                  SpeedPoint* const speed_point) const;
 

modules/planning/common/trajectory/BUILD

@@ -25,7 +25,7 @@ cc_library(
     deps = [
         ":trajectory",
         "//modules/common/proto:path_point_proto",
-        "//modules/planning/common/path:path_point_util",
+        "//modules/planning/common:planning_util",
         "@eigen//:eigen",
     ],
 )

modules/planning/common/trajectory/discretized_trajectory.cc

@@ -20,10 +20,11 @@
 
 #include "modules/planning/common/trajectory/discretized_trajectory.h"
 
+#include <algorithm>
 #include <climits>
 
 #include "modules/common/log.h"
-#include "modules/planning/common/path/path_point_util.h"
+#include "modules/planning/common/planning_util.h"
 
 namespace apollo {
 namespace planning {

modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc

@@ -25,7 +25,7 @@
 #include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/macro.h"
 #include "modules/common/util/util.h"
-#include "modules/planning/common/path/sl_point_util.h"
+#include "modules/planning/common/planning_util.h"
 #include "modules/planning/math/double.h"
 #include "modules/planning/proto/planning.pb.h"
 

modules/planning/optimizer/qp_spline_st_speed/BUILD

@@ -34,9 +34,9 @@ cc_library(
         "//modules/common/proto:error_code_proto",
         "//modules/common/proto:path_point_proto",
         "//modules/common/util",
+        "//modules/planning/common:planning_util",
         "//modules/planning/math/smoothing_spline:spline_1d_generator",
         "//modules/planning/optimizer/st_graph:st_graph_data",
-        "//modules/planning/util:planning_util",
     ],
 )
 

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -24,7 +24,6 @@
 #include <utility>
 
 #include "modules/common/log.h"
-#include "modules/planning/util/planning_util.h"
 
 namespace apollo {
 namespace planning {
@@ -91,8 +90,7 @@ Status QpSplineStGraph::search(const StGraphData& st_graph_data,
     double v = spline.derivative(time);
     double a = spline.second_order_derivative(time);
     double da = spline.third_order_derivative(time);
-    speed_data->mutable_speed_vector()->push_back(
-        util::MakeSpeedPoint(s, time, v, a, da));
+    speed_data->add_speed_point(s, time, v, a, da);
   }
 
   return Status::OK();
@@ -208,9 +206,9 @@ Status QpSplineStGraph::apply_kernel() {
 }
 
 Status QpSplineStGraph::solve() {
-  return _spline_generator->solve() ?
-      Status::OK() :
-      Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
+  return _spline_generator->solve()
+             ? Status::OK()
+             : Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
 }
 
 Status QpSplineStGraph::get_s_constraints_by_time(

modules/planning/planning.cc

@@ -16,6 +16,8 @@
 
 #include "modules/planning/planning.h"
 
+#include <algorithm>
+
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/time/time.h"
 #include "modules/common/vehicle_state/vehicle_state.h"

modules/planning/reference_line/reference_line.cc

@@ -20,6 +20,7 @@
 
 #include "modules/planning/reference_line/reference_line.h"
 
+#include <algorithm>
 #include <sstream>
 #include <string>
 #include <utility>

modules/planning/util/BUILD

-load("//tools:cpplint.bzl", "cpplint")
-
-package(default_visibility = ["//visibility:public"])
-
-cc_library(
-    name = "planning_util",
-    srcs = [
-        "planning_util.cc",
-    ],
-    hdrs = [
-        "planning_util.h",
-    ],
-    deps = [
-        "//modules/common/proto:path_point_proto",
-        "//modules/planning/proto:planning_proto",
-    ],
-)
-
-cpplint()

modules/planning/util/planning_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.
- *****************************************************************************/
-
-#include "modules/planning/util/planning_util.h"
-
-namespace apollo {
-namespace planning {
-namespace util {
-
-using SpeedPoint = apollo::planning::SpeedPoint;
-
-SpeedPoint MakeSpeedPoint(const double s, const double t, double v, double a,
-                          double da) {
-  SpeedPoint point;
-  point.set_s(s);
-  point.set_t(t);
-  point.set_v(v);
-  point.set_a(a);
-  point.set_da(da);
-  return point;
-}
-
-}  // namespace util
-}  // namespace planning
-}  // namespace apollo