apollo planning tasks optimizers road_graph comparable_cost

1. 描述

在 dp path 的时候 node 间的 cost 主要分为3部分：路径平滑、避开静态障碍物、避开动态障碍物，使用ComparableCost 类描述。

该类除包含常规的 safety_cost（无碰）和 smoothness_cost（平滑）外，还含有 3 个 bool 型的成员变量，以表示是否碰撞、是否超出边界、是否超出车道线, 如下:

  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.0f;
  // cost from deviation from lane center, path curvature etc
  double smoothness_cost = 0.0f;

comparable_cost，源文件在这里

modules/planning/tasks/optimizers/road_graph/comparable_cost.h
modules/planning/tasks/optimizers/road_graph/comparable_cost.cc

2. 代码

看一下完整代码,和测试的例子

(这个直接就是源码中的,因为和其他的模块没有关联,不用修改,可以直接运行)

#include <iostream>
#include <vector>
#include <string>
#include <queue>
#include <memory>
#include <algorithm>
#include <cmath>
#include <array>

#include <gtest/gtest.h>

using namespace std;
#include <cmath>

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;
    lhs += other;
    return lhs;
  }
  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.0f;
  // cost from deviation from lane center, path curvature etc
  double smoothness_cost = 0.0f;
};

}  // namespace planning
}  // namespace apollo

namespace apollo {
namespace planning {

TEST(ComparableCost, simple) {
  ComparableCost cc;
  EXPECT_FLOAT_EQ(cc.safety_cost, 0.0);
  EXPECT_FLOAT_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_FLOAT_EQ(cc.safety_cost, 16.22);
  EXPECT_FLOAT_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_FLOAT_EQ(cc1.safety_cost, 16.22);
  EXPECT_FLOAT_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_FLOAT_EQ(cc7.safety_cost, 16.22);
  EXPECT_FLOAT_EQ(cc7.smoothness_cost, 5.96);

  EXPECT_TRUE(cc5 < cc6);
}

TEST(ComparableCost, compare_to) {
  ComparableCost cc1(true, false, false, 10.12, 2.51);
  ComparableCost cc2(false, false, true, 6.1, 3.45);
  EXPECT_TRUE(cc1 > cc2);

  ComparableCost cc3(false, true, false, 10.12, 2.51);
  ComparableCost cc4(false, false, false, 6.1, 3.45);
  EXPECT_TRUE(cc3 > cc4);

  ComparableCost cc5(false, false, true, 10.12, 2.51);
  ComparableCost cc6(false, false, false, 6.1, 3.45);
  EXPECT_TRUE(cc5 > cc6);

  ComparableCost cc7(false, false, false, 10.12, 2.51);
  ComparableCost cc8(false, false, false, 6.1, 3.45);
  EXPECT_TRUE(cc7 > cc8);

  ComparableCost cc9(false, false, false, 0.12, 2.51);
  ComparableCost cc10(false, false, false, 6.1, 3.45);
  EXPECT_TRUE(cc9 < cc10);

  ComparableCost cc11(false, false, false, 0.1, 100.0);
  ComparableCost cc12(false, false, false, 6.1, 3.45);
  EXPECT_TRUE(cc11 < cc12);
}

}  // namespace planning
}  // namespace apollo

• 结果:

/home/seivl/me/代码/规划/bbox_test/src/main.cpp:453: Failure
Value of: cc11 < cc12
  Actual: false
Expected: true
[  FAILED  ] ComparableCost.compare_to (0 ms)
[----------] 3 tests from ComparableCost (0 ms total)

[----------] Global test environment tear-down
[==========] 4 tests from 2 test suites ran. (0 ms total)
[  PASSED  ] 3 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] ComparableCost.compare_to

 1 FAILED TEST

3. 分析

很明显:

• 先判断是否碰撞、是否超出边界、是否超出车道线,一旦有一个是 true, 直接选择另一个 node

• 如果都没发生,那就比较两个 node 的 safety_cost 和 smoothness_cost 之和,选择小的