From f9a5bab37bf9e8ae13dbc438bccec88722a15ed0 Mon Sep 17 00:00:00 2001 From: Liangliang Zhang Date: Thu, 25 Oct 2018 17:35:08 -0700 Subject: [PATCH] Planning: replace PathObstacle with Obstacle. --- modules/planning/common/BUILD | 20 +- modules/planning/common/decision_data.cc | 48 +- modules/planning/common/decision_data.h | 30 +- modules/planning/common/ego_info.cc | 4 +- modules/planning/common/ego_info.h | 6 +- modules/planning/common/frame.cc | 54 +- modules/planning/common/frame.h | 24 +- .../common/{path_obstacle.cc => obstacle.cc} | 120 ++-- .../common/{path_obstacle.h => obstacle.h} | 27 +- modules/planning/common/obstacle_test.cc | 420 +++++++++++- modules/planning/common/path_decision.cc | 36 +- modules/planning/common/path_decision.h | 12 +- modules/planning/common/path_obstacle_test.cc | 631 ------------------ .../planning/common/reference_line_info.cc | 88 ++- modules/planning/common/reference_line_info.h | 10 +- .../common/speed_profile_generator_test.cc | 2 +- modules/planning/constraint_checker/BUILD | 2 +- .../constraint_checker/collision_checker.cc | 16 +- .../constraint_checker/collision_checker.h | 12 +- modules/planning/lattice/behavior/BUILD | 4 +- .../lattice/behavior/path_time_graph.cc | 10 +- .../lattice/behavior/path_time_graph.h | 10 +- .../lattice/behavior/prediction_querier.cc | 4 +- .../lattice/behavior/prediction_querier.h | 10 +- .../navi/decider/navi_obstacle_decider.cc | 19 +- .../navi/decider/navi_obstacle_decider.h | 21 +- .../decider/navi_obstacle_decider_test.cc | 34 +- .../navi/decider/navi_path_decider.cc | 22 +- .../planning/navi/decider/navi_path_decider.h | 12 +- .../navi/decider/navi_speed_decider.cc | 9 +- .../navi/decider/navi_speed_decider.h | 8 +- .../navi/decider/navi_speed_decider_test.cc | 22 +- modules/planning/open_space/BUILD | 4 +- .../distance_approach_problem_wrapper.cc | 5 +- modules/planning/open_space/hybrid_a_star.h | 2 +- .../planning/open_space/hybrid_a_star_test.cc | 7 +- .../open_space/hybrid_a_star_wrapper.cc | 5 +- modules/planning/planner/navi/navi_planner.cc | 20 +- modules/planning/planner/public_road/BUILD | 5 +- modules/planning/scenarios/lane_follow/BUILD | 2 - .../lane_follow/lane_follow_scenario.cc | 1 - .../lane_follow/lane_follow_stage.cc | 36 +- .../scenarios/side_pass/side_pass_scenario.cc | 24 +- .../scenarios/side_pass/side_pass_stage.cc | 19 +- .../stop_sign_unprotected_stage.cc | 19 +- .../stop_sign_unprotected_stage.h | 4 +- modules/planning/toolkits/BUILD | 1 - .../toolkits/deciders/decider_creep.cc | 12 +- .../toolkits/deciders/decider_stop_sign.cc | 4 +- .../deciders/side_pass_path_decider.cc | 14 +- .../toolkits/deciders/side_pass_safety.cc | 22 +- .../dp_poly_path/dp_poly_path_optimizer.cc | 2 +- .../toolkits/optimizers/dp_st_speed/BUILD | 4 +- .../optimizers/dp_st_speed/dp_st_cost.cc | 4 +- .../optimizers/dp_st_speed/dp_st_cost.h | 28 +- .../optimizers/dp_st_speed/dp_st_graph.cc | 2 +- .../optimizers/dp_st_speed/dp_st_graph.h | 6 +- .../dp_st_speed/dp_st_graph_test.cc | 12 +- .../dp_st_speed/dp_st_speed_optimizer.cc | 11 +- .../optimizers/path_decider/path_decider.cc | 62 +- .../optimizers/path_decider/path_decider.h | 3 +- .../toolkits/optimizers/poly_st_speed/BUILD | 69 -- .../optimizers/poly_st_speed/poly_st_graph.cc | 136 ---- .../optimizers/poly_st_speed/poly_st_graph.h | 93 --- .../poly_st_speed/poly_st_speed_optimizer.cc | 125 ---- .../poly_st_speed/poly_st_speed_optimizer.h | 52 -- .../poly_st_speed/speed_profile_cost.cc | 125 ---- .../poly_st_speed/speed_profile_cost.h | 57 -- .../poly_vt_speed/poly_vt_speed_optimizer.cc | 8 +- .../optimizers/qp_piecewise_jerk_path/BUILD | 2 +- .../qp_piecewise_jerk_path_optimizer.cc | 12 +- .../qp_piecewise_jerk_path_optimizer.h | 2 +- .../toolkits/optimizers/qp_spline_path/BUILD | 2 +- .../qp_spline_path/qp_frenet_frame.cc | 45 +- .../qp_spline_path/qp_frenet_frame.h | 11 +- .../qp_spline_path_generator.cc | 9 +- .../qp_spline_path/qp_spline_path_generator.h | 4 +- .../qp_spline_path_optimizer.cc | 9 +- .../qp_spline_st_speed_optimizer.cc | 8 +- .../toolkits/optimizers/road_graph/BUILD | 4 +- .../optimizers/road_graph/dp_road_graph.cc | 12 +- .../optimizers/road_graph/dp_road_graph.h | 6 +- .../road_graph/side_pass_waypoint_sampler.cc | 208 ------ .../road_graph/side_pass_waypoint_sampler.h | 44 -- .../optimizers/road_graph/trajectory_cost.cc | 35 +- .../optimizers/road_graph/trajectory_cost.h | 4 +- .../optimizers/road_graph/waypoint_sampler.cc | 2 +- .../optimizers/road_graph/waypoint_sampler.h | 2 +- .../toolkits/optimizers/side_pass_path/BUILD | 19 - .../side_pass_path_optimizer.cc | 66 -- .../side_pass_path/side_pass_path_optimizer.h | 48 -- .../optimizers/speed_decider/speed_decider.cc | 114 ++-- .../optimizers/speed_decider/speed_decider.h | 12 +- .../toolkits/optimizers/st_graph/BUILD | 2 +- .../st_graph/speed_limit_decider.cc | 20 +- .../optimizers/st_graph/speed_limit_decider.h | 4 +- .../st_graph/speed_limit_decider_test.cc | 2 +- .../optimizers/st_graph/st_boundary_mapper.cc | 82 ++- .../optimizers/st_graph/st_boundary_mapper.h | 11 +- .../st_graph/st_boundary_mapper_test.cc | 2 +- modules/planning/toolkits/task_factory.cc | 5 - .../traffic_rules/backside_vehicle.cc | 27 +- .../planning/traffic_rules/backside_vehicle.h | 2 +- modules/planning/traffic_rules/change_lane.cc | 38 +- modules/planning/traffic_rules/change_lane.h | 11 +- modules/planning/traffic_rules/creeper.cc | 4 +- modules/planning/traffic_rules/crosswalk.cc | 17 +- modules/planning/traffic_rules/destination.cc | 4 +- .../planning/traffic_rules/front_vehicle.cc | 32 +- modules/planning/traffic_rules/pull_over.cc | 12 +- .../traffic_rules/reference_line_end.cc | 2 +- .../planning/traffic_rules/signal_light.cc | 8 +- modules/planning/traffic_rules/stop_sign.cc | 43 +- modules/planning/traffic_rules/stop_sign.h | 6 +- .../planning/traffic_rules/traffic_decider.cc | 2 +- .../planning/traffic_rules/traffic_decider.h | 2 +- .../autotuning_raw_feature_generator.cc | 2 +- 117 files changed, 1227 insertions(+), 2545 deletions(-) rename modules/planning/common/{path_obstacle.cc => obstacle.cc} (86%) rename modules/planning/common/{path_obstacle.h => obstacle.h} (93%) delete mode 100644 modules/planning/common/path_obstacle_test.cc delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/BUILD delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.cc delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.cc delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.cc delete mode 100644 modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h delete mode 100644 modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.cc delete mode 100644 modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.h delete mode 100644 modules/planning/toolkits/optimizers/side_pass_path/BUILD delete mode 100644 modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.cc delete mode 100644 modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.h diff --git a/modules/planning/common/BUILD b/modules/planning/common/BUILD index 1155a4fb6a..c8235121a6 100644 --- a/modules/planning/common/BUILD +++ b/modules/planning/common/BUILD @@ -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", diff --git a/modules/planning/common/decision_data.cc b/modules/planning/common/decision_data.cc index 5c27a2fe70..eef2ddb26d 100644 --- a/modules/planning/common/decision_data.cc +++ b/modules/planning/common/decision_data.cc @@ -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 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 DecisionData::GetObstacleByType( +std::vector DecisionData::GetObstacleByType( const VirtualObjectType& type) { std::lock_guard lock(transaction_mutex_); std::unordered_set ids = GetObstacleIdByType(type); if (ids.empty()) { - return std::vector(); + return std::vector(); } - std::vector ret; + std::vector ret; for (const std::string& id : ids) { ret.emplace_back(GetObstacleById(id)); if (ret.back() == nullptr) { @@ -120,23 +120,23 @@ std::unordered_set DecisionData::GetObstacleIdByType( return it->second; } -const std::vector& DecisionData::GetStaticObstacle() const { +const std::vector& DecisionData::GetStaticObstacle() const { return static_obstacle_; } -const std::vector& DecisionData::GetDynamicObstacle() const { +const std::vector& DecisionData::GetDynamicObstacle() const { return dynamic_obstacle_; } -const std::vector& DecisionData::GetVirtualObstacle() const { +const std::vector& DecisionData::GetVirtualObstacle() const { return virtual_obstacle_; } -const std::vector& DecisionData::GetPracticalObstacle() const { +const std::vector& DecisionData::GetPracticalObstacle() const { return practical_obstacle_; } -const std::vector& DecisionData::GetAllObstacle() const { +const std::vector& 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; } diff --git a/modules/planning/common/decision_data.h b/modules/planning/common/decision_data.h index 9a5559e6d1..77422243c8 100644 --- a/modules/planning/common/decision_data.h +++ b/modules/planning/common/decision_data.h @@ -25,7 +25,7 @@ #include #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 GetObstacleByType(const VirtualObjectType& type); + Obstacle* GetObstacleById(const std::string& id); + std::vector GetObstacleByType(const VirtualObjectType& type); std::unordered_set GetObstacleIdByType( const VirtualObjectType& type); - const std::vector& GetStaticObstacle() const; - const std::vector& GetDynamicObstacle() const; - const std::vector& GetVirtualObstacle() const; - const std::vector& GetPracticalObstacle() const; - const std::vector& GetAllObstacle() const; + const std::vector& GetStaticObstacle() const; + const std::vector& GetDynamicObstacle() const; + const std::vector& GetVirtualObstacle() const; + const std::vector& GetPracticalObstacle() const; + const std::vector& GetAllObstacle() const; public: bool CreateVirtualObstacle(const ReferencePoint& point, @@ -83,16 +83,16 @@ class DecisionData { std::string* const id); private: - std::vector static_obstacle_; - std::vector dynamic_obstacle_; - std::vector virtual_obstacle_; - std::vector practical_obstacle_; - std::vector all_obstacle_; + std::vector static_obstacle_; + std::vector dynamic_obstacle_; + std::vector virtual_obstacle_; + std::vector practical_obstacle_; + std::vector all_obstacle_; private: const ReferenceLine& reference_line_; - std::list> path_obstacles_; - std::unordered_map obstacle_map_; + std::list> obstacles_; + std::unordered_map obstacle_map_; std::unordered_map, EnumClassHash> virtual_obstacle_id_map_; diff --git a/modules/planning/common/ego_info.cc b/modules/planning/common/ego_info.cc index 12cd2e82a9..2ed561a2f7 100644 --- a/modules/planning/common/ego_info.cc +++ b/modules/planning/common/ego_info.cc @@ -36,7 +36,7 @@ EgoInfo::EgoInfo() { bool EgoInfo::Update(const common::TrajectoryPoint& start_point, const common::VehicleState& vehicle_state, - const std::vector& obstacles) { + const std::vector& 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& obstacles) { + const std::vector& obstacles) { Vec2d position(vehicle_state_.x(), vehicle_state_.y()); const auto& param = ego_vehicle_config_.vehicle_param(); diff --git a/modules/planning/common/ego_info.h b/modules/planning/common/ego_info.h index e2d917fbb2..a48a1b4688 100644 --- a/modules/planning/common/ego_info.h +++ b/modules/planning/common/ego_info.h @@ -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& obstacles); + const std::vector& obstacles); void Clear(); common::TrajectoryPoint start_point() const { return start_point_; } @@ -66,7 +66,7 @@ class EgoInfo { } void CalculateFrontObstacleClearDistance( - const std::vector& obstacles); + const std::vector& obstacles); // stitched point (at stitching mode) // or real vehicle point (at non-stitching mode) diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc index 6ace565e1c..0084b5dccd 100644 --- a/modules/planning/common/frame.cc +++ b/modules/planning/common/frame.cc @@ -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 Frame::obstacles() const { +const std::vector 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 obstacle = - PathObstacle::CreateStaticVirtualObstacles( - obstacles_.Items().at(i)->Id(), original_box); + std::unique_ptr 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 left_boundary_obstacle = - PathObstacle::CreateStaticVirtualObstacles("left_boundary", - left_boundary_box); + std::unique_ptr left_boundary_obstacle = + Obstacle::CreateStaticVirtualObstacles("left_boundary", + left_boundary_box); openspace_warmstart_obstacles_.Add(left_boundary_obstacle->Id(), *left_boundary_obstacle); - std::unique_ptr down_boundary_obstacle = - PathObstacle::CreateStaticVirtualObstacles("down_boundary", - down_boundary_box); + std::unique_ptr down_boundary_obstacle = + Obstacle::CreateStaticVirtualObstacles("down_boundary", + down_boundary_box); openspace_warmstart_obstacles_.Add(down_boundary_obstacle->Id(), *down_boundary_obstacle); - std::unique_ptr right_boundary_obstacle = - PathObstacle::CreateStaticVirtualObstacles("right_boundary", - right_boundary_box); + std::unique_ptr right_boundary_obstacle = + Obstacle::CreateStaticVirtualObstacles("right_boundary", + right_boundary_box); openspace_warmstart_obstacles_.Add(right_boundary_obstacle->Id(), *right_boundary_obstacle); - std::unique_ptr up_boundary_obstacle = - PathObstacle::CreateStaticVirtualObstacles("up_boundary", - up_boundary_box); + std::unique_ptr up_boundary_obstacle = + Obstacle::CreateStaticVirtualObstacles("up_boundary", up_boundary_box); openspace_warmstart_obstacles_.Add(up_boundary_obstacle->Id(), *up_boundary_obstacle); diff --git a/modules/planning/common/frame.h b/modules/planning/common/frame.h index 73893a3ef6..cec2c13979 100644 --- a/modules/planning/common/frame.h +++ b/modules/planning/common/frame.h @@ -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 &reference_line_info() const; std::list *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 obstacles() const; + const std::vector 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; diff --git a/modules/planning/common/path_obstacle.cc b/modules/planning/common/obstacle.cc similarity index 86% rename from modules/planning/common/path_obstacle.cc rename to modules/planning/common/obstacle.cc index 19a9da3799..a559733a26 100644 --- a/modules/planning/common/path_obstacle.cc +++ b/modules/planning/common/obstacle.cc @@ -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 - 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 - 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> PathObstacle::CreateObstacles( +std::list> Obstacle::CreateObstacles( const prediction::PredictionObstacles& predictions) { - std::list> obstacles; + std::list> 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> 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> PathObstacle::CreateObstacles( return obstacles; } -std::unique_ptr PathObstacle::CreateStaticVirtualObstacles( +std::unique_ptr 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::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(obstacle); + return std::unique_ptr(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& PathObstacle::decider_tags() const { +const std::vector& Obstacle::decider_tags() const { return decider_tags_; } -const std::vector& PathObstacle::decisions() const { +const std::vector& 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(); diff --git a/modules/planning/common/path_obstacle.h b/modules/planning/common/obstacle.h similarity index 93% rename from modules/planning/common/path_obstacle.h rename to modules/planning/common/obstacle.h index 91e1cee84e..6d573f8a69 100644 --- a/modules/planning/common/path_obstacle.h +++ b/modules/planning/common/obstacle.h @@ -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> CreateObstacles( + static std::list> CreateObstacles( const prediction::PredictionObstacles& predictions); - static std::unique_ptr CreateStaticVirtualObstacles( + static std::unique_ptr 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 IndexedObstacles; -typedef ThreadSafeIndexedList - ThreadSafeIndexedObstacles; +typedef IndexedList IndexedObstacles; +typedef ThreadSafeIndexedList ThreadSafeIndexedObstacles; } // namespace planning } // namespace apollo diff --git a/modules/planning/common/obstacle_test.cc b/modules/planning/common/obstacle_test.cc index f5489440b6..4a3a8abf16 100644 --- a/modules/planning/common/obstacle_test.cc +++ b/modules/planning/common/obstacle_test.cc @@ -18,6 +18,8 @@ * @file **/ +#include "modules/planning/common/obstacle.h" + #include #include #include @@ -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 diff --git a/modules/planning/common/path_decision.cc b/modules/planning/common/path_decision.cc index 8db6f3a434..a7426ef48f 100644 --- a/modules/planning/common/path_decision.cc +++ b/modules/planning/common/path_decision.cc @@ -28,28 +28,26 @@ namespace apollo { namespace planning { -using IndexedPathObstacles = IndexedList; +using IndexedObstacles = IndexedList; -PathObstacle *PathDecision::AddPathObstacle(const PathObstacle &path_obstacle) { +Obstacle *PathDecision::AddObstacle(const Obstacle &obstacle) { std::lock_guard 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; } diff --git a/modules/planning/common/path_decision.h b/modules/planning/common/path_decision.h index fc7e7ecd74..24ca2ce8c5 100644 --- a/modules/planning/common/path_decision.h +++ b/modules/planning/common/path_decision.h @@ -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 &path_obstacles() const; + const IndexedList &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 path_obstacles_; + IndexedList obstacles_; MainStop main_stop_; double stop_reference_line_s_ = std::numeric_limits::max(); }; diff --git a/modules/planning/common/path_obstacle_test.cc b/modules/planning/common/path_obstacle_test.cc deleted file mode 100644 index 9262c47161..0000000000 --- a/modules/planning/common/path_obstacle_test.cc +++ /dev/null @@ -1,631 +0,0 @@ -/****************************************************************************** - * 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/common/path_obstacle.h" - -#include -#include -#include - -#include "gtest/gtest.h" - -#include "modules/perception/proto/perception_obstacle.pb.h" -#include "modules/prediction/proto/prediction_obstacle.pb.h" - -#include "modules/common/util/file.h" -#include "modules/common/util/util.h" -#include "modules/planning/common/planning_gflags.h" - -namespace apollo { -namespace planning { - -using apollo::perception::PerceptionObstacle; - -TEST(PathObstacle, IsStaticObstacle) { - PerceptionObstacle perception_obstacle; - EXPECT_TRUE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.mutable_velocity()->set_x(2.5); - perception_obstacle.mutable_velocity()->set_y(0.5); - EXPECT_FALSE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.set_type(PerceptionObstacle::UNKNOWN); - EXPECT_FALSE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.set_type(PerceptionObstacle::UNKNOWN_UNMOVABLE); - EXPECT_TRUE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.set_type(PerceptionObstacle::UNKNOWN_MOVABLE); - EXPECT_FALSE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.set_type(PerceptionObstacle::PEDESTRIAN); - EXPECT_FALSE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.set_type(PerceptionObstacle::BICYCLE); - EXPECT_FALSE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.set_type(PerceptionObstacle::VEHICLE); - EXPECT_FALSE(PathObstacle::IsStaticObstacle(perception_obstacle)); - - perception_obstacle.mutable_velocity()->set_x(0.5); - perception_obstacle.mutable_velocity()->set_y(0.5); - EXPECT_TRUE(PathObstacle::IsStaticObstacle(perception_obstacle)); -} - -class ObstacleTest : public ::testing::Test { - public: - virtual void SetUp() { - prediction::PredictionObstacles prediction_obstacles; - ASSERT_TRUE(common::util::GetProtoFromFile( - "modules/planning/common/testdata/sample_prediction.pb.txt", - &prediction_obstacles)); - auto obstacles = PathObstacle::CreateObstacles(prediction_obstacles); - ASSERT_EQ(5, obstacles.size()); - for (auto& obstacle : obstacles) { - const auto id = obstacle->Id(); - indexed_obstacles_.Add(id, *obstacle); - } - } - - protected: - IndexedObstacles indexed_obstacles_; -}; - -TEST_F(ObstacleTest, CreateObstacles) { - ASSERT_EQ(5, indexed_obstacles_.Items().size()); - EXPECT_TRUE(indexed_obstacles_.Find("2156_0")); - EXPECT_TRUE(indexed_obstacles_.Find("2156_1")); - EXPECT_TRUE(indexed_obstacles_.Find("2157_0")); - EXPECT_TRUE(indexed_obstacles_.Find("2157_1")); - EXPECT_TRUE(indexed_obstacles_.Find("2161")); -} - -TEST_F(ObstacleTest, Id) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - EXPECT_EQ("2156_0", obstacle->Id()); - EXPECT_EQ(2156, obstacle->PerceptionId()); -} - -TEST_F(ObstacleTest, GetPointAtTime) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - - // first - const auto first_point = obstacle->GetPointAtTime(0.0); - EXPECT_FLOAT_EQ(0.0, first_point.relative_time()); - EXPECT_FLOAT_EQ(76.684071405, first_point.path_point().x()); - EXPECT_FLOAT_EQ(350.481852505, first_point.path_point().y()); - - // last - const auto last_point = obstacle->GetPointAtTime(10.04415320); - EXPECT_FLOAT_EQ(10.0441531943, last_point.relative_time()); - EXPECT_FLOAT_EQ(186.259371951, last_point.path_point().x()); - EXPECT_FLOAT_EQ(341.853799387, last_point.path_point().y()); - - // middle - const auto middle_point = obstacle->GetPointAtTime(3.7300); - EXPECT_LE(3.68968892853, middle_point.relative_time()); - EXPECT_GE(3.89467164678, middle_point.relative_time()); - EXPECT_GE(139.091700103, middle_point.path_point().x()); - EXPECT_LE(135.817210975, middle_point.path_point().x()); - EXPECT_GE(349.875902219, middle_point.path_point().y()); - EXPECT_LE(349.549888973, middle_point.path_point().y()); -} - -TEST_F(ObstacleTest, PerceptionBoundingBox) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - const auto& box = obstacle->PerceptionBoundingBox(); - - std::vector corners; - box.GetAllCorners(&corners); - EXPECT_EQ(4, corners.size()); - EXPECT_FLOAT_EQ(3.832477, box.length()); - EXPECT_FLOAT_EQ(1.73200099013, box.width()); - EXPECT_FLOAT_EQ(76.684071405, box.center_x()); - EXPECT_FLOAT_EQ(350.481852505, box.center_y()); - EXPECT_FLOAT_EQ(0.00531211859358, box.heading()); -} - -TEST_F(ObstacleTest, GetBoundingBox) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - const auto& point = obstacle->Trajectory().trajectory_point(2); - const auto& box = obstacle->GetBoundingBox(point); - std::vector corners; - box.GetAllCorners(&corners); - EXPECT_EQ(4, corners.size()); - EXPECT_FLOAT_EQ(3.832477, box.length()); - EXPECT_FLOAT_EQ(1.73200099013, box.width()); - EXPECT_FLOAT_EQ(83.2581699369, box.center_x()); - EXPECT_FLOAT_EQ(350.779556678, box.center_y()); - EXPECT_FLOAT_EQ(0.040689919, box.heading()); -} - -TEST_F(ObstacleTest, PerceptionPolygon) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - const auto& polygon = obstacle->PerceptionPolygon(); - - const auto& points = polygon.points(); - EXPECT_EQ(16, points.size()); - EXPECT_FLOAT_EQ(74.766182, points[0].x()); - EXPECT_FLOAT_EQ(350.72986, points[0].y()); - EXPECT_FLOAT_EQ(74.783195, points[1].x()); - EXPECT_FLOAT_EQ(350.32602, points[1].y()); - EXPECT_FLOAT_EQ(74.770554, points[15].x()); - EXPECT_FLOAT_EQ(350.87857, points[15].y()); -} - -TEST_F(ObstacleTest, Trajectory) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - const auto& points = obstacle->Trajectory().trajectory_point(); - EXPECT_EQ(50, points.size()); -} - -TEST_F(ObstacleTest, Perception) { - const auto* obstacle = indexed_obstacles_.Find("2156_0"); - ASSERT_TRUE(obstacle); - const auto& perception_obstacle = obstacle->Perception(); - EXPECT_EQ(2156, perception_obstacle.id()); -} - -TEST(Obstacle, CreateStaticVirtualObstacle) { - common::math::Box2d box({0, 0}, 0.0, 4.0, 2.0); - std::unique_ptr obstacle = - PathObstacle::CreateStaticVirtualObstacles("abc", box); - EXPECT_EQ("abc", obstacle->Id()); - EXPECT_EQ(-314721735, obstacle->PerceptionId()); - EXPECT_TRUE(PathObstacle::IsStaticObstacle(obstacle->Perception())); - EXPECT_TRUE(PathObstacle::IsVirtualObstacle(obstacle->Perception())); - auto& perception_box = obstacle->PerceptionBoundingBox(); - EXPECT_FLOAT_EQ(0.0, perception_box.center().x()); - EXPECT_FLOAT_EQ(0.0, perception_box.center().y()); - EXPECT_FLOAT_EQ(4.0, perception_box.length()); - EXPECT_FLOAT_EQ(2.0, perception_box.width()); - EXPECT_FLOAT_EQ(0.0, perception_box.heading()); -} - -TEST(IsLateralDecision, AllDecisions) { - ObjectDecisionType decision_ignore; - decision_ignore.mutable_ignore(); - EXPECT_TRUE(PathObstacle::IsLateralDecision(decision_ignore)); - - ObjectDecisionType decision_overtake; - decision_overtake.mutable_overtake(); - EXPECT_FALSE(PathObstacle::IsLateralDecision(decision_overtake)); - - ObjectDecisionType decision_follow; - decision_follow.mutable_follow(); - EXPECT_FALSE(PathObstacle::IsLateralDecision(decision_follow)); - - ObjectDecisionType decision_yield; - decision_yield.mutable_yield(); - EXPECT_FALSE(PathObstacle::IsLateralDecision(decision_yield)); - - ObjectDecisionType decision_stop; - decision_stop.mutable_stop(); - EXPECT_FALSE(PathObstacle::IsLateralDecision(decision_stop)); - - ObjectDecisionType decision_nudge; - decision_nudge.mutable_nudge(); - EXPECT_TRUE(PathObstacle::IsLateralDecision(decision_nudge)); - - ObjectDecisionType decision_sidepass; - decision_sidepass.mutable_sidepass(); - EXPECT_TRUE(PathObstacle::IsLateralDecision(decision_sidepass)); -} - -TEST(IsLongitudinalDecision, AllDecisions) { - ObjectDecisionType decision_ignore; - decision_ignore.mutable_ignore(); - EXPECT_TRUE(PathObstacle::IsLongitudinalDecision(decision_ignore)); - - ObjectDecisionType decision_overtake; - decision_overtake.mutable_overtake(); - EXPECT_TRUE(PathObstacle::IsLongitudinalDecision(decision_overtake)); - - ObjectDecisionType decision_follow; - decision_follow.mutable_follow(); - EXPECT_TRUE(PathObstacle::IsLongitudinalDecision(decision_follow)); - - ObjectDecisionType decision_yield; - decision_yield.mutable_yield(); - EXPECT_TRUE(PathObstacle::IsLongitudinalDecision(decision_yield)); - - ObjectDecisionType decision_stop; - decision_stop.mutable_stop(); - EXPECT_TRUE(PathObstacle::IsLongitudinalDecision(decision_stop)); - - ObjectDecisionType decision_nudge; - decision_nudge.mutable_nudge(); - EXPECT_FALSE(PathObstacle::IsLongitudinalDecision(decision_nudge)); - - ObjectDecisionType decision_sidepass; - decision_sidepass.mutable_sidepass(); - EXPECT_FALSE(PathObstacle::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( - PathObstacle::MergeLongitudinalDecision(decision_stop, decision_ignore) - .has_stop()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_stop, decision_overtake) - .has_stop()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_stop, decision_follow) - .has_stop()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_stop, decision_yield) - .has_stop()); - - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_yield, decision_ignore) - .has_yield()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_yield, decision_overtake) - .has_yield()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_yield, decision_follow) - .has_yield()); - - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_follow, decision_ignore) - .has_follow()); - EXPECT_TRUE(PathObstacle::MergeLongitudinalDecision(decision_follow, - decision_overtake) - .has_follow()); - - EXPECT_TRUE(PathObstacle::MergeLongitudinalDecision(decision_overtake, - decision_ignore) - .has_overtake()); - - EXPECT_TRUE(PathObstacle::MergeLongitudinalDecision(decision_ignore, - decision_overtake) - .has_overtake()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_ignore, decision_follow) - .has_follow()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_ignore, decision_yield) - .has_yield()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_ignore, decision_stop) - .has_stop()); - - EXPECT_TRUE(PathObstacle::MergeLongitudinalDecision(decision_overtake, - decision_follow) - .has_follow()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_overtake, decision_yield) - .has_yield()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_overtake, decision_stop) - .has_stop()); - - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_follow, decision_yield) - .has_yield()); - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_follow, decision_stop) - .has_stop()); - - EXPECT_TRUE( - PathObstacle::MergeLongitudinalDecision(decision_yield, decision_stop) - .has_stop()); - - EXPECT_TRUE( - PathObstacle::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, PathObstacle::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, PathObstacle::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, PathObstacle::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, PathObstacle::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( - PathObstacle::MergeLateralDecision(decision_nudge, decision_ignore) - .has_nudge()); - - EXPECT_TRUE( - PathObstacle::MergeLateralDecision(decision_ignore, decision_nudge) - .has_nudge()); - - ObjectDecisionType decision_nudge2; - decision_nudge2.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE); - EXPECT_TRUE( - PathObstacle::MergeLateralDecision(decision_nudge, decision_nudge2) - .has_nudge()); - decision_nudge2.mutable_nudge()->set_type(ObjectNudge::RIGHT_NUDGE); -} - -TEST(PathObstacleTest, add_decision_test) { - // init state - { - PathObstacle path_obstacle; - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_FALSE(path_obstacle.HasLongitudinalDecision()); - } - - // Ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - decision.mutable_ignore(); - path_obstacle.AddLongitudinalDecision("test_ignore", decision); - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_FALSE(path_obstacle.LateralDecision().has_ignore()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_ignore()); - } - - // stop and ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_stop(); - path_obstacle.AddLongitudinalDecision("test_stop", decision); - - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_stop()); - - decision.mutable_ignore(); - path_obstacle.AddLongitudinalDecision("test_ignore", decision); - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_FALSE(path_obstacle.LateralDecision().has_ignore()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_stop()); - } - - // left nudge and ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE); - path_obstacle.AddLateralDecision("test_nudge", decision); - - EXPECT_TRUE(path_obstacle.HasLateralDecision()); - EXPECT_FALSE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LateralDecision().has_nudge()); - - decision.mutable_ignore(); - path_obstacle.AddLateralDecision("test_ignore", decision); - EXPECT_TRUE(path_obstacle.HasLateralDecision()); - EXPECT_FALSE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LateralDecision().has_nudge()); - EXPECT_FALSE(path_obstacle.LongitudinalDecision().has_ignore()); - } - - // right nudge and ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_nudge()->set_type(ObjectNudge::RIGHT_NUDGE); - path_obstacle.AddLateralDecision("test_nudge", decision); - - EXPECT_TRUE(path_obstacle.HasLateralDecision()); - EXPECT_FALSE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LateralDecision().has_nudge()); - - decision.mutable_ignore(); - path_obstacle.AddLateralDecision("test_ignore", decision); - EXPECT_TRUE(path_obstacle.HasLateralDecision()); - EXPECT_FALSE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LateralDecision().has_nudge()); - EXPECT_FALSE(path_obstacle.LongitudinalDecision().has_ignore()); - } - - // left nudge and right nudge - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE); - path_obstacle.AddLateralDecision("test_left_nudge", decision); - - EXPECT_TRUE(path_obstacle.HasLateralDecision()); - EXPECT_FALSE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LateralDecision().has_nudge()); - } - - // overtake and ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_overtake(); - path_obstacle.AddLongitudinalDecision("test_overtake", decision); - - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_overtake()); - - decision.mutable_ignore(); - path_obstacle.AddLongitudinalDecision("test_ignore", decision); - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_FALSE(path_obstacle.LateralDecision().has_ignore()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_overtake()); - } - - // follow and ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_follow(); - path_obstacle.AddLongitudinalDecision("test_follow", decision); - - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_follow()); - - decision.mutable_ignore(); - path_obstacle.AddLongitudinalDecision("test_ignore", decision); - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_FALSE(path_obstacle.LateralDecision().has_ignore()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_follow()); - } - - // yield and ignore - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_yield(); - path_obstacle.AddLongitudinalDecision("test_yield", decision); - - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_yield()); - - decision.mutable_ignore(); - path_obstacle.AddLongitudinalDecision("test_ignore", decision); - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_FALSE(path_obstacle.LateralDecision().has_ignore()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_yield()); - } - - // stop and nudge - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_stop(); - path_obstacle.AddLongitudinalDecision("test_stop", decision); - - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_stop()); - - decision.mutable_nudge(); - path_obstacle.AddLateralDecision("test_nudge", decision); - EXPECT_TRUE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LateralDecision().has_nudge()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_stop()); - } - - // stop and yield - { - PathObstacle path_obstacle; - ObjectDecisionType decision; - - decision.mutable_stop(); - path_obstacle.AddLongitudinalDecision("test_stop", decision); - - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_stop()); - - decision.mutable_yield(); - path_obstacle.AddLongitudinalDecision("test_yield", decision); - EXPECT_FALSE(path_obstacle.HasLateralDecision()); - EXPECT_TRUE(path_obstacle.HasLongitudinalDecision()); - EXPECT_TRUE(path_obstacle.LongitudinalDecision().has_stop()); - } -} -} // namespace planning -} // namespace apollo diff --git a/modules/planning/common/reference_line_info.cc b/modules/planning/common/reference_line_info.cc index 2e0b4bc753..3f82379dea 100644 --- a/modules/planning/common/reference_line_info.cc +++ b/modules/planning/common/reference_line_info.cc @@ -56,8 +56,7 @@ ReferenceLineInfo::ReferenceLineInfo(const common::VehicleState& vehicle_state, reference_line_(reference_line), lanes_(segments) {} -bool ReferenceLineInfo::Init( - const std::vector& obstacles) { +bool ReferenceLineInfo::Init(const std::vector& 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((adc_planning_point_.v() - path_obstacle->speed()) * + static_cast((adc_planning_point_.v() - obstacle->speed()) * kSafeTime)); const float kBackwardSafeDistance = std::max( kBackwardMinSafeDistance, - static_cast((path_obstacle->speed() - adc_planning_point_.v()) * + static_cast((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& obstacle) { + const std::shared_ptr& 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& obstacles) { + const std::vector& obstacles) { if (FLAGS_use_multi_thread_to_add_obstacles) { - std::vector> results; + std::vector> 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::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(); } } diff --git a/modules/planning/common/reference_line_info.h b/modules/planning/common/reference_line_info.h index 82ef73f386..cce78b8676 100644 --- a/modules/planning/common/reference_line_info.h +++ b/modules/planning/common/reference_line_info.h @@ -56,13 +56,13 @@ class ReferenceLineInfo { const ReferenceLine& reference_line, const hdmap::RouteSegments& segments); - bool Init(const std::vector& obstacles); + bool Init(const std::vector& obstacles); bool IsInited() const; - bool AddObstacles(const std::vector& obstacles); - PathObstacle* AddObstacle(const PathObstacle* obstacle); - bool AddObstacleHelper(const std::shared_ptr& obstacle); + bool AddObstacles(const std::vector& obstacles); + Obstacle* AddObstacle(const Obstacle* obstacle); + bool AddObstacleHelper(const std::shared_ptr& 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; diff --git a/modules/planning/common/speed_profile_generator_test.cc b/modules/planning/common/speed_profile_generator_test.cc index 9a42b30c34..874b2c1383 100644 --- a/modules/planning/common/speed_profile_generator_test.cc +++ b/modules/planning/common/speed_profile_generator_test.cc @@ -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 obstacles; + const std::vector obstacles; EgoInfo::Instance()->Update(adc_planning_point, vs, obstacles); auto speed_data2 = SpeedProfileGenerator::GenerateFallbackSpeedProfile(); diff --git a/modules/planning/constraint_checker/BUILD b/modules/planning/constraint_checker/BUILD index 7b75220fd8..3e16268c05 100644 --- a/modules/planning/constraint_checker/BUILD +++ b/modules/planning/constraint_checker/BUILD @@ -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", diff --git a/modules/planning/constraint_checker/collision_checker.cc b/modules/planning/constraint_checker/collision_checker.cc index 5345294fd3..0ec25f5e6b 100644 --- a/modules/planning/constraint_checker/collision_checker.cc +++ b/modules/planning/constraint_checker/collision_checker.cc @@ -41,8 +41,8 @@ using apollo::common::math::PathMatcher; using apollo::common::math::Vec2d; CollisionChecker::CollisionChecker( - const std::vector& obstacles, - const double ego_vehicle_s, const double ego_vehicle_d, + const std::vector& obstacles, const double ego_vehicle_s, + const double ego_vehicle_d, const std::vector& discretized_reference_line, const ReferenceLineInfo* ptr_reference_line_info, const std::shared_ptr& ptr_path_time_graph) { @@ -83,16 +83,16 @@ bool CollisionChecker::InCollision( } void CollisionChecker::BuildPredictedEnvironment( - const std::vector& obstacles, - const double ego_vehicle_s, const double ego_vehicle_d, + const std::vector& obstacles, const double ego_vehicle_s, + const double ego_vehicle_d, const std::vector& 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 obstacles_considered; - for (const PathObstacle* obstacle : obstacles) { + std::vector 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 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& discretized_reference_line) { double half_lane_width = FLAGS_default_reference_line_width * 0.5; TrajectoryPoint point = obstacle->GetPointAtTime(0.0); diff --git a/modules/planning/constraint_checker/collision_checker.h b/modules/planning/constraint_checker/collision_checker.h index ca27dad376..a0f6275c10 100644 --- a/modules/planning/constraint_checker/collision_checker.h +++ b/modules/planning/constraint_checker/collision_checker.h @@ -25,7 +25,7 @@ #include #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& obstacles, - const double ego_vehicle_s, const double ego_vehicle_d, + const std::vector& obstacles, const double ego_vehicle_s, + const double ego_vehicle_d, const std::vector& discretized_reference_line, const ReferenceLineInfo* ptr_reference_line_info, const std::shared_ptr& ptr_path_time_graph); @@ -46,15 +46,15 @@ class CollisionChecker { private: void BuildPredictedEnvironment( - const std::vector& obstacles, - const double ego_vehicle_s, const double ego_vehicle_d, + const std::vector& obstacles, const double ego_vehicle_s, + const double ego_vehicle_d, const std::vector& 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& discretized_reference_line); private: diff --git a/modules/planning/lattice/behavior/BUILD b/modules/planning/lattice/behavior/BUILD index b57ec298eb..2f76ea8b55 100644 --- a/modules/planning/lattice/behavior/BUILD +++ b/modules/planning/lattice/behavior/BUILD @@ -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", ], diff --git a/modules/planning/lattice/behavior/path_time_graph.cc b/modules/planning/lattice/behavior/path_time_graph.cc index 85a50b592e..9d80e1f568 100644 --- a/modules/planning/lattice/behavior/path_time_graph.cc +++ b/modules/planning/lattice/behavior/path_time_graph.cc @@ -45,7 +45,7 @@ using apollo::common::TrajectoryPoint; using apollo::perception::PerceptionObstacle; PathTimeGraph::PathTimeGraph( - const std::vector& obstacles, + const std::vector& obstacles, const std::vector& 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& obstacles, + const std::vector& obstacles, const std::vector& 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& 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& discretized_ref_points) { double relative_time = time_range_.first; while (relative_time < time_range_.second) { diff --git a/modules/planning/lattice/behavior/path_time_graph.h b/modules/planning/lattice/behavior/path_time_graph.h index 0edc95ef38..38e83778d8 100644 --- a/modules/planning/lattice/behavior/path_time_graph.h +++ b/modules/planning/lattice/behavior/path_time_graph.h @@ -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& obstacles, + PathTimeGraph(const std::vector& obstacles, const std::vector& 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& obstacles, + const std::vector& obstacles, const std::vector& 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& discretized_ref_points); void SetDynamicObstacle( - const PathObstacle* obstacle, + const Obstacle* obstacle, const std::vector& discretized_ref_points); void UpdateLateralBoundsByObstacle( diff --git a/modules/planning/lattice/behavior/prediction_querier.cc b/modules/planning/lattice/behavior/prediction_querier.cc index 673c132503..9bdd0daff0 100644 --- a/modules/planning/lattice/behavior/prediction_querier.cc +++ b/modules/planning/lattice/behavior/prediction_querier.cc @@ -29,7 +29,7 @@ namespace apollo { namespace planning { PredictionQuerier::PredictionQuerier( - const std::vector& obstacles, + const std::vector& obstacles, const std::shared_ptr>& ptr_reference_line) : ptr_reference_line_(ptr_reference_line) { for (const auto ptr_obstacle : obstacles) { @@ -42,7 +42,7 @@ PredictionQuerier::PredictionQuerier( } } -std::vector PredictionQuerier::GetObstacles() const { +std::vector PredictionQuerier::GetObstacles() const { return obstacles_; } diff --git a/modules/planning/lattice/behavior/prediction_querier.h b/modules/planning/lattice/behavior/prediction_querier.h index 863c9089b6..2fd0b402d2 100644 --- a/modules/planning/lattice/behavior/prediction_querier.h +++ b/modules/planning/lattice/behavior/prediction_querier.h @@ -25,7 +25,7 @@ #include #include -#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& obstacles, + const std::vector& obstacles, const std::shared_ptr>& ptr_reference_line); virtual ~PredictionQuerier() = default; - std::vector GetObstacles() const; + std::vector GetObstacles() const; double ProjectVelocityAlongReferenceLine(const std::string& obstacle_id, const double s, const double t) const; private: - std::unordered_map id_obstacle_map_; + std::unordered_map id_obstacle_map_; - std::vector obstacles_; + std::vector obstacles_; std::shared_ptr> ptr_reference_line_; }; diff --git a/modules/planning/navi/decider/navi_obstacle_decider.cc b/modules/planning/navi/decider/navi_obstacle_decider.cc index a3bbd3be02..a9ed42079d 100644 --- a/modules/planning/navi/decider/navi_obstacle_decider.cc +++ b/modules/planning/navi/decider/navi_obstacle_decider.cc @@ -81,7 +81,7 @@ double NaviObstacleDecider::GetMinLaneWidth( void NaviObstacleDecider::AddObstacleOffsetDirection( const common::PathPoint& projection_point, const std::vector& 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& 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& obstacles, +void NaviObstacleDecider::ProcessObstacle( + const std::vector& obstacles, const std::vector& 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& obstacles, + const std::vector& obstacles, const ReferenceLine& reference_line, const PathDecision& path_decision, const std::vector& 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& path_data_points, - const std::vector& obstacles) { + const std::vector& obstacles) { // Find start point of the reference line. double reference_line_y = path_data_points[0].y(); diff --git a/modules/planning/navi/decider/navi_obstacle_decider.h b/modules/planning/navi/decider/navi_obstacle_decider.h index 635017f1a2..8437dd7e42 100644 --- a/modules/planning/navi/decider/navi_obstacle_decider.h +++ b/modules/planning/navi/decider/navi_obstacle_decider.h @@ -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 &obstacles, + const std::vector &obstacles, const ReferenceLine &reference_line, const PathDecision &path_decision, const std::vector &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 &path_data_points, - const std::vector &obstacles); + const std::vector &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 &obstacles, - const std::vector &path_data_points, - const PathDecision &path_decision, const double min_lane_width, - const common::VehicleState &vehicle_state); + void ProcessObstacle(const std::vector &obstacles, + const std::vector &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 &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 &path_data_points, const common::VehicleState &vehicle_state, diff --git a/modules/planning/navi/decider/navi_obstacle_decider_test.cc b/modules/planning/navi/decider/navi_obstacle_decider_test.cc index ece61e8261..f931eb2bbd 100644 --- a/modules/planning/navi/decider/navi_obstacle_decider_test.cc +++ b/modules/planning/navi/decider/navi_obstacle_decider_test.cc @@ -40,7 +40,7 @@ namespace planning { TEST(NaviObstacleDeciderTest, ComputeNudgeDist1) { NaviObstacleDecider obstacle_decider; - std::vector vec_obstacle; + std::vector vec_obstacle; std::vector 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 vec_obstacle; + std::vector vec_obstacle; std::vector 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 vec_obstacle; + std::vector vec_obstacle; std::vector 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 vec_obstacle; + std::vector vec_obstacle; std::vector 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 vec_obstacle; + std::vector vec_obstacle; std::vector 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); diff --git a/modules/planning/navi/decider/navi_path_decider.cc b/modules/planning/navi/decider/navi_path_decider.cc index 2d9c83e7ea..98fbba9feb 100644 --- a/modules/planning/navi/decider/navi_path_decider.cc +++ b/modules/planning/navi/decider/navi_path_decider.cc @@ -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& obstacles, + const std::vector& 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& path_data_points, - const std::vector& obstacles, + const std::vector& obstacles, const PathDecision& path_decision, const common::VehicleState& vehicle_state) { double nudge_position_y = 0.0; diff --git a/modules/planning/navi/decider/navi_path_decider.h b/modules/planning/navi/decider/navi_path_decider.h index 52a9cfd55e..3724d73438 100644 --- a/modules/planning/navi/decider/navi_path_decider.h +++ b/modules/planning/navi/decider/navi_path_decider.h @@ -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 &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 &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 &path_data_points, - const std::vector &obstacles, + const std::vector &obstacles, const PathDecision &path_decision, const common::VehicleState &vehicle_state); /** diff --git a/modules/planning/navi/decider/navi_speed_decider.cc b/modules/planning/navi/decider/navi_speed_decider.cc index 323ef1673b..e7645e970a 100644 --- a/modules/planning/navi/decider/navi_speed_decider.cc +++ b/modules/planning/navi/decider/navi_speed_decider.cc @@ -250,8 +250,8 @@ Status NaviSpeedDecider::Execute(Frame* frame, Status NaviSpeedDecider::MakeSpeedDecision( double start_v, double start_a, double start_da, const std::vector& path_points, - const std::vector& obstacles, - const std::function& find_obstacle, + const std::vector& obstacles, + const std::function& 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& path_points, - const std::vector& obstacles, - const std::function& - find_obstacle) { + const std::vector& obstacles, + const std::function& find_obstacle) { const auto& vehicle_config = VehicleConfigHelper::Instance()->GetConfig(); auto front_edge_to_center = vehicle_config.vehicle_param().front_edge_to_center(); diff --git a/modules/planning/navi/decider/navi_speed_decider.h b/modules/planning/navi/decider/navi_speed_decider.h index 196f080b3c..1ea696f66f 100644 --- a/modules/planning/navi/decider/navi_speed_decider.h +++ b/modules/planning/navi/decider/navi_speed_decider.h @@ -81,8 +81,8 @@ class NaviSpeedDecider : public NaviTask { apollo::common::Status MakeSpeedDecision( double start_v, double start_a, double start_da, const std::vector& path_points, - const std::vector& obstacles, - const std::function& + const std::vector& obstacles, + const std::function& 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& path_points, - const std::vector& obstacles, - const std::function& + const std::vector& obstacles, + const std::function& find_obstacle); /** diff --git a/modules/planning/navi/decider/navi_speed_decider_test.cc b/modules/planning/navi/decider/navi_speed_decider_test.cc index 3a6846758f..2142e91f71 100644 --- a/modules/planning/navi/decider/navi_speed_decider_test.cc +++ b/modules/planning/navi/decider/navi_speed_decider_test.cc @@ -67,8 +67,8 @@ TEST(NaviSpeedDeciderTest, CreateSpeedData) { speed_decider.kappa_threshold_ = 0.0; PerceptionObstacle perception_obstacle; - std::map obstacle_buf; - std::vector obstacles; + std::map obstacle_buf; + std::vector obstacles; std::vector 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 obstacle_buf; - std::vector obstacles; + std::map obstacle_buf; + std::vector obstacles; std::vector 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 obstacle_buf; - std::vector obstacles; + std::map obstacle_buf; + std::vector obstacles; std::vector 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 obstacle_buf; - std::vector obstacles; + std::map obstacle_buf; + std::vector obstacles; std::vector path_points; double s = 0.0; diff --git a/modules/planning/open_space/BUILD b/modules/planning/open_space/BUILD index 3780715122..deda1b6a15 100644 --- a/modules/planning/open_space/BUILD +++ b/modules/planning/open_space/BUILD @@ -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", ], diff --git a/modules/planning/open_space/distance_approach_problem_wrapper.cc b/modules/planning/open_space/distance_approach_problem_wrapper.cc index b4c1852bfc..5bab72a8a7 100644 --- a/modules/planning/open_space/distance_approach_problem_wrapper.cc +++ b/modules/planning/open_space/distance_approach_problem_wrapper.cc @@ -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 obstacle = - PathObstacle::CreateStaticVirtualObstacles(std::to_string(id), - obstacle_box); + std::unique_ptr obstacle = Obstacle::CreateStaticVirtualObstacles( + std::to_string(id), obstacle_box); obstacles_list.Add(obstacle->Id(), *obstacle); } diff --git a/modules/planning/open_space/hybrid_a_star.h b/modules/planning/open_space/hybrid_a_star.h index 772b5be67d..4fe7cd3595 100644 --- a/modules/planning/open_space/hybrid_a_star.h +++ b/modules/planning/open_space/hybrid_a_star.h @@ -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" diff --git a/modules/planning/open_space/hybrid_a_star_test.cc b/modules/planning/open_space/hybrid_a_star_test.cc index 6847c899c3..c9dfa39c06 100644 --- a/modules/planning/open_space/hybrid_a_star_test.cc +++ b/modules/planning/open_space/hybrid_a_star_test.cc @@ -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 obstacle = - PathObstacle::CreateStaticVirtualObstacles("a box in center", - obstacle_box); + std::unique_ptr 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)); diff --git a/modules/planning/open_space/hybrid_a_star_wrapper.cc b/modules/planning/open_space/hybrid_a_star_wrapper.cc index c7e3b423e7..c03a7236a7 100644 --- a/modules/planning/open_space/hybrid_a_star_wrapper.cc +++ b/modules/planning/open_space/hybrid_a_star_wrapper.cc @@ -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 obstacle = - PathObstacle::CreateStaticVirtualObstacles(std::to_string(id), - obstacle_box); + std::unique_ptr obstacle = Obstacle::CreateStaticVirtualObstacles( + std::to_string(id), obstacle_box); obstacles_list.Add(obstacle->Id(), *obstacle); } ThreadSafeIndexedObstacles* GetObstacleList() { return &obstacles_list; } diff --git a/modules/planning/planner/navi/navi_planner.cc b/modules/planning/planner/navi/navi_planner.cc index 3cdfcbc2f6..548282e4f4 100644 --- a/modules/planning/planner/navi/navi_planner.cc +++ b/modules/planning/planner/navi/navi_planner.cc @@ -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(); diff --git a/modules/planning/planner/public_road/BUILD b/modules/planning/planner/public_road/BUILD index c0d2493b99..511c4bc793 100644 --- a/modules/planning/planner/public_road/BUILD +++ b/modules/planning/planner/public_road/BUILD @@ -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", ], ) diff --git a/modules/planning/scenarios/lane_follow/BUILD b/modules/planning/scenarios/lane_follow/BUILD index d359ab911f..c5dcbe3c97 100644 --- a/modules/planning/scenarios/lane_follow/BUILD +++ b/modules/planning/scenarios/lane_follow/BUILD @@ -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", diff --git a/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc b/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc index 32dae58a7e..019657d0a9 100644 --- a/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc +++ b/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc @@ -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" diff --git a/modules/planning/scenarios/lane_follow/lane_follow_stage.cc b/modules/planning/scenarios/lane_follow/lane_follow_stage.cc index 8fbddcb4ca..58338d37b3 100644 --- a/modules/planning/scenarios/lane_follow/lane_follow_stage.cc +++ b/modules/planning/scenarios/lane_follow/lane_follow_stage.cc @@ -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; } diff --git a/modules/planning/scenarios/side_pass/side_pass_scenario.cc b/modules/planning/scenarios/side_pass/side_pass_scenario.cc index 1f02d9acdf..e40e409315 100644 --- a/modules/planning/scenarios/side_pass/side_pass_scenario.cc +++ b/modules/planning/scenarios/side_pass/side_pass_scenario.cc @@ -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 { diff --git a/modules/planning/scenarios/side_pass/side_pass_stage.cc b/modules/planning/scenarios/side_pass/side_pass_stage.cc index 864f467853..5cec84a449 100644 --- a/modules/planning/scenarios/side_pass/side_pass_stage.cc +++ b/modules/planning/scenarios/side_pass/side_pass_stage.cc @@ -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; } diff --git a/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.cc b/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.cc index 39a4d23ec7..18ad30ef49 100644 --- a/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.cc +++ b/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.cc @@ -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& watch_vehicle_ids, + const Obstacle& obstacle, const std::vector& 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(); diff --git a/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.h b/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.h index 6d93d3a366..ad12fc4d02 100644 --- a/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.h +++ b/modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.h @@ -65,7 +65,7 @@ class StopSignUnprotectedStop : public Stage { } int RemoveWatchVehicle( - const PathObstacle& path_obstacle, + const Obstacle& obstacle, const std::vector& watch_vehicle_ids, std::unordered_map>* watch_vehicles); @@ -89,7 +89,7 @@ class StopSignUnprotectedPreStop : public Stage { return GetContextAs(); } - int AddWatchVehicle(const PathObstacle& path_obstacle, + int AddWatchVehicle(const Obstacle& obstacle, std::unordered_map>* watch_vehicles); bool CheckADCStop(const ReferenceLineInfo& reference_line_info); diff --git a/modules/planning/toolkits/BUILD b/modules/planning/toolkits/BUILD index 90aed3c88c..4cca6bdfb0 100644 --- a/modules/planning/toolkits/BUILD +++ b/modules/planning/toolkits/BUILD @@ -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", diff --git a/modules/planning/toolkits/deciders/decider_creep.cc b/modules/planning/toolkits/deciders/decider_creep.cc index 157c4a0012..f0ae77b0f2 100644 --- a/modules/planning/toolkits/deciders/decider_creep.cc +++ b/modules/planning/toolkits/deciders/decider_creep.cc @@ -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; diff --git a/modules/planning/toolkits/deciders/decider_stop_sign.cc b/modules/planning/toolkits/deciders/decider_stop_sign.cc index 857d311f83..ba43069f31 100644 --- a/modules/planning/toolkits/deciders/decider_stop_sign.cc +++ b/modules/planning/toolkits/deciders/decider_stop_sign.cc @@ -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; } diff --git a/modules/planning/toolkits/deciders/side_pass_path_decider.cc b/modules/planning/toolkits/deciders/side_pass_path_decider.cc index e21e892518..6ddfdb14e6 100644 --- a/modules/planning/toolkits/deciders/side_pass_path_decider.cc +++ b/modules/planning/toolkits/deciders/side_pass_path_decider.cc @@ -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; } diff --git a/modules/planning/toolkits/deciders/side_pass_safety.cc b/modules/planning/toolkits/deciders/side_pass_safety.cc index 95d1619cf8..ba7e85b95e 100644 --- a/modules/planning/toolkits/deciders/side_pass_safety.cc +++ b/modules/planning/toolkits/deciders/side_pass_safety.cc @@ -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; } diff --git a/modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.cc b/modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.cc index 829583965f..eb38b0c895 100644 --- a/modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.cc +++ b/modules/planning/toolkits/optimizers/dp_poly_path/dp_poly_path_optimizer.cc @@ -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"); diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/BUILD b/modules/planning/toolkits/optimizers/dp_st_speed/BUILD index 502b6be7fd..77817f1812 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/BUILD +++ b/modules/planning/toolkits/optimizers/dp_st_speed/BUILD @@ -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", diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.cc b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.cc index 300f8be073..13ad5b7df3 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.cc +++ b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.cc @@ -33,7 +33,7 @@ constexpr float kInf = std::numeric_limits::infinity(); } DpStCost::DpStCost(const DpStSpeedConfig& config, - const std::vector& obstacles, + const std::vector& 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& obstacles) { + const std::vector& obstacles) { for (const auto& obstacle : obstacles) { if (obstacle->st_boundary().IsEmpty()) { continue; diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.h b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.h index ae39dcd370..0a0ca6ae78 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.h +++ b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_cost.h @@ -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& obstacles, + const std::vector& 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& obstacles); + void AddToKeepClearRange(const std::vector& obstacles); static void SortAndMergeRange( std::vector>* keep_clear_range_); bool InKeepClearRange(float s) const; const DpStSpeedConfig& config_; - const std::vector& obstacles_; + const std::vector& obstacles_; const common::TrajectoryPoint& init_point_; float unit_t_ = 0.0; diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.cc b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.cc index f449ed8479..ee2f932d7f 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.cc +++ b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.cc @@ -63,7 +63,7 @@ bool CheckOverlapOnDpStGraph(const std::vector& boundaries, DpStGraph::DpStGraph(const StGraphData& st_graph_data, const DpStSpeedConfig& dp_config, - const std::vector& obstacles, + const std::vector& obstacles, const common::TrajectoryPoint& init_point, const SLBoundary& adc_sl_boundary) : st_graph_data_(st_graph_data), diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.h b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.h index cf704d5464..933721d75f 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.h +++ b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph.h @@ -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& obstacles, + const std::vector& 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& obstacles_; + const std::vector& obstacles_; // vehicle configuration parameter const common::VehicleParam& vehicle_param_ = diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph_test.cc b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph_test.cc index b14848df20..d59dd8c9ff 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph_test.cc +++ b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_graph_test.cc @@ -77,7 +77,7 @@ class DpStGraphTest : public ::testing::Test { virtual void TearDown() {} protected: - std::list path_obstacle_list_; + std::list 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 obstacles_; - obstacles_.emplace_back(&(path_obstacle_list_.back())); + std::vector obstacles_; + obstacles_.emplace_back(&(obstacle_list_.back())); std::vector upper_points; std::vector 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 boundaries; boundaries.push_back(&(obstacles_.back()->st_boundary())); diff --git a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.cc b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.cc index d01caadd6e..066bf238e1 100644 --- a/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.cc +++ b/modules/planning/toolkits/optimizers/dp_st_speed/dp_st_speed_optimizer.cc @@ -55,7 +55,7 @@ bool DpStSpeedOptimizer::SearchStGraph( SpeedData* speed_data, PathDecision* path_decision, STGraphDebug* st_graph_debug) const { std::vector 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."; diff --git a/modules/planning/toolkits/optimizers/path_decider/path_decider.cc b/modules/planning/toolkits/optimizers/path_decider/path_decider.cc index f1e3192e83..4ea4fd3800 100644 --- a/modules/planning/toolkits/optimizers/path_decider/path_decider.cc +++ b/modules/planning/toolkits/optimizers/path_decider/path_decider.cc @@ -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()); diff --git a/modules/planning/toolkits/optimizers/path_decider/path_decider.h b/modules/planning/toolkits/optimizers/path_decider/path_decider.h index 8707777cca..068dc79a82 100644 --- a/modules/planning/toolkits/optimizers/path_decider/path_decider.h +++ b/modules/planning/toolkits/optimizers/path_decider/path_decider.h @@ -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 diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/BUILD b/modules/planning/toolkits/optimizers/poly_st_speed/BUILD deleted file mode 100644 index 1d1ff1eb1a..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/BUILD +++ /dev/null @@ -1,69 +0,0 @@ -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() diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.cc b/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.cc deleted file mode 100644 index 25d896da30..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.cc +++ /dev/null @@ -1,136 +0,0 @@ -/****************************************************************************** - * 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 - -#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 &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> 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> &points, - const std::vector &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::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> *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 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 diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h b/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h deleted file mode 100644 index ef66da107b..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_graph.h +++ /dev/null @@ -1,93 +0,0 @@ -/****************************************************************************** - * 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 -#include -#include -#include - -#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 &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> &points, - const std::vector &obstacles, - PolyStGraphNode *const min_cost_node); - - bool SampleStPoints(std::vector> *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 diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.cc b/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.cc deleted file mode 100644 index 92b11ff414..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.cc +++ /dev/null @@ -1,125 +0,0 @@ -/****************************************************************************** - * 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 -#include -#include -#include - -#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 diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h b/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h deleted file mode 100644 index 14081a01c6..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/poly_st_speed_optimizer.h +++ /dev/null @@ -1,52 +0,0 @@ -/****************************************************************************** - * 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 diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.cc b/modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.cc deleted file mode 100644 index f0c449c2b1..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.cc +++ /dev/null @@ -1,125 +0,0 @@ -/****************************************************************************** - * 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 - -#include "modules/planning/common/planning_gflags.h" - -namespace apollo { -namespace planning { -namespace { -constexpr auto kInfCost = std::numeric_limits::infinity(); -constexpr double kEpsilon = 1e-6; -} // namespace - -using apollo::common::TrajectoryPoint; - -SpeedProfileCost::SpeedProfileCost( - const PolyStSpeedConfig &config, - const std::vector &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 diff --git a/modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h b/modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h deleted file mode 100644 index c7f6f1ea28..0000000000 --- a/modules/planning/toolkits/optimizers/poly_st_speed/speed_profile_cost.h +++ /dev/null @@ -1,57 +0,0 @@ -/****************************************************************************** - * 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 - -#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 &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 &obstacles_; - const SpeedLimit &speed_limit_; - const common::TrajectoryPoint &init_point_; -}; - -} // namespace planning -} // namespace apollo diff --git a/modules/planning/toolkits/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc b/modules/planning/toolkits/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc index cdb1c5639a..b6a5a47d77 100644 --- a/modules/planning/toolkits/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc +++ b/modules/planning/toolkits/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc @@ -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!"); diff --git a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/BUILD b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/BUILD index d3e306fde4..8a6a39bf82 100644 --- a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/BUILD +++ b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/BUILD @@ -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", diff --git a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc index 044173625b..3aca56ab53 100644 --- a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc +++ b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc @@ -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& obstacles) { + const std::vector& obstacles) { const auto& qp_config = config_.qp_piecewise_jerk_path_config(); int size = std::max(2, static_cast(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); diff --git a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h index 47c71cbcfe..91abe41677 100644 --- a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h +++ b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h @@ -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& obstacles); + const std::vector& obstacles); std::vector> GetLateralSecondOrderDerivativeBounds( diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/BUILD b/modules/planning/toolkits/optimizers/qp_spline_path/BUILD index 2e041a4ca8..abc34696c4 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/BUILD +++ b/modules/planning/toolkits/optimizers/qp_spline_path/BUILD @@ -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", diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.cc b/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.cc index 6ff42cf324..3e159ec870 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.cc +++ b/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.cc @@ -67,8 +67,7 @@ QpFrenetFrame::QpFrenetFrame(const ReferenceLine& reference_line, DCHECK_GE(evaluated_s_.size(), 2); } -bool QpFrenetFrame::Init( - const std::vector& path_obstacles) { +bool QpFrenetFrame::Init(const std::vector& 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 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& path_obstacles) { - for (const auto ptr_path_obstacle : path_obstacles) { - if (!ptr_path_obstacle->HasLateralDecision()) { + const std::vector& 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; } diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.h b/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.h index 8237683304..33bb4ac72a 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.h +++ b/modules/planning/toolkits/optimizers/qp_spline_path/qp_frenet_frame.h @@ -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& evaluated_s); virtual ~QpFrenetFrame() = default; - bool Init(const std::vector& path_obstacles); + bool Init(const std::vector& 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& path_obstacles); + bool CalculateObstacleBound(const std::vector& obstacles); uint32_t FindIndex(const double s) const; diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc index 4a606dec8a..3da5a339ee 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc +++ b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc @@ -81,10 +81,9 @@ void QpSplinePathGenerator::SetChangeLane(bool is_change_lane_path) { } bool QpSplinePathGenerator::Generate( - const std::vector& 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& 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; } diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.h b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.h index 8c620cdd14..1a9cf1a653 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.h +++ b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.h @@ -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& path_obstacles, + bool Generate(const std::vector& obstacles, const SpeedData& speed_data, const common::TrajectoryPoint& init_point, const double boundary_extension, bool is_final_attempt, diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.cc b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.cc index 88a0ac15bf..36c9d97c31 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.cc +++ b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_optimizer.cc @@ -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."; diff --git a/modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc b/modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc index c32d3243e6..c75cd1398d 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc +++ b/modules/planning/toolkits/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc @@ -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 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!"); diff --git a/modules/planning/toolkits/optimizers/road_graph/BUILD b/modules/planning/toolkits/optimizers/road_graph/BUILD index c700b32d6a..16d5d9bbf1 100644 --- a/modules/planning/toolkits/optimizers/road_graph/BUILD +++ b/modules/planning/toolkits/optimizers/road_graph/BUILD @@ -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", diff --git a/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc b/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc index f1a4e71c84..d0780e283d 100644 --- a/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc +++ b/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc @@ -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 &obstacles, - PathData *const path_data) { +bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point, + const std::vector &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 &obstacles, + const std::vector &obstacles, std::vector *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); } diff --git a/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.h b/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.h index ce5c1f9758..49c2932399 100644 --- a/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.h +++ b/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.h @@ -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 &obstacles, + const std::vector &obstacles, PathData *const path_data); void SetDebugLogger(apollo::planning_internal::Debug *debug) { @@ -97,7 +97,7 @@ class DpRoadGraph { std::numeric_limits::infinity()}; QuinticPolynomialCurve1d min_cost_curve; }; - bool GenerateMinCostPath(const std::vector &obstacles, + bool GenerateMinCostPath(const std::vector &obstacles, std::vector *min_cost_path); bool IsValidCurve(const QuinticPolynomialCurve1d &curve) const; diff --git a/modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.cc b/modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.cc deleted file mode 100644 index 440e36a6cf..0000000000 --- a/modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.cc +++ /dev/null @@ -1,208 +0,0 @@ -/****************************************************************************** - * 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 -#include - -#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> *const points) { - CHECK_NOTNULL(points); - points->clear(); - points->insert(points->begin(), std::vector{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 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 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 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 diff --git a/modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.h b/modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.h deleted file mode 100644 index 4320bd3bf3..0000000000 --- a/modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.h +++ /dev/null @@ -1,44 +0,0 @@ -/****************************************************************************** - * 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 side_pass_waypoint_sampler.h - **/ - -#pragma once - -#include - -#include "modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.h" - -namespace apollo { -namespace planning { - -class SidePassWaypointSampler final : public WaypointSampler { - public: - explicit SidePassWaypointSampler(const WaypointSamplerConfig &config) - : WaypointSampler(config) {} - virtual ~SidePassWaypointSampler() = default; - - bool SamplePathWaypoints( - const common::TrajectoryPoint &init_point, - std::vector> *const points) override; - - bool HasSidepass(); -}; - -} // namespace planning -} // namespace apollo diff --git a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc index 0c833c15a4..ee6f606eb7 100644 --- a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc +++ b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc @@ -40,12 +40,13 @@ using apollo::common::math::Box2d; using apollo::common::math::Sigmoid; using apollo::common::math::Vec2d; -TrajectoryCost::TrajectoryCost( - const DpPolyPathConfig &config, const ReferenceLine &reference_line, - const bool is_change_lane_path, - const std::vector &obstacles, - const common::VehicleParam &vehicle_param, - const SpeedData &heuristic_speed_data, const common::SLPoint &init_sl_point) +TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config, + const ReferenceLine &reference_line, + const bool is_change_lane_path, + const std::vector &obstacles, + const common::VehicleParam &vehicle_param, + const SpeedData &heuristic_speed_data, + const common::SLPoint &init_sl_point) : config_(config), reference_line_(&reference_line), is_change_lane_path_(is_change_lane_path), @@ -58,13 +59,13 @@ TrajectoryCost::TrajectoryCost( num_of_time_stamps_ = static_cast( std::floor(total_time / config.eval_time_interval())); - for (const auto *ptr_path_obstacle : obstacles) { - if (ptr_path_obstacle->IsIgnore()) { + for (const auto *ptr_obstacle : obstacles) { + if (ptr_obstacle->IsIgnore()) { continue; - } else if (ptr_path_obstacle->LongitudinalDecision().has_stop()) { + } else if (ptr_obstacle->LongitudinalDecision().has_stop()) { continue; } - const auto &sl_boundary = ptr_path_obstacle->PerceptionSLBoundary(); + const auto &sl_boundary = ptr_obstacle->PerceptionSLBoundary(); const float adc_left_l = init_sl_point_.l() + vehicle_param_.left_edge_to_center(); @@ -77,26 +78,24 @@ TrajectoryCost::TrajectoryCost( } bool is_bycycle_or_pedestrian = - (ptr_path_obstacle->Perception().type() == + (ptr_obstacle->Perception().type() == perception::PerceptionObstacle::BICYCLE || - ptr_path_obstacle->Perception().type() == + ptr_obstacle->Perception().type() == perception::PerceptionObstacle::PEDESTRIAN); - if (PathObstacle::IsVirtualObstacle(ptr_path_obstacle->Perception())) { + if (Obstacle::IsVirtualObstacle(ptr_obstacle->Perception())) { // Virtual obstacle continue; - } else if (PathObstacle::IsStaticObstacle( - ptr_path_obstacle->Perception()) || + } else if (Obstacle::IsStaticObstacle(ptr_obstacle->Perception()) || is_bycycle_or_pedestrian) { static_obstacle_sl_boundaries_.push_back(std::move(sl_boundary)); } else { std::vector box_by_time; for (uint32_t t = 0; t <= num_of_time_stamps_; ++t) { TrajectoryPoint trajectory_point = - ptr_path_obstacle->GetPointAtTime(t * config.eval_time_interval()); + ptr_obstacle->GetPointAtTime(t * config.eval_time_interval()); - Box2d obstacle_box = - ptr_path_obstacle->GetBoundingBox(trajectory_point); + Box2d obstacle_box = ptr_obstacle->GetBoundingBox(trajectory_point); constexpr float kBuff = 0.5; Box2d expanded_obstacle_box = Box2d(obstacle_box.center(), obstacle_box.heading(), diff --git a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h index c78f356138..439ac8d86e 100644 --- a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h +++ b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h @@ -26,8 +26,8 @@ #include "modules/planning/proto/dp_poly_path_config.pb.h" #include "modules/common/math/box2d.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/math/curve1d/quintic_polynomial_curve1d.h" #include "modules/planning/reference_line/reference_line.h" @@ -42,7 +42,7 @@ class TrajectoryCost { explicit TrajectoryCost(const DpPolyPathConfig &config, const ReferenceLine &reference_line, const bool is_change_lane_path, - const std::vector &obstacles, + const std::vector &obstacles, const common::VehicleParam &vehicle_param, const SpeedData &heuristic_speed_data, const common::SLPoint &init_sl_point); diff --git a/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.cc b/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.cc index e8114f88d3..bc9b4032b1 100644 --- a/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.cc +++ b/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.cc @@ -204,7 +204,7 @@ bool WaypointSampler::SamplePathWaypoints( bool WaypointSampler::HasSidepass() { const auto &path_decision = reference_line_info_->path_decision(); - for (const auto &obstacle : path_decision.path_obstacles().Items()) { + for (const auto &obstacle : path_decision.obstacles().Items()) { if (obstacle->LateralDecision().has_sidepass()) { sidepass_ = obstacle->LateralDecision().sidepass(); return true; diff --git a/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.h b/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.h index 7b1c8cb91d..430cb18c5a 100644 --- a/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.h +++ b/modules/planning/toolkits/optimizers/road_graph/waypoint_sampler.h @@ -28,9 +28,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/trajectory/discretized_trajectory.h" #include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h" diff --git a/modules/planning/toolkits/optimizers/side_pass_path/BUILD b/modules/planning/toolkits/optimizers/side_pass_path/BUILD deleted file mode 100644 index 4a80e44082..0000000000 --- a/modules/planning/toolkits/optimizers/side_pass_path/BUILD +++ /dev/null @@ -1,19 +0,0 @@ -load("//tools:cpplint.bzl", "cpplint") - -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "side_pass_path_optimizer", - srcs = [ - "side_pass_path_optimizer.cc", - ], - hdrs = [ - "side_pass_path_optimizer.h", - ], - deps = [ - "//modules/planning/toolkits/optimizers:path_optimizer", - "//modules/planning/toolkits/optimizers/road_graph", - ], -) - -cpplint() diff --git a/modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.cc b/modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.cc deleted file mode 100644 index 8f28d4410c..0000000000 --- a/modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.cc +++ /dev/null @@ -1,66 +0,0 @@ -/****************************************************************************** - * 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 - **/ - -#include "modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.h" - -#include -#include - -#include "modules/common/util/file.h" -#include "modules/planning/common/planning_gflags.h" -#include "modules/planning/toolkits/optimizers/road_graph/dp_road_graph.h" -#include "modules/planning/toolkits/optimizers/road_graph/side_pass_waypoint_sampler.h" - -namespace apollo { -namespace planning { - -using apollo::common::ErrorCode; -using apollo::common::Status; - -SidePassPathOptimizer::SidePassPathOptimizer(const TaskConfig &config) - : PathOptimizer(config) { - CHECK(config_.has_dp_poly_path_config()); - SetName("SidePassPathOptimizer"); -} - -Status SidePassPathOptimizer::Process(const SpeedData &speed_data, - const ReferenceLine &, - const common::TrajectoryPoint &init_point, - PathData *const path_data) { - CHECK_NOTNULL(path_data); - DpRoadGraph dp_road_graph(config_.dp_poly_path_config(), - *reference_line_info_, speed_data); - dp_road_graph.SetDebugLogger(reference_line_info_->mutable_debug()); - dp_road_graph.SetWaypointSampler(new SidePassWaypointSampler( - config_.dp_poly_path_config().waypoint_sampler_config())); - - if (!dp_road_graph.FindPathTunnel( - init_point, - reference_line_info_->path_decision()->path_obstacles().Items(), - path_data)) { - AERROR << "Failed to find tunnel in road graph"; - return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation"); - } - - return Status::OK(); -} - -} // namespace planning -} // namespace apollo diff --git a/modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.h b/modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.h deleted file mode 100644 index 8215235d68..0000000000 --- a/modules/planning/toolkits/optimizers/side_pass_path/side_pass_path_optimizer.h +++ /dev/null @@ -1,48 +0,0 @@ -/****************************************************************************** - * 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 - **/ - -#pragma once - -#include "modules/planning/proto/dp_poly_path_config.pb.h" -#include "modules/planning/proto/planning_config.pb.h" - -#include "modules/planning/toolkits/optimizers/path_optimizer.h" - -namespace apollo { -namespace planning { - -/** - * @class SidePassPathOptimizer - * @brief SidePassPathOptimizer does path planning with dynamic programming - * algorithm. - */ -class SidePassPathOptimizer final : public PathOptimizer { - public: - explicit SidePassPathOptimizer(const TaskConfig &config); - - private: - apollo::common::Status Process(const SpeedData &speed_data, - const ReferenceLine &reference_line, - const common::TrajectoryPoint &init_point, - PathData *const path_data) override; -}; - -} // namespace planning -} // namespace apollo diff --git a/modules/planning/toolkits/optimizers/speed_decider/speed_decider.cc b/modules/planning/toolkits/optimizers/speed_decider/speed_decider.cc index d073e83b01..889bc6b608 100644 --- a/modules/planning/toolkits/optimizers/speed_decider/speed_decider.cc +++ b/modules/planning/toolkits/optimizers/speed_decider/speed_decider.cc @@ -130,21 +130,21 @@ SpeedDecider::StPosition SpeedDecider::GetStPosition( return st_position; } -bool SpeedDecider::IsFollowTooClose(const PathObstacle& path_obstacle) const { - if (!path_obstacle.IsBlockingObstacle()) { +bool SpeedDecider::IsFollowTooClose(const Obstacle& obstacle) const { + if (!obstacle.IsBlockingObstacle()) { return false; } - if (path_obstacle.st_boundary().min_t() > 0.0) { + if (obstacle.st_boundary().min_t() > 0.0) { return false; } - const double obs_speed = path_obstacle.speed(); + const double obs_speed = obstacle.speed(); const double ego_speed = init_point_.v(); if (obs_speed > ego_speed) { return false; } const double distance = - path_obstacle.st_boundary().min_s() - FLAGS_min_stop_distance_obstacle; + obstacle.st_boundary().min_s() - FLAGS_min_stop_distance_obstacle; constexpr double decel = 1.0; return distance < std::pow((ego_speed - obs_speed), 2) * 0.5 / decel; } @@ -156,19 +156,19 @@ Status SpeedDecider::MakeObjectDecision( AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - for (const auto* obstacle : path_decision->path_obstacles().Items()) { - auto* path_obstacle = path_decision->Find(obstacle->Id()); - const auto& boundary = path_obstacle->st_boundary(); + for (const auto* obstacle : path_decision->obstacles().Items()) { + auto* mutable_obstacle = path_decision->Find(obstacle->Id()); + const auto& boundary = mutable_obstacle->st_boundary(); if (boundary.IsEmpty() || boundary.max_s() < 0.0 || boundary.max_t() < 0.0 || boundary.min_t() > dp_st_speed_config_.total_time()) { - AppendIgnoreDecision(path_obstacle); + AppendIgnoreDecision(mutable_obstacle); continue; } - if (path_obstacle->HasLongitudinalDecision()) { - AppendIgnoreDecision(path_obstacle); + if (obstacle->HasLongitudinalDecision()) { + AppendIgnoreDecision(mutable_obstacle); continue; } @@ -177,36 +177,36 @@ Status SpeedDecider::MakeObjectDecision( case BELOW: if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) { ObjectDecisionType stop_decision; - if (CreateStopDecision(*path_obstacle, &stop_decision, + if (CreateStopDecision(*mutable_obstacle, &stop_decision, -FLAGS_stop_line_stop_distance)) { - path_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear", - stop_decision); + mutable_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear", + stop_decision); } } else if (CheckIsFollowByT(boundary) && (boundary.max_t() - boundary.min_t() > FLAGS_follow_min_time_sec)) { // stop for low_speed decelerating - if (IsFollowTooClose(*path_obstacle)) { + if (IsFollowTooClose(*mutable_obstacle)) { ObjectDecisionType stop_decision; - if (CreateStopDecision(*path_obstacle, &stop_decision, + if (CreateStopDecision(*mutable_obstacle, &stop_decision, -FLAGS_min_stop_distance_obstacle)) { - path_obstacle->AddLongitudinalDecision("dp_st_graph/too_close", - stop_decision); + mutable_obstacle->AddLongitudinalDecision("dp_st_graph/too_close", + stop_decision); } } else { // high speed or low speed accelerating // FOLLOW decision ObjectDecisionType follow_decision; - if (CreateFollowDecision(*path_obstacle, &follow_decision)) { - path_obstacle->AddLongitudinalDecision("dp_st_graph", - follow_decision); + if (CreateFollowDecision(*mutable_obstacle, &follow_decision)) { + mutable_obstacle->AddLongitudinalDecision("dp_st_graph", + follow_decision); } } } else { // YIELD decision ObjectDecisionType yield_decision; - if (CreateYieldDecision(*path_obstacle, &yield_decision)) { - path_obstacle->AddLongitudinalDecision("dp_st_graph", - yield_decision); + if (CreateYieldDecision(*mutable_obstacle, &yield_decision)) { + mutable_obstacle->AddLongitudinalDecision("dp_st_graph", + yield_decision); } } break; @@ -214,27 +214,27 @@ Status SpeedDecider::MakeObjectDecision( if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) { ObjectDecisionType ignore; ignore.mutable_ignore(); - path_obstacle->AddLongitudinalDecision("dp_st_graph", ignore); + mutable_obstacle->AddLongitudinalDecision("dp_st_graph", ignore); } else { // OVERTAKE decision ObjectDecisionType overtake_decision; - if (CreateOvertakeDecision(*path_obstacle, &overtake_decision)) { - path_obstacle->AddLongitudinalDecision("dp_st_graph/overtake", - overtake_decision); + if (CreateOvertakeDecision(*mutable_obstacle, &overtake_decision)) { + mutable_obstacle->AddLongitudinalDecision("dp_st_graph/overtake", + overtake_decision); } } break; case CROSS: - if (obstacle->IsBlockingObstacle()) { + if (mutable_obstacle->IsBlockingObstacle()) { ObjectDecisionType stop_decision; - if (CreateStopDecision(*path_obstacle, &stop_decision, + if (CreateStopDecision(*mutable_obstacle, &stop_decision, -FLAGS_min_stop_distance_obstacle)) { - path_obstacle->AddLongitudinalDecision("dp_st_graph/cross", - stop_decision); + mutable_obstacle->AddLongitudinalDecision("dp_st_graph/cross", + stop_decision); } const std::string msg = "Failed to find a solution for crossing obstacle:" + - obstacle->Id(); + mutable_obstacle->Id(); AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } @@ -242,28 +242,28 @@ Status SpeedDecider::MakeObjectDecision( default: AERROR << "Unknown position:" << position; } - AppendIgnoreDecision(path_obstacle); + AppendIgnoreDecision(mutable_obstacle); } return Status::OK(); } -void SpeedDecider::AppendIgnoreDecision(PathObstacle* path_obstacle) const { +void SpeedDecider::AppendIgnoreDecision(Obstacle* obstacle) const { ObjectDecisionType ignore_decision; ignore_decision.mutable_ignore(); - if (!path_obstacle->HasLongitudinalDecision()) { - path_obstacle->AddLongitudinalDecision("dp_st_graph", ignore_decision); + if (!obstacle->HasLongitudinalDecision()) { + obstacle->AddLongitudinalDecision("dp_st_graph", ignore_decision); } - if (!path_obstacle->HasLateralDecision()) { - path_obstacle->AddLateralDecision("dp_st_graph", ignore_decision); + if (!obstacle->HasLateralDecision()) { + obstacle->AddLateralDecision("dp_st_graph", ignore_decision); } } -bool SpeedDecider::CreateStopDecision(const PathObstacle& path_obstacle, +bool SpeedDecider::CreateStopDecision(const Obstacle& obstacle, ObjectDecisionType* const stop_decision, double stop_distance) const { DCHECK_NOTNULL(stop_decision); - const auto& boundary = path_obstacle.st_boundary(); + const auto& boundary = obstacle.st_boundary(); const double fence_s = adc_sl_boundary_.end_s() + boundary.min_s() + stop_distance; const double main_stop_s = @@ -288,23 +288,22 @@ bool SpeedDecider::CreateStopDecision(const PathObstacle& path_obstacle, stop->set_reason_code(StopReasonCode::STOP_REASON_CLEAR_ZONE); } - PerceptionObstacle::Type obstacle_type = path_obstacle.Perception().type(); - ADEBUG << "STOP: obstacle_id[" << path_obstacle.Id() << "] obstacle_type[" + PerceptionObstacle::Type obstacle_type = obstacle.Perception().type(); + ADEBUG << "STOP: obstacle_id[" << obstacle.Id() << "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type) << "]"; return true; } bool SpeedDecider::CreateFollowDecision( - const PathObstacle& path_obstacle, - ObjectDecisionType* const follow_decision) const { + const Obstacle& obstacle, ObjectDecisionType* const follow_decision) const { DCHECK_NOTNULL(follow_decision); const double follow_speed = init_point_.v(); const double follow_distance_s = -std::fmax( follow_speed * FLAGS_follow_time_buffer, FLAGS_follow_min_distance); - const auto& boundary = path_obstacle.st_boundary(); + const auto& boundary = obstacle.st_boundary(); const double reference_s = adc_sl_boundary_.end_s() + boundary.min_s() + follow_distance_s; const double main_stop_s = @@ -325,19 +324,18 @@ bool SpeedDecider::CreateFollowDecision( fence_point->set_z(0.0); follow->set_fence_heading(ref_point.heading()); - PerceptionObstacle::Type obstacle_type = path_obstacle.Perception().type(); - ADEBUG << "FOLLOW: obstacle_id[" << path_obstacle.Id() << "] obstacle_type[" + PerceptionObstacle::Type obstacle_type = obstacle.Perception().type(); + ADEBUG << "FOLLOW: obstacle_id[" << obstacle.Id() << "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type) << "]"; return true; } bool SpeedDecider::CreateYieldDecision( - const PathObstacle& path_obstacle, - ObjectDecisionType* const yield_decision) const { + const Obstacle& obstacle, ObjectDecisionType* const yield_decision) const { DCHECK_NOTNULL(yield_decision); - PerceptionObstacle::Type obstacle_type = path_obstacle.Perception().type(); + PerceptionObstacle::Type obstacle_type = obstacle.Perception().type(); double yield_distance = FLAGS_yield_distance; switch (obstacle_type) { case PerceptionObstacle::PEDESTRIAN: @@ -349,7 +347,7 @@ bool SpeedDecider::CreateYieldDecision( break; } - const auto& obstacle_boundary = path_obstacle.st_boundary(); + const auto& obstacle_boundary = obstacle.st_boundary(); const double yield_distance_s = std::max(-obstacle_boundary.min_s(), -yield_distance); @@ -372,21 +370,21 @@ bool SpeedDecider::CreateYieldDecision( yield->mutable_fence_point()->set_z(0.0); yield->set_fence_heading(ref_point.heading()); - ADEBUG << "YIELD: obstacle_id[" << path_obstacle.Id() << "] obstacle_type[" + ADEBUG << "YIELD: obstacle_id[" << obstacle.Id() << "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type) << "]"; return true; } bool SpeedDecider::CreateOvertakeDecision( - const PathObstacle& path_obstacle, + const Obstacle& obstacle, ObjectDecisionType* const overtake_decision) const { DCHECK_NOTNULL(overtake_decision); constexpr double kOvertakeTimeBuffer = 3.0; // in seconds constexpr double kMinOvertakeDistance = 10.0; // in meters - const auto& velocity = path_obstacle.Perception().velocity(); + const auto& velocity = obstacle.Perception().velocity(); const double obstacle_speed = common::math::Vec2d::CreateUnitVec2d(init_point_.path_point().theta()) .InnerProd(Vec2d(velocity.x(), velocity.y())); @@ -395,7 +393,7 @@ bool SpeedDecider::CreateOvertakeDecision( std::fmax(init_point_.v(), obstacle_speed) * kOvertakeTimeBuffer, kMinOvertakeDistance); - const auto& boundary = path_obstacle.st_boundary(); + const auto& boundary = obstacle.st_boundary(); const double reference_line_fence_s = adc_sl_boundary_.end_s() + boundary.min_s() + overtake_distance_s; const double main_stop_s = @@ -415,8 +413,8 @@ bool SpeedDecider::CreateOvertakeDecision( overtake->mutable_fence_point()->set_z(0.0); overtake->set_fence_heading(ref_point.heading()); - PerceptionObstacle::Type obstacle_type = path_obstacle.Perception().type(); - ADEBUG << "OVERTAKE: obstacle_id[" << path_obstacle.Id() << "] obstacle_type[" + PerceptionObstacle::Type obstacle_type = obstacle.Perception().type(); + ADEBUG << "OVERTAKE: obstacle_id[" << obstacle.Id() << "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type) << "]"; return true; diff --git a/modules/planning/toolkits/optimizers/speed_decider/speed_decider.h b/modules/planning/toolkits/optimizers/speed_decider/speed_decider.h index 447b22ed6c..8ecb0af0b7 100644 --- a/modules/planning/toolkits/optimizers/speed_decider/speed_decider.h +++ b/modules/planning/toolkits/optimizers/speed_decider/speed_decider.h @@ -54,40 +54,40 @@ class SpeedDecider : public Task { **/ bool CheckIsFollowByT(const StBoundary& boundary) const; - bool CreateStopDecision(const PathObstacle& path_obstacle, + bool CreateStopDecision(const Obstacle& obstacle, ObjectDecisionType* const stop_decision, double stop_distance) const; /** * @brief create follow decision based on the boundary **/ - bool CreateFollowDecision(const PathObstacle& path_obstacle, + bool CreateFollowDecision(const Obstacle& obstacle, ObjectDecisionType* const follow_decision) const; /** * @brief create yield decision based on the boundary **/ - bool CreateYieldDecision(const PathObstacle& path_obstacle, + bool CreateYieldDecision(const Obstacle& obstacle, ObjectDecisionType* const yield_decision) const; /** * @brief create overtake decision based on the boundary **/ bool CreateOvertakeDecision( - const PathObstacle& path_obstacle, + const Obstacle& obstacle, ObjectDecisionType* const overtake_decision) const; apollo::common::Status MakeObjectDecision( const SpeedData& speed_profile, PathDecision* const path_decision) const; - void AppendIgnoreDecision(PathObstacle* path_obstacle) const; + void AppendIgnoreDecision(Obstacle* obstacle) const; /** * @brief "too close" is determined by whether ego vehicle will hit the front * obstacle if the obstacle drive at current speed and ego vehicle use some * reasonable deceleration **/ - bool IsFollowTooClose(const PathObstacle& path_obstacle) const; + bool IsFollowTooClose(const Obstacle& obstacle) const; private: DpStSpeedConfig dp_st_speed_config_; diff --git a/modules/planning/toolkits/optimizers/st_graph/BUILD b/modules/planning/toolkits/optimizers/st_graph/BUILD index 44ebb16e6e..8f1e6c015b 100644 --- a/modules/planning/toolkits/optimizers/st_graph/BUILD +++ b/modules/planning/toolkits/optimizers/st_graph/BUILD @@ -35,8 +35,8 @@ cc_library( "//modules/map/pnc_map", "//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:speed_limit", "//modules/planning/common/path:discretized_path", diff --git a/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.cc b/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.cc index 851b7b2ee5..555720c319 100644 --- a/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.cc +++ b/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.cc @@ -83,7 +83,7 @@ void SpeedLimitDecider::GetAvgKappa( } Status SpeedLimitDecider::GetSpeedLimits( - const IndexedList& path_obstacles, + const IndexedList& obstacles, SpeedLimit* const speed_limit_data) const { CHECK_NOTNULL(speed_limit_data); @@ -128,11 +128,11 @@ Status SpeedLimitDecider::GetSpeedLimits( // (3) speed limit from nudge obstacles double nudge_obstacle_speed_limit = std::numeric_limits::max(); - for (const auto* const_path_obstacle : path_obstacles.Items()) { - if (const_path_obstacle->IsVirtual()) { + for (const auto* const_obstacle : obstacles.Items()) { + if (const_obstacle->IsVirtual()) { continue; } - if (!const_path_obstacle->LateralDecision().has_nudge()) { + if (!const_obstacle->LateralDecision().has_nudge()) { continue; } @@ -143,13 +143,13 @@ Status SpeedLimitDecider::GetSpeedLimits( * ------------| obstacle |------ */ if (frenet_point_s + vehicle_param_.front_edge_to_center() < - const_path_obstacle->PerceptionSLBoundary().start_s() || + const_obstacle->PerceptionSLBoundary().start_s() || frenet_point_s - vehicle_param_.back_edge_to_center() > - const_path_obstacle->PerceptionSLBoundary().end_s()) { + const_obstacle->PerceptionSLBoundary().end_s()) { continue; } constexpr double kRange = 1.0; // meters - const auto& nudge = const_path_obstacle->LateralDecision().nudge(); + const auto& nudge = const_obstacle->LateralDecision().nudge(); // Please notice the differences between adc_l and frenet_point_l const double frenet_point_l = frenet_path_points.at(i).l(); @@ -158,17 +158,17 @@ Status SpeedLimitDecider::GetSpeedLimits( bool is_close_on_left = (nudge.type() == ObjectNudge::LEFT_NUDGE) && (frenet_point_l - vehicle_param_.right_edge_to_center() - kRange < - const_path_obstacle->PerceptionSLBoundary().end_l()); + const_obstacle->PerceptionSLBoundary().end_l()); // obstacle is on the left of ego vehicle (at path point i) bool is_close_on_right = (nudge.type() == ObjectNudge::RIGHT_NUDGE) && - (const_path_obstacle->PerceptionSLBoundary().start_l() - kRange < + (const_obstacle->PerceptionSLBoundary().start_l() - kRange < frenet_point_l + vehicle_param_.left_edge_to_center()); if (is_close_on_left || is_close_on_right) { double nudge_speed_ratio = 1.0; - if (const_path_obstacle->IsStatic()) { + if (const_obstacle->IsStatic()) { nudge_speed_ratio = st_boundary_config_.static_obs_nudge_speed_ratio(); } else { diff --git a/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.h b/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.h index 0bb8f4cc4a..b8cbbe2740 100644 --- a/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.h +++ b/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.h @@ -27,8 +27,8 @@ #include "modules/planning/proto/st_boundary_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_obstacle.h" #include "modules/planning/common/speed_limit.h" #include "modules/planning/reference_line/reference_line.h" @@ -45,7 +45,7 @@ class SpeedLimitDecider { virtual ~SpeedLimitDecider() = default; virtual apollo::common::Status GetSpeedLimits( - const IndexedList& path_obstacles, + const IndexedList& obstacles, SpeedLimit* const speed_limit_data) const; private: diff --git a/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider_test.cc b/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider_test.cc index 5723fba3d9..c3931fd27e 100644 --- a/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider_test.cc +++ b/modules/planning/toolkits/optimizers/st_graph/speed_limit_decider_test.cc @@ -24,7 +24,7 @@ #include "cyber/common/log.h" #include "modules/map/hdmap/hdmap_util.h" -#include "modules/planning/common/path_obstacle.h" +#include "modules/planning/common/obstacle.h" #include "modules/planning/reference_line/qp_spline_reference_line_smoother.h" namespace apollo { diff --git a/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.cc b/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.cc index aecbe4ef4f..f26948d75e 100644 --- a/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.cc +++ b/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.cc @@ -74,7 +74,7 @@ StBoundaryMapper::StBoundaryMapper(const SLBoundary& adc_sl_boundary, is_change_lane_(is_change_lane) {} Status StBoundaryMapper::CreateStBoundary(PathDecision* path_decision) const { - const auto& path_obstacles = path_decision->path_obstacles(); + const auto& obstacles = path_decision->obstacles(); if (planning_time_ < 0.0) { const std::string msg = "Fail to get params since planning_time_ < 0."; @@ -90,24 +90,24 @@ Status StBoundaryMapper::CreateStBoundary(PathDecision* path_decision) const { "Fail to get params because of too few path points"); } - PathObstacle* stop_obstacle = nullptr; + Obstacle* stop_obstacle = nullptr; ObjectDecisionType stop_decision; double min_stop_s = std::numeric_limits::max(); - for (const auto* const_path_obstacle : path_obstacles.Items()) { - auto* path_obstacle = path_decision->Find(const_path_obstacle->Id()); - if (!path_obstacle->HasLongitudinalDecision()) { - if (!MapWithoutDecision(path_obstacle).ok()) { - std::string msg = StrCat("Fail to map obstacle ", path_obstacle->Id(), + for (const auto* const_obstacle : obstacles.Items()) { + auto* obstacle = path_decision->Find(const_obstacle->Id()); + if (!obstacle->HasLongitudinalDecision()) { + if (!MapWithoutDecision(obstacle).ok()) { + std::string msg = StrCat("Fail to map obstacle ", obstacle->Id(), " without decision."); AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } continue; } - const auto& decision = path_obstacle->LongitudinalDecision(); + const auto& decision = obstacle->LongitudinalDecision(); if (decision.has_stop()) { - const double stop_s = path_obstacle->PerceptionSLBoundary().start_s() + + const double stop_s = obstacle->PerceptionSLBoundary().start_s() + decision.stop().distance_s(); // this is a rough estimation based on reference line s, so that a large // buffer is used. @@ -120,14 +120,14 @@ Status StBoundaryMapper::CreateStBoundary(PathDecision* path_decision) const { return Status(ErrorCode::PLANNING_ERROR, "invalid decision"); } if (stop_s < min_stop_s) { - stop_obstacle = path_obstacle; + stop_obstacle = obstacle; min_stop_s = stop_s; stop_decision = decision; } } else if (decision.has_follow() || decision.has_overtake() || decision.has_yield()) { - if (!MapWithDecision(path_obstacle, decision).ok()) { - AERROR << "Fail to map obstacle " << path_obstacle->Id() + if (!MapWithDecision(obstacle, decision).ok()) { + AERROR << "Fail to map obstacle " << obstacle->Id() << " with decision: " << decision.DebugString(); return Status(ErrorCode::PLANNING_ERROR, "Fail to map overtake/yield decision"); @@ -151,7 +151,7 @@ Status StBoundaryMapper::CreateStBoundary(PathDecision* path_decision) const { Status StBoundaryMapper::CreateStBoundaryWithHistory( const ObjectDecisions& history_decisions, PathDecision* path_decision) const { - const auto& path_obstacles = path_decision->path_obstacles(); + const auto& obstacles = path_decision->obstacles(); if (planning_time_ < 0.0) { const std::string msg = "Fail to get params since planning_time_ < 0."; AERROR << msg; @@ -169,7 +169,7 @@ Status StBoundaryMapper::CreateStBoundaryWithHistory( std::unordered_map prev_decision_map; for (const auto& history_decision : history_decisions.decision()) { for (const auto& decision : history_decision.object_decision()) { - if (PathObstacle::IsLongitudinalDecision(decision) && + if (Obstacle::IsLongitudinalDecision(decision) && !decision.has_ignore()) { prev_decision_map[history_decision.id()] = decision; break; @@ -177,13 +177,13 @@ Status StBoundaryMapper::CreateStBoundaryWithHistory( } } - PathObstacle* stop_obstacle = nullptr; + Obstacle* stop_obstacle = nullptr; ObjectDecisionType stop_decision; double min_stop_s = std::numeric_limits::max(); - for (const auto* const_path_obstacle : path_obstacles.Items()) { - auto* path_obstacle = path_decision->Find(const_path_obstacle->Id()); - auto iter = prev_decision_map.find(path_obstacle->Id()); + for (const auto* const_obstacle : obstacles.Items()) { + auto* obstacle = path_decision->Find(const_obstacle->Id()); + auto iter = prev_decision_map.find(obstacle->Id()); ObjectDecisionType decision; if (iter == prev_decision_map.end()) { decision.mutable_ignore(); @@ -191,22 +191,22 @@ Status StBoundaryMapper::CreateStBoundaryWithHistory( decision = iter->second; } - if (!path_obstacle->HasLongitudinalDecision()) { - if (!MapWithoutDecision(path_obstacle).ok()) { - std::string msg = StrCat("Fail to map obstacle ", path_obstacle->Id(), + if (!obstacle->HasLongitudinalDecision()) { + if (!MapWithoutDecision(obstacle).ok()) { + std::string msg = StrCat("Fail to map obstacle ", obstacle->Id(), " without decision."); AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - if (path_obstacle->st_boundary().IsEmpty() || decision.has_ignore()) { + if (obstacle->st_boundary().IsEmpty() || decision.has_ignore()) { continue; } } - if (path_obstacle->HasLongitudinalDecision()) { - decision = path_obstacle->LongitudinalDecision(); + if (obstacle->HasLongitudinalDecision()) { + decision = obstacle->LongitudinalDecision(); } if (decision.has_stop()) { - const double stop_s = path_obstacle->PerceptionSLBoundary().start_s() + + const double stop_s = obstacle->PerceptionSLBoundary().start_s() + decision.stop().distance_s(); // this is a rough estimation based on reference line s, so that a large // buffer is used. @@ -219,14 +219,14 @@ Status StBoundaryMapper::CreateStBoundaryWithHistory( return Status(ErrorCode::PLANNING_ERROR, "invalid decision"); } if (stop_s < min_stop_s) { - stop_obstacle = path_obstacle; + stop_obstacle = obstacle; min_stop_s = stop_s; stop_decision = decision; } } else if (decision.has_follow() || decision.has_overtake() || decision.has_yield()) { - if (!MapWithDecision(path_obstacle, decision).ok()) { - AERROR << "Fail to map obstacle " << path_obstacle->Id() + if (!MapWithDecision(obstacle, decision).ok()) { + AERROR << "Fail to map obstacle " << obstacle->Id() << " with decision: " << decision.DebugString(); return Status(ErrorCode::PLANNING_ERROR, "Fail to map overtake/yield decision"); @@ -248,8 +248,7 @@ Status StBoundaryMapper::CreateStBoundaryWithHistory( } bool StBoundaryMapper::MapStopDecision( - PathObstacle* stop_obstacle, - const ObjectDecisionType& stop_decision) const { + Obstacle* stop_obstacle, const ObjectDecisionType& stop_decision) const { DCHECK(stop_decision.has_stop()) << "Must have stop decision"; if (stop_obstacle->PerceptionSLBoundary().start_s() > @@ -297,34 +296,33 @@ bool StBoundaryMapper::MapStopDecision( return true; } -Status StBoundaryMapper::MapWithoutDecision(PathObstacle* path_obstacle) const { +Status StBoundaryMapper::MapWithoutDecision(Obstacle* obstacle) const { std::vector lower_points; std::vector upper_points; if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(), - *path_obstacle, &upper_points, &lower_points)) { + *obstacle, &upper_points, &lower_points)) { return Status::OK(); } auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points) .ExpandByS(boundary_s_buffer) .ExpandByT(boundary_t_buffer); - boundary.SetId(path_obstacle->Id()); - const auto& prev_st_boundary = path_obstacle->st_boundary(); - const auto& ref_line_st_boundary = - path_obstacle->reference_line_st_boundary(); + boundary.SetId(obstacle->Id()); + const auto& prev_st_boundary = obstacle->st_boundary(); + const auto& ref_line_st_boundary = obstacle->reference_line_st_boundary(); if (!prev_st_boundary.IsEmpty()) { boundary.SetBoundaryType(prev_st_boundary.boundary_type()); } else if (!ref_line_st_boundary.IsEmpty()) { boundary.SetBoundaryType(ref_line_st_boundary.boundary_type()); } - path_obstacle->SetStBoundary(boundary); + obstacle->SetStBoundary(boundary); return Status::OK(); } bool StBoundaryMapper::GetOverlapBoundaryPoints( - const std::vector& path_points, const PathObstacle& obstacle, + const std::vector& path_points, const Obstacle& obstacle, std::vector* upper_points, std::vector* lower_points) const { DCHECK_NOTNULL(upper_points); @@ -458,7 +456,7 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints( } Status StBoundaryMapper::MapWithDecision( - PathObstacle* path_obstacle, const ObjectDecisionType& decision) const { + Obstacle* obstacle, const ObjectDecisionType& decision) const { DCHECK(decision.has_follow() || decision.has_yield() || decision.has_overtake()) << "decision is " << decision.DebugString() @@ -468,7 +466,7 @@ Status StBoundaryMapper::MapWithDecision( std::vector upper_points; if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(), - *path_obstacle, &upper_points, &lower_points)) { + *obstacle, &upper_points, &lower_points)) { return Status::OK(); } @@ -508,9 +506,9 @@ Status StBoundaryMapper::MapWithDecision( << decision.DebugString(); } boundary.SetBoundaryType(b_type); - boundary.SetId(path_obstacle->Id()); + boundary.SetId(obstacle->Id()); boundary.SetCharacteristicLength(characteristic_length); - path_obstacle->SetStBoundary(boundary); + obstacle->SetStBoundary(boundary); return Status::OK(); } diff --git a/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.h b/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.h index cb1820ba5f..91e72a13a2 100644 --- a/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.h +++ b/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper.h @@ -53,8 +53,7 @@ class StBoundaryMapper { PathDecision* path_decision) const; apollo::common::Status CreateStBoundary( - PathObstacle* path_obstacle, - const ObjectDecisionType& external_decision) const; + Obstacle* obstacle, const ObjectDecisionType& external_decision) const; private: FRIEND_TEST(StBoundaryMapperTest, check_overlap_test); @@ -69,16 +68,16 @@ class StBoundaryMapper { */ bool GetOverlapBoundaryPoints( const std::vector& path_points, - const PathObstacle& obstacle, std::vector* upper_points, + const Obstacle& obstacle, std::vector* upper_points, std::vector* lower_points) const; - apollo::common::Status MapWithoutDecision(PathObstacle* path_obstacle) const; + apollo::common::Status MapWithoutDecision(Obstacle* obstacle) const; - bool MapStopDecision(PathObstacle* stop_obstacle, + bool MapStopDecision(Obstacle* stop_obstacle, const ObjectDecisionType& decision) const; apollo::common::Status MapWithDecision( - PathObstacle* path_obstacle, const ObjectDecisionType& decision) const; + Obstacle* obstacle, const ObjectDecisionType& decision) const; private: const SLBoundary& adc_sl_boundary_; diff --git a/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper_test.cc b/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper_test.cc index 1a6a3b6d35..248587b830 100644 --- a/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper_test.cc +++ b/modules/planning/toolkits/optimizers/st_graph/st_boundary_mapper_test.cc @@ -24,7 +24,7 @@ #include "cyber/common/log.h" #include "modules/map/hdmap/hdmap_util.h" -#include "modules/planning/common/path_obstacle.h" +#include "modules/planning/common/obstacle.h" #include "modules/planning/reference_line/qp_spline_reference_line_smoother.h" #include "modules/planning/toolkits/optimizers/st_graph/speed_limit_decider.h" diff --git a/modules/planning/toolkits/task_factory.cc b/modules/planning/toolkits/task_factory.cc index 66183df37d..6529c34239 100644 --- a/modules/planning/toolkits/task_factory.cc +++ b/modules/planning/toolkits/task_factory.cc @@ -30,7 +30,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" @@ -78,10 +77,6 @@ void TaskFactory::Init(const PlanningConfig& config) { [](const TaskConfig& config) -> Task* { return new QpPiecewiseJerkPathOptimizer(config); }); - task_factory_.Register(TaskConfig::POLY_ST_SPEED_OPTIMIZER, - [](const TaskConfig& config) -> Task* { - return new PolyStSpeedOptimizer(config); - }); task_factory_.Register(TaskConfig::DECIDER_CREEP, [](const TaskConfig& config) -> Task* { return new DeciderCreep(config); diff --git a/modules/planning/traffic_rules/backside_vehicle.cc b/modules/planning/traffic_rules/backside_vehicle.cc index 89873997f9..873b783bb3 100644 --- a/modules/planning/traffic_rules/backside_vehicle.cc +++ b/modules/planning/traffic_rules/backside_vehicle.cc @@ -34,40 +34,39 @@ void BacksideVehicle::MakeLaneKeepingObstacleDecision( ignore.mutable_ignore(); const double adc_length_s = adc_sl_boundary.end_s() - adc_sl_boundary.start_s(); - for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - if (path_obstacle->PerceptionSLBoundary().end_s() >= + for (const auto* obstacle : path_decision->obstacles().Items()) { + if (obstacle->PerceptionSLBoundary().end_s() >= adc_sl_boundary.end_s()) { // don't ignore such vehicles. continue; } - if (path_obstacle->reference_line_st_boundary().IsEmpty()) { + if (obstacle->reference_line_st_boundary().IsEmpty()) { path_decision->AddLongitudinalDecision("backside_vehicle/no-st-region", - path_obstacle->Id(), ignore); + obstacle->Id(), ignore); path_decision->AddLateralDecision("backside_vehicle/no-st-region", - path_obstacle->Id(), ignore); + obstacle->Id(), ignore); continue; } // Ignore the car comes from back of ADC - if (path_obstacle->reference_line_st_boundary().min_s() < -adc_length_s) { + if (obstacle->reference_line_st_boundary().min_s() < -adc_length_s) { path_decision->AddLongitudinalDecision("backside_vehicle/st-min-s < adc", - path_obstacle->Id(), ignore); + obstacle->Id(), ignore); path_decision->AddLateralDecision("backside_vehicle/st-min-s < adc", - path_obstacle->Id(), ignore); + obstacle->Id(), ignore); continue; } const double lane_boundary = config_.backside_vehicle().backside_lane_width(); - if (path_obstacle->PerceptionSLBoundary().start_s() < - adc_sl_boundary.end_s()) { - if (path_obstacle->PerceptionSLBoundary().start_l() > lane_boundary || - path_obstacle->PerceptionSLBoundary().end_l() < -lane_boundary) { + if (obstacle->PerceptionSLBoundary().start_s() < adc_sl_boundary.end_s()) { + if (obstacle->PerceptionSLBoundary().start_l() > lane_boundary || + obstacle->PerceptionSLBoundary().end_l() < -lane_boundary) { continue; } path_decision->AddLongitudinalDecision("backside_vehicle/sl < adc.end_s", - path_obstacle->Id(), ignore); + obstacle->Id(), ignore); path_decision->AddLateralDecision("backside_vehicle/sl < adc.end_s", - path_obstacle->Id(), ignore); + obstacle->Id(), ignore); continue; } } diff --git a/modules/planning/traffic_rules/backside_vehicle.h b/modules/planning/traffic_rules/backside_vehicle.h index 9e9e29d2f5..cc48015bb8 100644 --- a/modules/planning/traffic_rules/backside_vehicle.h +++ b/modules/planning/traffic_rules/backside_vehicle.h @@ -33,7 +33,7 @@ class BacksideVehicle : public TrafficRule { virtual ~BacksideVehicle() = default; common::Status ApplyRule(Frame* const frame, - ReferenceLineInfo* const reference_line_info); + ReferenceLineInfo* const reference_line_info); private: /** diff --git a/modules/planning/traffic_rules/change_lane.cc b/modules/planning/traffic_rules/change_lane.cc index 8ee255f86b..26b3c05668 100644 --- a/modules/planning/traffic_rules/change_lane.cc +++ b/modules/planning/traffic_rules/change_lane.cc @@ -41,26 +41,25 @@ bool ChangeLane::FilterObstacles(ReferenceLineInfo* reference_line_info) { const auto& reference_line = reference_line_info->reference_line(); const auto& adc_sl_boundary = reference_line_info->AdcSlBoundary(); const auto& path_decision = reference_line_info->path_decision(); - const PathObstacle* first_guard_vehicle = nullptr; + const Obstacle* first_guard_vehicle = nullptr; constexpr double kGuardForwardDistance = 60; double max_s = 0.0; const double min_overtake_time = config_.change_lane().min_overtake_time(); - for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - if (!path_obstacle->HasTrajectory()) { + for (const auto* obstacle : path_decision->obstacles().Items()) { + if (!obstacle->HasTrajectory()) { continue; } - if (path_obstacle->PerceptionSLBoundary().start_s() > - adc_sl_boundary.end_s()) { + if (obstacle->PerceptionSLBoundary().start_s() > adc_sl_boundary.end_s()) { continue; } - if (path_obstacle->PerceptionSLBoundary().end_s() < + if (obstacle->PerceptionSLBoundary().end_s() < adc_sl_boundary.start_s() - std::max(config_.change_lane().min_overtake_distance(), - path_obstacle->speed() * min_overtake_time)) { - overtake_obstacles_.push_back(path_obstacle); + obstacle->speed() * min_overtake_time)) { + overtake_obstacles_.push_back(obstacle); } const auto& last_point = - *(path_obstacle->Trajectory().trajectory_point().rbegin()); + *(obstacle->Trajectory().trajectory_point().rbegin()); if (last_point.v() < config_.change_lane().min_guard_speed()) { continue; } @@ -77,7 +76,7 @@ bool ChangeLane::FilterObstacles(ReferenceLineInfo* reference_line_info) { } if (last_sl.s() > max_s) { max_s = last_sl.s(); - first_guard_vehicle = path_obstacle; + first_guard_vehicle = obstacle; } } if (first_guard_vehicle) { @@ -87,7 +86,7 @@ bool ChangeLane::FilterObstacles(ReferenceLineInfo* reference_line_info) { } bool ChangeLane::CreateGuardObstacle( - const ReferenceLineInfo* reference_line_info, PathObstacle* obstacle) { + const ReferenceLineInfo* reference_line_info, Obstacle* obstacle) { if (!obstacle || !obstacle->HasTrajectory()) { return false; } @@ -146,8 +145,8 @@ Status ChangeLane::ApplyRule(Frame* const frame, } if (config_.change_lane().enable_guard_obstacle() && !guard_obstacles_.empty()) { - for (const auto path_obstacle : guard_obstacles_) { - auto* guard_obstacle = frame->Find(path_obstacle->Id()); + for (const auto obstacle : guard_obstacles_) { + auto* guard_obstacle = frame->Find(obstacle->Id()); if (guard_obstacle && CreateGuardObstacle(reference_line_info, guard_obstacle)) { AINFO << "Created guard obstacle: " << guard_obstacle->Id(); @@ -158,25 +157,24 @@ Status ChangeLane::ApplyRule(Frame* const frame, if (!overtake_obstacles_.empty()) { auto* path_decision = reference_line_info->path_decision(); const auto& reference_line = reference_line_info->reference_line(); - for (const auto* path_obstacle : overtake_obstacles_) { - auto overtake = CreateOvertakeDecision(reference_line, path_obstacle); + for (const auto* obstacle : overtake_obstacles_) { + auto overtake = CreateOvertakeDecision(reference_line, obstacle); path_decision->AddLongitudinalDecision( - TrafficRuleConfig::RuleId_Name(Id()), path_obstacle->Id(), overtake); + TrafficRuleConfig::RuleId_Name(Id()), obstacle->Id(), overtake); } } return Status::OK(); } ObjectDecisionType ChangeLane::CreateOvertakeDecision( - const ReferenceLine& reference_line, - const PathObstacle* path_obstacle) const { + const ReferenceLine& reference_line, const Obstacle* obstacle) const { ObjectDecisionType overtake; overtake.mutable_overtake(); - const double speed = path_obstacle->speed(); + const double speed = obstacle->speed(); double distance = std::max(speed * config_.change_lane().min_overtake_time(), config_.change_lane().min_overtake_distance()); overtake.mutable_overtake()->set_distance_s(distance); - double fence_s = path_obstacle->PerceptionSLBoundary().end_s() + distance; + double fence_s = obstacle->PerceptionSLBoundary().end_s() + distance; auto point = reference_line.GetReferencePoint(fence_s); overtake.mutable_overtake()->set_time_buffer( config_.change_lane().min_overtake_time()); diff --git a/modules/planning/traffic_rules/change_lane.h b/modules/planning/traffic_rules/change_lane.h index 97c252a82b..fc30555238 100644 --- a/modules/planning/traffic_rules/change_lane.h +++ b/modules/planning/traffic_rules/change_lane.h @@ -52,17 +52,16 @@ class ChangeLane : public TrafficRule { *even when it is not on the target lane yet. **/ bool CreateGuardObstacle(const ReferenceLineInfo* reference_line_info, - PathObstacle* obstacle); + Obstacle* obstacle); /** * @brief create overtake decision for the give path obstacle */ - ObjectDecisionType CreateOvertakeDecision( - const ReferenceLine& reference_line, - const PathObstacle* path_obstacle) const; + ObjectDecisionType CreateOvertakeDecision(const ReferenceLine& reference_line, + const Obstacle* obstacle) const; - std::vector guard_obstacles_; - std::vector overtake_obstacles_; + std::vector guard_obstacles_; + std::vector overtake_obstacles_; }; } // namespace planning diff --git a/modules/planning/traffic_rules/creeper.cc b/modules/planning/traffic_rules/creeper.cc index 1c65389ebb..bc7f2c33b4 100644 --- a/modules/planning/traffic_rules/creeper.cc +++ b/modules/planning/traffic_rules/creeper.cc @@ -80,9 +80,9 @@ bool Creeper::BuildStopDecision(const PathOverlap& overlap, 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; } diff --git a/modules/planning/traffic_rules/crosswalk.cc b/modules/planning/traffic_rules/crosswalk.cc index 3e1351f5a0..a52f089001 100644 --- a/modules/planning/traffic_rules/crosswalk.cc +++ b/modules/planning/traffic_rules/crosswalk.cc @@ -122,10 +122,9 @@ void Crosswalk::MakeDecisions(Frame* const frame, } std::vector pedestrians; - for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - const PerceptionObstacle& perception_obstacle = - path_obstacle->Perception(); - const std::string& obstacle_id = path_obstacle->Id(); + for (const auto* obstacle : path_decision->obstacles().Items()) { + const PerceptionObstacle& perception_obstacle = obstacle->Perception(); + const std::string& obstacle_id = obstacle->Id(); PerceptionObstacle::Type obstacle_type = perception_obstacle.type(); std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle_type); @@ -165,11 +164,11 @@ void Crosswalk::MakeDecisions(Frame* const frame, double obstacle_l_distance = std::fabs(obstacle_sl_point.l()); const bool is_on_lane = - reference_line.IsOnLane(path_obstacle->PerceptionSLBoundary()); + reference_line.IsOnLane(obstacle->PerceptionSLBoundary()); const bool is_on_road = - reference_line.IsOnRoad(path_obstacle->PerceptionSLBoundary()); + reference_line.IsOnRoad(obstacle->PerceptionSLBoundary()); const bool is_path_cross = - !path_obstacle->reference_line_st_boundary().IsEmpty(); + !obstacle->reference_line_st_boundary().IsEmpty(); ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name << "] crosswalk_id[" << crosswalk_id << "] obstacle_l[" @@ -361,9 +360,9 @@ int Crosswalk::BuildStopDecision(Frame* const frame, AERROR << "Failed to create obstacle[" << virtual_obstacle_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: " << virtual_obstacle_id; + AERROR << "Failed to create obstacle for: " << virtual_obstacle_id; return -1; } diff --git a/modules/planning/traffic_rules/destination.cc b/modules/planning/traffic_rules/destination.cc index a1a0d084f2..a045d117e2 100644 --- a/modules/planning/traffic_rules/destination.cc +++ b/modules/planning/traffic_rules/destination.cc @@ -131,9 +131,9 @@ int Destination::Stop(Frame* const frame, 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; } diff --git a/modules/planning/traffic_rules/front_vehicle.cc b/modules/planning/traffic_rules/front_vehicle.cc index d6ee471af9..68e008bba2 100644 --- a/modules/planning/traffic_rules/front_vehicle.cc +++ b/modules/planning/traffic_rules/front_vehicle.cc @@ -233,20 +233,20 @@ std::string FrontVehicle::FindPassableObstacle( std::string passable_obstacle_id; const auto& adc_sl_boundary = reference_line_info->AdcSlBoundary(); auto* path_decision = reference_line_info->path_decision(); - for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - const PerceptionObstacle& perception_obstacle = path_obstacle->Perception(); + for (const auto* obstacle : path_decision->obstacles().Items()) { + 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); - if (path_obstacle->IsVirtual() || !path_obstacle->IsStatic()) { + if (obstacle->IsVirtual() || !obstacle->IsStatic()) { ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name << "] VIRTUAL or NOT STATIC. SKIP"; continue; } - const auto& obstacle_sl = path_obstacle->PerceptionSLBoundary(); + const auto& obstacle_sl = obstacle->PerceptionSLBoundary(); if (obstacle_sl.start_s() <= adc_sl_boundary.end_s()) { ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name << "] behind ADC. SKIP"; @@ -272,8 +272,8 @@ std::string FrontVehicle::FindPassableObstacle( } 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() > @@ -295,7 +295,7 @@ std::string FrontVehicle::FindPassableObstacle( } } if (!is_blocked_by_others) { - passable_obstacle_id = path_obstacle->Id(); + passable_obstacle_id = obstacle->Id(); break; } } @@ -309,21 +309,21 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) { const auto& vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param(); const double adc_width = vehicle_param.width(); - for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - const PerceptionObstacle& perception_obstacle = path_obstacle->Perception(); + for (const auto* obstacle : path_decision->obstacles().Items()) { + 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); - if (path_obstacle->IsVirtual() || !path_obstacle->IsStatic()) { + if (obstacle->IsVirtual() || !obstacle->IsStatic()) { ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name << "] VIRTUAL or NOT STATIC. SKIP"; continue; } bool is_on_road = reference_line_info->reference_line().HasOverlap( - path_obstacle->PerceptionBoundingBox()); + obstacle->PerceptionBoundingBox()); if (!is_on_road) { // skip obstacles not on reference line ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name @@ -332,7 +332,7 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) { } const auto& vehicle_sl = reference_line_info->VehicleSlBoundary(); - const auto& obstacle_sl = path_obstacle->PerceptionSLBoundary(); + const auto& obstacle_sl = obstacle->PerceptionSLBoundary(); if (obstacle_sl.end_s() <= adc_sl.start_s() || obstacle_sl.end_s() <= vehicle_sl.start_s()) { // skip backside vehicles @@ -342,7 +342,7 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) { } // check SIDE_PASS decision - if (path_obstacle->LateralDecision().has_sidepass()) { + if (obstacle->LateralDecision().has_sidepass()) { ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name << "] SIDE_PASS. SKIP"; continue; @@ -378,7 +378,7 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) { // build stop decision const double stop_distance = - path_obstacle->MinRadiusStopDistance(vehicle_param); + obstacle->MinRadiusStopDistance(vehicle_param); const double stop_s = obstacle_sl.start_s() - stop_distance; auto stop_point = reference_line.GetReferencePoint(stop_s); const double stop_heading = @@ -400,8 +400,8 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) { stop_decision->mutable_stop_point()->set_y(stop_point.y()); stop_decision->mutable_stop_point()->set_z(0.0); - path_decision->AddLongitudinalDecision("front_vehicle", - path_obstacle->Id(), stop); + path_decision->AddLongitudinalDecision("front_vehicle", obstacle->Id(), + stop); } } } diff --git a/modules/planning/traffic_rules/pull_over.cc b/modules/planning/traffic_rules/pull_over.cc index bf3f418e37..41357b2681 100644 --- a/modules/planning/traffic_rules/pull_over.cc +++ b/modules/planning/traffic_rules/pull_over.cc @@ -145,20 +145,20 @@ PullOver::ValidateStopPointCode PullOver::IsValidStop( // check obstacles auto* path_decision = reference_line_info_->path_decision(); - for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - const PerceptionObstacle& perception_obstacle = path_obstacle->Perception(); + for (const auto* obstacle : path_decision->obstacles().Items()) { + 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); - if (path_obstacle->IsVirtual() || !path_obstacle->IsStatic()) { + if (obstacle->IsVirtual() || !obstacle->IsStatic()) { ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name << "] VIRTUAL or NOT STATIC. SKIP"; continue; } - const auto& obstacle_sl = path_obstacle->PerceptionSLBoundary(); + const auto& obstacle_sl = obstacle->PerceptionSLBoundary(); if (!(parking_spot_boundary.start_s() > obstacle_sl.end_s() || obstacle_sl.start_s() > parking_spot_boundary.end_s() || parking_spot_boundary.start_l() > obstacle_sl.end_l() || @@ -650,9 +650,9 @@ int PullOver::BuildStopDecision(const std::string& vistual_obstacle_id_postfix, AERROR << "Failed to create obstacle[" << virtual_obstacle_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 " << virtual_obstacle_id; + AERROR << "Failed to create obstacle for " << virtual_obstacle_id; return -1; } diff --git a/modules/planning/traffic_rules/reference_line_end.cc b/modules/planning/traffic_rules/reference_line_end.cc index 9bbb833937..c5dc8eeb55 100644 --- a/modules/planning/traffic_rules/reference_line_end.cc +++ b/modules/planning/traffic_rules/reference_line_end.cc @@ -57,7 +57,7 @@ Status ReferenceLineEnd::ApplyRule( return Status(common::PLANNING_ERROR, "Failed to create reference line end obstacle"); } - PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle); + Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle); if (!stop_wall) { return Status( common::PLANNING_ERROR, diff --git a/modules/planning/traffic_rules/signal_light.cc b/modules/planning/traffic_rules/signal_light.cc index d864f2f2aa..6a03c914b7 100644 --- a/modules/planning/traffic_rules/signal_light.cc +++ b/modules/planning/traffic_rules/signal_light.cc @@ -157,8 +157,8 @@ void SignalLight::SetCreepForwardSignalDecision( const double creep_s_buffer = config_.signal_light().righ_turn_creep().min_boundary_s(); const auto& path_decision = reference_line_info->path_decision(); - for (const auto& path_obstacle : path_decision->path_obstacles().Items()) { - const auto& st_boundary = path_obstacle->reference_line_st_boundary(); + for (const auto& obstacle : path_decision->obstacles().Items()) { + const auto& st_boundary = obstacle->reference_line_st_boundary(); const double stop_s = signal_light->start_s - config_.signal_light().stop_distance(); if (reference_line_info->AdcSlBoundary().end_s() + st_boundary.min_s() < @@ -207,9 +207,9 @@ bool SignalLight::BuildStopDecision( 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; } diff --git a/modules/planning/traffic_rules/stop_sign.cc b/modules/planning/traffic_rules/stop_sign.cc index 4f763a9dc4..1b9394f96f 100644 --- a/modules/planning/traffic_rules/stop_sign.cc +++ b/modules/planning/traffic_rules/stop_sign.cc @@ -92,10 +92,9 @@ void StopSign::MakeDecisions(Frame* frame, stop_status_ == StopSignStatus::WAIT) { auto* path_decision = reference_line_info->path_decision(); if (stop_status_ == StopSignStatus::DRIVE) { - 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); } } else if (!watch_vehicles.empty() && (stop_status_ == StopSignStatus::STOP || @@ -108,10 +107,9 @@ void StopSign::MakeDecisions(Frame* frame, std::back_inserter(watch_vehicle_ids)); } - 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); } } @@ -284,12 +282,12 @@ bool StopSign::CheckCreepDone(ReferenceLineInfo* const reference_line_info) { creep_stop_s - reference_line_info->AdcSlBoundary().end_s(); if (distance < config_.stop_sign().creep().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() < config_.stop_sign().creep().min_boundary_t()) { all_far_away = false; break; @@ -472,11 +470,11 @@ void StopSign::UpdateWatchVehicles(StopSignLaneVehicles* watch_vehicles) { /** * @brief: add a watch vehicle which arrives at stop sign ahead of adc */ -int StopSign::AddWatchVehicle(const PathObstacle& path_obstacle, +int StopSign::AddWatchVehicle(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); @@ -568,12 +566,11 @@ int StopSign::AddWatchVehicle(const PathObstacle& path_obstacle, * @brief: remove a watch vehicle which not stopping at stop sign any more */ int StopSign::RemoveWatchVehicle( - const PathObstacle& path_obstacle, - const std::vector& watch_vehicle_ids, + const Obstacle& obstacle, const std::vector& 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); @@ -614,7 +611,7 @@ int StopSign::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(); @@ -691,13 +688,13 @@ int StopSign::ClearWatchVehicle(ReferenceLineInfo* const reference_line_info, CHECK_NOTNULL(reference_line_info); CHECK_NOTNULL(watch_vehicles); - const auto& path_obstacles = - reference_line_info->path_decision()->path_obstacles().Items(); + const auto& obstacles = + reference_line_info->path_decision()->obstacles().Items(); std::unordered_set obstacle_ids; - std::transform(path_obstacles.begin(), path_obstacles.end(), + std::transform(obstacles.begin(), obstacles.end(), std::inserter(obstacle_ids, obstacle_ids.end()), - [](const PathObstacle* path_obstacle) { - return std::to_string(path_obstacle->Perception().id()); + [](const Obstacle* obstacle) { + return std::to_string(obstacle->Perception().id()); }); for (StopSignLaneVehicles::iterator it = watch_vehicles->begin(); @@ -764,9 +761,9 @@ int StopSign::BuildStopDecision(Frame* frame, 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; } diff --git a/modules/planning/traffic_rules/stop_sign.h b/modules/planning/traffic_rules/stop_sign.h index 292f420c73..0c9efcaa0f 100644 --- a/modules/planning/traffic_rules/stop_sign.h +++ b/modules/planning/traffic_rules/stop_sign.h @@ -55,11 +55,11 @@ class StopSign : public TrafficRule { StopSignLaneVehicles* watch_vehicles); bool CheckADCStop(ReferenceLineInfo* const reference_line_info); void GetWatchVehicles(const hdmap::StopSignInfo& stop_sign_info, - StopSignLaneVehicles* watch_vehicles); + StopSignLaneVehicles* watch_vehicles); void UpdateWatchVehicles(StopSignLaneVehicles* watch_vehicles); - int AddWatchVehicle(const PathObstacle& path_obstacle, + int AddWatchVehicle(const Obstacle& obstacle, StopSignLaneVehicles* watch_vehicles); - int RemoveWatchVehicle(const PathObstacle& path_obstacle, + int RemoveWatchVehicle(const Obstacle& obstacle, const std::vector& watch_vehicle_ids, StopSignLaneVehicles* watch_vehicles); int ClearWatchVehicle(ReferenceLineInfo* const reference_line_info, diff --git a/modules/planning/traffic_rules/traffic_decider.cc b/modules/planning/traffic_rules/traffic_decider.cc index 2403b6620b..6d4b97304e 100644 --- a/modules/planning/traffic_rules/traffic_decider.cc +++ b/modules/planning/traffic_rules/traffic_decider.cc @@ -107,7 +107,7 @@ void TrafficDecider::BuildPlanningTarget( double min_s = std::numeric_limits::infinity(); StopPoint stop_point; for (const auto *obstacle : - reference_line_info->path_decision()->path_obstacles().Items()) { + reference_line_info->path_decision()->obstacles().Items()) { if (obstacle->IsVirtual() && obstacle->HasLongitudinalDecision() && obstacle->LongitudinalDecision().has_stop() && obstacle->PerceptionSLBoundary().start_s() < min_s) { diff --git a/modules/planning/traffic_rules/traffic_decider.h b/modules/planning/traffic_rules/traffic_decider.h index f573b06d65..feeacf7c87 100644 --- a/modules/planning/traffic_rules/traffic_decider.h +++ b/modules/planning/traffic_rules/traffic_decider.h @@ -38,7 +38,7 @@ namespace planning { * @class TrafficDecider * @brief Create traffic related decision in this class. * The created obstacles is added to obstacles_, and the decision is added to - * path_obstacles_ + * obstacles_ * Traffic obstacle examples include: * * Traffic Light * * End of routing diff --git a/modules/planning/tuning/autotuning_raw_feature_generator.cc b/modules/planning/tuning/autotuning_raw_feature_generator.cc index 83a2d90130..e80d344794 100644 --- a/modules/planning/tuning/autotuning_raw_feature_generator.cc +++ b/modules/planning/tuning/autotuning_raw_feature_generator.cc @@ -148,7 +148,7 @@ void AutotuningRawFeatureGenerator::GenerateSTBoundaries( const ReferenceLineInfo& reference_line_info) { const auto& path_decision = reference_line_info.path_decision(); const auto& adc_sl_boundary = reference_line_info.AdcSlBoundary(); - for (auto* obstacle : path_decision.path_obstacles().Items()) { + for (auto* obstacle : path_decision.obstacles().Items()) { auto id = obstacle->Id(); const double speed = obstacle->speed(); if (!obstacle->st_boundary().IsEmpty()) { -- GitLab