Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 f9a5bab3 编写于 作者: L Liangliang Zhang 提交者： Jiangtao Hu
浏览文件
操作

Planning: replace PathObstacle with Obstacle.

上级 e770e666
展开全部 隐藏空白更改
内联 并排
Showing with 1227 addition and 2545 deletion
modules/planning/common/BUILD
浏览文件 @ f9a5bab3
......@@ -59,12 +59,12 @@ filegroup(
)
cc_library(
name = "path_obstacle",
name = "obstacle",
srcs = [
"path_obstacle.cc",
"obstacle.cc",
],
hdrs = [
"path_obstacle.h",
"obstacle.h",
],
deps = [
":indexed_list",
......@@ -77,16 +77,16 @@ cc_library(
)
cc_test(
name = "path_obstacle_test",
name = "obstacle_test",
size = "small",
srcs = [
"path_obstacle_test.cc",
"obstacle_test.cc",
],
data = [
"//modules/planning/common:common_testdata",
],
deps = [
":path_obstacle",
":obstacle",
"//modules/common/util",
"//modules/perception/proto:perception_proto",
"@gtest//:main",
......@@ -121,7 +121,7 @@ cc_library(
"path_decision.h",
],
deps = [
":path_obstacle",
":obstacle",
"//modules/planning/reference_line",
],
)
......@@ -271,7 +271,7 @@ cc_library(
":change_lane_decider",
":indexed_queue",
#":lag_prediction",
":path_obstacle",
":obstacle",
":reference_line_info",
"//modules/common/monitor_log",
"//cyber/common:log",
......@@ -341,7 +341,7 @@ cc_library(
"ego_info.h",
],
deps = [
":path_obstacle",
":obstacle",
"//cyber/common:log",
"//modules/common/configs:vehicle_config_helper",
"//modules/common/configs/proto:vehicle_config_proto",
......@@ -375,7 +375,7 @@ cc_library(
"decision_data.h",
],
deps = [
":path_obstacle",
":obstacle",
"//modules/common/configs:vehicle_config_helper",
"//modules/planning/reference_line",
"//modules/prediction/proto:prediction_proto",
......
modules/planning/common/decision_data.cc
浏览文件 @ f9a5bab3
......@@ -54,12 +54,12 @@ DecisionData::DecisionData(
const std::string perception_id =
std::to_string(prediction_obstacle.perception_obstacle().id());
if (prediction_obstacle.trajectory().empty()) {
path_obstacles_.emplace_back(new PathObstacle(
obstacles_.emplace_back(new Obstacle(
perception_id, prediction_obstacle.perception_obstacle()));
all_obstacle_.emplace_back(path_obstacles_.back().get());
practical_obstacle_.emplace_back(path_obstacles_.back().get());
static_obstacle_.emplace_back(path_obstacles_.back().get());
obstacle_map_[perception_id] = path_obstacles_.back().get();
all_obstacle_.emplace_back(obstacles_.back().get());
practical_obstacle_.emplace_back(obstacles_.back().get());
static_obstacle_.emplace_back(obstacles_.back().get());
obstacle_map_[perception_id] = obstacles_.back().get();
continue;
}
int trajectory_index = 0;
......@@ -70,18 +70,18 @@ DecisionData::DecisionData(
}
const std::string obstacle_id =
apollo::common::util::StrCat(perception_id, "_", trajectory_index);
path_obstacles_.emplace_back(new PathObstacle(
obstacles_.emplace_back(new Obstacle(
obstacle_id, prediction_obstacle.perception_obstacle(), trajectory));
all_obstacle_.emplace_back(path_obstacles_.back().get());
practical_obstacle_.emplace_back(path_obstacles_.back().get());
dynamic_obstacle_.emplace_back(path_obstacles_.back().get());
obstacle_map_[obstacle_id] = path_obstacles_.back().get();
all_obstacle_.emplace_back(obstacles_.back().get());
practical_obstacle_.emplace_back(obstacles_.back().get());
dynamic_obstacle_.emplace_back(obstacles_.back().get());
obstacle_map_[obstacle_id] = obstacles_.back().get();
++trajectory_index;
}
}
}
PathObstacle* DecisionData::GetObstacleById(const std::string& id) {
Obstacle* DecisionData::GetObstacleById(const std::string& id) {
std::lock_guard<std::mutex> lock(mutex_);
const auto it = obstacle_map_.find(id);
if (it == obstacle_map_.end()) {
......@@ -90,15 +90,15 @@ PathObstacle* DecisionData::GetObstacleById(const std::string& id) {
return it->second;
}
std::vector<PathObstacle*> DecisionData::GetObstacleByType(
std::vector<Obstacle*> DecisionData::GetObstacleByType(
const VirtualObjectType& type) {
std::lock_guard<std::mutex> lock(transaction_mutex_);
std::unordered_set<std::string> ids = GetObstacleIdByType(type);
if (ids.empty()) {
return std::vector<PathObstacle*>();
return std::vector<Obstacle*>();
}
std::vector<PathObstacle*> ret;
std::vector<Obstacle*> ret;
for (const std::string& id : ids) {
ret.emplace_back(GetObstacleById(id));
if (ret.back() == nullptr) {
......@@ -120,23 +120,23 @@ std::unordered_set<std::string> DecisionData::GetObstacleIdByType(
return it->second;
}
const std::vector<PathObstacle*>& DecisionData::GetStaticObstacle() const {
const std::vector<Obstacle*>& DecisionData::GetStaticObstacle() const {
return static_obstacle_;
}
const std::vector<PathObstacle*>& DecisionData::GetDynamicObstacle() const {
const std::vector<Obstacle*>& DecisionData::GetDynamicObstacle() const {
return dynamic_obstacle_;
}
const std::vector<PathObstacle*>& DecisionData::GetVirtualObstacle() const {
const std::vector<Obstacle*>& DecisionData::GetVirtualObstacle() const {
return virtual_obstacle_;
}
const std::vector<PathObstacle*>& DecisionData::GetPracticalObstacle() const {
const std::vector<Obstacle*>& DecisionData::GetPracticalObstacle() const {
return practical_obstacle_;
}
const std::vector<PathObstacle*>& DecisionData::GetAllObstacle() const {
const std::vector<Obstacle*>& DecisionData::GetAllObstacle() const {
return all_obstacle_;
}
......@@ -204,14 +204,14 @@ bool DecisionData::CreateVirtualObstacle(
point->set_y(corner_point.y());
}
*id = std::to_string(perception_obstacle.id());
path_obstacles_.emplace_back(new PathObstacle(*id, perception_obstacle));
all_obstacle_.emplace_back(path_obstacles_.back().get());
virtual_obstacle_.emplace_back(path_obstacles_.back().get());
obstacles_.emplace_back(new Obstacle(*id, perception_obstacle));
all_obstacle_.emplace_back(obstacles_.back().get());
virtual_obstacle_.emplace_back(obstacles_.back().get());
// would be changed if some virtual type is not static one
static_obstacle_.emplace_back(path_obstacles_.back().get());
static_obstacle_.emplace_back(obstacles_.back().get());
virtual_obstacle_id_map_[type].insert(*id);
obstacle_map_[*id] = path_obstacles_.back().get();
obstacle_map_[*id] = obstacles_.back().get();
return true;
}
......
modules/planning/common/decision_data.h
浏览文件 @ f9a5bab3
......@@ -25,7 +25,7 @@
#include <vector>
#include "modules/common/math/box2d.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/reference_line/reference_line.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
......@@ -57,15 +57,15 @@ class DecisionData {
~DecisionData() = default;
public:
PathObstacle* GetObstacleById(const std::string& id);
std::vector<PathObstacle*> GetObstacleByType(const VirtualObjectType& type);
Obstacle* GetObstacleById(const std::string& id);
std::vector<Obstacle*> GetObstacleByType(const VirtualObjectType& type);
std::unordered_set<std::string> GetObstacleIdByType(
const VirtualObjectType& type);
const std::vector<PathObstacle*>& GetStaticObstacle() const;
const std::vector<PathObstacle*>& GetDynamicObstacle() const;
const std::vector<PathObstacle*>& GetVirtualObstacle() const;
const std::vector<PathObstacle*>& GetPracticalObstacle() const;
const std::vector<PathObstacle*>& GetAllObstacle() const;
const std::vector<Obstacle*>& GetStaticObstacle() const;
const std::vector<Obstacle*>& GetDynamicObstacle() const;
const std::vector<Obstacle*>& GetVirtualObstacle() const;
const std::vector<Obstacle*>& GetPracticalObstacle() const;
const std::vector<Obstacle*>& GetAllObstacle() const;
public:
bool CreateVirtualObstacle(const ReferencePoint& point,
......@@ -83,16 +83,16 @@ class DecisionData {
std::string* const id);
private:
std::vector<PathObstacle*> static_obstacle_;
std::vector<PathObstacle*> dynamic_obstacle_;
std::vector<PathObstacle*> virtual_obstacle_;
std::vector<PathObstacle*> practical_obstacle_;
std::vector<PathObstacle*> all_obstacle_;
std::vector<Obstacle*> static_obstacle_;
std::vector<Obstacle*> dynamic_obstacle_;
std::vector<Obstacle*> virtual_obstacle_;
std::vector<Obstacle*> practical_obstacle_;
std::vector<Obstacle*> all_obstacle_;
private:
const ReferenceLine& reference_line_;
std::list<std::unique_ptr<PathObstacle>> path_obstacles_;
std::unordered_map<std::string, PathObstacle*> obstacle_map_;
std::list<std::unique_ptr<Obstacle>> obstacles_;
std::unordered_map<std::string, Obstacle*> obstacle_map_;
std::unordered_map<VirtualObjectType, std::unordered_set<std::string>,
EnumClassHash>
virtual_obstacle_id_map_;
......
modules/planning/common/ego_info.cc
浏览文件 @ f9a5bab3
......@@ -36,7 +36,7 @@ EgoInfo::EgoInfo() {
bool EgoInfo::Update(const common::TrajectoryPoint& start_point,
const common::VehicleState& vehicle_state,
const std::vector<const PathObstacle*>& obstacles) {
const std::vector<const Obstacle*>& obstacles) {
set_start_point(start_point);
set_vehicle_state(vehicle_state);
CalculateFrontObstacleClearDistance(obstacles);
......@@ -50,7 +50,7 @@ void EgoInfo::Clear() {
}
void EgoInfo::CalculateFrontObstacleClearDistance(
const std::vector<const PathObstacle*>& obstacles) {
const std::vector<const Obstacle*>& obstacles) {
Vec2d position(vehicle_state_.x(), vehicle_state_.y());
const auto& param = ego_vehicle_config_.vehicle_param();
......
modules/planning/common/ego_info.h
浏览文件 @ f9a5bab3
......@@ -32,7 +32,7 @@
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/reference_line/reference_line.h"
......@@ -45,7 +45,7 @@ class EgoInfo {
bool Update(const common::TrajectoryPoint& start_point,
const common::VehicleState& vehicle_state,
const std::vector<const PathObstacle*>& obstacles);
const std::vector<const Obstacle*>& obstacles);
void Clear();
common::TrajectoryPoint start_point() const { return start_point_; }
......@@ -66,7 +66,7 @@ class EgoInfo {
}
void CalculateFrontObstacleClearDistance(
const std::vector<const PathObstacle*>& obstacles);
const std::vector<const Obstacle*>& obstacles);
// stitched point (at stitching mode)
// or real vehicle point (at non-stitching mode)
......
modules/planning/common/frame.cc
浏览文件 @ f9a5bab3
......@@ -232,7 +232,7 @@ bool Frame::CreateReferenceLineInfo(
* @brief: create static virtual object with lane width,
* mainly used for virtual stop wall
*/
const PathObstacle *Frame::CreateStopObstacle(
const Obstacle *Frame::CreateStopObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_s) {
if (reference_line_info == nullptr) {
......@@ -257,9 +257,9 @@ const PathObstacle *Frame::CreateStopObstacle(
* @brief: create static virtual object with lane width,
* mainly used for virtual stop wall
*/
const PathObstacle *Frame::CreateStopObstacle(const std::string &obstacle_id,
const std::string &lane_id,
const double lane_s) {
const Obstacle *Frame::CreateStopObstacle(const std::string &obstacle_id,
const std::string &lane_id,
const double lane_s) {
if (!hdmap_) {
AERROR << "Invalid HD Map.";
return nullptr;
......@@ -288,7 +288,7 @@ const PathObstacle *Frame::CreateStopObstacle(const std::string &obstacle_id,
/**
* @brief: create static virtual object with lane width,
*/
const PathObstacle *Frame::CreateStaticObstacle(
const Obstacle *Frame::CreateStaticObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_start_s,
const double obstacle_end_s) {
......@@ -333,15 +333,15 @@ const PathObstacle *Frame::CreateStaticObstacle(
return CreateStaticVirtualObstacle(obstacle_id, obstacle_box);
}
const PathObstacle *Frame::CreateStaticVirtualObstacle(const std::string &id,
const Box2d &box) {
const Obstacle *Frame::CreateStaticVirtualObstacle(const std::string &id,
const Box2d &box) {
const auto *object = obstacles_.Find(id);
if (object) {
AWARN << "obstacle " << id << " already exist.";
return object;
}
auto *ptr =
obstacles_.Add(id, *PathObstacle::CreateStaticVirtualObstacles(id, box));
obstacles_.Add(id, *Obstacle::CreateStaticVirtualObstacles(id, box));
if (!ptr) {
AERROR << "Failed to create virtual obstacle " << id;
}
......@@ -375,7 +375,7 @@ Status Frame::Init(
local_view_.prediction_obstacles->CopyFrom(prediction);
}
for (auto &ptr :
PathObstacle::CreateObstacles(*local_view_.prediction_obstacles)) {
Obstacle::CreateObstacles(*local_view_.prediction_obstacles)) {
AddObstacle(*ptr);
}
if (FLAGS_enable_collision_detection) {
......@@ -398,7 +398,7 @@ Status Frame::Init(
return Status::OK();
}
const PathObstacle *Frame::FindCollisionObstacleWithInExtraRadius(
const Obstacle *Frame::FindCollisionObstacleWithInExtraRadius(
const double radius) const {
CHECK_GT(radius, kMathEpsilon)
<< "Extra safety radius must be positive number!";
......@@ -514,9 +514,9 @@ void Frame::AlignPredictionTime(const double planning_start_time,
}
}
PathObstacle *Frame::Find(const std::string &id) { return obstacles_.Find(id); }
Obstacle *Frame::Find(const std::string &id) { return obstacles_.Find(id); }
void Frame::AddObstacle(const PathObstacle &obstacle) {
void Frame::AddObstacle(const Obstacle &obstacle) {
obstacles_.Add(obstacle.Id(), obstacle);
}
......@@ -537,7 +537,7 @@ const ReferenceLineInfo *Frame::DriveReferenceLineInfo() const {
return drive_reference_line_info_;
}
const std::vector<const PathObstacle *> Frame::obstacles() const {
const std::vector<const Obstacle *> Frame::obstacles() const {
return obstacles_.Items();
}
......@@ -563,9 +563,8 @@ bool Frame::VPresentationObstacle() {
Box2d original_box = obstacles_.Items().at(i)->PerceptionBoundingBox();
original_box.Shift(-1 * origin_point_);
original_box.RotateFromCenter(-origin_heading_);
std::unique_ptr<PathObstacle> obstacle =
PathObstacle::CreateStaticVirtualObstacles(
obstacles_.Items().at(i)->Id(), original_box);
std::unique_ptr<Obstacle> obstacle = Obstacle::CreateStaticVirtualObstacles(
obstacles_.Items().at(i)->Id(), original_box);
openspace_warmstart_obstacles_.Add(obstacle->Id(), *obstacle);
}
......@@ -622,30 +621,29 @@ bool Frame::VPresentationObstacle() {
Box2d up_boundary_box(up_boundary_center, up_boundary_heading,
up_boundary_length, up_boundary_width);
std::unique_ptr<PathObstacle> left_boundary_obstacle =
PathObstacle::CreateStaticVirtualObstacles("left_boundary",
left_boundary_box);
std::unique_ptr<Obstacle> left_boundary_obstacle =
Obstacle::CreateStaticVirtualObstacles("left_boundary",
left_boundary_box);
openspace_warmstart_obstacles_.Add(left_boundary_obstacle->Id(),
*left_boundary_obstacle);
std::unique_ptr<PathObstacle> down_boundary_obstacle =
PathObstacle::CreateStaticVirtualObstacles("down_boundary",
down_boundary_box);
std::unique_ptr<Obstacle> down_boundary_obstacle =
Obstacle::CreateStaticVirtualObstacles("down_boundary",
down_boundary_box);
openspace_warmstart_obstacles_.Add(down_boundary_obstacle->Id(),
*down_boundary_obstacle);
std::unique_ptr<PathObstacle> right_boundary_obstacle =
PathObstacle::CreateStaticVirtualObstacles("right_boundary",
right_boundary_box);
std::unique_ptr<Obstacle> right_boundary_obstacle =
Obstacle::CreateStaticVirtualObstacles("right_boundary",
right_boundary_box);
openspace_warmstart_obstacles_.Add(right_boundary_obstacle->Id(),
*right_boundary_obstacle);
std::unique_ptr<PathObstacle> up_boundary_obstacle =
PathObstacle::CreateStaticVirtualObstacles("up_boundary",
up_boundary_box);
std::unique_ptr<Obstacle> up_boundary_obstacle =
Obstacle::CreateStaticVirtualObstacles("up_boundary", up_boundary_box);
openspace_warmstart_obstacles_.Add(up_boundary_obstacle->Id(),
*up_boundary_obstacle);
......
modules/planning/common/frame.h
浏览文件 @ f9a5bab3
......@@ -43,7 +43,7 @@
#include "modules/planning/common/indexed_queue.h"
// #include "modules/planning/common/lag_prediction.h"
#include "modules/planning/common/local_view.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/trajectory/publishable_trajectory.h"
#include "modules/planning/reference_line/reference_line_provider.h"
......@@ -91,23 +91,23 @@ class Frame {
const std::list<ReferenceLineInfo> &reference_line_info() const;
std::list<ReferenceLineInfo> *mutable_reference_line_info();
PathObstacle *Find(const std::string &id);
Obstacle *Find(const std::string &id);
const ReferenceLineInfo *FindDriveReferenceLineInfo();
const ReferenceLineInfo *DriveReferenceLineInfo() const;
const std::vector<const PathObstacle *> obstacles() const;
const std::vector<const Obstacle *> obstacles() const;
const PathObstacle *CreateStopObstacle(
const Obstacle *CreateStopObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_s);
const PathObstacle *CreateStopObstacle(const std::string &obstacle_id,
const std::string &lane_id,
const double lane_s);
const Obstacle *CreateStopObstacle(const std::string &obstacle_id,
const std::string &lane_id,
const double lane_s);
const PathObstacle *CreateStaticObstacle(
const Obstacle *CreateStaticObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_start_s,
const double obstacle_end_s);
......@@ -196,16 +196,16 @@ class Frame {
* @return pointer to the obstacle if such obstacle exists, otherwise
* @return false if no colliding obstacle.
*/
const PathObstacle *FindCollisionObstacleWithInExtraRadius(
const Obstacle *FindCollisionObstacleWithInExtraRadius(
const double radius) const;
/**
* @brief create a static virtual obstacle
*/
const PathObstacle *CreateStaticVirtualObstacle(
const std::string &id, const common::math::Box2d &box);
const Obstacle *CreateStaticVirtualObstacle(const std::string &id,
const common::math::Box2d &box);
void AddObstacle(const PathObstacle &obstacle);
void AddObstacle(const Obstacle &obstacle);
private:
uint32_t sequence_num_ = 0;
......
modules/planning/common/path_obstacle.cc → modules/planning/common/obstacle.cc
浏览文件 @ f9a5bab3
......@@ -29,7 +29,7 @@
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/util/map_util.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed/st_boundary.h"
......@@ -47,8 +47,8 @@ const double kStBoundaryDeltaT = 0.05; // seconds
} // namespace
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
PathObstacle::ObjectTagCaseHash>
PathObstacle::s_longitudinal_decision_safety_sorter_ = {
Obstacle::ObjectTagCaseHash>
Obstacle::s_longitudinal_decision_safety_sorter_ = {
{ObjectDecisionType::kIgnore, 0},
{ObjectDecisionType::kOvertake, 100},
{ObjectDecisionType::kFollow, 300},
......@@ -56,14 +56,14 @@ const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
{ObjectDecisionType::kStop, 500}};
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
PathObstacle::ObjectTagCaseHash>
PathObstacle::s_lateral_decision_safety_sorter_ = {
Obstacle::ObjectTagCaseHash>
Obstacle::s_lateral_decision_safety_sorter_ = {
{ObjectDecisionType::kIgnore, 0},
{ObjectDecisionType::kNudge, 100},
{ObjectDecisionType::kSidepass, 200}};
PathObstacle::PathObstacle(const std::string& id,
const PerceptionObstacle& perception_obstacle)
Obstacle::Obstacle(const std::string& id,
const PerceptionObstacle& perception_obstacle)
: id_(id),
perception_id_(perception_obstacle.id()),
perception_obstacle_(perception_obstacle),
......@@ -93,10 +93,10 @@ PathObstacle::PathObstacle(const std::string& id,
perception_obstacle.velocity().y());
}
PathObstacle::PathObstacle(const std::string& id,
const PerceptionObstacle& perception_obstacle,
const prediction::Trajectory& trajectory)
: PathObstacle(id, perception_obstacle) {
Obstacle::Obstacle(const std::string& id,
const PerceptionObstacle& perception_obstacle,
const prediction::Trajectory& trajectory)
: Obstacle(id, perception_obstacle) {
trajectory_ = trajectory;
auto& trajectory_points = *trajectory_.mutable_trajectory_point();
double cumulative_s = 0.0;
......@@ -117,7 +117,7 @@ PathObstacle::PathObstacle(const std::string& id,
}
}
common::TrajectoryPoint PathObstacle::GetPointAtTime(
common::TrajectoryPoint Obstacle::GetPointAtTime(
const double relative_time) const {
const auto& points = trajectory_.trajectory_point();
if (points.size() < 2) {
......@@ -152,7 +152,7 @@ common::TrajectoryPoint PathObstacle::GetPointAtTime(
}
}
common::math::Box2d PathObstacle::GetBoundingBox(
common::math::Box2d Obstacle::GetBoundingBox(
const common::TrajectoryPoint& point) const {
return common::math::Box2d({point.path_point().x(), point.path_point().y()},
point.path_point().theta(),
......@@ -160,8 +160,7 @@ common::math::Box2d PathObstacle::GetBoundingBox(
perception_obstacle_.width());
}
bool PathObstacle::IsStaticObstacle(
const PerceptionObstacle& perception_obstacle) {
bool Obstacle::IsStaticObstacle(const PerceptionObstacle& perception_obstacle) {
if (perception_obstacle.type() == PerceptionObstacle::UNKNOWN_UNMOVABLE) {
return true;
}
......@@ -171,14 +170,14 @@ bool PathObstacle::IsStaticObstacle(
moving_speed <= FLAGS_static_obstacle_speed_threshold;
}
std::list<std::unique_ptr<PathObstacle>> PathObstacle::CreateObstacles(
std::list<std::unique_ptr<Obstacle>> Obstacle::CreateObstacles(
const prediction::PredictionObstacles& predictions) {
std::list<std::unique_ptr<PathObstacle>> obstacles;
std::list<std::unique_ptr<Obstacle>> obstacles;
for (const auto& prediction_obstacle : predictions.prediction_obstacle()) {
const auto perception_id =
std::to_string(prediction_obstacle.perception_obstacle().id());
if (prediction_obstacle.trajectory().empty()) {
obstacles.emplace_back(new PathObstacle(
obstacles.emplace_back(new Obstacle(
perception_id, prediction_obstacle.perception_obstacle()));
continue;
}
......@@ -201,7 +200,7 @@ std::list<std::unique_ptr<PathObstacle>> PathObstacle::CreateObstacles(
const std::string obstacle_id =
apollo::common::util::StrCat(perception_id, "_", trajectory_index);
obstacles.emplace_back(new PathObstacle(
obstacles.emplace_back(new Obstacle(
obstacle_id, prediction_obstacle.perception_obstacle(), trajectory));
++trajectory_index;
}
......@@ -209,7 +208,7 @@ std::list<std::unique_ptr<PathObstacle>> PathObstacle::CreateObstacles(
return obstacles;
}
std::unique_ptr<PathObstacle> PathObstacle::CreateStaticVirtualObstacles(
std::unique_ptr<Obstacle> Obstacle::CreateStaticVirtualObstacles(
const std::string& id, const common::math::Box2d& obstacle_box) {
// create a "virtual" perception_obstacle
perception::PerceptionObstacle perception_obstacle;
......@@ -237,13 +236,12 @@ std::unique_ptr<PathObstacle> PathObstacle::CreateStaticVirtualObstacles(
point->set_x(corner_point.x());
point->set_y(corner_point.y());
}
auto* obstacle = new PathObstacle(id, perception_obstacle);
auto* obstacle = new Obstacle(id, perception_obstacle);
obstacle->is_virtual_ = true;
return std::unique_ptr<PathObstacle>(obstacle);
return std::unique_ptr<Obstacle>(obstacle);
}
bool PathObstacle::IsValidTrajectoryPoint(
const common::TrajectoryPoint& point) {
bool Obstacle::IsValidTrajectoryPoint(const common::TrajectoryPoint& point) {
return !((!point.has_path_point()) || std::isnan(point.path_point().x()) ||
std::isnan(point.path_point().y()) ||
std::isnan(point.path_point().z()) ||
......@@ -254,11 +252,11 @@ bool PathObstacle::IsValidTrajectoryPoint(
std::isnan(point.a()) || std::isnan(point.relative_time()));
}
void PathObstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) {
void Obstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) {
perception_sl_boundary_ = sl_boundary;
}
double PathObstacle::MinRadiusStopDistance(
double Obstacle::MinRadiusStopDistance(
const common::VehicleParam& vehicle_param) const {
if (min_radius_stop_distance_ > 0) {
return min_radius_stop_distance_;
......@@ -281,8 +279,8 @@ double PathObstacle::MinRadiusStopDistance(
return stop_distance;
}
void PathObstacle::BuildReferenceLineStBoundary(
const ReferenceLine& reference_line, const double adc_start_s) {
void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
const double adc_start_s) {
const auto& adc_param =
VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
const double adc_width = adc_param.width();
......@@ -315,9 +313,9 @@ void PathObstacle::BuildReferenceLineStBoundary(
}
}
bool PathObstacle::BuildTrajectoryStBoundary(
const ReferenceLine& reference_line, const double adc_start_s,
StBoundary* const st_boundary) {
bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
const double adc_start_s,
StBoundary* const st_boundary) {
if (!IsValidObstacle(perception_obstacle_)) {
AERROR << "Fail to build trajectory st boundary because object is not "
"valid. PerceptionObstacle: "
......@@ -477,31 +475,31 @@ bool PathObstacle::BuildTrajectoryStBoundary(
return true;
}
const StBoundary& PathObstacle::reference_line_st_boundary() const {
const StBoundary& Obstacle::reference_line_st_boundary() const {
return reference_line_st_boundary_;
}
const StBoundary& PathObstacle::st_boundary() const { return st_boundary_; }
const StBoundary& Obstacle::st_boundary() const { return st_boundary_; }
const std::vector<std::string>& PathObstacle::decider_tags() const {
const std::vector<std::string>& Obstacle::decider_tags() const {
return decider_tags_;
}
const std::vector<ObjectDecisionType>& PathObstacle::decisions() const {
const std::vector<ObjectDecisionType>& Obstacle::decisions() const {
return decisions_;
}
bool PathObstacle::IsLateralDecision(const ObjectDecisionType& decision) {
bool Obstacle::IsLateralDecision(const ObjectDecisionType& decision) {
return decision.has_ignore() || decision.has_nudge() ||
decision.has_sidepass();
}
bool PathObstacle::IsLongitudinalDecision(const ObjectDecisionType& decision) {
bool Obstacle::IsLongitudinalDecision(const ObjectDecisionType& decision) {
return decision.has_ignore() || decision.has_stop() || decision.has_yield() ||
decision.has_follow() || decision.has_overtake();
}
ObjectDecisionType PathObstacle::MergeLongitudinalDecision(
ObjectDecisionType Obstacle::MergeLongitudinalDecision(
const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {
if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
return rhs;
......@@ -536,27 +534,27 @@ ObjectDecisionType PathObstacle::MergeLongitudinalDecision(
return lhs; // stop compiler complaining
}
const ObjectDecisionType& PathObstacle::LongitudinalDecision() const {
const ObjectDecisionType& Obstacle::LongitudinalDecision() const {
return longitudinal_decision_;
}
const ObjectDecisionType& PathObstacle::LateralDecision() const {
const ObjectDecisionType& Obstacle::LateralDecision() const {
return lateral_decision_;
}
bool PathObstacle::IsIgnore() const {
bool Obstacle::IsIgnore() const {
return IsLongitudinalIgnore() && IsLateralIgnore();
}
bool PathObstacle::IsLongitudinalIgnore() const {
bool Obstacle::IsLongitudinalIgnore() const {
return longitudinal_decision_.has_ignore();
}
bool PathObstacle::IsLateralIgnore() const {
bool Obstacle::IsLateralIgnore() const {
return lateral_decision_.has_ignore();
}
ObjectDecisionType PathObstacle::MergeLateralDecision(
ObjectDecisionType Obstacle::MergeLateralDecision(
const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {
if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
return rhs;
......@@ -590,23 +588,23 @@ ObjectDecisionType PathObstacle::MergeLateralDecision(
return lhs;
}
bool PathObstacle::HasLateralDecision() const {
bool Obstacle::HasLateralDecision() const {
return lateral_decision_.object_tag_case() !=
ObjectDecisionType::OBJECT_TAG_NOT_SET;
}
bool PathObstacle::HasLongitudinalDecision() const {
bool Obstacle::HasLongitudinalDecision() const {
return longitudinal_decision_.object_tag_case() !=
ObjectDecisionType::OBJECT_TAG_NOT_SET;
}
bool PathObstacle::HasNonIgnoreDecision() const {
bool Obstacle::HasNonIgnoreDecision() const {
return (HasLateralDecision() && !IsLateralIgnore()) ||
(HasLongitudinalDecision() && !IsLongitudinalIgnore());
}
void PathObstacle::AddLongitudinalDecision(const std::string& decider_tag,
const ObjectDecisionType& decision) {
void Obstacle::AddLongitudinalDecision(const std::string& decider_tag,
const ObjectDecisionType& decision) {
DCHECK(IsLongitudinalDecision(decision))
<< "Decision: " << decision.ShortDebugString()
<< " is not a longitudinal decision";
......@@ -620,8 +618,8 @@ void PathObstacle::AddLongitudinalDecision(const std::string& decider_tag,
decider_tags_.push_back(decider_tag);
}
void PathObstacle::AddLateralDecision(const std::string& decider_tag,
const ObjectDecisionType& decision) {
void Obstacle::AddLateralDecision(const std::string& decider_tag,
const ObjectDecisionType& decision) {
DCHECK(IsLateralDecision(decision))
<< "Decision: " << decision.ShortDebugString()
<< " is not a lateral decision";
......@@ -634,9 +632,9 @@ void PathObstacle::AddLateralDecision(const std::string& decider_tag,
decider_tags_.push_back(decider_tag);
}
const std::string PathObstacle::DebugString() const {
const std::string Obstacle::DebugString() const {
std::stringstream ss;
ss << "PathObstacle id: " << id_;
ss << "Obstacle id: " << id_;
for (std::size_t i = 0; i < decisions_.size(); ++i) {
ss << " decision: " << decisions_[i].DebugString() << ", made by "
<< decider_tags_[i];
......@@ -653,34 +651,34 @@ const std::string PathObstacle::DebugString() const {
return ss.str();
}
const SLBoundary& PathObstacle::PerceptionSLBoundary() const {
const SLBoundary& Obstacle::PerceptionSLBoundary() const {
return perception_sl_boundary_;
}
void PathObstacle::SetStBoundary(const StBoundary& boundary) {
void Obstacle::SetStBoundary(const StBoundary& boundary) {
st_boundary_ = boundary;
}
void PathObstacle::SetStBoundaryType(const StBoundary::BoundaryType type) {
void Obstacle::SetStBoundaryType(const StBoundary::BoundaryType type) {
st_boundary_.SetBoundaryType(type);
}
void PathObstacle::EraseStBoundary() { st_boundary_ = StBoundary(); }
void Obstacle::EraseStBoundary() { st_boundary_ = StBoundary(); }
void PathObstacle::SetReferenceLineStBoundary(const StBoundary& boundary) {
void Obstacle::SetReferenceLineStBoundary(const StBoundary& boundary) {
reference_line_st_boundary_ = boundary;
}
void PathObstacle::SetReferenceLineStBoundaryType(
void Obstacle::SetReferenceLineStBoundaryType(
const StBoundary::BoundaryType type) {
reference_line_st_boundary_.SetBoundaryType(type);
}
void PathObstacle::EraseReferenceLineStBoundary() {
void Obstacle::EraseReferenceLineStBoundary() {
reference_line_st_boundary_ = StBoundary();
}
bool PathObstacle::IsValidObstacle(
bool Obstacle::IsValidObstacle(
const perception::PerceptionObstacle& perception_obstacle) {
const double object_width = perception_obstacle.width();
const double object_length = perception_obstacle.length();
......
modules/planning/common/path_obstacle.h → modules/planning/common/obstacle.h
浏览文件 @ f9a5bab3
......@@ -42,7 +42,7 @@ namespace apollo {
namespace planning {
/**
* @class PathObstacle
* @class Obstacle
* @brief This is the class that associates an Obstacle with its path
* properties. An obstacle's path properties relative to a path.
* The `s` and `l` values are examples of path properties.
......@@ -58,16 +58,14 @@ namespace planning {
* Ignore decision belongs to both lateral decision and longitudinal decision,
* and it has the lowest priority.
*/
class PathObstacle {
class Obstacle {
public:
PathObstacle() = default;
explicit PathObstacle(
const std::string& id,
const perception::PerceptionObstacle& perception_obstacle);
explicit PathObstacle(
const std::string& id,
const perception::PerceptionObstacle& perception_obstacle,
const prediction::Trajectory& trajectory);
Obstacle() = default;
explicit Obstacle(const std::string& id,
const perception::PerceptionObstacle& perception_obstacle);
explicit Obstacle(const std::string& id,
const perception::PerceptionObstacle& perception_obstacle,
const prediction::Trajectory& trajectory);
const std::string& Id() const { return id_; }
void SetId(const std::string& id) { id_ = id; }
......@@ -109,10 +107,10 @@ class PathObstacle {
* @param predictions The prediction results
* @return obstacles The output obstacles saved in a list of unique_ptr.
*/
static std::list<std::unique_ptr<PathObstacle>> CreateObstacles(
static std::list<std::unique_ptr<Obstacle>> CreateObstacles(
const prediction::PredictionObstacles& predictions);
static std::unique_ptr<PathObstacle> CreateStaticVirtualObstacles(
static std::unique_ptr<Obstacle> CreateStaticVirtualObstacles(
const std::string& id, const common::math::Box2d& obstacle_box);
static bool IsStaticObstacle(
......@@ -265,9 +263,8 @@ class PathObstacle {
s_longitudinal_decision_safety_sorter_;
};
typedef IndexedList<std::string, PathObstacle> IndexedObstacles;
typedef ThreadSafeIndexedList<std::string, PathObstacle>
ThreadSafeIndexedObstacles;
typedef IndexedList<std::string, Obstacle> IndexedObstacles;
typedef ThreadSafeIndexedList<std::string, Obstacle> ThreadSafeIndexedObstacles;
} // namespace planning
} // namespace apollo
modules/planning/common/obstacle_test.cc
浏览文件 @ f9a5bab3
......@@ -18,6 +18,8 @@
* @file
**/
#include "modules/planning/common/obstacle.h"
#include <memory>
#include <unordered_map>
#include <vector>
......@@ -29,7 +31,6 @@
#include "modules/common/util/file.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/planning_gflags.h"
namespace apollo {
......@@ -204,5 +205,422 @@ TEST(Obstacle, CreateStaticVirtualObstacle) {
EXPECT_FLOAT_EQ(0.0, perception_box.heading());
}
TEST(IsLateralDecision, AllDecisions) {
ObjectDecisionType decision_ignore;
decision_ignore.mutable_ignore();
EXPECT_TRUE(Obstacle::IsLateralDecision(decision_ignore));
ObjectDecisionType decision_overtake;
decision_overtake.mutable_overtake();
EXPECT_FALSE(Obstacle::IsLateralDecision(decision_overtake));
ObjectDecisionType decision_follow;
decision_follow.mutable_follow();
EXPECT_FALSE(Obstacle::IsLateralDecision(decision_follow));
ObjectDecisionType decision_yield;
decision_yield.mutable_yield();
EXPECT_FALSE(Obstacle::IsLateralDecision(decision_yield));
ObjectDecisionType decision_stop;
decision_stop.mutable_stop();
EXPECT_FALSE(Obstacle::IsLateralDecision(decision_stop));
ObjectDecisionType decision_nudge;
decision_nudge.mutable_nudge();
EXPECT_TRUE(Obstacle::IsLateralDecision(decision_nudge));
ObjectDecisionType decision_sidepass;
decision_sidepass.mutable_sidepass();
EXPECT_TRUE(Obstacle::IsLateralDecision(decision_sidepass));
}
TEST(IsLongitudinalDecision, AllDecisions) {
ObjectDecisionType decision_ignore;
decision_ignore.mutable_ignore();
EXPECT_TRUE(Obstacle::IsLongitudinalDecision(decision_ignore));
ObjectDecisionType decision_overtake;
decision_overtake.mutable_overtake();
EXPECT_TRUE(Obstacle::IsLongitudinalDecision(decision_overtake));
ObjectDecisionType decision_follow;
decision_follow.mutable_follow();
EXPECT_TRUE(Obstacle::IsLongitudinalDecision(decision_follow));
ObjectDecisionType decision_yield;
decision_yield.mutable_yield();
EXPECT_TRUE(Obstacle::IsLongitudinalDecision(decision_yield));
ObjectDecisionType decision_stop;
decision_stop.mutable_stop();
EXPECT_TRUE(Obstacle::IsLongitudinalDecision(decision_stop));
ObjectDecisionType decision_nudge;
decision_nudge.mutable_nudge();
EXPECT_FALSE(Obstacle::IsLongitudinalDecision(decision_nudge));
ObjectDecisionType decision_sidepass;
decision_sidepass.mutable_sidepass();
EXPECT_FALSE(Obstacle::IsLongitudinalDecision(decision_sidepass));
}
TEST(MergeLongitudinalDecision, AllDecisions) {
ObjectDecisionType decision_ignore;
decision_ignore.mutable_ignore();
ObjectDecisionType decision_overtake;
decision_overtake.mutable_overtake();
ObjectDecisionType decision_follow;
decision_follow.mutable_follow();
ObjectDecisionType decision_yield;
decision_yield.mutable_yield();
ObjectDecisionType decision_stop;
decision_stop.mutable_stop();
ObjectDecisionType decision_nudge;
decision_nudge.mutable_nudge();
ObjectDecisionType decision_sidepass;
decision_sidepass.mutable_sidepass();
// vertical decision comparison
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_stop, decision_ignore)
.has_stop());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_stop, decision_overtake)
.has_stop());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_stop, decision_follow)
.has_stop());
EXPECT_TRUE(Obstacle::MergeLongitudinalDecision(decision_stop, decision_yield)
.has_stop());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_yield, decision_ignore)
.has_yield());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_yield, decision_overtake)
.has_yield());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_yield, decision_follow)
.has_yield());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_follow, decision_ignore)
.has_follow());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_follow, decision_overtake)
.has_follow());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_overtake, decision_ignore)
.has_overtake());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_ignore, decision_overtake)
.has_overtake());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_ignore, decision_follow)
.has_follow());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_ignore, decision_yield)
.has_yield());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_ignore, decision_stop)
.has_stop());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_overtake, decision_follow)
.has_follow());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_overtake, decision_yield)
.has_yield());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_overtake, decision_stop)
.has_stop());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_follow, decision_yield)
.has_yield());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_follow, decision_stop)
.has_stop());
EXPECT_TRUE(Obstacle::MergeLongitudinalDecision(decision_yield, decision_stop)
.has_stop());
EXPECT_TRUE(
Obstacle::MergeLongitudinalDecision(decision_ignore, decision_ignore)
.has_ignore());
ObjectDecisionType decision_overtake1;
decision_overtake1.mutable_overtake()->set_distance_s(1);
ObjectDecisionType decision_overtake2;
decision_overtake2.mutable_overtake()->set_distance_s(2);
EXPECT_EQ(2, Obstacle::MergeLongitudinalDecision(decision_overtake1,
decision_overtake2)
.overtake()
.distance_s());
ObjectDecisionType decision_follow1;
decision_follow1.mutable_follow()->set_distance_s(-1);
ObjectDecisionType decision_follow2;
decision_follow2.mutable_follow()->set_distance_s(-2);
EXPECT_EQ(-2, Obstacle::MergeLongitudinalDecision(decision_follow1,
decision_follow2)
.follow()
.distance_s());
ObjectDecisionType decision_yield1;
decision_yield1.mutable_yield()->set_distance_s(-1);
ObjectDecisionType decision_yield2;
decision_yield2.mutable_yield()->set_distance_s(-2);
EXPECT_EQ(
-2, Obstacle::MergeLongitudinalDecision(decision_yield1, decision_yield2)
.yield()
.distance_s());
ObjectDecisionType decision_stop1;
decision_stop1.mutable_stop()->set_distance_s(-1);
ObjectDecisionType decision_stop2;
decision_stop2.mutable_stop()->set_distance_s(-2);
EXPECT_EQ(-2,
Obstacle::MergeLongitudinalDecision(decision_stop1, decision_stop2)
.stop()
.distance_s());
}
TEST(MergeLateralDecision, AllDecisions) {
ObjectDecisionType decision_ignore;
decision_ignore.mutable_ignore();
ObjectDecisionType decision_overtake;
decision_overtake.mutable_overtake();
ObjectDecisionType decision_follow;
decision_follow.mutable_follow();
ObjectDecisionType decision_yield;
decision_yield.mutable_yield();
ObjectDecisionType decision_stop;
decision_stop.mutable_stop();
ObjectDecisionType decision_nudge;
decision_nudge.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE);
ObjectDecisionType decision_sidepass;
decision_sidepass.mutable_sidepass();
EXPECT_TRUE(Obstacle::MergeLateralDecision(decision_nudge, decision_ignore)
.has_nudge());
EXPECT_TRUE(Obstacle::MergeLateralDecision(decision_ignore, decision_nudge)
.has_nudge());
ObjectDecisionType decision_nudge2;
decision_nudge2.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE);
EXPECT_TRUE(Obstacle::MergeLateralDecision(decision_nudge, decision_nudge2)
.has_nudge());
decision_nudge2.mutable_nudge()->set_type(ObjectNudge::RIGHT_NUDGE);
}
TEST(ObstacleMergeTest, add_decision_test) {
// init state
{
Obstacle obstacle;
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_FALSE(obstacle.HasLongitudinalDecision());
}
// Ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_ignore();
obstacle.AddLongitudinalDecision("test_ignore", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_FALSE(obstacle.LateralDecision().has_ignore());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_ignore());
}
// stop and ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_stop();
obstacle.AddLongitudinalDecision("test_stop", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_stop());
decision.mutable_ignore();
obstacle.AddLongitudinalDecision("test_ignore", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_FALSE(obstacle.LateralDecision().has_ignore());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_stop());
}
// left nudge and ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE);
obstacle.AddLateralDecision("test_nudge", decision);
EXPECT_TRUE(obstacle.HasLateralDecision());
EXPECT_FALSE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LateralDecision().has_nudge());
decision.mutable_ignore();
obstacle.AddLateralDecision("test_ignore", decision);
EXPECT_TRUE(obstacle.HasLateralDecision());
EXPECT_FALSE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LateralDecision().has_nudge());
EXPECT_FALSE(obstacle.LongitudinalDecision().has_ignore());
}
// right nudge and ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_nudge()->set_type(ObjectNudge::RIGHT_NUDGE);
obstacle.AddLateralDecision("test_nudge", decision);
EXPECT_TRUE(obstacle.HasLateralDecision());
EXPECT_FALSE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LateralDecision().has_nudge());
decision.mutable_ignore();
obstacle.AddLateralDecision("test_ignore", decision);
EXPECT_TRUE(obstacle.HasLateralDecision());
EXPECT_FALSE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LateralDecision().has_nudge());
EXPECT_FALSE(obstacle.LongitudinalDecision().has_ignore());
}
// left nudge and right nudge
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE);
obstacle.AddLateralDecision("test_left_nudge", decision);
EXPECT_TRUE(obstacle.HasLateralDecision());
EXPECT_FALSE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LateralDecision().has_nudge());
}
// overtake and ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_overtake();
obstacle.AddLongitudinalDecision("test_overtake", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_overtake());
decision.mutable_ignore();
obstacle.AddLongitudinalDecision("test_ignore", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_FALSE(obstacle.LateralDecision().has_ignore());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_overtake());
}
// follow and ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_follow();
obstacle.AddLongitudinalDecision("test_follow", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_follow());
decision.mutable_ignore();
obstacle.AddLongitudinalDecision("test_ignore", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_FALSE(obstacle.LateralDecision().has_ignore());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_follow());
}
// yield and ignore
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_yield();
obstacle.AddLongitudinalDecision("test_yield", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_yield());
decision.mutable_ignore();
obstacle.AddLongitudinalDecision("test_ignore", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_FALSE(obstacle.LateralDecision().has_ignore());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_yield());
}
// stop and nudge
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_stop();
obstacle.AddLongitudinalDecision("test_stop", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_stop());
decision.mutable_nudge();
obstacle.AddLateralDecision("test_nudge", decision);
EXPECT_TRUE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LateralDecision().has_nudge());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_stop());
}
// stop and yield
{
Obstacle obstacle;
ObjectDecisionType decision;
decision.mutable_stop();
obstacle.AddLongitudinalDecision("test_stop", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_stop());
decision.mutable_yield();
obstacle.AddLongitudinalDecision("test_yield", decision);
EXPECT_FALSE(obstacle.HasLateralDecision());
EXPECT_TRUE(obstacle.HasLongitudinalDecision());
EXPECT_TRUE(obstacle.LongitudinalDecision().has_stop());
}
}
} // namespace planning
} // namespace apollo
modules/planning/common/path_decision.cc
浏览文件 @ f9a5bab3
......@@ -28,28 +28,26 @@
namespace apollo {
namespace planning {
using IndexedPathObstacles = IndexedList<std::string, PathObstacle>;
using IndexedObstacles = IndexedList<std::string, Obstacle>;
PathObstacle *PathDecision::AddPathObstacle(const PathObstacle &path_obstacle) {
Obstacle *PathDecision::AddObstacle(const Obstacle &obstacle) {
std::lock_guard<std::mutex> lock(obstacle_mutex_);
return path_obstacles_.Add(path_obstacle.Id(), path_obstacle);
return obstacles_.Add(obstacle.Id(), obstacle);
}
const IndexedPathObstacles &PathDecision::path_obstacles() const {
return path_obstacles_;
}
const IndexedObstacles &PathDecision::obstacles() const { return obstacles_; }
PathObstacle *PathDecision::Find(const std::string &object_id) {
return path_obstacles_.Find(object_id);
Obstacle *PathDecision::Find(const std::string &object_id) {
return obstacles_.Find(object_id);
}
const PathObstacle *PathDecision::Find(const std::string &object_id) const {
return path_obstacles_.Find(object_id);
const Obstacle *PathDecision::Find(const std::string &object_id) const {
return obstacles_.Find(object_id);
}
void PathDecision::SetStBoundary(const std::string &id,
const StBoundary &boundary) {
auto *obstacle = path_obstacles_.Find(id);
auto *obstacle = obstacles_.Find(id);
if (!obstacle) {
AERROR << "Failed to find obstacle : " << id;
......@@ -62,18 +60,18 @@ void PathDecision::SetStBoundary(const std::string &id,
bool PathDecision::AddLateralDecision(const std::string &tag,
const std::string &object_id,
const ObjectDecisionType &decision) {
auto *path_obstacle = path_obstacles_.Find(object_id);
if (!path_obstacle) {
auto *obstacle = obstacles_.Find(object_id);
if (!obstacle) {
AERROR << "failed to find obstacle";
return false;
}
path_obstacle->AddLateralDecision(tag, decision);
obstacle->AddLateralDecision(tag, decision);
return true;
}
void PathDecision::EraseStBoundaries() {
for (const auto *path_obstacle : path_obstacles_.Items()) {
auto *obstacle_ptr = path_obstacles_.Find(path_obstacle->Id());
for (const auto *obstacle : obstacles_.Items()) {
auto *obstacle_ptr = obstacles_.Find(obstacle->Id());
obstacle_ptr->EraseStBoundary();
}
}
......@@ -81,12 +79,12 @@ void PathDecision::EraseStBoundaries() {
bool PathDecision::AddLongitudinalDecision(const std::string &tag,
const std::string &object_id,
const ObjectDecisionType &decision) {
auto *path_obstacle = path_obstacles_.Find(object_id);
if (!path_obstacle) {
auto *obstacle = obstacles_.Find(object_id);
if (!obstacle) {
AERROR << "failed to find obstacle";
return false;
}
path_obstacle->AddLongitudinalDecision(tag, decision);
obstacle->AddLongitudinalDecision(tag, decision);
return true;
}
......
modules/planning/common/path_decision.h
浏览文件 @ f9a5bab3
......@@ -30,7 +30,7 @@
#include "modules/planning/proto/decision.pb.h"
#include "modules/planning/common/indexed_list.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
namespace apollo {
namespace planning {
......@@ -44,9 +44,9 @@ class PathDecision {
public:
PathDecision() = default;
PathObstacle *AddPathObstacle(const PathObstacle &path_obstacle);
Obstacle *AddObstacle(const Obstacle &obstacle);
const IndexedList<std::string, PathObstacle> &path_obstacles() const;
const IndexedList<std::string, Obstacle> &obstacles() const;
bool AddLateralDecision(const std::string &tag, const std::string &object_id,
const ObjectDecisionType &decision);
......@@ -54,9 +54,9 @@ class PathDecision {
const std::string &object_id,
const ObjectDecisionType &decision);
const PathObstacle *Find(const std::string &object_id) const;
const Obstacle *Find(const std::string &object_id) const;
PathObstacle *Find(const std::string &object_id);
Obstacle *Find(const std::string &object_id);
void SetStBoundary(const std::string &id, const StBoundary &boundary);
void EraseStBoundaries();
......@@ -68,7 +68,7 @@ class PathDecision {
private:
std::mutex obstacle_mutex_;
IndexedList<std::string, PathObstacle> path_obstacles_;
IndexedList<std::string, Obstacle> obstacles_;
MainStop main_stop_;
double stop_reference_line_s_ = std::numeric_limits<double>::max();
};
......
modules/planning/common/path_obstacle_test.cc 已删除 100644 → 0
浏览文件 @ e770e666
此差异已折叠。
modules/planning/common/reference_line_info.cc
浏览文件 @ f9a5bab3
......@@ -56,8 +56,7 @@ ReferenceLineInfo::ReferenceLineInfo(const common::VehicleState& vehicle_state,
reference_line_(reference_line),
lanes_(segments) {}
bool ReferenceLineInfo::Init(
const std::vector<const PathObstacle*>& obstacles) {
bool ReferenceLineInfo::Init(const std::vector<const Obstacle*>& obstacles) {
const auto& param = VehicleConfigHelper::GetConfig().vehicle_param();
// stitching point
const auto& path_point = adc_planning_point_.path_point();
......@@ -142,7 +141,7 @@ void ReferenceLineInfo::InitFirstOverlaps() {
first_encounter_overlaps_.push_back({STOP_SIGN, overlap});
break;
}
for (const auto& obstacle : path_decision_.path_obstacles().Items()) {
for (const auto& obstacle : path_decision_.obstacles().Items()) {
if (!obstacle->IsBlockingObstacle()) {
continue;
}
......@@ -214,8 +213,8 @@ bool ReferenceLineInfo::CheckChangeLane() const {
return false;
}
for (const auto* path_obstacle : path_decision_.path_obstacles().Items()) {
const auto& sl_boundary = path_obstacle->PerceptionSLBoundary();
for (const auto* obstacle : path_decision_.obstacles().Items()) {
const auto& sl_boundary = obstacle->PerceptionSLBoundary();
constexpr float kLateralShift = 2.5;
if (sl_boundary.start_l() < -kLateralShift ||
......@@ -229,11 +228,11 @@ bool ReferenceLineInfo::CheckChangeLane() const {
const float kForwardSafeDistance = std::max(
kForwardMinSafeDistance,
static_cast<float>((adc_planning_point_.v() - path_obstacle->speed()) *
static_cast<float>((adc_planning_point_.v() - obstacle->speed()) *
kSafeTime));
const float kBackwardSafeDistance = std::max(
kBackwardMinSafeDistance,
static_cast<float>((path_obstacle->speed() - adc_planning_point_.v()) *
static_cast<float>((obstacle->speed() - adc_planning_point_.v()) *
kSafeTime));
if (sl_boundary.end_s() > sl_boundary_info_.adc_sl_boundary_.start_s() -
kBackwardSafeDistance &&
......@@ -278,18 +277,18 @@ void ReferenceLineInfo::SetTrajectory(const DiscretizedTrajectory& trajectory) {
}
bool ReferenceLineInfo::AddObstacleHelper(
const std::shared_ptr<PathObstacle>& obstacle) {
const std::shared_ptr<Obstacle>& obstacle) {
return AddObstacle(obstacle.get()) != nullptr;
}
// AddObstacle is thread safe
PathObstacle* ReferenceLineInfo::AddObstacle(const PathObstacle* obstacle) {
Obstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) {
if (!obstacle) {
AERROR << "The provided obstacle is empty";
return nullptr;
}
auto* path_obstacle = path_decision_.AddPathObstacle(*obstacle);
if (!path_obstacle) {
auto* mutable_obstacle = path_decision_.AddObstacle(*obstacle);
if (!mutable_obstacle) {
AERROR << "failed to add obstacle " << obstacle->Id();
return nullptr;
}
......@@ -298,11 +297,11 @@ PathObstacle* ReferenceLineInfo::AddObstacle(const PathObstacle* obstacle) {
if (!reference_line_.GetSLBoundary(obstacle->PerceptionBoundingBox(),
&perception_sl)) {
AERROR << "Failed to get sl boundary for obstacle: " << obstacle->Id();
return path_obstacle;
return mutable_obstacle;
}
path_obstacle->SetPerceptionSlBoundary(perception_sl);
mutable_obstacle->SetPerceptionSlBoundary(perception_sl);
if (IsUnrelaventObstacle(path_obstacle)) {
if (IsUnrelaventObstacle(mutable_obstacle)) {
ObjectDecisionType ignore;
ignore.mutable_ignore();
path_decision_.AddLateralDecision("reference_line_filter", obstacle->Id(),
......@@ -312,23 +311,22 @@ PathObstacle* ReferenceLineInfo::AddObstacle(const PathObstacle* obstacle) {
ADEBUG << "NO build reference line st boundary. id:" << obstacle->Id();
} else {
ADEBUG << "build reference line st boundary. id:" << obstacle->Id();
path_obstacle->BuildReferenceLineStBoundary(
mutable_obstacle->BuildReferenceLineStBoundary(
reference_line_, sl_boundary_info_.adc_sl_boundary_.start_s());
ADEBUG << "reference line st boundary: "
<< path_obstacle->reference_line_st_boundary().min_t() << ", "
<< path_obstacle->reference_line_st_boundary().max_t()
<< ", s_max: " << path_obstacle->reference_line_st_boundary().max_s()
<< ", s_min: "
<< path_obstacle->reference_line_st_boundary().min_s();
<< obstacle->reference_line_st_boundary().min_t() << ", "
<< obstacle->reference_line_st_boundary().max_t()
<< ", s_max: " << obstacle->reference_line_st_boundary().max_s()
<< ", s_min: " << obstacle->reference_line_st_boundary().min_s();
}
return path_obstacle;
return mutable_obstacle;
}
bool ReferenceLineInfo::AddObstacles(
const std::vector<const PathObstacle*>& obstacles) {
const std::vector<const Obstacle*>& obstacles) {
if (FLAGS_use_multi_thread_to_add_obstacles) {
std::vector<std::future<PathObstacle*>> results;
std::vector<std::future<Obstacle*>> results;
for (const auto* obstacle : obstacles) {
results.push_back(
cyber::Async(&ReferenceLineInfo::AddObstacle, this, obstacle));
......@@ -351,16 +349,15 @@ bool ReferenceLineInfo::AddObstacles(
return true;
}
bool ReferenceLineInfo::IsUnrelaventObstacle(PathObstacle* path_obstacle) {
bool ReferenceLineInfo::IsUnrelaventObstacle(const Obstacle* obstacle) {
// if adc is on the road, and obstacle behind adc, ignore
if (path_obstacle->PerceptionSLBoundary().end_s() >
reference_line_.Length()) {
if (obstacle->PerceptionSLBoundary().end_s() > reference_line_.Length()) {
return true;
}
if (is_on_reference_line_ &&
path_obstacle->PerceptionSLBoundary().end_s() <
obstacle->PerceptionSLBoundary().end_s() <
sl_boundary_info_.adc_sl_boundary_.end_s() &&
reference_line_.IsOnLane(path_obstacle->PerceptionSLBoundary())) {
reference_line_.IsOnLane(obstacle->PerceptionSLBoundary())) {
return true;
}
return false;
......@@ -616,11 +613,11 @@ void ReferenceLineInfo::MakeMainMissionCompleteDecision(
int ReferenceLineInfo::MakeMainStopDecision(
DecisionResult* decision_result) const {
double min_stop_line_s = std::numeric_limits<double>::infinity();
const PathObstacle* stop_obstacle = nullptr;
const Obstacle* stop_obstacle = nullptr;
const ObjectStop* stop_decision = nullptr;
for (const auto path_obstacle : path_decision_.path_obstacles().Items()) {
const auto& object_decision = path_obstacle->LongitudinalDecision();
for (const auto* obstacle : path_decision_.obstacles().Items()) {
const auto& object_decision = obstacle->LongitudinalDecision();
if (!object_decision.has_stop()) {
continue;
}
......@@ -631,7 +628,7 @@ int ReferenceLineInfo::MakeMainStopDecision(
double stop_line_s = stop_line_sl.s();
if (stop_line_s < 0 || stop_line_s > reference_line_.Length()) {
AERROR << "Ignore object:" << path_obstacle->Id() << " fence route_s["
AERROR << "Ignore object:" << obstacle->Id() << " fence route_s["
<< stop_line_s << "] not in range[0, " << reference_line_.Length()
<< "]";
continue;
......@@ -640,7 +637,7 @@ int ReferenceLineInfo::MakeMainStopDecision(
// check stop_line_s vs adc_s
if (stop_line_s < min_stop_line_s) {
min_stop_line_s = stop_line_s;
stop_obstacle = path_obstacle;
stop_obstacle = obstacle;
stop_decision = &(object_decision.stop());
}
}
......@@ -667,23 +664,22 @@ int ReferenceLineInfo::MakeMainStopDecision(
void ReferenceLineInfo::SetObjectDecisions(
ObjectDecisions* object_decisions) const {
for (const auto path_obstacle : path_decision_.path_obstacles().Items()) {
if (!path_obstacle->HasNonIgnoreDecision()) {
for (const auto obstacle : path_decision_.obstacles().Items()) {
if (!obstacle->HasNonIgnoreDecision()) {
continue;
}
auto* object_decision = object_decisions->add_decision();
object_decision->set_id(path_obstacle->Id());
object_decision->set_perception_id(path_obstacle->PerceptionId());
if (path_obstacle->HasLateralDecision() &&
!path_obstacle->IsLateralIgnore()) {
object_decision->set_id(obstacle->Id());
object_decision->set_perception_id(obstacle->PerceptionId());
if (obstacle->HasLateralDecision() && !obstacle->IsLateralIgnore()) {
object_decision->add_object_decision()->CopyFrom(
path_obstacle->LateralDecision());
obstacle->LateralDecision());
}
if (path_obstacle->HasLongitudinalDecision() &&
!path_obstacle->IsLongitudinalIgnore()) {
if (obstacle->HasLongitudinalDecision() &&
!obstacle->IsLongitudinalIgnore()) {
object_decision->add_object_decision()->CopyFrom(
path_obstacle->LongitudinalDecision());
obstacle->LongitudinalDecision());
}
}
}
......@@ -747,10 +743,10 @@ void ReferenceLineInfo::MakeEStopDecision(
// set object decisions
ObjectDecisions* object_decisions =
decision_result->mutable_object_decision();
for (const auto path_obstacle : path_decision_.path_obstacles().Items()) {
for (const auto obstacle : path_decision_.obstacles().Items()) {
auto* object_decision = object_decisions->add_decision();
object_decision->set_id(path_obstacle->Id());
object_decision->set_perception_id(path_obstacle->PerceptionId());
object_decision->set_id(obstacle->Id());
object_decision->set_perception_id(obstacle->PerceptionId());
object_decision->add_object_decision()->mutable_avoid();
}
}
......
modules/planning/common/reference_line_info.h
浏览文件 @ f9a5bab3
......@@ -56,13 +56,13 @@ class ReferenceLineInfo {
const ReferenceLine& reference_line,
const hdmap::RouteSegments& segments);
bool Init(const std::vector<const PathObstacle*>& obstacles);
bool Init(const std::vector<const Obstacle*>& obstacles);
bool IsInited() const;
bool AddObstacles(const std::vector<const PathObstacle*>& obstacles);
PathObstacle* AddObstacle(const PathObstacle* obstacle);
bool AddObstacleHelper(const std::shared_ptr<PathObstacle>& obstacle);
bool AddObstacles(const std::vector<const Obstacle*>& obstacles);
Obstacle* AddObstacle(const Obstacle* obstacle);
bool AddObstacleHelper(const std::shared_ptr<Obstacle>& obstacle);
PathDecision* path_decision();
const PathDecision& path_decision() const;
......@@ -186,7 +186,7 @@ class ReferenceLineInfo {
bool CheckChangeLane() const;
void ExportTurnSignal(common::VehicleSignal* signal) const;
bool IsUnrelaventObstacle(PathObstacle* path_obstacle);
bool IsUnrelaventObstacle(const Obstacle* obstacle);
void MakeDecision(DecisionResult* decision_result) const;
int MakeMainStopDecision(DecisionResult* decision_result) const;
......
modules/planning/common/speed_profile_generator_test.cc
浏览文件 @ f9a5bab3
......@@ -36,7 +36,7 @@ TEST(SpeedProfileGeneratorTest, GenerateFallbackSpeedProfile) {
adc_planning_point.set_v(FLAGS_polynomial_speed_fallback_velocity + 0.1);
common::VehicleState vs;
const std::vector<const PathObstacle*> obstacles;
const std::vector<const Obstacle*> obstacles;
EgoInfo::Instance()->Update(adc_planning_point, vs, obstacles);
auto speed_data2 = SpeedProfileGenerator::GenerateFallbackSpeedProfile();
......
modules/planning/constraint_checker/BUILD
浏览文件 @ f9a5bab3
......@@ -51,7 +51,7 @@ cc_library(
"//modules/common/math:geometry",
"//modules/common/math:path_matcher",
"//modules/planning/common:frame",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:obstacle",
"//modules/planning/common/trajectory:discretized_trajectory",
"//modules/planning/lattice/behavior:path_time_graph",
"//modules/prediction/proto:prediction_proto",
......
modules/planning/constraint_checker/collision_checker.cc
浏览文件 @ f9a5bab3
......@@ -41,8 +41,8 @@ using apollo::common::math::PathMatcher;
using apollo::common::math::Vec2d;
CollisionChecker::CollisionChecker(
const std::vector<const PathObstacle*>& obstacles,
const double ego_vehicle_s, const double ego_vehicle_d,
const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
const double ego_vehicle_d,
const std::vector<PathPoint>& discretized_reference_line,
const ReferenceLineInfo* ptr_reference_line_info,
const std::shared_ptr<PathTimeGraph>& ptr_path_time_graph) {
......@@ -83,16 +83,16 @@ bool CollisionChecker::InCollision(
}
void CollisionChecker::BuildPredictedEnvironment(
const std::vector<const PathObstacle*>& obstacles,
const double ego_vehicle_s, const double ego_vehicle_d,
const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
const double ego_vehicle_d,
const std::vector<PathPoint>& discretized_reference_line) {
CHECK(predicted_bounding_rectangles_.empty());
// If the ego vehicle is in lane,
// then, ignore all obstacles from the same lane.
bool ego_vehicle_in_lane = IsEgoVehicleInLane(ego_vehicle_s, ego_vehicle_d);
std::vector<const PathObstacle*> obstacles_considered;
for (const PathObstacle* obstacle : obstacles) {
std::vector<const Obstacle*> obstacles_considered;
for (const Obstacle* obstacle : obstacles) {
if (obstacle->IsVirtual()) {
continue;
}
......@@ -109,7 +109,7 @@ void CollisionChecker::BuildPredictedEnvironment(
double relative_time = 0.0;
while (relative_time < FLAGS_trajectory_time_length) {
std::vector<Box2d> predicted_env;
for (const PathObstacle* obstacle : obstacles_considered) {
for (const Obstacle* obstacle : obstacles_considered) {
// If an obstacle has no trajectory, it is considered as static.
// Obstacle::GetPointAtTime has handled this case.
TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
......@@ -133,7 +133,7 @@ bool CollisionChecker::IsEgoVehicleInLane(const double ego_vehicle_s,
}
bool CollisionChecker::IsObstacleBehindEgoVehicle(
const PathObstacle* obstacle, const double ego_vehicle_s,
const Obstacle* obstacle, const double ego_vehicle_s,
const std::vector<PathPoint>& discretized_reference_line) {
double half_lane_width = FLAGS_default_reference_line_width * 0.5;
TrajectoryPoint point = obstacle->GetPointAtTime(0.0);
......
modules/planning/constraint_checker/collision_checker.h
浏览文件 @ f9a5bab3
......@@ -25,7 +25,7 @@
#include <vector>
#include "modules/common/math/box2d.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/lattice/behavior/path_time_graph.h"
......@@ -36,8 +36,8 @@ namespace planning {
class CollisionChecker {
public:
explicit CollisionChecker(
const std::vector<const PathObstacle*>& obstacles,
const double ego_vehicle_s, const double ego_vehicle_d,
const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
const double ego_vehicle_d,
const std::vector<common::PathPoint>& discretized_reference_line,
const ReferenceLineInfo* ptr_reference_line_info,
const std::shared_ptr<PathTimeGraph>& ptr_path_time_graph);
......@@ -46,15 +46,15 @@ class CollisionChecker {
private:
void BuildPredictedEnvironment(
const std::vector<const PathObstacle*>& obstacles,
const double ego_vehicle_s, const double ego_vehicle_d,
const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
const double ego_vehicle_d,
const std::vector<common::PathPoint>& discretized_reference_line);
bool IsEgoVehicleInLane(const double ego_vehicle_s,
const double ego_vehicle_d);
bool IsObstacleBehindEgoVehicle(
const PathObstacle* obstacle, const double ego_vehicle_s,
const Obstacle* obstacle, const double ego_vehicle_s,
const std::vector<apollo::common::PathPoint>& discretized_reference_line);
private:
......
modules/planning/lattice/behavior/BUILD
浏览文件 @ f9a5bab3
......@@ -32,7 +32,7 @@ cc_library(
"//modules/common/math:path_matcher",
"//modules/common/proto:pnc_point_proto",
"//modules/planning/common:frame",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:obstacle",
"//modules/planning/common:planning_gflags",
"//modules/planning/proto:lattice_structure_proto",
"//modules/planning/reference_line",
......@@ -50,7 +50,7 @@ cc_library(
deps = [
"//modules/common/math",
"//modules/common/math:path_matcher",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:obstacle",
"//modules/planning/proto:lattice_structure_proto",
"//modules/planning/proto:planning_proto",
],
......
modules/planning/lattice/behavior/path_time_graph.cc
浏览文件 @ f9a5bab3
......@@ -45,7 +45,7 @@ using apollo::common::TrajectoryPoint;
using apollo::perception::PerceptionObstacle;
PathTimeGraph::PathTimeGraph(
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const std::vector<PathPoint>& discretized_ref_points,
const ReferenceLineInfo* ptr_reference_line_info, const double s_start,
const double s_end, const double t_start, const double t_end,
......@@ -89,9 +89,9 @@ SLBoundary PathTimeGraph::ComputeObstacleBoundary(
}
void PathTimeGraph::SetupObstacles(
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const std::vector<PathPoint>& discretized_ref_points) {
for (const PathObstacle* obstacle : obstacles) {
for (const Obstacle* obstacle : obstacles) {
if (obstacle->IsVirtual()) {
continue;
}
......@@ -126,7 +126,7 @@ void PathTimeGraph::SetupObstacles(
}
void PathTimeGraph::SetStaticObstacle(
const PathObstacle* obstacle,
const Obstacle* obstacle,
const std::vector<PathPoint>& discretized_ref_points) {
const Polygon2d& polygon = obstacle->PerceptionPolygon();
......@@ -166,7 +166,7 @@ void PathTimeGraph::SetStaticObstacle(
}
void PathTimeGraph::SetDynamicObstacle(
const PathObstacle* obstacle,
const Obstacle* obstacle,
const std::vector<PathPoint>& discretized_ref_points) {
double relative_time = time_range_.first;
while (relative_time < time_range_.second) {
......
modules/planning/lattice/behavior/path_time_graph.h
浏览文件 @ f9a5bab3
......@@ -32,7 +32,7 @@
#include "modules/common/math/polygon2d.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/reference_line/reference_line.h"
......@@ -41,7 +41,7 @@ namespace planning {
class PathTimeGraph {
public:
PathTimeGraph(const std::vector<const PathObstacle*>& obstacles,
PathTimeGraph(const std::vector<const Obstacle*>& obstacles,
const std::vector<common::PathPoint>& discretized_ref_points,
const ReferenceLineInfo* ptr_reference_line_info,
const double s_start, const double s_end, const double t_start,
......@@ -73,7 +73,7 @@ class PathTimeGraph {
private:
void SetupObstacles(
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const std::vector<common::PathPoint>& discretized_ref_points);
SLBoundary ComputeObstacleBoundary(
......@@ -84,11 +84,11 @@ class PathTimeGraph {
const double t) const;
void SetStaticObstacle(
const PathObstacle* obstacle,
const Obstacle* obstacle,
const std::vector<common::PathPoint>& discretized_ref_points);
void SetDynamicObstacle(
const PathObstacle* obstacle,
const Obstacle* obstacle,
const std::vector<common::PathPoint>& discretized_ref_points);
void UpdateLateralBoundsByObstacle(
......
modules/planning/lattice/behavior/prediction_querier.cc
浏览文件 @ f9a5bab3
......@@ -29,7 +29,7 @@ namespace apollo {
namespace planning {
PredictionQuerier::PredictionQuerier(
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const std::shared_ptr<std::vector<common::PathPoint>>& ptr_reference_line)
: ptr_reference_line_(ptr_reference_line) {
for (const auto ptr_obstacle : obstacles) {
......@@ -42,7 +42,7 @@ PredictionQuerier::PredictionQuerier(
}
}
std::vector<const PathObstacle*> PredictionQuerier::GetObstacles() const {
std::vector<const Obstacle*> PredictionQuerier::GetObstacles() const {
return obstacles_;
}
......
modules/planning/lattice/behavior/prediction_querier.h
浏览文件 @ f9a5bab3
......@@ -25,7 +25,7 @@
#include <unordered_map>
#include <vector>
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
namespace apollo {
namespace planning {
......@@ -33,22 +33,22 @@ namespace planning {
class PredictionQuerier {
public:
explicit PredictionQuerier(
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const std::shared_ptr<std::vector<common::PathPoint>>&
ptr_reference_line);
virtual ~PredictionQuerier() = default;
std::vector<const PathObstacle*> GetObstacles() const;
std::vector<const Obstacle*> GetObstacles() const;
double ProjectVelocityAlongReferenceLine(const std::string& obstacle_id,
const double s,
const double t) const;
private:
std::unordered_map<std::string, const PathObstacle*> id_obstacle_map_;
std::unordered_map<std::string, const Obstacle*> id_obstacle_map_;
std::vector<const PathObstacle*> obstacles_;
std::vector<const Obstacle*> obstacles_;
std::shared_ptr<std::vector<common::PathPoint>> ptr_reference_line_;
};
......
modules/planning/navi/decider/navi_obstacle_decider.cc
浏览文件 @ f9a5bab3
......@@ -81,7 +81,7 @@ double NaviObstacleDecider::GetMinLaneWidth(
void NaviObstacleDecider::AddObstacleOffsetDirection(
const common::PathPoint& projection_point,
const std::vector<common::PathPoint>& path_data_points,
const PathObstacle* current_obstacle, const double proj_len, double* dist) {
const Obstacle* current_obstacle, const double proj_len, double* dist) {
Vec2d p1(0.0, 0.0);
Vec2d p2(0.0, 0.0);
......@@ -105,8 +105,7 @@ void NaviObstacleDecider::AddObstacleOffsetDirection(
}
bool NaviObstacleDecider::IsNeedFilterObstacle(
const PathObstacle* current_obstacle,
const PathPoint& vehicle_projection_point,
const Obstacle* current_obstacle, const PathPoint& vehicle_projection_point,
const std::vector<common::PathPoint>& path_data_points,
const common::VehicleState& vehicle_state,
PathPoint* projection_point_ptr) {
......@@ -135,7 +134,7 @@ bool NaviObstacleDecider::IsNeedFilterObstacle(
}
if ((obstacle_start_position - vehicle_frontedge_position) >
config_.safe_distance()) {
if (!PathObstacle::IsStaticObstacle(current_obstacle->Perception())) {
if (!Obstacle::IsStaticObstacle(current_obstacle->Perception())) {
if (current_obstacle->Perception().velocity().x() > 0.0) {
return is_filter;
}
......@@ -145,8 +144,8 @@ bool NaviObstacleDecider::IsNeedFilterObstacle(
return is_filter;
}
void NaviObstacleDecider::ProcessPathObstacle(
const std::vector<const PathObstacle*>& obstacles,
void NaviObstacleDecider::ProcessObstacle(
const std::vector<const Obstacle*>& obstacles,
const std::vector<common::PathPoint>& path_data_points,
const PathDecision& path_decision, const double min_lane_width,
const common::VehicleState& vehicle_state) {
......@@ -274,7 +273,7 @@ void NaviObstacleDecider::SmoothNudgeDistance(
}
double NaviObstacleDecider::GetNudgeDistance(
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const ReferenceLine& reference_line, const PathDecision& path_decision,
const std::vector<common::PathPoint>& path_data_points,
const common::VehicleState& vehicle_state, int* lane_obstacles_num) {
......@@ -306,8 +305,8 @@ double NaviObstacleDecider::GetNudgeDistance(
// Calculation of the number of current Lane obstacles
obstacle_lat_dist_.clear();
ProcessPathObstacle(obstacles, path_data_points, path_decision,
min_lane_width, vehicle_state);
ProcessObstacle(obstacles, path_data_points, path_decision, min_lane_width,
vehicle_state);
for (auto iter = obstacle_lat_dist_.begin(); iter != obstacle_lat_dist_.end();
iter++) {
auto actual_dist = GetObstacleActualOffsetDistance(
......@@ -370,7 +369,7 @@ void NaviObstacleDecider::KeepNudgePosition(const double nudge_dist,
void NaviObstacleDecider::GetUnsafeObstaclesInfo(
const std::vector<common::PathPoint>& path_data_points,
const std::vector<const PathObstacle*>& obstacles) {
const std::vector<const Obstacle*>& obstacles) {
// Find start point of the reference line.
double reference_line_y = path_data_points[0].y();
......
modules/planning/navi/decider/navi_obstacle_decider.h
浏览文件 @ f9a5bab3
......@@ -31,7 +31,7 @@
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/vec2d.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/navi/decider/navi_task.h"
/**
......@@ -66,7 +66,7 @@ class NaviObstacleDecider : public NaviTask {
* @return actual nudgable distance
*/
double GetNudgeDistance(
const std::vector<const PathObstacle *> &obstacles,
const std::vector<const Obstacle *> &obstacles,
const ReferenceLine &reference_line, const PathDecision &path_decision,
const std::vector<common::PathPoint> &path_data_points,
const common::VehicleState &vehicle_state, int *lane_obstacles_num);
......@@ -77,7 +77,7 @@ class NaviObstacleDecider : public NaviTask {
*/
void GetUnsafeObstaclesInfo(
const std::vector<common::PathPoint> &path_data_points,
const std::vector<const PathObstacle *> &obstacles);
const std::vector<const Obstacle *> &obstacles);
/**
* @brief Get unsafe obstacles' ID
......@@ -102,11 +102,11 @@ class NaviObstacleDecider : public NaviTask {
* @brief process path's obstacles info
* @return Number of obstacles in the current lane
*/
void ProcessPathObstacle(
const std::vector<const PathObstacle *> &obstacles,
const std::vector<common::PathPoint> &path_data_points,
const PathDecision &path_decision, const double min_lane_width,
const common::VehicleState &vehicle_state);
void ProcessObstacle(const std::vector<const Obstacle *> &obstacles,
const std::vector<common::PathPoint> &path_data_points,
const PathDecision &path_decision,
const double min_lane_width,
const common::VehicleState &vehicle_state);
/**
* @brief According to the relation between the obstacle and the path data,
......@@ -116,15 +116,14 @@ class NaviObstacleDecider : public NaviTask {
void AddObstacleOffsetDirection(
const common::PathPoint &projection_point,
const std::vector<common::PathPoint> &path_data_points,
const PathObstacle *current_obstacle, const double proj_len,
double *dist);
const Obstacle *current_obstacle, const double proj_len, double *dist);
/**
* @brief Remove safe obstacles
* @return whether filter the obstacle
*/
bool IsNeedFilterObstacle(
const PathObstacle *current_obstacle,
const Obstacle *current_obstacle,
const common::PathPoint &vehicle_projection_point,
const std::vector<common::PathPoint> &path_data_points,
const common::VehicleState &vehicle_state,
......
modules/planning/navi/decider/navi_obstacle_decider_test.cc
浏览文件 @ f9a5bab3
......@@ -40,7 +40,7 @@ namespace planning {
TEST(NaviObstacleDeciderTest, ComputeNudgeDist1) {
NaviObstacleDecider obstacle_decider;
std::vector<const PathObstacle*> vec_obstacle;
std::vector<const Obstacle*> vec_obstacle;
std::vector<common::PathPoint> vec_points;
PerceptionObstacle perception_obstacle;
PathDecision path_decision;
......@@ -52,7 +52,7 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist1) {
perception_obstacle.set_length(1.0);
perception_obstacle.mutable_position()->set_x(2.0);
perception_obstacle.mutable_position()->set_y(1.0);
PathObstacle b1("1", perception_obstacle);
Obstacle b1("1", perception_obstacle);
PathPoint p1 = MakePathPoint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p1.set_s(0.0);
......@@ -64,7 +64,7 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist1) {
obstacle_boundary.set_start_l(1.5);
b1.SetPerceptionSlBoundary(obstacle_boundary);
path_decision.AddPathObstacle(b1);
path_decision.AddObstacle(b1);
vehicle_state.set_linear_velocity(5.556);
int lane_obstacles_num = 0;
......@@ -82,7 +82,7 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist1) {
TEST(NaviObstacleDeciderTest, ComputeNudgeDist2) {
NaviObstacleDecider obstacle_decider;
std::vector<const PathObstacle*> vec_obstacle;
std::vector<const Obstacle*> vec_obstacle;
std::vector<common::PathPoint> vec_points;
PerceptionObstacle perception_obstacle;
PathDecision path_decision;
......@@ -94,9 +94,9 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist2) {
perception_obstacle.set_length(1.0);
perception_obstacle.mutable_position()->set_x(-2.0);
perception_obstacle.mutable_position()->set_y(1.0);
PathObstacle b1("1", perception_obstacle);
Obstacle b1("1", perception_obstacle);
b1.SetPerceptionSlBoundary(obstacle_boundary);
path_decision.AddPathObstacle(b1);
path_decision.AddObstacle(b1);
PathPoint p1 = MakePathPoint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p1.set_s(0.0);
......@@ -122,7 +122,7 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist2) {
TEST(NaviObstacleDeciderTest, ComputeNudgeDist3) {
NaviObstacleDecider obstacle_decider;
std::vector<const PathObstacle*> vec_obstacle;
std::vector<const Obstacle*> vec_obstacle;
std::vector<common::PathPoint> vec_points;
PerceptionObstacle perception_obstacle;
PathDecision path_decision;
......@@ -135,20 +135,20 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist3) {
perception_obstacle.set_length(1.0);
perception_obstacle.mutable_position()->set_x(3.0);
perception_obstacle.mutable_position()->set_y(0.0);
PathObstacle b1("1", perception_obstacle);
Obstacle b1("1", perception_obstacle);
obstacle_boundary.set_start_l(1.6);
b1.SetPerceptionSlBoundary(obstacle_boundary);
path_decision.AddPathObstacle(b1);
path_decision.AddObstacle(b1);
// obstacle 2
perception_obstacle.set_width(2.6);
perception_obstacle.set_length(1.0);
perception_obstacle.mutable_position()->set_x(4.0);
perception_obstacle.mutable_position()->set_y(0.0);
PathObstacle b2("2", perception_obstacle);
Obstacle b2("2", perception_obstacle);
obstacle_boundary.set_start_l(1.5);
b2.SetPerceptionSlBoundary(obstacle_boundary);
path_decision.AddPathObstacle(b2);
path_decision.AddObstacle(b2);
PathPoint p1 = MakePathPoint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p1.set_s(0.0);
......@@ -184,7 +184,7 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist3) {
TEST(NaviObstacleDeciderTest, ComputeNudgeDist4) {
NaviObstacleDecider obstacle_decider;
std::vector<const PathObstacle*> vec_obstacle;
std::vector<const Obstacle*> vec_obstacle;
std::vector<common::PathPoint> vec_points;
PerceptionObstacle perception_obstacle;
PathDecision path_decision;
......@@ -196,10 +196,10 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist4) {
perception_obstacle.set_length(1.0);
perception_obstacle.mutable_position()->set_x(-3.0);
perception_obstacle.mutable_position()->set_y(1.0);
PathObstacle b1("1", perception_obstacle);
Obstacle b1("1", perception_obstacle);
b1.SetPerceptionSlBoundary(obstacle_boundary);
path_decision.AddPathObstacle(b1);
path_decision.AddObstacle(b1);
PathPoint p1 = MakePathPoint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p1.set_s(0.0);
......@@ -224,7 +224,7 @@ TEST(NaviObstacleDeciderTest, ComputeNudgeDist4) {
TEST(NaviObstacleDeciderTest, GetUnsafeObstaclesID) {
NaviObstacleDecider obstacle_decider;
std::vector<const PathObstacle*> vec_obstacle;
std::vector<const Obstacle*> vec_obstacle;
std::vector<common::PathPoint> vec_points;
PerceptionObstacle perception_obstacle;
......@@ -235,7 +235,7 @@ TEST(NaviObstacleDeciderTest, GetUnsafeObstaclesID) {
perception_obstacle.mutable_position()->set_y(3.0);
perception_obstacle.mutable_velocity()->set_x(10.0);
perception_obstacle.mutable_velocity()->set_y(0.0);
PathObstacle b1("5", perception_obstacle);
Obstacle b1("5", perception_obstacle);
// obstacle 2
perception_obstacle.set_width(2.6);
......@@ -244,7 +244,7 @@ TEST(NaviObstacleDeciderTest, GetUnsafeObstaclesID) {
perception_obstacle.mutable_position()->set_y(-1.0);
perception_obstacle.mutable_velocity()->set_x(5.0);
perception_obstacle.mutable_velocity()->set_y(0.0);
PathObstacle b2("6", perception_obstacle);
Obstacle b2("6", perception_obstacle);
PathPoint p1 = MakePathPoint(0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p1.set_s(0.0);
......
modules/planning/navi/decider/navi_path_decider.cc
浏览文件 @ f9a5bab3
......@@ -99,7 +99,7 @@ Status NaviPathDecider::Execute(Frame* frame,
apollo::common::Status NaviPathDecider::Process(
const ReferenceLine& reference_line,
const common::TrajectoryPoint& init_point,
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
PathDecision* const path_decision, PathData* const path_data) {
CHECK_NOTNULL(path_decision);
CHECK_NOTNULL(path_data);
......@@ -304,8 +304,8 @@ bool NaviPathDecider::IsSafeChangeLane(const ReferenceLine& reference_line,
return false;
}
for (const auto* path_obstacle : path_decision.path_obstacles().Items()) {
const auto& sl_boundary = path_obstacle->PerceptionSLBoundary();
for (const auto* obstacle : path_decision.obstacles().Items()) {
const auto& sl_boundary = obstacle->PerceptionSLBoundary();
constexpr double kLateralShift = 6.0;
if (sl_boundary.start_l() < -kLateralShift ||
sl_boundary.end_l() > kLateralShift) {
......@@ -316,14 +316,12 @@ bool NaviPathDecider::IsSafeChangeLane(const ReferenceLine& reference_line,
constexpr double kForwardMinSafeDistance = 6.0;
constexpr double kBackwardMinSafeDistance = 8.0;
const double kForwardSafeDistance =
std::max(kForwardMinSafeDistance,
((vehicle_state_.linear_velocity() - path_obstacle->speed()) *
kSafeTime));
const double kBackwardSafeDistance =
std::max(kBackwardMinSafeDistance,
((path_obstacle->speed() - vehicle_state_.linear_velocity()) *
kSafeTime));
const double kForwardSafeDistance = std::max(
kForwardMinSafeDistance,
((vehicle_state_.linear_velocity() - obstacle->speed()) * kSafeTime));
const double kBackwardSafeDistance = std::max(
kBackwardMinSafeDistance,
((obstacle->speed() - vehicle_state_.linear_velocity()) * kSafeTime));
if (sl_boundary.end_s() >
adc_sl_boundary.start_s() - kBackwardSafeDistance &&
sl_boundary.start_s() <
......@@ -338,7 +336,7 @@ bool NaviPathDecider::IsSafeChangeLane(const ReferenceLine& reference_line,
double NaviPathDecider::NudgeProcess(
const ReferenceLine& reference_line,
const std::vector<common::PathPoint>& path_data_points,
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const PathDecision& path_decision,
const common::VehicleState& vehicle_state) {
double nudge_position_y = 0.0;
......
modules/planning/navi/decider/navi_path_decider.h
浏览文件 @ f9a5bab3
......@@ -80,11 +80,11 @@ class NaviPathDecider : public NaviTask {
* system
* @return Status::OK() if a suitable path is created; error otherwise.
*/
apollo::common::Status Process(
const ReferenceLine &reference_line,
const common::TrajectoryPoint &init_point,
const std::vector<const PathObstacle *> &obstacles,
PathDecision *const path_decision, PathData *const path_data);
apollo::common::Status Process(const ReferenceLine &reference_line,
const common::TrajectoryPoint &init_point,
const std::vector<const Obstacle *> &obstacles,
PathDecision *const path_decision,
PathData *const path_data);
/**
* @brief take a section of the reference line as the initial path trajectory.
......@@ -133,7 +133,7 @@ class NaviPathDecider : public NaviTask {
*/
double NudgeProcess(const ReferenceLine &reference_line,
const std::vector<common::PathPoint> &path_data_points,
const std::vector<const PathObstacle *> &obstacles,
const std::vector<const Obstacle *> &obstacles,
const PathDecision &path_decision,
const common::VehicleState &vehicle_state);
/**
......
modules/planning/navi/decider/navi_speed_decider.cc
浏览文件 @ f9a5bab3
......@@ -250,8 +250,8 @@ Status NaviSpeedDecider::Execute(Frame* frame,
Status NaviSpeedDecider::MakeSpeedDecision(
double start_v, double start_a, double start_da,
const std::vector<PathPoint>& path_points,
const std::vector<const PathObstacle*>& obstacles,
const std::function<const PathObstacle*(const std::string&)>& find_obstacle,
const std::vector<const Obstacle*>& obstacles,
const std::function<const Obstacle*(const std::string&)>& find_obstacle,
SpeedData* const speed_data) {
CHECK_NOTNULL(speed_data);
CHECK_GE(path_points.size(), 2);
......@@ -366,9 +366,8 @@ Status NaviSpeedDecider::AddPerceptionRangeConstraints() {
Status NaviSpeedDecider::AddObstaclesConstraints(
double vehicle_speed, double path_length,
const std::vector<PathPoint>& path_points,
const std::vector<const PathObstacle*>& obstacles,
const std::function<const PathObstacle*(const std::string&)>&
find_obstacle) {
const std::vector<const Obstacle*>& obstacles,
const std::function<const Obstacle*(const std::string&)>& find_obstacle) {
const auto& vehicle_config = VehicleConfigHelper::Instance()->GetConfig();
auto front_edge_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
......
modules/planning/navi/decider/navi_speed_decider.h
浏览文件 @ f9a5bab3
......@@ -81,8 +81,8 @@ class NaviSpeedDecider : public NaviTask {
apollo::common::Status MakeSpeedDecision(
double start_v, double start_a, double start_da,
const std::vector<common::PathPoint>& path_points,
const std::vector<const PathObstacle*>& obstacles,
const std::function<const PathObstacle*(const std::string& id)>&
const std::vector<const Obstacle*>& obstacles,
const std::function<const Obstacle*(const std::string& id)>&
find_obstacle,
SpeedData* const speed_data);
......@@ -104,8 +104,8 @@ class NaviSpeedDecider : public NaviTask {
apollo::common::Status AddObstaclesConstraints(
double vehicle_speed, double path_length,
const std::vector<common::PathPoint>& path_points,
const std::vector<const PathObstacle*>& obstacles,
const std::function<const PathObstacle*(const std::string& id)>&
const std::vector<const Obstacle*>& obstacles,
const std::function<const Obstacle*(const std::string& id)>&
find_obstacle);
/**
......
modules/planning/navi/decider/navi_speed_decider_test.cc
浏览文件 @ f9a5bab3
......@@ -67,8 +67,8 @@ TEST(NaviSpeedDeciderTest, CreateSpeedData) {
speed_decider.kappa_threshold_ = 0.0;
PerceptionObstacle perception_obstacle;
std::map<std::string, PathObstacle> obstacle_buf;
std::vector<const PathObstacle*> obstacles;
std::map<std::string, Obstacle> obstacle_buf;
std::vector<const Obstacle*> obstacles;
std::vector<PathPoint> path_points;
path_points.emplace_back(GenPathPoint(0.0));
......@@ -111,8 +111,8 @@ TEST(NaviSpeedDeciderTest, CreateSpeedDataForStaticObstacle) {
speed_decider.kappa_threshold_ = 0.0;
PerceptionObstacle perception_obstacle;
std::map<std::string, PathObstacle> obstacle_buf;
std::vector<const PathObstacle*> obstacles;
std::map<std::string, Obstacle> obstacle_buf;
std::vector<const Obstacle*> obstacles;
std::vector<PathPoint> path_points;
path_points.emplace_back(GenPathPoint(0.0));
......@@ -126,7 +126,7 @@ TEST(NaviSpeedDeciderTest, CreateSpeedDataForStaticObstacle) {
perception_obstacle.set_length(3.0);
perception_obstacle.set_width(3.0);
std::string id = "1";
obstacle_buf.emplace(id, PathObstacle(id, perception_obstacle));
obstacle_buf.emplace(id, Obstacle(id, perception_obstacle));
obstacles.emplace_back(&obstacle_buf[id]);
SpeedData speed_data;
......@@ -164,8 +164,8 @@ TEST(NaviSpeedDeciderTest, CreateSpeedDataForObstacles) {
speed_decider.kappa_threshold_ = 0.0;
PerceptionObstacle perception_obstacle;
std::map<std::string, PathObstacle> obstacle_buf;
std::vector<const PathObstacle*> obstacles;
std::map<std::string, Obstacle> obstacle_buf;
std::vector<const Obstacle*> obstacles;
std::vector<PathPoint> path_points;
path_points.emplace_back(GenPathPoint(0.0));
......@@ -179,7 +179,7 @@ TEST(NaviSpeedDeciderTest, CreateSpeedDataForObstacles) {
perception_obstacle.set_length(3.0);
perception_obstacle.set_width(3.0);
std::string id = "1";
obstacle_buf.emplace(id, PathObstacle(id, perception_obstacle));
obstacle_buf.emplace(id, Obstacle(id, perception_obstacle));
obstacles.emplace_back(&obstacle_buf[id]);
// obstacle2
......@@ -190,7 +190,7 @@ TEST(NaviSpeedDeciderTest, CreateSpeedDataForObstacles) {
perception_obstacle.set_length(3.0);
perception_obstacle.set_width(3.0);
id = "2";
obstacle_buf.emplace(id, PathObstacle(id, perception_obstacle));
obstacle_buf.emplace(id, Obstacle(id, perception_obstacle));
obstacles.emplace_back(&obstacle_buf[id]);
SpeedData speed_data;
......@@ -229,8 +229,8 @@ TEST(NaviSpeedDeciderTest, CreateSpeedDataForCurve) {
speed_decider.kappa_threshold_ = 0.0;
PerceptionObstacle perception_obstacle;
std::map<std::string, PathObstacle> obstacle_buf;
std::vector<const PathObstacle*> obstacles;
std::map<std::string, Obstacle> obstacle_buf;
std::vector<const Obstacle*> obstacles;
std::vector<PathPoint> path_points;
double s = 0.0;
......
modules/planning/open_space/BUILD
浏览文件 @ f9a5bab3
......@@ -9,7 +9,7 @@ cc_library(
deps = [
"//cyber/common:log",
"//modules/common/math",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:obstacle",
"//modules/planning/constraint_checker:collision_checker",
"//modules/planning/proto:planner_open_space_config_proto",
],
......@@ -193,7 +193,7 @@ cc_library(
deps = [
"//cyber/common:log",
"//modules/common/configs:vehicle_config_helper",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:obstacle",
"//modules/planning/open_space:open_space_utils",
"//modules/planning/proto:planner_open_space_config_proto",
],
......
modules/planning/open_space/distance_approach_problem_wrapper.cc
浏览文件 @ f9a5bab3
......@@ -138,9 +138,8 @@ class ObstacleContainer {
const double width, const int id) {
Vec2d obstacle_center(x, y);
Box2d obstacle_box(obstacle_center, heading, length, width);
std::unique_ptr<PathObstacle> obstacle =
PathObstacle::CreateStaticVirtualObstacles(std::to_string(id),
obstacle_box);
std::unique_ptr<Obstacle> obstacle = Obstacle::CreateStaticVirtualObstacles(
std::to_string(id), obstacle_box);
obstacles_list.Add(obstacle->Id(), *obstacle);
}
......
modules/planning/open_space/hybrid_a_star.h
浏览文件 @ f9a5bab3
......@@ -46,7 +46,7 @@
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/time/time.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/proto/planner_open_space_config.pb.h"
......
modules/planning/open_space/hybrid_a_star_test.cc
浏览文件 @ f9a5bab3
......@@ -23,7 +23,7 @@
#include "gtest/gtest.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/open_space/hybrid_a_star.h"
namespace apollo {
......@@ -60,9 +60,8 @@ TEST_F(HybridATest, test1) {
Result result;
Vec2d obstacle_center(0.0, 0.0);
Box2d obstacle_box(obstacle_center, 0.0, 5.0, 5.0);
std::unique_ptr<PathObstacle> obstacle =
PathObstacle::CreateStaticVirtualObstacles("a box in center",
obstacle_box);
std::unique_ptr<Obstacle> obstacle =
Obstacle::CreateStaticVirtualObstacles("a box in center", obstacle_box);
obstacles_list.Add(obstacle->Id(), *obstacle);
ASSERT_TRUE(
hybrid_test->Plan(sx, sy, sphi, ex, ey, ephi, &obstacles_list, &result));
......
modules/planning/open_space/hybrid_a_star_wrapper.cc
浏览文件 @ f9a5bab3
......@@ -32,9 +32,8 @@ class HybridAObstacleContainer {
const int id) {
Vec2d obstacle_center(x, y);
Box2d obstacle_box(obstacle_center, heading, length, width);
std::unique_ptr<PathObstacle> obstacle =
PathObstacle::CreateStaticVirtualObstacles(std::to_string(id),
obstacle_box);
std::unique_ptr<Obstacle> obstacle = Obstacle::CreateStaticVirtualObstacles(
std::to_string(id), obstacle_box);
obstacles_list.Add(obstacle->Id(), *obstacle);
}
ThreadSafeIndexedObstacles* GetObstacleList() { return &obstacles_list; }
......
modules/planning/planner/navi/navi_planner.cc
浏览文件 @ f9a5bab3
......@@ -204,15 +204,15 @@ Status NaviPlanner::PlanOnReferenceLine(
return Status(ErrorCode::PLANNING_ERROR, msg);
}
for (const auto* path_obstacle :
reference_line_info->path_decision()->path_obstacles().Items()) {
if (path_obstacle->IsVirtual()) {
for (const auto* obstacle :
reference_line_info->path_decision()->obstacles().Items()) {
if (obstacle->IsVirtual()) {
continue;
}
if (!path_obstacle->IsStatic()) {
if (!obstacle->IsStatic()) {
continue;
}
if (path_obstacle->LongitudinalDecision().has_stop()) {
if (obstacle->LongitudinalDecision().has_stop()) {
constexpr double kRefrenceLineStaticObsCost = 1e3;
reference_line_info->AddCost(kRefrenceLineStaticObsCost);
}
......@@ -241,13 +241,13 @@ void NaviPlanner::RecordObstacleDebugInfo(
auto ptr_debug = reference_line_info->mutable_debug();
const auto path_decision = reference_line_info->path_decision();
for (const auto path_obstacle : path_decision->path_obstacles().Items()) {
for (const auto obstacle : path_decision->obstacles().Items()) {
auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
obstacle_debug->set_id(path_obstacle->Id());
obstacle_debug->set_id(obstacle->Id());
obstacle_debug->mutable_sl_boundary()->CopyFrom(
path_obstacle->PerceptionSLBoundary());
const auto& decider_tags = path_obstacle->decider_tags();
const auto& decisions = path_obstacle->decisions();
obstacle->PerceptionSLBoundary());
const auto& decider_tags = obstacle->decider_tags();
const auto& decisions = obstacle->decisions();
if (decider_tags.size() != decisions.size()) {
AERROR << "decider_tags size: " << decider_tags.size()
<< " different from decisions size:" << decisions.size();
......
modules/planning/planner/public_road/BUILD
浏览文件 @ f9a5bab3
......@@ -11,8 +11,8 @@ cc_library(
"public_road_planner.h",
],
deps = [
"//external:gflags",
"//cyber/common:log",
"//external:gflags",
"//modules/common/proto:pnc_point_proto",
"//modules/common/status",
"//modules/common/time",
......@@ -33,11 +33,10 @@ cc_library(
"//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",
"//modules/planning/toolkits/optimizers/dp_st_speed:dp_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/path_decider",
"//modules/planning/toolkits/optimizers/poly_st_speed:poly_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/qp_spline_path",
"//modules/planning/toolkits/optimizers/qp_spline_st_speed:qp_spline_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/speed_decider",
"@eigen//:eigen",
"@eigen",
],
)
......
modules/planning/scenarios/lane_follow/BUILD
浏览文件 @ f9a5bab3
......@@ -42,7 +42,6 @@ cc_library(
"//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",
"//modules/planning/toolkits/optimizers/dp_st_speed:dp_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/path_decider",
"//modules/planning/toolkits/optimizers/poly_st_speed:poly_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/qp_piecewise_jerk_path:qp_piecewise_jerk_path_optimizer",
"//modules/planning/toolkits/optimizers/qp_spline_path",
"//modules/planning/toolkits/optimizers/qp_spline_st_speed:qp_spline_st_speed_optimizer",
......@@ -84,7 +83,6 @@ cc_library(
"//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",
"//modules/planning/toolkits/optimizers/dp_st_speed:dp_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/path_decider",
"//modules/planning/toolkits/optimizers/poly_st_speed:poly_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/qp_piecewise_jerk_path:qp_piecewise_jerk_path_optimizer",
"//modules/planning/toolkits/optimizers/qp_spline_path",
"//modules/planning/toolkits/optimizers/qp_spline_st_speed:qp_spline_st_speed_optimizer",
......
modules/planning/scenarios/lane_follow/lane_follow_scenario.cc
浏览文件 @ f9a5bab3
......@@ -41,7 +41,6 @@
#include "modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.h"
#include "modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.h"
#include "modules/planning/toolkits/optimizers/path_decider/path_decider.h"
#include "modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h"
#include "modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h"
#include "modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.h"
#include "modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.h"
......
modules/planning/scenarios/lane_follow/lane_follow_stage.cc
浏览文件 @ f9a5bab3
......@@ -40,7 +40,6 @@
#include "modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.h"
#include "modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.h"
#include "modules/planning/toolkits/optimizers/path_decider/path_decider.h"
#include "modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h"
#include "modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h"
#include "modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.h"
#include "modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.h"
......@@ -76,13 +75,13 @@ void LaneFollowStage::RecordObstacleDebugInfo(
auto ptr_debug = reference_line_info->mutable_debug();
const auto path_decision = reference_line_info->path_decision();
for (const auto path_obstacle : path_decision->path_obstacles().Items()) {
for (const auto obstacle : path_decision->obstacles().Items()) {
auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
obstacle_debug->set_id(path_obstacle->Id());
obstacle_debug->set_id(obstacle->Id());
obstacle_debug->mutable_sl_boundary()->CopyFrom(
path_obstacle->PerceptionSLBoundary());
const auto& decider_tags = path_obstacle->decider_tags();
const auto& decisions = path_obstacle->decisions();
obstacle->PerceptionSLBoundary());
const auto& decider_tags = obstacle->decider_tags();
const auto& decisions = obstacle->decisions();
if (decider_tags.size() != decisions.size()) {
AERROR << "decider_tags size: " << decider_tags.size()
<< " different from decisions size:" << decisions.size();
......@@ -210,33 +209,32 @@ Status LaneFollowStage::PlanOnReferenceLine(
// determine if there is a destination on reference line.
double dest_stop_s = -1.0;
for (const auto* path_obstacle :
reference_line_info->path_decision()->path_obstacles().Items()) {
if (path_obstacle->LongitudinalDecision().has_stop() &&
path_obstacle->LongitudinalDecision().stop().reason_code() ==
for (const auto* obstacle :
reference_line_info->path_decision()->obstacles().Items()) {
if (obstacle->LongitudinalDecision().has_stop() &&
obstacle->LongitudinalDecision().stop().reason_code() ==
STOP_REASON_DESTINATION) {
SLPoint dest_sl = GetStopSL(path_obstacle->LongitudinalDecision().stop(),
SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
reference_line_info->reference_line());
dest_stop_s = dest_sl.s();
}
}
for (const auto* path_obstacle :
reference_line_info->path_decision()->path_obstacles().Items()) {
if (path_obstacle->IsVirtual()) {
for (const auto* obstacle :
reference_line_info->path_decision()->obstacles().Items()) {
if (obstacle->IsVirtual()) {
continue;
}
if (!path_obstacle->IsStatic()) {
if (!obstacle->IsStatic()) {
continue;
}
if (path_obstacle->LongitudinalDecision().has_stop()) {
if (obstacle->LongitudinalDecision().has_stop()) {
bool add_stop_obstacle_cost = false;
if (dest_stop_s < 0.0) {
add_stop_obstacle_cost = true;
} else {
SLPoint stop_sl =
GetStopSL(path_obstacle->LongitudinalDecision().stop(),
reference_line_info->reference_line());
SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
reference_line_info->reference_line());
if (stop_sl.s() < dest_stop_s) {
add_stop_obstacle_cost = true;
}
......
modules/planning/scenarios/side_pass/side_pass_scenario.cc
浏览文件 @ f9a5bab3
......@@ -132,42 +132,42 @@ bool SidePassScenario::HasBlockingObstacle(
const PathDecision& path_decision) const {
// a blocking obstacle is an obstacle blocks the road when it is not blocked
// (by other obstacles or traffic rules)
for (const auto* path_obstacle : path_decision.path_obstacles().Items()) {
if (path_obstacle->IsVirtual() || !path_obstacle->IsStatic()) {
for (const auto* obstacle : path_decision.obstacles().Items()) {
if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
continue;
}
CHECK(path_obstacle->IsStatic());
CHECK(obstacle->IsStatic());
if (path_obstacle->PerceptionSLBoundary().start_s() <=
if (obstacle->PerceptionSLBoundary().start_s() <=
adc_sl_boundary.end_s()) { // such vehicles are behind the ego car.
continue;
}
constexpr double kAdcDistanceThreshold = 15.0; // unit: m
if (path_obstacle->PerceptionSLBoundary().start_s() >
if (obstacle->PerceptionSLBoundary().start_s() >
adc_sl_boundary.end_s() +
kAdcDistanceThreshold) { // vehicles are far away
continue;
}
if (path_obstacle->PerceptionSLBoundary().start_l() > 1.0 ||
path_obstacle->PerceptionSLBoundary().end_l() < -1.0) {
if (obstacle->PerceptionSLBoundary().start_l() > 1.0 ||
obstacle->PerceptionSLBoundary().end_l() < -1.0) {
continue;
}
bool is_blocked_by_others = false;
for (const auto* other_obstacle : path_decision.path_obstacles().Items()) {
if (other_obstacle->Id() == path_obstacle->Id()) {
for (const auto* other_obstacle : path_decision.obstacles().Items()) {
if (other_obstacle->Id() == obstacle->Id()) {
continue;
}
if (other_obstacle->PerceptionSLBoundary().start_l() >
path_obstacle->PerceptionSLBoundary().end_l() ||
obstacle->PerceptionSLBoundary().end_l() ||
other_obstacle->PerceptionSLBoundary().end_l() <
path_obstacle->PerceptionSLBoundary().start_l()) {
obstacle->PerceptionSLBoundary().start_l()) {
// not blocking the backside vehicle
continue;
}
double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -
path_obstacle->PerceptionSLBoundary().end_s();
obstacle->PerceptionSLBoundary().end_s();
if (delta_s < 0.0 || delta_s > kAdcDistanceThreshold) {
continue;
} else {
......
modules/planning/scenarios/side_pass/side_pass_stage.cc
浏览文件 @ f9a5bab3
......@@ -196,13 +196,13 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process(
bool all_far_away = true;
// Go through every obstacle, check if there is any in the no_obs_zone,
// which will used by the proceed_with_caution movement.
for (const auto* path_obstacle : path_decision.path_obstacles().Items()) {
if (path_obstacle->IsVirtual()) {
for (const auto* obstacle : path_decision.obstacles().Items()) {
if (obstacle->IsVirtual()) {
continue;
}
// Check the s-direction.
double obs_start_s = path_obstacle->PerceptionSLBoundary().start_s();
double obs_end_s = path_obstacle->PerceptionSLBoundary().end_s();
double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
if (obs_end_s < no_obs_zone_start_s || obs_start_s > no_obs_zone_end_s) {
continue;
}
......@@ -219,8 +219,8 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process(
std::abs(lane_left_width_at_end_s));
double lane_right_width = std::min(std::abs(lane_right_width_at_start_s),
std::abs(lane_right_width_at_end_s));
double obs_start_l = path_obstacle->PerceptionSLBoundary().start_l();
double obs_end_l = path_obstacle->PerceptionSLBoundary().end_l();
double obs_start_l = obstacle->PerceptionSLBoundary().start_l();
double obs_end_l = obstacle->PerceptionSLBoundary().end_l();
if (obs_start_l > lane_left_width || -obs_end_l > lane_right_width) {
continue;
}
......@@ -262,6 +262,7 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process(
* @brief:
* STAGE: SidePassDetectSafety
*/
Stage::StageStatus SidePassDetectSafety::Process(
const TrajectoryPoint& planning_start_point, Frame* frame) {
if (PlanningOnReferenceLine(planning_start_point, frame)) {
......@@ -274,9 +275,9 @@ Stage::StageStatus SidePassDetectSafety::Process(
const PathDecision& path_decision =
frame->reference_line_info().front().path_decision();
for (const auto* path_obstacle : path_decision.path_obstacles().Items()) {
if (path_obstacle->IsVirtual() &&
path_obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
for (const auto* obstacle : path_decision.obstacles().Items()) {
if (obstacle->IsVirtual() &&
obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
is_safe = false;
break;
}
......
modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.cc
浏览文件 @ f9a5bab3
......@@ -67,9 +67,9 @@ Stage::StageStatus StopSignUnprotectedPreStop::Process(
const PathDecision& path_decision = reference_line_info.path_decision();
auto& watch_vehicles = GetContext()->watch_vehicles;
for (const auto* path_obstacle : path_decision.path_obstacles().Items()) {
for (const auto* obstacle : path_decision.obstacles().Items()) {
// add to watch_vehicles if adc is still proceeding to stop sign
AddWatchVehicle(*path_obstacle, &watch_vehicles);
AddWatchVehicle(*obstacle, &watch_vehicles);
}
bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
......@@ -108,9 +108,9 @@ Stage::StageStatus StopSignUnprotectedStop::Process(
const auto& reference_line_info = frame->reference_line_info().front();
const PathDecision& path_decision = reference_line_info.path_decision();
for (const auto* path_obstacle : path_decision.path_obstacles().Items()) {
for (const auto* obstacle : path_decision.obstacles().Items()) {
// remove from watch_vehicles_ if adc is stopping/waiting at stop sign
RemoveWatchVehicle(*path_obstacle, watch_vehicle_ids, &watch_vehicles);
RemoveWatchVehicle(*obstacle, watch_vehicle_ids, &watch_vehicles);
}
return Stage::RUNNING;
......@@ -213,10 +213,10 @@ int StopSignUnprotectedScenario::GetAssociatedLanes(
* @brief: add a watch vehicle which arrives at stop sign ahead of adc
*/
int StopSignUnprotectedPreStop::AddWatchVehicle(
const PathObstacle& path_obstacle, StopSignLaneVehicles* watch_vehicles) {
const Obstacle& obstacle, StopSignLaneVehicles* watch_vehicles) {
CHECK_NOTNULL(watch_vehicles);
const PerceptionObstacle& perception_obstacle = path_obstacle.Perception();
const PerceptionObstacle& perception_obstacle = obstacle.Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle_type);
......@@ -308,12 +308,11 @@ int StopSignUnprotectedPreStop::AddWatchVehicle(
* @brief: remove a watch vehicle which not stopping at stop sign any more
*/
int StopSignUnprotectedStop::RemoveWatchVehicle(
const PathObstacle& path_obstacle,
const std::vector<std::string>& watch_vehicle_ids,
const Obstacle& obstacle, const std::vector<std::string>& watch_vehicle_ids,
StopSignLaneVehicles* watch_vehicles) {
CHECK_NOTNULL(watch_vehicles);
const PerceptionObstacle& perception_obstacle = path_obstacle.Perception();
const PerceptionObstacle& perception_obstacle = obstacle.Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle_type);
......@@ -354,7 +353,7 @@ int StopSignUnprotectedStop::RemoveWatchVehicle(
bool erase = false;
bool is_path_cross = !path_obstacle.reference_line_st_boundary().IsEmpty();
bool is_path_cross = !obstacle.reference_line_st_boundary().IsEmpty();
// check obstacle is on an associate lane guarded by stop sign
const std::string& obstable_lane_id = obstacle_lane.get()->id().id();
......
modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.h
浏览文件 @ f9a5bab3
......@@ -65,7 +65,7 @@ class StopSignUnprotectedStop : public Stage {
}
int RemoveWatchVehicle(
const PathObstacle& path_obstacle,
const Obstacle& obstacle,
const std::vector<std::string>& watch_vehicle_ids,
std::unordered_map<std::string, std::vector<std::string>>*
watch_vehicles);
......@@ -89,7 +89,7 @@ class StopSignUnprotectedPreStop : public Stage {
return GetContextAs<StopSignUnprotectedContext>();
}
int AddWatchVehicle(const PathObstacle& path_obstacle,
int AddWatchVehicle(const Obstacle& obstacle,
std::unordered_map<std::string, std::vector<std::string>>*
watch_vehicles);
bool CheckADCStop(const ReferenceLineInfo& reference_line_info);
......
modules/planning/toolkits/BUILD
浏览文件 @ f9a5bab3
......@@ -35,7 +35,6 @@ cc_library(
"//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",
"//modules/planning/toolkits/optimizers/dp_st_speed:dp_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/path_decider",
"//modules/planning/toolkits/optimizers/poly_st_speed:poly_st_speed_optimizer",
"//modules/planning/toolkits/optimizers/qp_piecewise_jerk_path:qp_piecewise_jerk_path_optimizer",
"//modules/planning/toolkits/optimizers/qp_spline_path",
"//modules/planning/toolkits/optimizers/qp_spline_st_speed:qp_spline_st_speed_optimizer",
......
modules/planning/toolkits/deciders/decider_creep.cc
浏览文件 @ f9a5bab3
......@@ -74,9 +74,9 @@ bool DeciderCreep::BuildStopDecision(const double stop_sign_overlap_end_s,
AERROR << "Failed to create obstacle [" << virtual_obstacle_id << "]";
return false;
}
PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
if (!stop_wall) {
AERROR << "Failed to create path_obstacle for: " << virtual_obstacle_id;
AERROR << "Failed to create obstacle for: " << virtual_obstacle_id;
return false;
}
......@@ -113,12 +113,12 @@ bool DeciderCreep::CheckCreepDone(const Frame& frame,
creep_stop_s - reference_line_info.AdcSlBoundary().end_s();
if (distance < creep_config.max_valid_stop_distance()) {
bool all_far_away = true;
for (auto* path_obstacle :
reference_line_info.path_decision().path_obstacles().Items()) {
if (path_obstacle->IsVirtual() || !path_obstacle->IsStatic()) {
for (auto* obstacle :
reference_line_info.path_decision().obstacles().Items()) {
if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
continue;
}
if (path_obstacle->reference_line_st_boundary().min_t() <
if (obstacle->reference_line_st_boundary().min_t() <
creep_config.min_boundary_t()) {
all_far_away = false;
break;
......
modules/planning/toolkits/deciders/decider_stop_sign.cc
浏览文件 @ f9a5bab3
......@@ -76,9 +76,9 @@ bool DeciderStopSign::BuildStopDecision(
AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
return -1;
}
PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
if (!stop_wall) {
AERROR << "Failed to create path_obstacle for: " << stop_wall_id;
AERROR << "Failed to create obstacle for: " << stop_wall_id;
return -1;
}
......
modules/planning/toolkits/deciders/side_pass_path_decider.cc
浏览文件 @ f9a5bab3
......@@ -101,11 +101,11 @@ bool SidePassPathDecider::GeneratePath(Frame *frame,
double nearest_obs_end_s = 0.0;
double nearest_obs_start_l = 0.0;
double nearest_obs_end_l = 0.0;
for (const auto *path_obstacle :
reference_line_info->path_decision()->path_obstacles().Items()) {
for (const auto *obstacle :
reference_line_info->path_decision()->obstacles().Items()) {
// Filter out obstacles that are behind ADC.
double obs_start_s = path_obstacle->PerceptionSLBoundary().start_s();
double obs_end_s = path_obstacle->PerceptionSLBoundary().end_s();
double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
if (obs_end_s < adc_end_s) {
continue;
}
......@@ -129,13 +129,13 @@ bool SidePassPathDecider::GeneratePath(Frame *frame,
std::abs(lane_left_width_at_end_s));
double lane_right_width = std::min(std::abs(lane_right_width_at_start_s),
std::abs(lane_right_width_at_end_s));
double obs_start_l = path_obstacle->PerceptionSLBoundary().start_l();
double obs_end_l = path_obstacle->PerceptionSLBoundary().end_l();
double obs_start_l = obstacle->PerceptionSLBoundary().start_l();
double obs_end_l = obstacle->PerceptionSLBoundary().end_l();
if (obs_start_l > lane_left_width || -obs_end_l > lane_right_width) {
continue;
}
// Only select vehicle obstacles.
if (path_obstacle->Perception().type() !=
if (obstacle->Perception().type() !=
perception::PerceptionObstacle::VEHICLE) {
continue;
}
......
modules/planning/toolkits/deciders/side_pass_safety.cc
浏览文件 @ f9a5bab3
......@@ -66,38 +66,38 @@ bool SidePassSafety::IsSafeSidePass(
Frame *frame, ReferenceLineInfo *const reference_line_info) {
double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s();
const auto &path_obstacles =
reference_line_info->path_decision()->path_obstacles().Items();
const auto &obstacles =
reference_line_info->path_decision()->obstacles().Items();
for (const auto *path_obstacle : path_obstacles) {
for (const auto *obstacle : obstacles) {
// ignore virtual and static obstacles
if (path_obstacle->IsVirtual() || path_obstacle->IsStatic()) {
if (obstacle->IsVirtual() || obstacle->IsStatic()) {
continue;
}
// ignore the obstacles behind adc and is able to catch adc after 5 secs.
if (path_obstacle->PerceptionSLBoundary().end_s() < adc_front_edge_s &&
path_obstacle->st_boundary().min_t() >
if (obstacle->PerceptionSLBoundary().end_s() < adc_front_edge_s &&
obstacle->st_boundary().min_t() >
Config().side_pass_safety_config().safe_duration_reach_ref_line()) {
continue;
}
// not overlapped obstacles
if (path_obstacle->st_boundary().IsEmpty()) {
if (obstacle->st_boundary().IsEmpty()) {
continue;
}
// if near side obstacles exist, it is unsafe.
if (std::abs(path_obstacle->PerceptionSLBoundary().start_l()) <=
if (std::abs(obstacle->PerceptionSLBoundary().start_l()) <=
Config()
.side_pass_safety_config()
.min_obstacle_lateral_distance() &&
std::abs(path_obstacle->PerceptionSLBoundary().end_l()) <=
std::abs(obstacle->PerceptionSLBoundary().end_l()) <=
Config()
.side_pass_safety_config()
.min_obstacle_lateral_distance()) {
return false;
}
// overlap is more than 5 meters
double s_range = path_obstacle->st_boundary().max_s() -
path_obstacle->st_boundary().min_s();
double s_range =
obstacle->st_boundary().max_s() - obstacle->st_boundary().min_s();
if (s_range > Config().side_pass_safety_config().max_overlap_s_range()) {
return false;
}
......
modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.cc
浏览文件 @ f9a5bab3
......@@ -55,7 +55,7 @@ Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
if (!dp_road_graph.FindPathTunnel(
init_point,
reference_line_info_->path_decision()->path_obstacles().Items(),
reference_line_info_->path_decision()->obstacles().Items(),
path_data)) {
AERROR << "Failed to find tunnel in road graph";
return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation");
......
modules/planning/toolkits/optimizers/dp_st_speed/BUILD
浏览文件 @ f9a5bab3
......@@ -28,7 +28,7 @@ cc_library(
":st_graph_point",
"//modules/common/proto:pnc_point_proto",
"//modules/planning/common:frame",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:obstacle",
"//modules/planning/common/speed:st_boundary",
"//modules/planning/proto:dp_st_speed_config_proto",
],
......@@ -51,8 +51,8 @@ cc_library(
"//modules/common/proto:geometry_proto",
"//modules/common/proto:pnc_point_proto",
"//modules/common/status",
"//modules/planning/common:obstacle",
"//modules/planning/common:path_decision",
"//modules/planning/common:path_obstacle",
"//modules/planning/common/speed:speed_data",
"//modules/planning/proto:planning_config_proto",
"//modules/planning/proto:planning_proto",
......
modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.cc
浏览文件 @ f9a5bab3
......@@ -33,7 +33,7 @@ constexpr float kInf = std::numeric_limits<float>::infinity();
}
DpStCost::DpStCost(const DpStSpeedConfig& config,
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const common::TrajectoryPoint& init_point)
: config_(config), obstacles_(obstacles), init_point_(init_point) {
int index = 0;
......@@ -53,7 +53,7 @@ DpStCost::DpStCost(const DpStSpeedConfig& config,
}
void DpStCost::AddToKeepClearRange(
const std::vector<const PathObstacle*>& obstacles) {
const std::vector<const Obstacle*>& obstacles) {
for (const auto& obstacle : obstacles) {
if (obstacle->st_boundary().IsEmpty()) {
continue;
......
modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.h
浏览文件 @ f9a5bab3
......@@ -28,7 +28,7 @@
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/dp_st_speed_config.pb.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/speed/st_boundary.h"
#include "modules/planning/common/speed/st_point.h"
#include "modules/planning/toolkits/optimizers/dp_st_speed/st_graph_point.h"
......@@ -39,44 +39,44 @@ namespace planning {
class DpStCost {
public:
explicit DpStCost(const DpStSpeedConfig& dp_st_speed_config,
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const common::TrajectoryPoint& init_point);
float GetObstacleCost(const StGraphPoint& point);
float GetReferenceCost(const STPoint& point,
const STPoint& reference_point) const;
const STPoint& reference_point) const;
float GetSpeedCost(const STPoint& first, const STPoint& second,
const float speed_limit) const;
const float speed_limit) const;
float GetAccelCostByTwoPoints(const float pre_speed, const STPoint& first,
const STPoint& second);
const STPoint& second);
float GetAccelCostByThreePoints(const STPoint& first, const STPoint& second,
const STPoint& third);
const STPoint& third);
float GetJerkCostByTwoPoints(const float pre_speed, const float pre_acc,
const STPoint& pre_point,
const STPoint& curr_point);
const STPoint& pre_point,
const STPoint& curr_point);
float GetJerkCostByThreePoints(const float first_speed,
const STPoint& first_point,
const STPoint& second_point,
const STPoint& third_point);
const STPoint& first_point,
const STPoint& second_point,
const STPoint& third_point);
float GetJerkCostByFourPoints(const STPoint& first, const STPoint& second,
const STPoint& third, const STPoint& fourth);
const STPoint& third, const STPoint& fourth);
private:
float GetAccelCost(const float accel);
float JerkCost(const float jerk);
void AddToKeepClearRange(const std::vector<const PathObstacle*>& obstacles);
void AddToKeepClearRange(const std::vector<const Obstacle*>& obstacles);
static void SortAndMergeRange(
std::vector<std::pair<float, float>>* keep_clear_range_);
bool InKeepClearRange(float s) const;
const DpStSpeedConfig& config_;
const std::vector<const PathObstacle*>& obstacles_;
const std::vector<const Obstacle*>& obstacles_;
const common::TrajectoryPoint& init_point_;
float unit_t_ = 0.0;
......
modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.cc
浏览文件 @ f9a5bab3
......@@ -63,7 +63,7 @@ bool CheckOverlapOnDpStGraph(const std::vector<const StBoundary*>& boundaries,
DpStGraph::DpStGraph(const StGraphData& st_graph_data,
const DpStSpeedConfig& dp_config,
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const common::TrajectoryPoint& init_point,
const SLBoundary& adc_sl_boundary)
: st_graph_data_(st_graph_data),
......
modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.h
浏览文件 @ f9a5bab3
......@@ -30,8 +30,8 @@
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/status/status.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/speed/st_point.h"
#include "modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.h"
......@@ -44,7 +44,7 @@ namespace planning {
class DpStGraph {
public:
DpStGraph(const StGraphData& st_graph_data, const DpStSpeedConfig& dp_config,
const std::vector<const PathObstacle*>& obstacles,
const std::vector<const Obstacle*>& obstacles,
const common::TrajectoryPoint& init_point,
const SLBoundary& adc_sl_boundary);
......@@ -84,7 +84,7 @@ class DpStGraph {
DpStSpeedConfig dp_st_speed_config_;
// obstacles based on the current reference line
const std::vector<const PathObstacle*>& obstacles_;
const std::vector<const Obstacle*>& obstacles_;
// vehicle configuration parameter
const common::VehicleParam& vehicle_param_ =
......
modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph_test.cc
浏览文件 @ f9a5bab3
......@@ -77,7 +77,7 @@ class DpStGraphTest : public ::testing::Test {
virtual void TearDown() {}
protected:
std::list<PathObstacle> path_obstacle_list_;
std::list<Obstacle> obstacle_list_;
StGraphData st_graph_data_;
SpeedLimit speed_limit_;
......@@ -89,12 +89,12 @@ class DpStGraphTest : public ::testing::Test {
};
TEST_F(DpStGraphTest, simple) {
PathObstacle o1;
Obstacle o1;
o1.SetId("o1");
path_obstacle_list_.push_back(o1);
obstacle_list_.push_back(o1);
std::vector<const PathObstacle*> obstacles_;
obstacles_.emplace_back(&(path_obstacle_list_.back()));
std::vector<const Obstacle*> obstacles_;
obstacles_.emplace_back(&(obstacle_list_.back()));
std::vector<STPoint> upper_points;
std::vector<STPoint> lower_points;
......@@ -108,7 +108,7 @@ TEST_F(DpStGraphTest, simple) {
point_pairs.emplace_back(lower_points[0], upper_points[0]);
point_pairs.emplace_back(lower_points[1], upper_points[1]);
path_obstacle_list_.back().SetStBoundary(StBoundary(point_pairs));
obstacle_list_.back().SetStBoundary(StBoundary(point_pairs));
std::vector<const StBoundary*> boundaries;
boundaries.push_back(&(obstacles_.back()->st_boundary()));
......
modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.cc
浏览文件 @ f9a5bab3
......@@ -55,7 +55,7 @@ bool DpStSpeedOptimizer::SearchStGraph(
SpeedData* speed_data, PathDecision* path_decision,
STGraphDebug* st_graph_debug) const {
std::vector<const StBoundary*> boundaries;
for (auto* obstacle : path_decision->path_obstacles().Items()) {
for (auto* obstacle : path_decision->obstacles().Items()) {
auto id = obstacle->Id();
if (!obstacle->st_boundary().IsEmpty()) {
if (obstacle->st_boundary().boundary_type() ==
......@@ -99,7 +99,7 @@ bool DpStSpeedOptimizer::SearchStGraph(
// step 2 perform graph search
SpeedLimit speed_limit;
if (!speed_limit_decider
.GetSpeedLimits(path_decision->path_obstacles(), &speed_limit)
.GetSpeedLimits(path_decision->obstacles(), &speed_limit)
.ok()) {
AERROR << "Getting speed limits for dp st speed optimizer failed!";
return false;
......@@ -108,10 +108,9 @@ bool DpStSpeedOptimizer::SearchStGraph(
const float path_length = path_data.discretized_path().Length();
StGraphData st_graph_data(boundaries, init_point_, speed_limit, path_length);
DpStGraph st_graph(
st_graph_data, dp_st_speed_config_,
reference_line_info_->path_decision()->path_obstacles().Items(),
init_point_, adc_sl_boundary_);
DpStGraph st_graph(st_graph_data, dp_st_speed_config_,
reference_line_info_->path_decision()->obstacles().Items(),
init_point_, adc_sl_boundary_);
if (!st_graph.Search(speed_data).ok()) {
AERROR << "failed to search graph with dynamic programming.";
......
modules/planning/toolkits/optimizers/path_decider/path_decider.cc
浏览文件 @ f9a5bab3
......@@ -88,35 +88,35 @@ bool PathDecider::MakeStaticObstacleDecision(
const double lateral_stop_radius =
half_width + FLAGS_static_decision_nudge_l_buffer;
for (const auto *path_obstacle : path_decision->path_obstacles().Items()) {
for (const auto *obstacle : path_decision->obstacles().Items()) {
bool is_bycycle_or_pedestrain =
(path_obstacle->Perception().type() ==
(obstacle->Perception().type() ==
perception::PerceptionObstacle::BICYCLE ||
path_obstacle->Perception().type() ==
obstacle->Perception().type() ==
perception::PerceptionObstacle::PEDESTRIAN);
if (!is_bycycle_or_pedestrain && !path_obstacle->IsStatic()) {
if (!is_bycycle_or_pedestrain && !obstacle->IsStatic()) {
continue;
}
if (path_obstacle->HasLongitudinalDecision() &&
path_obstacle->LongitudinalDecision().has_ignore() &&
path_obstacle->HasLateralDecision() &&
path_obstacle->LateralDecision().has_ignore()) {
if (obstacle->HasLongitudinalDecision() &&
obstacle->LongitudinalDecision().has_ignore() &&
obstacle->HasLateralDecision() &&
obstacle->LateralDecision().has_ignore()) {
continue;
}
if (path_obstacle->HasLongitudinalDecision() &&
path_obstacle->LongitudinalDecision().has_stop()) {
if (obstacle->HasLongitudinalDecision() &&
obstacle->LongitudinalDecision().has_stop()) {
// STOP decision
continue;
}
if (path_obstacle->HasLateralDecision() &&
path_obstacle->LateralDecision().has_sidepass()) {
if (obstacle->HasLateralDecision() &&
obstacle->LateralDecision().has_sidepass()) {
// SIDE_PASS decision
continue;
}
if (path_obstacle->reference_line_st_boundary().boundary_type() ==
if (obstacle->reference_line_st_boundary().boundary_type() ==
StBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
......@@ -125,14 +125,14 @@ bool PathDecider::MakeStaticObstacleDecision(
ObjectDecisionType object_decision;
object_decision.mutable_ignore();
const auto &sl_boundary = path_obstacle->PerceptionSLBoundary();
const auto &sl_boundary = obstacle->PerceptionSLBoundary();
if (sl_boundary.end_s() < frenet_points.front().s() ||
sl_boundary.start_s() > frenet_points.back().s()) {
path_decision->AddLongitudinalDecision(
"PathDecider/not-in-s", path_obstacle->Id(), object_decision);
path_decision->AddLateralDecision("PathDecider/not-in-s",
path_obstacle->Id(), object_decision);
path_decision->AddLongitudinalDecision("PathDecider/not-in-s",
obstacle->Id(), object_decision);
path_decision->AddLateralDecision("PathDecider/not-in-s", obstacle->Id(),
object_decision);
continue;
}
......@@ -141,26 +141,24 @@ bool PathDecider::MakeStaticObstacleDecision(
if (curr_l - lateral_radius > sl_boundary.end_l() ||
curr_l + lateral_radius < sl_boundary.start_l()) {
// ignore
path_decision->AddLateralDecision("PathDecider/not-in-l",
path_obstacle->Id(), object_decision);
path_decision->AddLateralDecision("PathDecider/not-in-l", obstacle->Id(),
object_decision);
} else if (curr_l - lateral_stop_radius < sl_boundary.end_l() &&
curr_l + lateral_stop_radius > sl_boundary.start_l()) {
// stop
*object_decision.mutable_stop() =
GenerateObjectStopDecision(*path_obstacle);
*object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);
if (path_decision->MergeWithMainStop(
object_decision.stop(), path_obstacle->Id(),
object_decision.stop(), obstacle->Id(),
reference_line_info_->reference_line(),
reference_line_info_->AdcSlBoundary())) {
path_decision->AddLongitudinalDecision(
"PathDecider/nearest-stop", path_obstacle->Id(), object_decision);
path_decision->AddLongitudinalDecision("PathDecider/nearest-stop",
obstacle->Id(), object_decision);
} else {
ObjectDecisionType object_decision;
object_decision.mutable_ignore();
path_decision->AddLongitudinalDecision("PathDecider/not-nearest-stop",
path_obstacle->Id(),
object_decision);
obstacle->Id(), object_decision);
}
} else if (FLAGS_enable_nudge_decision) {
// nudge
......@@ -170,14 +168,14 @@ bool PathDecider::MakeStaticObstacleDecision(
object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);
object_nudge_ptr->set_distance_l(FLAGS_nudge_distance_obstacle);
path_decision->AddLateralDecision("PathDecider/left-nudge",
path_obstacle->Id(), object_decision);
obstacle->Id(), object_decision);
} else {
// RIGHT_NUDGE
ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);
object_nudge_ptr->set_distance_l(-FLAGS_nudge_distance_obstacle);
path_decision->AddLateralDecision("PathDecider/right-nudge",
path_obstacle->Id(), object_decision);
obstacle->Id(), object_decision);
}
}
}
......@@ -186,16 +184,16 @@ bool PathDecider::MakeStaticObstacleDecision(
}
ObjectStop PathDecider::GenerateObjectStopDecision(
const PathObstacle &path_obstacle) const {
const Obstacle &obstacle) const {
ObjectStop object_stop;
double stop_distance = path_obstacle.MinRadiusStopDistance(
double stop_distance = obstacle.MinRadiusStopDistance(
VehicleConfigHelper::GetConfig().vehicle_param());
object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
object_stop.set_distance_s(-stop_distance);
const double stop_ref_s =
path_obstacle.PerceptionSLBoundary().start_s() - stop_distance;
obstacle.PerceptionSLBoundary().start_s() - stop_distance;
const auto stop_ref_point =
reference_line_info_->reference_line().GetReferencePoint(stop_ref_s);
object_stop.mutable_stop_point()->set_x(stop_ref_point.x());
......
modules/planning/toolkits/optimizers/path_decider/path_decider.h
浏览文件 @ f9a5bab3
......@@ -45,8 +45,7 @@ class PathDecider : public Task {
bool MakeStaticObstacleDecision(const PathData &path_data,
PathDecision *const path_decision);
ObjectStop GenerateObjectStopDecision(
const PathObstacle &path_obstacle) const;
ObjectStop GenerateObjectStopDecision(const Obstacle &obstacle) const;
};
} // namespace planning
......
modules/planning/toolkits/optimizers/poly_st_speed/BUILD 已删除 100644 → 0
浏览文件 @ e770e666
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "speed_profile_cost",
srcs = [
"speed_profile_cost.cc",
],
hdrs = [
"speed_profile_cost.h",
],
deps = [
"//modules/planning/math/curve1d:quartic_polynomial_curve1d",
"//modules/planning/proto:planning_config_proto",
"//modules/planning/proto:poly_st_speed_config_proto",
"//modules/planning/toolkits/optimizers:speed_optimizer",
"//modules/planning/toolkits/optimizers/st_graph:st_boundary_mapper",
],
)
cc_library(
name = "poly_st_graph",
srcs = [
"poly_st_graph.cc",
],
hdrs = [
"poly_st_graph.h",
],
deps = [
":speed_profile_cost",
"//cyber/common:log",
"//modules/common/configs:vehicle_config_helper",
"//modules/common/configs/proto:vehicle_config_proto",
"//modules/common/proto:pnc_point_proto",
"//modules/localization/proto:pose_proto",
"//modules/map/hdmap",
"//modules/planning/math/curve1d:quartic_polynomial_curve1d",
"//modules/planning/proto:planning_config_proto",
"//modules/planning/proto:poly_st_speed_config_proto",
"//modules/planning/toolkits/optimizers:speed_optimizer",
"//modules/planning/toolkits/optimizers/st_graph:st_boundary_mapper",
],
)
cc_library(
name = "poly_st_speed_optimizer",
srcs = [
"poly_st_speed_optimizer.cc",
],
hdrs = [
"poly_st_speed_optimizer.h",
],
deps = [
":poly_st_graph",
"//cyber/common:log",
"//modules/common/configs:vehicle_config_helper",
"//modules/common/configs/proto:vehicle_config_proto",
"//modules/common/proto:pnc_point_proto",
"//modules/localization/proto:pose_proto",
"//modules/map/hdmap",
"//modules/planning/proto:planning_config_proto",
"//modules/planning/proto:poly_st_speed_config_proto",
"//modules/planning/toolkits/optimizers:speed_optimizer",
"//modules/planning/toolkits/optimizers/st_graph:st_boundary_mapper",
],
)
cpplint()
modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.cc 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* 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
**/
#include "modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h"
#include <algorithm>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h"
#include "modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h"
namespace apollo {
namespace planning {
namespace {
constexpr double kEpsilon = 1e-6;
}
using apollo::common::ErrorCode;
using apollo::common::Status;
PolyStGraph::PolyStGraph(const PolyStSpeedConfig &config,
const ReferenceLineInfo *reference_line_info,
const SpeedLimit &speed_limit)
: config_(config),
reference_line_info_(reference_line_info),
reference_line_(reference_line_info->reference_line()),
speed_limit_(speed_limit) {}
bool PolyStGraph::FindStTunnel(
const common::TrajectoryPoint &init_point,
const std::vector<const PathObstacle *> &obstacles,
SpeedData *const speed_data) {
CHECK_NOTNULL(speed_data);
// set init point
init_point_ = init_point;
unit_s_ = std::fmax(0.1, init_point_.v() / 4.0);
// sample end points
std::vector<std::vector<STPoint>> points;
if (!SampleStPoints(&points)) {
AERROR << "Fail to sample st points.";
return false;
}
PolyStGraphNode min_cost_node;
if (!GenerateMinCostSpeedProfile(points, obstacles, &min_cost_node)) {
AERROR << "Fail to search min cost speed profile.";
return false;
}
ADEBUG << "min_cost_node s = " << min_cost_node.st_point.s()
<< ", t = " << min_cost_node.st_point.t();
speed_data->Clear();
constexpr double delta_t = 0.1; // output resolution, in seconds
const auto curve = min_cost_node.speed_profile;
for (double t = 0.0; t < planning_time_; t += delta_t) {
const double s = curve.Evaluate(0, t);
const double v = curve.Evaluate(1, t);
const double a = curve.Evaluate(2, t);
const double da = curve.Evaluate(3, t);
speed_data->AppendSpeedPoint(s, t, v, a, da);
}
return true;
}
bool PolyStGraph::GenerateMinCostSpeedProfile(
const std::vector<std::vector<STPoint>> &points,
const std::vector<const PathObstacle *> &obstacles,
PolyStGraphNode *const min_cost_node) {
CHECK_NOTNULL(min_cost_node);
PolyStGraphNode start_node = {STPoint(0.0, 0.0), init_point_.v(),
init_point_.a()};
SpeedProfileCost cost(config_, obstacles, speed_limit_, init_point_);
double min_cost = std::numeric_limits<double>::max();
for (const auto &level : points) {
for (const auto &st_point : level) {
const double speed_limit = speed_limit_.GetSpeedLimitByS(st_point.s());
constexpr int num_speed = 10;
for (double v = 0; v < speed_limit + kEpsilon;
v += speed_limit / num_speed) {
PolyStGraphNode node = {st_point, v, 0.0};
node.speed_profile = QuarticPolynomialCurve1d(
0.0, start_node.speed, start_node.accel, node.st_point.s(),
node.speed, node.st_point.t());
const double c =
cost.Calculate(node.speed_profile, st_point.t(), min_cost);
if (c < min_cost) {
*min_cost_node = node;
min_cost = c;
}
}
}
}
return true;
}
bool PolyStGraph::SampleStPoints(
std::vector<std::vector<STPoint>> *const points) {
CHECK_NOTNULL(points);
constexpr double start_t = 6.0;
constexpr double start_s = 0.0;
for (double t = start_t; t <= planning_time_; t += unit_t_) {
std::vector<STPoint> level_points;
for (double s = start_s; s < planning_distance_ + kEpsilon; s += unit_s_) {
level_points.emplace_back(s, t);
}
points->push_back(std::move(level_points));
}
return true;
}
} // namespace planning
} // namespace apollo
modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* 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
**/
#pragma once
#include <limits>
#include <string>
#include <utility>
#include <vector>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/poly_st_speed_config.pb.h"
#include "modules/common/status/status.h"
#include "modules/planning/common/path/path_data.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h"
#include "modules/planning/reference_line/reference_point.h"
namespace apollo {
namespace planning {
class PolyStGraph {
public:
explicit PolyStGraph(const PolyStSpeedConfig &config,
const ReferenceLineInfo *reference_line_info,
const SpeedLimit &speed_limit);
~PolyStGraph() = default;
bool FindStTunnel(const common::TrajectoryPoint &init_point,
const std::vector<const PathObstacle *> &obstacles,
SpeedData *const speed_data);
private:
struct PolyStGraphNode {
public:
PolyStGraphNode() = default;
PolyStGraphNode(const STPoint &point_st, const double speed,
const double accel)
: st_point(point_st), speed(speed), accel(accel) {}
STPoint st_point;
double speed = 0.0;
double accel = 0.0;
QuarticPolynomialCurve1d speed_profile;
};
bool GenerateMinCostSpeedProfile(
const std::vector<std::vector<STPoint>> &points,
const std::vector<const PathObstacle *> &obstacles,
PolyStGraphNode *const min_cost_node);
bool SampleStPoints(std::vector<std::vector<STPoint>> *const points);
private:
PolyStSpeedConfig config_;
common::TrajectoryPoint init_point_;
const ReferenceLineInfo *reference_line_info_ = nullptr;
const ReferenceLine &reference_line_;
const SpeedLimit &speed_limit_;
double unit_t_ = 1.0;
double unit_s_ = 5.0;
double planning_distance_ = 100.0;
double planning_time_ = 6.0;
};
} // namespace planning
} // namespace apollo
modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.cc 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* 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
**/
#include "modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h"
#include <algorithm>
#include <string>
#include <utility>
#include <vector>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/file.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h"
#include "modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.h"
#include "modules/planning/toolkits/optimizers/st_graph/st_graph_data.h"
namespace apollo {
namespace planning {
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::planning_internal::STGraphDebug;
PolyStSpeedOptimizer::PolyStSpeedOptimizer(const TaskConfig& config)
: SpeedOptimizer(config) {
SetName("PolyStSpeedOptimizer");
CHECK(config.has_poly_st_speed_config());
poly_st_speed_config_ = config.poly_st_speed_config();
st_boundary_config_ = poly_st_speed_config_.st_boundary_config();
}
Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
const PathData& path_data,
const TrajectoryPoint& init_point,
const ReferenceLine& reference_line,
const SpeedData& reference_speed_data,
PathDecision* const path_decision,
SpeedData* const speed_data) {
if (reference_line_info_->ReachedDestination()) {
return Status::OK();
}
if (path_data.discretized_path().NumOfPoints() == 0) {
std::string msg("Empty path data");
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
StBoundaryMapper boundary_mapper(adc_sl_boundary, st_boundary_config_,
reference_line, path_data,
poly_st_speed_config_.total_path_length(),
poly_st_speed_config_.total_time(),
reference_line_info_->IsChangeLanePath());
for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {
DCHECK(path_obstacle->HasLongitudinalDecision());
}
// step 1 get boundaries
path_decision->EraseStBoundaries();
if (boundary_mapper.CreateStBoundary(path_decision).code() ==
ErrorCode::PLANNING_ERROR) {
return Status(ErrorCode::PLANNING_ERROR,
"Mapping obstacle for qp st speed optimizer failed!");
}
for (const auto* obstacle : path_decision->path_obstacles().Items()) {
auto id = obstacle->Id();
auto* mutable_obstacle = path_decision->Find(id);
if (!obstacle->st_boundary().IsEmpty()) {
mutable_obstacle->SetBlockingObstacle(true);
} else {
path_decision->SetStBoundary(
id, path_decision->Find(id)->reference_line_st_boundary());
}
}
SpeedLimitDecider speed_limit_decider(adc_sl_boundary, st_boundary_config_,
reference_line, path_data);
SpeedLimit speed_limits;
if (speed_limit_decider.GetSpeedLimits(path_decision->path_obstacles(),
&speed_limits) != Status::OK()) {
return Status(ErrorCode::PLANNING_ERROR,
"GetSpeedLimits for qp st speed optimizer failed!");
}
// step 2 perform graph search
// make a poly_st_graph and perform search here.
PolyStGraph poly_st_graph(poly_st_speed_config_, reference_line_info_,
speed_limits);
auto ret = poly_st_graph.FindStTunnel(
init_point,
reference_line_info_->path_decision()->path_obstacles().Items(),
speed_data);
if (!ret) {
return Status(ErrorCode::PLANNING_ERROR,
"Fail to find st tunnel in PolyStGraph.");
}
return Status::OK();
}
} // namespace planning
} // namespace apollo
modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* 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
**/
#pragma once
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/planning/proto/planning_config.pb.h"
#include "modules/planning/proto/poly_st_speed_config.pb.h"
#include "modules/planning/proto/st_boundary_config.pb.h"
#include "modules/planning/toolkits/optimizers/speed_optimizer.h"
#include "modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.h"
namespace apollo {
namespace planning {
class PolyStSpeedOptimizer : public SpeedOptimizer {
public:
explicit PolyStSpeedOptimizer(const TaskConfig& config);
private:
common::Status Process(const SLBoundary& adc_sl_boundary,
const PathData& path_data,
const apollo::common::TrajectoryPoint& init_point,
const ReferenceLine& reference_line,
const SpeedData& reference_speed_data,
PathDecision* const path_decision,
SpeedData* const speed_data) override;
PolyStSpeedConfig poly_st_speed_config_;
StBoundaryConfig st_boundary_config_;
};
} // namespace planning
} // namespace apollo
modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.cc 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* 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
**/
#include "modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h"
#include <limits>
#include "modules/planning/common/planning_gflags.h"
namespace apollo {
namespace planning {
namespace {
constexpr auto kInfCost = std::numeric_limits<double>::infinity();
constexpr double kEpsilon = 1e-6;
} // namespace
using apollo::common::TrajectoryPoint;
SpeedProfileCost::SpeedProfileCost(
const PolyStSpeedConfig &config,
const std::vector<const PathObstacle *> &obstacles,
const SpeedLimit &speed_limit, const common::TrajectoryPoint &init_point)
: config_(config),
obstacles_(obstacles),
speed_limit_(speed_limit),
init_point_(init_point) {}
double SpeedProfileCost::Calculate(const QuarticPolynomialCurve1d &curve,
const double end_time,
const double curr_min_cost) const {
double cost = 0.0;
constexpr double kDeltaT = 0.5;
for (double t = kDeltaT; t < end_time + kEpsilon; t += kDeltaT) {
if (cost > curr_min_cost) {
return cost;
}
cost += CalculatePointCost(curve, t);
}
return cost;
}
double SpeedProfileCost::CalculatePointCost(
const QuarticPolynomialCurve1d &curve, const double t) const {
const double s = curve.Evaluate(0, t);
const double v = curve.Evaluate(1, t);
const double a = curve.Evaluate(2, t);
const double da = curve.Evaluate(3, t);
if (s < 0.0) {
return kInfCost;
}
const double speed_limit = speed_limit_.GetSpeedLimitByS(s);
if (v < 0.0 || v > speed_limit * (1.0 + config_.speed_limit_buffer())) {
return kInfCost;
}
if (a > config_.preferred_accel() || a < config_.preferred_decel()) {
return kInfCost;
}
double cost = 0.0;
for (const auto *obstacle : obstacles_) {
auto boundary = obstacle->st_boundary();
const double kIgnoreDistance = 100.0;
if (boundary.min_s() > kIgnoreDistance) {
continue;
}
if (t < boundary.min_t() || t > boundary.max_t()) {
continue;
}
if (obstacle->IsBlockingObstacle() &&
boundary.IsPointInBoundary(STPoint(s, t))) {
return kInfCost;
}
double s_upper = 0.0;
double s_lower = 0.0;
boundary.GetBoundarySRange(t, &s_upper, &s_lower);
if (s < s_lower) {
const double len = v * FLAGS_follow_time_buffer;
if (s + len < s_lower) {
continue;
} else {
cost += config_.obstacle_weight() * std::pow((len - s_lower + s), 2);
}
} else if (s > s_upper) {
const double kSafeDistance = 15.0; // or calculated from velocity
if (s > s_upper + kSafeDistance) {
continue;
} else {
cost += config_.obstacle_weight() *
std::pow((kSafeDistance + s_upper - s), 2);
}
} else {
if (!obstacle->IsBlockingObstacle()) {
cost += config_.unblocking_obstacle_cost();
}
}
}
cost += config_.speed_weight() * std::pow((v - speed_limit), 2);
cost += config_.jerk_weight() * std::pow(da, 2);
ADEBUG << "t = " << t << ", s = " << s << ", v = " << v << ", a = " << a
<< ", da = " << da << ", cost = " << cost;
return cost;
}
} // namespace planning
} // namespace apollo
modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* 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
**/
#pragma once
#include <vector>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/poly_st_speed_config.pb.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h"
namespace apollo {
namespace planning {
class SpeedProfileCost {
public:
explicit SpeedProfileCost(const PolyStSpeedConfig &config,
const std::vector<const PathObstacle *> &obstacles,
const SpeedLimit &speed_limit,
const common::TrajectoryPoint &init_point);
double Calculate(const QuarticPolynomialCurve1d &curve, const double end_time,
const double curr_min_cost) const;
private:
double CalculatePointCost(const QuarticPolynomialCurve1d &curve,
const double t) const;
const PolyStSpeedConfig config_;
const std::vector<const PathObstacle *> &obstacles_;
const SpeedLimit &speed_limit_;
const common::TrajectoryPoint &init_point_;
};
} // namespace planning
} // namespace apollo
modules/planning/toolkits/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc
浏览文件 @ f9a5bab3
......@@ -90,8 +90,8 @@ apollo::common::Status PolyVTSpeedOptimizer::Execute(
poly_vt_config.total_s(), poly_vt_config.total_time(),
reference_line_info->IsChangeLanePath());
for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {
DCHECK(path_obstacle->HasLongitudinalDecision());
for (const auto* obstacle : path_decision->obstacles().Items()) {
DCHECK(obstacle->HasLongitudinalDecision());
}
// step 1 : get boundaries
path_decision->EraseStBoundaries();
......@@ -101,7 +101,7 @@ apollo::common::Status PolyVTSpeedOptimizer::Execute(
"Mapping obstacle for qp st speed optimizer failed!");
}
for (const auto* obstacle : path_decision->path_obstacles().Items()) {
for (const auto* obstacle : path_decision->obstacles().Items()) {
auto id = obstacle->Id();
auto* mutable_obstacle = path_decision->Find(id);
......@@ -116,7 +116,7 @@ apollo::common::Status PolyVTSpeedOptimizer::Execute(
SpeedLimitDecider speed_limit_decider(adc_sl_boundary, st_boundary_config_,
reference_line, path_data);
SpeedLimit speed_limits;
if (speed_limit_decider.GetSpeedLimits(path_decision->path_obstacles(),
if (speed_limit_decider.GetSpeedLimits(path_decision->obstacles(),
&speed_limits) != Status::OK()) {
return Status(ErrorCode::PLANNING_ERROR,
"GetSpeedLimits for qp st speed optimizer failed!");
......
modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/BUILD
浏览文件 @ f9a5bab3
......@@ -18,8 +18,8 @@ cc_library(
"//modules/common/proto:pnc_point_proto",
"//modules/common/util",
"//modules/map/proto:map_proto",
"//modules/planning/common:obstacle",
"//modules/planning/common:path_decision",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:planning_gflags",
"//modules/planning/common/path:discretized_path",
"//modules/planning/common/path:frenet_frame_path",
......
modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc
浏览文件 @ f9a5bab3
......@@ -80,7 +80,7 @@ QpPiecewiseJerkPathOptimizer::GetLateralBounds(
const SLBoundary& adc_sl, const common::FrenetFramePoint& frenet_point,
const double qp_delta_s, double path_length,
const ReferenceLine& reference_line,
const std::vector<const PathObstacle*>& obstacles) {
const std::vector<const Obstacle*>& obstacles) {
const auto& qp_config = config_.qp_piecewise_jerk_path_config();
int size = std::max(2, static_cast<int>(path_length / qp_delta_s));
const double buffered_adc_width =
......@@ -128,14 +128,14 @@ QpPiecewiseJerkPathOptimizer::GetLateralBounds(
return lateral_bounds.begin() + index;
};
for (const auto* path_obstacle :
reference_line_info_->path_decision()->path_obstacles().Items()) {
for (const auto* obstacle :
reference_line_info_->path_decision()->obstacles().Items()) {
// only takes care of static obstacles
if (!path_obstacle->IsStatic()) {
if (!obstacle->IsStatic()) {
continue;
}
// ignore obstacles that are not in longitudinal range.
const auto& obstacle_sl = path_obstacle->PerceptionSLBoundary();
const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
if (obstacle_sl.end_s() < start_s ||
obstacle_sl.start_s() > accumulated_s) {
continue;
......@@ -220,7 +220,7 @@ Status QpPiecewiseJerkPathOptimizer::Process(
qp_config.min_look_ahead_distance());
auto lateral_bounds = GetLateralBounds(
adc_sl, frenet_point, qp_delta_s, path_length, reference_line,
reference_line_info_->path_decision()->path_obstacles().Items());
reference_line_info_->path_decision()->obstacles().Items());
auto lateral_second_order_derivative_bounds =
GetLateralSecondOrderDerivativeBounds(init_point, qp_delta_s);
......
modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h
浏览文件 @ f9a5bab3
......@@ -50,7 +50,7 @@ class QpPiecewiseJerkPathOptimizer : public PathOptimizer {
const SLBoundary& adc_sl, const common::FrenetFramePoint& frenet_point,
const double qp_delta_s, double path_length,
const ReferenceLine& reference_line,
const std::vector<const PathObstacle*>& obstacles);
const std::vector<const Obstacle*>& obstacles);
std::vector<std::tuple<double, double, double>>
GetLateralSecondOrderDerivativeBounds(
......
modules/planning/toolkits/optimizers/qp_spline_path/BUILD
浏览文件 @ f9a5bab3
......@@ -22,8 +22,8 @@ cc_library(
"//modules/common/proto:pnc_point_proto",
"//modules/common/util",
"//modules/map/proto:map_proto",
"//modules/planning/common:obstacle",
"//modules/planning/common:path_decision",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:planning_gflags",
"//modules/planning/common/path:discretized_path",
"//modules/planning/common/path:frenet_frame_path",
......
modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.cc
浏览文件 @ f9a5bab3
......@@ -67,8 +67,7 @@ QpFrenetFrame::QpFrenetFrame(const ReferenceLine& reference_line,
DCHECK_GE(evaluated_s_.size(), 2);
}
bool QpFrenetFrame::Init(
const std::vector<const PathObstacle*>& path_obstacles) {
bool QpFrenetFrame::Init(const std::vector<const Obstacle*>& obstacles) {
if (!CalculateDiscretizedVehicleLocation()) {
AERROR << "Fail to calculate discretized vehicle location!";
return false;
......@@ -79,7 +78,7 @@ bool QpFrenetFrame::Init(
return false;
}
if (!CalculateObstacleBound(path_obstacles)) {
if (!CalculateObstacleBound(obstacles)) {
AERROR << "Calculate obstacle bound failed!";
return false;
}
......@@ -137,13 +136,12 @@ bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
return true;
}
bool QpFrenetFrame::MapDynamicObstacleWithDecision(
const PathObstacle& path_obstacle) {
if (!path_obstacle.HasLateralDecision()) {
bool QpFrenetFrame::MapDynamicObstacleWithDecision(const Obstacle& obstacle) {
if (!obstacle.HasLateralDecision()) {
ADEBUG << "object has no lateral decision";
return false;
}
const auto& decision = path_obstacle.LateralDecision();
const auto& decision = obstacle.LateralDecision();
if (!decision.has_nudge()) {
ADEBUG << "only support nudge now";
return true;
......@@ -151,10 +149,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
const auto& nudge = decision.nudge();
for (const SpeedPoint& veh_point : discretized_vehicle_location_) {
double time = veh_point.t();
common::TrajectoryPoint trajectory_point =
path_obstacle.GetPointAtTime(time);
common::math::Box2d obs_box =
path_obstacle.GetBoundingBox(trajectory_point);
common::TrajectoryPoint trajectory_point = obstacle.GetPointAtTime(time);
common::math::Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
// project obs_box on reference line
std::vector<common::math::Vec2d> corners;
obs_box.GetAllCorners(&corners);
......@@ -209,21 +205,20 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
return true;
}
bool QpFrenetFrame::MapStaticObstacleWithDecision(
const PathObstacle& path_obstacle) {
if (!path_obstacle.HasLateralDecision()) {
bool QpFrenetFrame::MapStaticObstacleWithDecision(const Obstacle& obstacle) {
if (!obstacle.HasLateralDecision()) {
ADEBUG << "obstacle has no lateral decision";
return false;
}
const auto& decision = path_obstacle.LateralDecision();
const auto& decision = obstacle.LateralDecision();
if (!decision.has_nudge()) {
ADEBUG << "only support nudge decision now";
return true;
}
if (!MapNudgePolygon(
common::math::Polygon2d(path_obstacle.PerceptionBoundingBox()),
common::math::Polygon2d(obstacle.PerceptionBoundingBox()),
decision.nudge(), &static_obstacle_bound_)) {
AERROR << "fail to map polygon with id " << path_obstacle.Id()
AERROR << "fail to map polygon with id " << obstacle.Id()
<< " in qp frenet frame";
return false;
}
......@@ -409,20 +404,20 @@ bool QpFrenetFrame::CalculateHDMapBound() {
}
bool QpFrenetFrame::CalculateObstacleBound(
const std::vector<const PathObstacle*>& path_obstacles) {
for (const auto ptr_path_obstacle : path_obstacles) {
if (!ptr_path_obstacle->HasLateralDecision()) {
const std::vector<const Obstacle*>& obstacles) {
for (const auto ptr_obstacle : obstacles) {
if (!ptr_obstacle->HasLateralDecision()) {
continue;
}
if (ptr_path_obstacle->IsStatic()) {
if (!MapStaticObstacleWithDecision(*ptr_path_obstacle)) {
AERROR << "mapping obstacle with id [" << ptr_path_obstacle->Id()
if (ptr_obstacle->IsStatic()) {
if (!MapStaticObstacleWithDecision(*ptr_obstacle)) {
AERROR << "mapping obstacle with id [" << ptr_obstacle->Id()
<< "] failed in qp frenet frame.";
return false;
}
} else {
if (!MapDynamicObstacleWithDecision(*ptr_path_obstacle)) {
AERROR << "mapping obstacle with id [" << ptr_path_obstacle->Id()
if (!MapDynamicObstacleWithDecision(*ptr_obstacle)) {
AERROR << "mapping obstacle with id [" << ptr_obstacle->Id()
<< "] failed in qp frenet frame.";
return false;
}
......
modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.h
浏览文件 @ f9a5bab3
......@@ -31,7 +31,7 @@
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/reference_line/reference_line.h"
......@@ -47,7 +47,7 @@ class QpFrenetFrame {
const std::vector<double>& evaluated_s);
virtual ~QpFrenetFrame() = default;
bool Init(const std::vector<const PathObstacle*>& path_obstacles);
bool Init(const std::vector<const Obstacle*>& obstacles);
void LogQpBound(apollo::planning_internal::Debug* planning_debug);
......@@ -58,9 +58,9 @@ class QpFrenetFrame {
private:
bool CalculateDiscretizedVehicleLocation();
bool MapDynamicObstacleWithDecision(const PathObstacle& path_obstacle);
bool MapDynamicObstacleWithDecision(const Obstacle& obstacle);
bool MapStaticObstacleWithDecision(const PathObstacle& path_obstacle);
bool MapStaticObstacleWithDecision(const Obstacle& obstacle);
bool MapNudgePolygon(const common::math::Polygon2d& polygon,
const ObjectNudge& nudge,
......@@ -80,8 +80,7 @@ class QpFrenetFrame {
bool CalculateHDMapBound();
bool CalculateObstacleBound(
const std::vector<const PathObstacle*>& path_obstacles);
bool CalculateObstacleBound(const std::vector<const Obstacle*>& obstacles);
uint32_t FindIndex(const double s) const;
......
modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc
浏览文件 @ f9a5bab3
......@@ -81,10 +81,9 @@ void QpSplinePathGenerator::SetChangeLane(bool is_change_lane_path) {
}
bool QpSplinePathGenerator::Generate(
const std::vector<const PathObstacle*>& path_obstacles,
const SpeedData& speed_data, const common::TrajectoryPoint& init_point,
const double boundary_extension, bool is_final_attempt,
PathData* const path_data) {
const std::vector<const Obstacle*>& obstacles, const SpeedData& speed_data,
const common::TrajectoryPoint& init_point, const double boundary_extension,
bool is_final_attempt, PathData* const path_data) {
ADEBUG << "Init point: " << init_point.DebugString();
init_trajectory_point_ = init_point;
......@@ -123,7 +122,7 @@ bool QpSplinePathGenerator::Generate(
QpFrenetFrame qp_frenet_frame(reference_line_, speed_data, init_frenet_point_,
qp_spline_path_config_.time_resolution(),
evaluated_s_);
if (!qp_frenet_frame.Init(path_obstacles)) {
if (!qp_frenet_frame.Init(obstacles)) {
AERROR << "Fail to initialize qp frenet frame";
return false;
}
......
modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.h
浏览文件 @ f9a5bab3
......@@ -29,9 +29,9 @@
#include "modules/planning/proto/qp_spline_path_config.pb.h"
#include "modules/planning/proto/sl_boundary.pb.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/path/path_data.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/math/smoothing_spline/active_set_spline_1d_solver.h"
#include "modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.h"
......@@ -50,7 +50,7 @@ class QpSplinePathGenerator {
void SetChangeLane(bool is_change_lane_path);
bool Generate(const std::vector<const PathObstacle*>& path_obstacles,
bool Generate(const std::vector<const Obstacle*>& obstacles,
const SpeedData& speed_data,
const common::TrajectoryPoint& init_point,
const double boundary_extension, bool is_final_attempt,
......
modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.cc
浏览文件 @ f9a5bab3
......@@ -55,8 +55,8 @@ Status QpSplinePathOptimizer::Process(const SpeedData& speed_data,
bool is_final_attempt = false;
bool ret = path_generator.Generate(
reference_line_info_->path_decision()->path_obstacles().Items(),
speed_data, init_point, boundary_extension, is_final_attempt, path_data);
reference_line_info_->path_decision()->obstacles().Items(), speed_data,
init_point, boundary_extension, is_final_attempt, path_data);
if (!ret) {
AERROR << "failed to generate spline path with boundary_extension = 0.";
......@@ -64,9 +64,8 @@ Status QpSplinePathOptimizer::Process(const SpeedData& speed_data,
is_final_attempt = true;
ret = path_generator.Generate(
reference_line_info_->path_decision()->path_obstacles().Items(),
speed_data, init_point, boundary_extension, is_final_attempt,
path_data);
reference_line_info_->path_decision()->obstacles().Items(), speed_data,
init_point, boundary_extension, is_final_attempt, path_data);
if (!ret) {
const std::string msg =
"failed to generate spline path at final attempt.";
......
modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
浏览文件 @ f9a5bab3
......@@ -79,8 +79,8 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
qp_st_speed_config_.total_path_length(), qp_st_speed_config_.total_time(),
reference_line_info_->IsChangeLanePath());
for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {
DCHECK(path_obstacle->HasLongitudinalDecision());
for (const auto* obstacle : path_decision->obstacles().Items()) {
DCHECK(obstacle->HasLongitudinalDecision());
}
// step 1 get boundaries
path_decision->EraseStBoundaries();
......@@ -91,7 +91,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
}
std::vector<const StBoundary*> boundaries;
for (auto* obstacle : path_decision->path_obstacles().Items()) {
for (auto* obstacle : path_decision->obstacles().Items()) {
auto id = obstacle->Id();
if (!obstacle->st_boundary().IsEmpty()) {
path_decision->Find(id)->SetBlockingObstacle(true);
......@@ -137,7 +137,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
SpeedLimitDecider speed_limit_decider(adc_sl_boundary, st_boundary_config_,
reference_line, path_data);
SpeedLimit speed_limits;
if (speed_limit_decider.GetSpeedLimits(path_decision->path_obstacles(),
if (speed_limit_decider.GetSpeedLimits(path_decision->obstacles(),
&speed_limits) != Status::OK()) {
return Status(ErrorCode::PLANNING_ERROR,
"GetSpeedLimits for qp st speed optimizer failed!");
......
modules/planning/toolkits/optimizers/road_graph/BUILD
浏览文件 @ f9a5bab3
......@@ -6,14 +6,12 @@ cc_library(
name = "road_graph",
srcs = [
"dp_road_graph.cc",
"side_pass_waypoint_sampler.cc",
"trajectory_cost.cc",
"waypoint_sampler.cc",
],
hdrs = [
"comparable_cost.h",
"dp_road_graph.h",
"side_pass_waypoint_sampler.h",
"trajectory_cost.h",
"waypoint_sampler.h",
],
......@@ -24,8 +22,8 @@ cc_library(
"//modules/common/status",
"//modules/map/proto:map_proto",
"//modules/planning/common:frame",
"//modules/planning/common:obstacle",
"//modules/planning/common:path_decision",
"//modules/planning/common:path_obstacle",
"//modules/planning/common:planning_gflags",
"//modules/planning/common/path:path_data",
"//modules/planning/common/speed:speed_data",
......
modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc
浏览文件 @ f9a5bab3
......@@ -56,10 +56,9 @@ DpRoadGraph::DpRoadGraph(const DpPolyPathConfig &config,
reference_line_(reference_line_info.reference_line()),
speed_data_(speed_data) {}
bool DpRoadGraph::FindPathTunnel(
const common::TrajectoryPoint &init_point,
const std::vector<const PathObstacle *> &obstacles,
PathData *const path_data) {
bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
const std::vector<const Obstacle *> &obstacles,
PathData *const path_data) {
CHECK_NOTNULL(path_data);
init_point_ = init_point;
......@@ -117,7 +116,7 @@ bool DpRoadGraph::FindPathTunnel(
}
bool DpRoadGraph::GenerateMinCostPath(
const std::vector<const PathObstacle *> &obstacles,
const std::vector<const Obstacle *> &obstacles,
std::vector<DpRoadGraphNode> *min_cost_path) {
CHECK(min_cost_path != nullptr);
......@@ -169,8 +168,7 @@ bool DpRoadGraph::GenerateMinCostPath(
&(graph_nodes.back().back()));
if (FLAGS_enable_multi_thread_in_dp_poly_path) {
results.emplace_back(
cyber::Async(&DpRoadGraph::UpdateNode, this, msg));
results.emplace_back(cyber::Async(&DpRoadGraph::UpdateNode, this, msg));
} else {
UpdateNode(msg);
}
......
modules/planning/toolkits/optimizers/road_graph/dp_road_graph.h
浏览文件 @ f9a5bab3
......@@ -29,9 +29,9 @@
#include "modules/planning/proto/dp_poly_path_config.pb.h"
#include "modules/common/status/status.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/path/path_data.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
......@@ -52,7 +52,7 @@ class DpRoadGraph {
~DpRoadGraph() = default;
bool FindPathTunnel(const common::TrajectoryPoint &init_point,
const std::vector<const PathObstacle *> &obstacles,
const std::vector<const Obstacle *> &obstacles,
PathData *const path_data);
void SetDebugLogger(apollo::planning_internal::Debug *debug) {
......@@ -97,7 +97,7 @@ class DpRoadGraph {
std::numeric_limits<float>::infinity()};
QuinticPolynomialCurve1d min_cost_curve;
};
bool GenerateMinCostPath(const std::vector<const PathObstacle *> &obstacles,
bool GenerateMinCostPath(const std::vector<const Obstacle *> &obstacles,
std::vector<DpRoadGraphNode> *min_cost_path);
bool IsValidCurve(const QuinticPolynomialCurve1d &curve) const;
......
modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.cc 已删除 100644 → 0
浏览文件 @ e770e666
/******************************************************************************
* Copyright 2018 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 waypoint_sampler.cc
**/
#include "modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.h"
#include <algorithm>
#include <utility>
#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/util/util.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/path/frenet_frame_path.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
namespace apollo {
namespace planning {
using apollo::common::SLPoint;
using apollo::common::Status;
using apollo::common::util::MakeSLPoint;
bool SidePassWaypointSampler::SamplePathWaypoints(
const common::TrajectoryPoint &init_point,
std::vector<std::vector<common::SLPoint>> *const points) {
CHECK_NOTNULL(points);
points->clear();
points->insert(points->begin(), std::vector<common::SLPoint>{init_sl_point_});
const float kMinSampleDistance = 40.0;
const float total_length = std::fmin(
init_sl_point_.s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
reference_line_info_->reference_line().Length());
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const float half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
const size_t num_sample_per_level =
FLAGS_use_navigation_mode ? config_.navigator_sample_num_each_level()
: config_.sample_points_num_each_level();
const bool has_sidepass = HasSidepass();
constexpr float kSamplePointLookForwardTime = 4.0;
const float step_length =
common::math::Clamp(init_point.v() * kSamplePointLookForwardTime,
config_.step_length_min(), config_.step_length_max());
const float level_distance =
(init_point.v() > FLAGS_max_stop_speed) ? step_length : step_length / 2.0;
float accumulated_s = init_sl_point_.s();
float prev_s = accumulated_s;
auto *status = PlanningContext::MutablePlanningStatus();
if (!status->has_pull_over() && status->pull_over().in_pull_over()) {
status->mutable_pull_over()->set_status(PullOverStatus::IN_OPERATION);
const auto &start_point = status->pull_over().start_point();
SLPoint start_point_sl;
if (!reference_line_info_->reference_line().XYToSL(start_point,
&start_point_sl)) {
AERROR << "Fail to change xy to sl.";
return false;
}
if (init_sl_point_.s() > start_point_sl.s()) {
const auto &stop_point = status->pull_over().stop_point();
SLPoint stop_point_sl;
if (!reference_line_info_->reference_line().XYToSL(stop_point,
&stop_point_sl)) {
AERROR << "Fail to change xy to sl.";
return false;
}
std::vector<common::SLPoint> level_points(1, stop_point_sl);
points->emplace_back(level_points);
return true;
}
}
for (std::size_t i = 0; accumulated_s < total_length; ++i) {
accumulated_s += level_distance;
if (accumulated_s + level_distance / 2.0 > total_length) {
accumulated_s = total_length;
}
const float s = std::fmin(accumulated_s, total_length);
constexpr float kMinAllowedSampleStep = 1.0;
if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
continue;
}
prev_s = s;
double left_width = 0.0;
double right_width = 0.0;
reference_line_info_->reference_line().GetLaneWidth(s, &left_width,
&right_width);
constexpr float kBoundaryBuff = 0.20;
const float eff_right_width = right_width - half_adc_width - kBoundaryBuff;
const float eff_left_width = left_width - half_adc_width - kBoundaryBuff;
// the heuristic shift of L for lane change scenarios
const double delta_dl = 1.2 / 20.0;
const double kChangeLaneDeltaL = common::math::Clamp(
level_distance * (std::fabs(init_frenet_frame_point_.dl()) + delta_dl),
1.2, 3.5);
float kDefaultUnitL = kChangeLaneDeltaL / (num_sample_per_level - 1);
if (reference_line_info_->IsChangeLanePath() &&
!reference_line_info_->IsSafeToChangeLane()) {
kDefaultUnitL = 1.0;
}
const float sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
float sample_right_boundary = -eff_right_width;
float sample_left_boundary = eff_left_width;
const float kLargeDeviationL = 1.75;
if (reference_line_info_->IsChangeLanePath() ||
std::fabs(init_sl_point_.l()) > kLargeDeviationL) {
sample_right_boundary = std::fmin(-eff_right_width, init_sl_point_.l());
sample_left_boundary = std::fmax(eff_left_width, init_sl_point_.l());
if (init_sl_point_.l() > eff_left_width) {
sample_right_boundary = std::fmax(sample_right_boundary,
init_sl_point_.l() - sample_l_range);
}
if (init_sl_point_.l() < eff_right_width) {
sample_left_boundary = std::fmin(sample_left_boundary,
init_sl_point_.l() + sample_l_range);
}
}
std::vector<float> sample_l;
if (reference_line_info_->IsChangeLanePath() &&
!reference_line_info_->IsSafeToChangeLane()) {
sample_l.push_back(reference_line_info_->OffsetToOtherReferenceLine());
} else if (has_sidepass) {
// currently only left nudge is supported. Need road hard boundary for
// both sides
switch (sidepass_.type()) {
case ObjectSidePass::LEFT: {
sample_l.push_back(eff_left_width + config_.sidepass_distance());
break;
}
case ObjectSidePass::RIGHT: {
sample_l.push_back(-eff_right_width - config_.sidepass_distance());
break;
}
default:
break;
}
} else {
common::util::uniform_slice(sample_right_boundary, sample_left_boundary,
num_sample_per_level - 1, &sample_l);
}
std::vector<common::SLPoint> level_points;
planning_internal::SampleLayerDebug sample_layer_debug;
for (size_t j = 0; j < sample_l.size(); ++j) {
common::SLPoint sl = common::util::MakeSLPoint(s, sample_l[j]);
sample_layer_debug.add_sl_point()->CopyFrom(sl);
level_points.push_back(std::move(sl));
}
if (!reference_line_info_->IsChangeLanePath() && has_sidepass) {
auto sl_zero = common::util::MakeSLPoint(s, 0.0);
sample_layer_debug.add_sl_point()->CopyFrom(sl_zero);
level_points.push_back(std::move(sl_zero));
}
if (!level_points.empty()) {
planning_debug_->mutable_planning_data()
->mutable_dp_poly_graph()
->add_sample_layer()
->CopyFrom(sample_layer_debug);
points->emplace_back(level_points);
}
}
return true;
}
bool SidePassWaypointSampler::HasSidepass() {
const auto &path_decision = reference_line_info_->path_decision();
for (const auto &obstacle : path_decision.path_obstacles().Items()) {
if (obstacle->LateralDecision().has_sidepass()) {
sidepass_ = obstacle->LateralDecision().sidepass();
return true;
}
}
return false;
}
} // namespace planning
} // namespace apollo
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或