Planning: added test for trajectory cost. Seperated comparable cost from trajectory cost.

modules/planning/tasks/dp_poly_path/BUILD

@@ -9,6 +9,7 @@ cc_library(
         "trajectory_cost.cc",
     ],
     hdrs = [
+        "comparable_cost.h",
         "dp_road_graph.h",
         "trajectory_cost.h",
     ],
@@ -28,7 +29,7 @@ cc_library(
         "//modules/planning/math/curve1d:quintic_polynomial_curve1d",
         "//modules/planning/proto:dp_poly_path_config_proto",
         "//modules/planning/reference_line",
-        "@eigen//:eigen",
+        "@eigen",
     ],
 )
 
@@ -46,6 +47,18 @@ cc_library(
     ],
 )
 
+cc_test(
+    name = "comparable_cost_test",
+    size = "small",
+    srcs = [
+        "comparable_cost_test.cc",
+    ],
+    deps = [
+        ":dp_poly_path",
+        "@gtest//:main",
+    ],
+)
+
 cc_test(
     name = "trajectory_cost_test",
     size = "small",

modules/planning/tasks/dp_poly_path/comparable_cost.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
+ **/
+
+#ifndef MODULES_PLANNING_TASKS_DP_POLY_PATH_COMPARABLE_COST_H_
+#define MODULES_PLANNING_TASKS_DP_POLY_PATH_COMPARABLE_COST_H_
+
+#include <cmath>
+#include <cstdlib>
+
+#include <array>
+#include <vector>
+
+namespace apollo {
+namespace planning {
+
+class ComparableCost {
+ public:
+  ComparableCost() = default;
+  ComparableCost(const bool has_collision, const bool out_of_boundary,
+                 const bool out_of_lane, const double safety_cost_,
+                 const double smoothness_cost_)
+      : safety_cost(safety_cost_), smoothness_cost(smoothness_cost_) {
+    cost_items[HAS_COLLISION] = has_collision;
+    cost_items[OUT_OF_BOUNDARY] = out_of_boundary;
+    cost_items[OUT_OF_LANE] = out_of_lane;
+  }
+  ComparableCost(const ComparableCost &) = default;
+
+  int CompareTo(const ComparableCost &other) const {
+    for (size_t i = 0; i < cost_items.size(); ++i) {
+      if (cost_items[i]) {
+        if (other.cost_items[i]) {
+          continue;
+        } else {
+          return 1;
+        }
+      } else {
+        if (other.cost_items[i]) {
+          return -1;
+        } else {
+          continue;
+        }
+      }
+    }
+
+    constexpr double kEpsilon = 1e-12;
+    const double diff = safety_cost + smoothness_cost - other.safety_cost -
+                        other.smoothness_cost;
+    if (std::fabs(diff) < kEpsilon) {
+      return 0;
+    } else if (diff > 0) {
+      return 1;
+    } else {
+      return -1;
+    }
+  }
+  ComparableCost operator+(const ComparableCost &other) {
+    ComparableCost lhs = *this;
+    return lhs += other;
+  }
+  ComparableCost &operator+=(const ComparableCost &other) {
+    for (size_t i = 0; i < cost_items.size(); ++i) {
+      cost_items[i] = (cost_items[i] || other.cost_items[i]);
+    }
+    safety_cost += other.safety_cost;
+    smoothness_cost += other.smoothness_cost;
+    return *this;
+  }
+  bool operator>(const ComparableCost &other) const {
+    return this->CompareTo(other) > 0;
+  }
+  bool operator>=(const ComparableCost &other) const {
+    return this->CompareTo(other) >= 0;
+  }
+  bool operator<(const ComparableCost &other) const {
+    return this->CompareTo(other) < 0;
+  }
+  bool operator<=(const ComparableCost &other) const {
+    return this->CompareTo(other) <= 0;
+  }
+  /*
+   * cost_items represents an array of factors that affect the cost,
+   * The level is from most critical to less critical.
+   * It includes:
+   * (0) has_collision or out_of_boundary
+   * (1) out_of_lane
+   *
+   * NOTICE: Items could have same critical levels
+   */
+  static const size_t HAS_COLLISION = 0;
+  static const size_t OUT_OF_BOUNDARY = 1;
+  static const size_t OUT_OF_LANE = 2;
+  std::array<bool, 3> cost_items = {{false, false, false}};
+
+  // cost from distance to obstacles or boundaries
+  double safety_cost = 0.0;
+  // cost from deviation from lane center, path curvature etc
+  double smoothness_cost = 0.0;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_TASKS_DP_POLY_PATH_COMPARABLE_COST_H_

modules/planning/tasks/dp_poly_path/comparable_cost_test.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/tasks/dp_poly_path/comparable_cost.h"
+
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace planning {
+
+TEST(ComparableCost, simple) {
+  ComparableCost cc;
+  EXPECT_DOUBLE_EQ(cc.safety_cost, 0.0);
+  EXPECT_DOUBLE_EQ(cc.smoothness_cost, 0.0);
+  EXPECT_FALSE(cc.cost_items[ComparableCost::HAS_COLLISION]);
+  EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
+  EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_LANE]);
+}
+
+TEST(ComparableCost, add_cost) {
+  ComparableCost cc1(true, false, false, 10.12, 2.51);
+  ComparableCost cc2(false, false, true, 6.1, 3.45);
+
+  ComparableCost cc = cc1 + cc2;
+
+  EXPECT_TRUE(cc.cost_items[ComparableCost::HAS_COLLISION]);
+  EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
+  EXPECT_TRUE(cc.cost_items[ComparableCost::OUT_OF_LANE]);
+  EXPECT_DOUBLE_EQ(cc.safety_cost, 16.22);
+  EXPECT_DOUBLE_EQ(cc.smoothness_cost, 5.96);
+
+  EXPECT_TRUE(cc1 > cc2);
+
+  cc1 += cc2;
+
+  EXPECT_TRUE(cc1.cost_items[ComparableCost::HAS_COLLISION]);
+  EXPECT_FALSE(cc1.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
+  EXPECT_TRUE(cc1.cost_items[ComparableCost::OUT_OF_LANE]);
+  EXPECT_DOUBLE_EQ(cc1.safety_cost, 16.22);
+  EXPECT_DOUBLE_EQ(cc1.smoothness_cost, 5.96);
+
+  ComparableCost cc3(true, false, false, 10.12, 2.51);
+  ComparableCost cc4(false, true, true, 6.1, 3.45);
+
+  EXPECT_TRUE(cc3 > cc4);
+
+  ComparableCost cc5(false, false, false, 10.12, 2.51);
+  ComparableCost cc6(false, true, true, 6.1, 3.45);
+
+  EXPECT_TRUE(cc5 < cc6);
+
+  ComparableCost cc7 = cc5 + cc6;
+
+  EXPECT_FALSE(cc7.cost_items[ComparableCost::HAS_COLLISION]);
+  EXPECT_TRUE(cc7.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
+  EXPECT_TRUE(cc7.cost_items[ComparableCost::OUT_OF_LANE]);
+  EXPECT_DOUBLE_EQ(cc7.safety_cost, 16.22);
+  EXPECT_DOUBLE_EQ(cc7.smoothness_cost, 5.96);
+
+  EXPECT_TRUE(cc5 < cc6);
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/tasks/dp_poly_path/trajectory_cost.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file trajectory_cost.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H_
@@ -32,97 +32,14 @@
 #include "modules/planning/common/speed/speed_data.h"
 #include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
 #include "modules/planning/reference_line/reference_line.h"
+#include "modules/planning/tasks/dp_poly_path/comparable_cost.h"
 
 namespace apollo {
 namespace planning {
 
-class ComparableCost {
- public:
-  ComparableCost() = default;
-  ComparableCost(const bool has_collision, const bool out_of_boundary,
-                 const bool out_of_lane, const double safety_cost_,
-                 const double smoothness_cost_)
-      : safety_cost(safety_cost_), smoothness_cost(smoothness_cost_) {
-    cost_items[HAS_COLLISION] = has_collision;
-    cost_items[OUT_OF_BOUNDARY] = out_of_boundary;
-    cost_items[OUT_OF_LANE] = out_of_lane;
-  }
-  ComparableCost(const ComparableCost &) = default;
-
-  int CompareTo(const ComparableCost &other) const {
-    for (size_t i = 0; i < cost_items.size(); ++i) {
-      if (cost_items[i]) {
-        if (other.cost_items[i]) {
-          continue;
-        } else {
-          return 1;
-        }
-      } else {
-        if (other.cost_items[i]) {
-          return -1;
-        } else {
-          continue;
-        }
-      }
-    }
-
-    constexpr double kEpsilon = 1e-12;
-    const double diff = safety_cost + smoothness_cost - other.safety_cost -
-                        other.smoothness_cost;
-    if (std::fabs(diff) < kEpsilon) {
-      return 0;
-    } else if (diff > 0) {
-      return 1;
-    } else {
-      return -1;
-    }
-  }
-  ComparableCost operator+(const ComparableCost &other) {
-    ComparableCost lhs = *this;
-    return lhs += other;
-  }
-  ComparableCost &operator+=(const ComparableCost &other) {
-    for (size_t i = 0; i < cost_items.size(); ++i) {
-      cost_items[i] = (cost_items[i] || other.cost_items[i]);
-    }
-    safety_cost += other.safety_cost;
-    smoothness_cost += other.smoothness_cost;
-    return *this;
-  }
-  bool operator>(const ComparableCost &other) const {
-    return this->CompareTo(other) > 0;
-  }
-  bool operator>=(const ComparableCost &other) const {
-    return this->CompareTo(other) >= 0;
-  }
-  bool operator<(const ComparableCost &other) const {
-    return this->CompareTo(other) < 0;
-  }
-  bool operator<=(const ComparableCost &other) const {
-    return this->CompareTo(other) <= 0;
-  }
-  /*
-   * cost_items represents an array of factors that affect the cost,
-   * The level is from most critical to less critical.
-   * It includes:
-   * (0) has_collision or out_of_boundary
-   * (1) out_of_lane
-   *
-   * NOTICE: Items could have same critical levels
-   */
-  static const size_t HAS_COLLISION = 0;
-  static const size_t OUT_OF_BOUNDARY = 1;
-  static const size_t OUT_OF_LANE = 2;
-  std::array<bool, 3> cost_items = {{false, false, false}};
-
-  // cost from distance to obstacles or boundaries
-  double safety_cost = 0.0;
-  // cost from deviation from lane center, path curvature etc
-  double smoothness_cost = 0.0;
-};
-
 class TrajectoryCost {
  public:
+  TrajectoryCost() = default;
   explicit TrajectoryCost(const DpPolyPathConfig &config,
                           const ReferenceLine &reference_line,
                           const bool is_change_lane_path,
@@ -150,6 +67,7 @@ class TrajectoryCost {
       const common::math::Box2d &ego_box,
       const common::math::Box2d &obstacle_box) const;
 
+  FRIEND_TEST(AllTrajectoryTests, GetCostFromObsSL);
   ComparableCost GetCostFromObsSL(const double adc_s, const double adc_l,
                                   const SLBoundary &obs_sl_boundary);
 

modules/planning/tasks/dp_poly_path/trajectory_cost_test.cc

@@ -21,56 +21,35 @@
 namespace apollo {
 namespace planning {
 
-TEST(ComparableCost, simple) {
-  ComparableCost cc;
-  EXPECT_DOUBLE_EQ(cc.safety_cost, 0.0);
-  EXPECT_DOUBLE_EQ(cc.smoothness_cost, 0.0);
-  EXPECT_FALSE(cc.cost_items[ComparableCost::HAS_COLLISION]);
-  EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
-  EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_LANE]);
-}
-
-TEST(ComparableCost, add_cost) {
-  ComparableCost cc1(true, false, false, 10.12, 2.51);
-  ComparableCost cc2(false, false, true, 6.1, 3.45);
-
-  ComparableCost cc = cc1 + cc2;
-
-  EXPECT_TRUE(cc.cost_items[ComparableCost::HAS_COLLISION]);
-  EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
-  EXPECT_TRUE(cc.cost_items[ComparableCost::OUT_OF_LANE]);
-  EXPECT_DOUBLE_EQ(cc.safety_cost, 16.22);
-  EXPECT_DOUBLE_EQ(cc.smoothness_cost, 5.96);
-
-  EXPECT_TRUE(cc1 > cc2);
-
-  cc1 += cc2;
-
-  EXPECT_TRUE(cc1.cost_items[ComparableCost::HAS_COLLISION]);
-  EXPECT_FALSE(cc1.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
-  EXPECT_TRUE(cc1.cost_items[ComparableCost::OUT_OF_LANE]);
-  EXPECT_DOUBLE_EQ(cc1.safety_cost, 16.22);
-  EXPECT_DOUBLE_EQ(cc1.smoothness_cost, 5.96);
-
-  ComparableCost cc3(true, false, false, 10.12, 2.51);
-  ComparableCost cc4(false, true, true, 6.1, 3.45);
-
-  EXPECT_TRUE(cc3 > cc4);
-
-  ComparableCost cc5(false, false, false, 10.12, 2.51);
-  ComparableCost cc6(false, true, true, 6.1, 3.45);
-
-  EXPECT_TRUE(cc5 < cc6);
-
-  ComparableCost cc7 = cc5 + cc6;
-
-  EXPECT_FALSE(cc7.cost_items[ComparableCost::HAS_COLLISION]);
-  EXPECT_TRUE(cc7.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
-  EXPECT_TRUE(cc7.cost_items[ComparableCost::OUT_OF_LANE]);
-  EXPECT_DOUBLE_EQ(cc7.safety_cost, 16.22);
-  EXPECT_DOUBLE_EQ(cc7.smoothness_cost, 5.96);
-
-  EXPECT_TRUE(cc5 < cc6);
+TEST(AllTrajectoryTests, GetCostFromObsSL) {
+  // left nudge
+  TrajectoryCost tc;
+  SLBoundary obs_sl_boundary;
+  obs_sl_boundary.set_start_s(20.0);
+  obs_sl_boundary.set_end_s(25.0);
+  obs_sl_boundary.set_start_l(-1.5);
+  obs_sl_boundary.set_end_l(-0.2);
+  auto cost = tc.GetCostFromObsSL(5.0, 0.5, obs_sl_boundary);
+  EXPECT_DOUBLE_EQ(cost.safety_cost, 240.48911372030088);
+  EXPECT_DOUBLE_EQ(cost.smoothness_cost, 0.0);
+  EXPECT_FALSE(cost.cost_items.at(0));
+  EXPECT_FALSE(cost.cost_items.at(1));
+  EXPECT_FALSE(cost.cost_items.at(2));
+
+  // collisioned obstacle
+  TrajectoryCost tc1;
+  SLBoundary obs_sl_boundary1;
+  obs_sl_boundary1.set_start_s(20.0);
+  obs_sl_boundary1.set_end_s(25.0);
+  obs_sl_boundary1.set_start_l(-1.5);
+  obs_sl_boundary1.set_end_l(-0.2);
+  auto cost1 = tc.GetCostFromObsSL(21.0, -0.5, obs_sl_boundary1);
+
+  EXPECT_DOUBLE_EQ(cost1.safety_cost, 676.73517161369182);
+  EXPECT_DOUBLE_EQ(cost1.smoothness_cost, 0.0);
+  EXPECT_TRUE(cost1.cost_items.at(0));
+  EXPECT_FALSE(cost1.cost_items.at(1));
+  EXPECT_FALSE(cost1.cost_items.at(2));
 }
 
 }  // namespace planning