From ef253e655539d6f45e2ecc1f6f1ee8c392549ea6 Mon Sep 17 00:00:00 2001 From: Dong Li Date: Mon, 2 Oct 2017 10:28:46 -0700 Subject: [PATCH] planning: added st boundary for keep clear --- .../planning/proto/dp_st_speed_config.proto | 2 + .../tasks/dp_poly_path/trajectory_cost.cc | 2 +- modules/planning/tasks/dp_st_speed/BUILD | 1 + .../planning/tasks/dp_st_speed/dp_st_cost.cc | 22 ++-- .../planning/tasks/dp_st_speed/dp_st_cost.h | 4 +- .../planning/tasks/dp_st_speed/dp_st_graph.cc | 3 +- .../tasks/path_decider/path_decider.cc | 5 + .../tasks/speed_decider/speed_decider.h | 2 +- .../tasks/st_graph/st_boundary_mapper.cc | 102 ++++++++---------- .../tasks/st_graph/st_boundary_mapper.h | 5 + 10 files changed, 79 insertions(+), 69 deletions(-) diff --git a/modules/planning/proto/dp_st_speed_config.proto b/modules/planning/proto/dp_st_speed_config.proto index 1c31cd0bb5..3e6b5738d2 100644 --- a/modules/planning/proto/dp_st_speed_config.proto +++ b/modules/planning/proto/dp_st_speed_config.proto @@ -40,4 +40,6 @@ message DpStSpeedConfig { optional double max_deceleration = 22 [ default = -4.5 ]; optional apollo.planning.StBoundaryConfig st_boundary_config = 33; + + optional double keep_clear_cost_factor = 50 [ default = 1000.0 ]; } diff --git a/modules/planning/tasks/dp_poly_path/trajectory_cost.cc b/modules/planning/tasks/dp_poly_path/trajectory_cost.cc index a91f642233..baf076d4e5 100644 --- a/modules/planning/tasks/dp_poly_path/trajectory_cost.cc +++ b/modules/planning/tasks/dp_poly_path/trajectory_cost.cc @@ -54,7 +54,7 @@ TrajectoryCost::TrajectoryCost( continue; } const auto ptr_obstacle = ptr_path_obstacle->obstacle(); - if (ptr_obstacle->PerceptionId() < 0) { + if (Obstacle::IsVirtualObstacle(ptr_obstacle->Perception())) { // Virtual obsatcle continue; } diff --git a/modules/planning/tasks/dp_st_speed/BUILD b/modules/planning/tasks/dp_st_speed/BUILD index 645b71b937..5509ab01e4 100644 --- a/modules/planning/tasks/dp_st_speed/BUILD +++ b/modules/planning/tasks/dp_st_speed/BUILD @@ -25,6 +25,7 @@ cc_library( "dp_st_cost.h", ], deps = [ + ":st_graph_point", "//modules/common/proto:pnc_point_proto", "//modules/planning/common:frame", "//modules/planning/common/speed:st_boundary", diff --git a/modules/planning/tasks/dp_st_speed/dp_st_cost.cc b/modules/planning/tasks/dp_st_speed/dp_st_cost.cc index 6d1229038b..2b94adf67c 100644 --- a/modules/planning/tasks/dp_st_speed/dp_st_cost.cc +++ b/modules/planning/tasks/dp_st_speed/dp_st_cost.cc @@ -32,22 +32,32 @@ DpStCost::DpStCost(const DpStSpeedConfig& dp_st_speed_config) unit_s_(dp_st_speed_config_.total_path_length() / dp_st_speed_config_.matrix_dimension_s()), unit_t_(dp_st_speed_config_.total_time() / - dp_st_speed_config_.matrix_dimension_t()) {} + dp_st_speed_config_.matrix_dimension_t()) { + unit_v_ = unit_s_ / unit_t_; +} // TODO(all): normalize cost with time double DpStCost::GetObstacleCost( - const STPoint& point, + const StGraphPoint& st_graph_point, const std::vector& st_boundaries) const { double total_cost = 0.0; constexpr double inf = std::numeric_limits::infinity(); - if (point.s() < 0) { + const double unit_v = unit_s_ / unit_t_; + const auto& st_point = st_graph_point.point(); + if (st_point.s() < 0) { return inf; } for (const StBoundary* boundary : st_boundaries) { - if (boundary->IsPointInBoundary(point)) { - return inf; + if (boundary->IsPointInBoundary(st_point)) { + if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) { + total_cost += unit_v * ((st_graph_point.index_s() + 1.0) / + (st_graph_point.index_t() + 1.0)) * + dp_st_speed_config_.keep_clear_cost_factor(); + } else { + return inf; + } } else { - const double distance = boundary->DistanceS(point); + const double distance = boundary->DistanceS(st_point); total_cost += dp_st_speed_config_.default_obstacle_cost() * std::exp(dp_st_speed_config_.obstacle_cost_factor() / boundary->characteristic_length() * distance); diff --git a/modules/planning/tasks/dp_st_speed/dp_st_cost.h b/modules/planning/tasks/dp_st_speed/dp_st_cost.h index c74cdb1b4a..9d30c66ffe 100644 --- a/modules/planning/tasks/dp_st_speed/dp_st_cost.h +++ b/modules/planning/tasks/dp_st_speed/dp_st_cost.h @@ -28,6 +28,7 @@ #include "modules/planning/common/speed/st_boundary.h" #include "modules/planning/common/speed/st_point.h" +#include "modules/planning/tasks/dp_st_speed/st_graph_point.h" namespace apollo { namespace planning { @@ -37,7 +38,7 @@ class DpStCost { explicit DpStCost(const DpStSpeedConfig& dp_st_speed_config); double GetObstacleCost( - const STPoint& point, + const StGraphPoint& point, const std::vector& st_boundaries) const; double GetReferenceCost(const STPoint& point, @@ -70,6 +71,7 @@ class DpStCost { const DpStSpeedConfig& dp_st_speed_config_; double unit_s_ = 0.0; double unit_t_ = 0.0; + double unit_v_ = 0.0; }; } // namespace planning diff --git a/modules/planning/tasks/dp_st_speed/dp_st_graph.cc b/modules/planning/tasks/dp_st_speed/dp_st_graph.cc index a700264395..f9b7b0a804 100644 --- a/modules/planning/tasks/dp_st_speed/dp_st_graph.cc +++ b/modules/planning/tasks/dp_st_speed/dp_st_graph.cc @@ -133,8 +133,7 @@ void DpStGraph::CalculatePointwiseCost( for (auto& st_graph_point : cost_table_[i]) { double ref_cost = dp_st_cost_.GetReferenceCost(st_graph_point.point(), reference_points[i]); - double obs_cost = - dp_st_cost_.GetObstacleCost(st_graph_point.point(), boundaries); + double obs_cost = dp_st_cost_.GetObstacleCost(st_graph_point, boundaries); st_graph_point.SetReferenceCost(ref_cost); st_graph_point.SetObstacleCost(obs_cost); st_graph_point.SetTotalCost(std::numeric_limits::infinity()); diff --git a/modules/planning/tasks/path_decider/path_decider.cc b/modules/planning/tasks/path_decider/path_decider.cc index ee0c3c7406..65365b1f0a 100644 --- a/modules/planning/tasks/path_decider/path_decider.cc +++ b/modules/planning/tasks/path_decider/path_decider.cc @@ -96,6 +96,11 @@ bool PathDecider::MakeStaticObstacleDecision( continue; } + if (path_obstacle->st_boundary().boundary_type() == + StBoundary::BoundaryType::KEEP_CLEAR) { + continue; + } + // IGNORE by default ObjectDecisionType object_decision; object_decision.mutable_ignore(); diff --git a/modules/planning/tasks/speed_decider/speed_decider.h b/modules/planning/tasks/speed_decider/speed_decider.h index c51135e9c4..50dbb08f01 100644 --- a/modules/planning/tasks/speed_decider/speed_decider.h +++ b/modules/planning/tasks/speed_decider/speed_decider.h @@ -35,7 +35,7 @@ class SpeedDecider : public Task { SpeedDecider(); ~SpeedDecider() = default; - bool Init(const PlanningConfig& config); + bool Init(const PlanningConfig& config) override; apollo::common::Status Execute( Frame* frame, ReferenceLineInfo* reference_line_info) override; diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.cc b/modules/planning/tasks/st_graph/st_boundary_mapper.cc index 2a82fb0ce5..dd319fb952 100644 --- a/modules/planning/tasks/st_graph/st_boundary_mapper.cc +++ b/modules/planning/tasks/st_graph/st_boundary_mapper.cc @@ -206,20 +206,15 @@ Status StBoundaryMapper::MapWithoutDecision(PathObstacle* path_obstacle) const { return Status::OK(); } - if (lower_points.size() != upper_points.size()) { - std::string msg = common::util::StrCat( - "lower_points.size()[", lower_points.size(), - "] and upper_points.size()[", upper_points.size(), "] does NOT match."); - return Status(ErrorCode::PLANNING_ERROR, msg); - } - - if (lower_points.size() > 1 && upper_points.size() > 1) { - auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points) - .ExpandByS(boundary_s_buffer) - .ExpandByT(boundary_t_buffer); - boundary.SetId(path_obstacle->Id()); - path_obstacle->SetStBoundary(boundary); + 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(); + if (!prev_st_boundary.IsEmpty()) { + boundary.SetBoundaryType(prev_st_boundary.boundary_type()); } + path_obstacle->SetStBoundary(boundary); return Status::OK(); } @@ -359,7 +354,7 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints( } } DCHECK_EQ(lower_points->size(), upper_points->size()); - return (lower_points->size() > 0 && upper_points->size() > 0); + return (lower_points->size() > 1 && upper_points->size() > 1); } Status StBoundaryMapper::MapWithPredictionTrajectory( @@ -376,55 +371,46 @@ Status StBoundaryMapper::MapWithPredictionTrajectory( if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(), *(path_obstacle->obstacle()), &upper_points, &lower_points)) { - return Status(ErrorCode::PLANNING_ERROR, "PLANNING_ERROR"); + return Status::OK(); } - if (lower_points.size() != upper_points.size()) { - std::string msg = common::util::StrCat( - "lower_points.size()[", lower_points.size(), - "] and upper_points.size()[", upper_points.size(), "] does NOT match."); - return Status(ErrorCode::PLANNING_ERROR, msg); + if (obj_decision.has_follow() && lower_points.back().t() < planning_time_) { + const double diff_s = lower_points.back().s() - lower_points.front().s(); + const double diff_t = lower_points.back().t() - lower_points.front().t(); + double extend_lower_s = + diff_s / diff_t * (planning_time_ - lower_points.front().t()) + + lower_points.front().s(); + const double extend_upper_s = + extend_lower_s + (upper_points.back().s() - lower_points.back().s()) + + 1.0; + upper_points.emplace_back(extend_upper_s, planning_time_); + lower_points.emplace_back(extend_lower_s, planning_time_); } - if (lower_points.size() > 1 && upper_points.size() > 1) { - if (obj_decision.has_follow() && lower_points.back().t() < planning_time_) { - const double diff_s = lower_points.back().s() - lower_points.front().s(); - const double diff_t = lower_points.back().t() - lower_points.front().t(); - double extend_lower_s = - diff_s / diff_t * (planning_time_ - lower_points.front().t()) + - lower_points.front().s(); - const double extend_upper_s = - extend_lower_s + (upper_points.back().s() - lower_points.back().s()) + - 1.0; - upper_points.emplace_back(extend_upper_s, planning_time_); - lower_points.emplace_back(extend_lower_s, planning_time_); - } - - auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points) - .ExpandByS(boundary_s_buffer) - .ExpandByT(boundary_t_buffer); - - // get characteristic_length and boundary_type. - StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN; - double characteristic_length = 0.0; - if (obj_decision.has_follow()) { - characteristic_length = std::fabs(obj_decision.follow().distance_s()); - b_type = StBoundary::BoundaryType::FOLLOW; - } else if (obj_decision.has_yield()) { - characteristic_length = std::fabs(obj_decision.yield().distance_s()); - b_type = StBoundary::BoundaryType::YIELD; - } else if (obj_decision.has_overtake()) { - characteristic_length = std::fabs(obj_decision.overtake().distance_s()); - b_type = StBoundary::BoundaryType::OVERTAKE; - } else { - DCHECK(false) << "Obj decision should be either yield or overtake: " - << obj_decision.DebugString(); - } - boundary.SetBoundaryType(b_type); - boundary.SetId(path_obstacle->obstacle()->Id()); - boundary.SetCharacteristicLength(characteristic_length); - path_obstacle->SetStBoundary(boundary); + auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points) + .ExpandByS(boundary_s_buffer) + .ExpandByT(boundary_t_buffer); + + // get characteristic_length and boundary_type. + StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN; + double characteristic_length = 0.0; + if (obj_decision.has_follow()) { + characteristic_length = std::fabs(obj_decision.follow().distance_s()); + b_type = StBoundary::BoundaryType::FOLLOW; + } else if (obj_decision.has_yield()) { + characteristic_length = std::fabs(obj_decision.yield().distance_s()); + b_type = StBoundary::BoundaryType::YIELD; + } else if (obj_decision.has_overtake()) { + characteristic_length = std::fabs(obj_decision.overtake().distance_s()); + b_type = StBoundary::BoundaryType::OVERTAKE; + } else { + DCHECK(false) << "Obj decision should be either yield or overtake: " + << obj_decision.DebugString(); } + boundary.SetBoundaryType(b_type); + boundary.SetId(path_obstacle->obstacle()->Id()); + boundary.SetCharacteristicLength(characteristic_length); + path_obstacle->SetStBoundary(boundary); return Status::OK(); } diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.h b/modules/planning/tasks/st_graph/st_boundary_mapper.h index f8b4678cb7..9bd0c50640 100644 --- a/modules/planning/tasks/st_graph/st_boundary_mapper.h +++ b/modules/planning/tasks/st_graph/st_boundary_mapper.h @@ -59,6 +59,11 @@ class StBoundaryMapper { const apollo::common::math::Box2d& obs_box, const double buffer) const; + /** + * Creates valid st boundary upper_points and lower_points + * If return true, upper_points.size() > 1 and + * upper_points.size() = lower_points.size() + */ bool GetOverlapBoundaryPoints( const std::vector& path_points, const Obstacle& obstacle, std::vector* upper_points, -- GitLab