diff --git a/modules/planning/common/path_obstacle.h b/modules/planning/common/path_obstacle.h index 673227411975192275b118bc606ff09013526226..397f5bcfce5aced1028545fb3626c6aec557e5c6 100644 --- a/modules/planning/common/path_obstacle.h +++ b/modules/planning/common/path_obstacle.h @@ -78,7 +78,8 @@ class PathObstacle { std::vector decisions_; std::vector decider_tags_; SLBoundary sl_boundary_; - StGraphBoundary st_boundary_; + // TODO: add st_boundary_ here. + // StGraphBoundary st_boundary_; }; } // namespace planning diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc index 00fb15846ebfa6d02d9be95154b410efd19e2e58..01e547ad1ef0364358ad135adfffc6007fbf5738 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc @@ -220,64 +220,14 @@ Status StBoundaryMapper::MapObstacleWithoutDecision( std::vector lower_points; std::vector upper_points; - const auto* obstacle = path_obstacle.Obstacle(); - bool skip = true; std::vector boundary_points; - const auto& adc_path_points = path_data_.discretized_path().path_points(); - if (adc_path_points.size() == 0) { - std::string msg = common::util::StrCat( - "Too few points in path_data_.discretized_path(); size = ", - adc_path_points.size()); - AERROR << msg; - return Status(ErrorCode::PLANNING_ERROR, msg); - } - const auto& trajectory = obstacle->Trajectory(); - if (trajectory.trajectory_point_size() == 0) { - std::string msg = common::util::StrCat("Obstacle (id = ", obstacle->Id(), - ") has NO prediction trajectory."); - AERROR << msg; - return Status(ErrorCode::PLANNING_ERROR, msg); + if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(), + *(path_obstacle.Obstacle()), &upper_points, + &lower_points)) { + return Status(ErrorCode::PLANNING_ERROR, "PLANNING_ERROR"); } - for (int j = 0; j < trajectory.trajectory_point_size(); ++j) { - const auto& trajectory_point = trajectory.trajectory_point(j); - double trajectory_point_time = trajectory_point.relative_time(); - const Box2d obs_box = obstacle->GetBoundingBox(trajectory_point); - int64_t low = 0; - int64_t high = adc_path_points.size() - 1; - bool find_low = false; - bool find_high = false; - while (low < high) { - if (find_low && find_high) { - break; - } - if (!find_low) { - if (!CheckOverlap(adc_path_points[low], obs_box, - st_boundary_config_.boundary_buffer())) { - ++low; - } else { - find_low = true; - } - } - if (!find_high) { - if (!CheckOverlap(adc_path_points[high], obs_box, - st_boundary_config_.boundary_buffer())) { - --high; - } else { - find_high = true; - } - } - } - if (find_high && find_low) { - lower_points.emplace_back( - adc_path_points[low].s() - st_boundary_config_.point_extension(), - trajectory_point_time); - upper_points.emplace_back( - adc_path_points[high].s() + st_boundary_config_.point_extension(), - trajectory_point_time); - } - } if (lower_points.size() > 0 && upper_points.size() > 0) { boundary_points.clear(); const double buffer = st_boundary_config_.follow_buffer(); @@ -298,7 +248,7 @@ Status StBoundaryMapper::MapObstacleWithoutDecision( const double area = GetArea(boundary_points); if (Double::Compare(area, 0.0) > 0) { boundary->emplace_back(&path_obstacle, boundary_points); - boundary->back().set_id(obstacle->Id()); + boundary->back().set_id(path_obstacle.Obstacle()->Id()); skip = false; } } @@ -378,235 +328,232 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory( if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(), *(path_obstacle.Obstacle()), &upper_points, &lower_points)) { - return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP") - : Status::OK(); - ; - - if (lower_points.size() > 0 && upper_points.size() > 0) { - boundary_points.clear(); - const double buffer = st_boundary_config_.follow_buffer(); - boundary_points.emplace_back(lower_points.at(0).s() - buffer, - lower_points.at(0).t()); - boundary_points.emplace_back(lower_points.back().s() - buffer, - lower_points.back().t()); - boundary_points.emplace_back(upper_points.back().s() + buffer + - st_boundary_config_.boundary_buffer(), - upper_points.back().t()); - boundary_points.emplace_back(upper_points.at(0).s() + buffer, - upper_points.at(0).t()); - if (lower_points.at(0).t() > lower_points.back().t() || - upper_points.at(0).t() > upper_points.back().t()) { - AWARN << "lower/upper points are reversed."; - } + return Status(ErrorCode::PLANNING_ERROR, "PLANNING_ERROR"); + } + if (lower_points.size() > 0 && upper_points.size() > 0) { + boundary_points.clear(); + const double buffer = st_boundary_config_.follow_buffer(); + boundary_points.emplace_back(lower_points.at(0).s() - buffer, + lower_points.at(0).t()); + boundary_points.emplace_back(lower_points.back().s() - buffer, + lower_points.back().t()); + boundary_points.emplace_back(upper_points.back().s() + buffer + + st_boundary_config_.boundary_buffer(), + upper_points.back().t()); + boundary_points.emplace_back(upper_points.at(0).s() + buffer, + upper_points.at(0).t()); + if (lower_points.at(0).t() > lower_points.back().t() || + upper_points.at(0).t() > upper_points.back().t()) { + AWARN << "lower/upper points are reversed."; + } - // change boundary according to obj_decision. - StGraphBoundary::BoundaryType b_type = - StGraphBoundary::BoundaryType::UNKNOWN; - double characteristic_length = 0.0; - if (obj_decision.has_follow()) { - const auto& speed = obstacle->Perception().velocity(); - const double scalar_speed = std::hypot(speed.x(), speed.y()); - const double minimal_follow_time = - st_boundary_config_.minimal_follow_time(); - characteristic_length = - std::fmax(scalar_speed * minimal_follow_time, - std::fabs(obj_decision.follow().distance_s())) + - vehicle_param_.front_edge_to_center(); - - boundary_points.at(0).set_s(boundary_points.at(0).s()); - boundary_points.at(1).set_s(boundary_points.at(1).s()); - boundary_points.at(3).set_t(-1.0); - b_type = StGraphBoundary::BoundaryType::FOLLOW; - } else if (obj_decision.has_yield()) { - const double dis = std::fabs(obj_decision.yield().distance_s()); - characteristic_length = dis; - // TODO(all): remove the arbitrary numbers in this part. - if (boundary_points.at(0).s() - dis < 0.0) { - boundary_points.at(0).set_s( - std::fmax(boundary_points.at(0).s() - 2.0, 0.0)); - } else { - boundary_points.at(0).set_s( - std::fmax(boundary_points.at(0).s() - dis, 0.0)); - } - if (boundary_points.at(1).s() - dis < 0.0) { - boundary_points.at(1).set_s( - std::fmax(boundary_points.at(0).s() - 4.0, 0.0)); - } else { - boundary_points.at(1).set_s( - std::fmax(boundary_points.at(0).s() - dis, 0.0)); - } - b_type = StGraphBoundary::BoundaryType::YIELD; - } else if (obj_decision.has_overtake()) { - const double dis = std::fabs(obj_decision.overtake().distance_s()); - characteristic_length = dis; - boundary_points.at(2).set_s(boundary_points.at(2).s() + dis); - boundary_points.at(3).set_s(boundary_points.at(3).s() + dis); + // change boundary according to obj_decision. + StGraphBoundary::BoundaryType b_type = + StGraphBoundary::BoundaryType::UNKNOWN; + double characteristic_length = 0.0; + if (obj_decision.has_follow()) { + const auto& speed = path_obstacle.Obstacle()->Perception().velocity(); + const double scalar_speed = std::hypot(speed.x(), speed.y()); + const double minimal_follow_time = + st_boundary_config_.minimal_follow_time(); + characteristic_length = + std::fmax(scalar_speed * minimal_follow_time, + std::fabs(obj_decision.follow().distance_s())) + + vehicle_param_.front_edge_to_center(); + + boundary_points.at(0).set_s(boundary_points.at(0).s()); + boundary_points.at(1).set_s(boundary_points.at(1).s()); + boundary_points.at(3).set_t(-1.0); + b_type = StGraphBoundary::BoundaryType::FOLLOW; + } else if (obj_decision.has_yield()) { + const double dis = std::fabs(obj_decision.yield().distance_s()); + characteristic_length = dis; + // TODO(all): remove the arbitrary numbers in this part. + if (boundary_points.at(0).s() - dis < 0.0) { + boundary_points.at(0).set_s( + std::fmax(boundary_points.at(0).s() - 2.0, 0.0)); + } else { + boundary_points.at(0).set_s( + std::fmax(boundary_points.at(0).s() - dis, 0.0)); } - - const double area = GetArea(boundary_points); - if (Double::Compare(area, 0.0) > 0) { - boundary->emplace_back(&path_obstacle, boundary_points); - boundary->back().SetBoundaryType(b_type); - boundary->back().set_id(obstacle->Id()); - boundary->back().SetCharacteristicLength(characteristic_length); - skip = false; + if (boundary_points.at(1).s() - dis < 0.0) { + boundary_points.at(1).set_s( + std::fmax(boundary_points.at(0).s() - 4.0, 0.0)); + } else { + boundary_points.at(1).set_s( + std::fmax(boundary_points.at(0).s() - dis, 0.0)); } + b_type = StGraphBoundary::BoundaryType::YIELD; + } else if (obj_decision.has_overtake()) { + const double dis = std::fabs(obj_decision.overtake().distance_s()); + characteristic_length = dis; + boundary_points.at(2).set_s(boundary_points.at(2).s() + dis); + boundary_points.at(3).set_s(boundary_points.at(3).s() + dis); } - return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP") - : Status::OK(); - } - - Status StBoundaryMapper::MapFollowDecision( - const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision, - StGraphBoundary* const boundary) const { - if (!obj_decision.has_follow()) { - std::string msg = common::util::StrCat( - "Map obstacle without prediction trajectory is ONLY supported when " - "the " - "object decision is follow. The current object decision is: \n", - obj_decision.DebugString()); - AERROR << msg; - return Status(ErrorCode::PLANNING_ERROR, msg); + const double area = GetArea(boundary_points); + if (Double::Compare(area, 0.0) > 0) { + boundary->emplace_back(&path_obstacle, boundary_points); + boundary->back().SetBoundaryType(b_type); + boundary->back().set_id(path_obstacle.Obstacle()->Id()); + boundary->back().SetCharacteristicLength(characteristic_length); + skip = false; } + } + return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP") + : Status::OK(); +} - const auto* obstacle = path_obstacle.Obstacle(); - - const auto& speed = obstacle->Perception().velocity(); - const double scalar_speed = std::hypot(speed.x(), speed.y()); +Status StBoundaryMapper::MapFollowDecision( + const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision, + StGraphBoundary* const boundary) const { + if (!obj_decision.has_follow()) { + std::string msg = common::util::StrCat( + "Map obstacle without prediction trajectory is ONLY supported when " + "the " + "object decision is follow. The current object decision is: \n", + obj_decision.DebugString()); + AERROR << msg; + return Status(ErrorCode::PLANNING_ERROR, msg); + } - const auto& perception = obstacle->Perception(); - const PathPoint ref_point = reference_line_.get_reference_point( - perception.position().x(), perception.position().y()); - const double speed_coeff = std::cos(perception.theta() - ref_point.theta()); - if (speed_coeff < 0.0) { - AERROR << "Obstacle is moving opposite to the reference line. Obstacle: " - << perception.DebugString(); - return common::Status(ErrorCode::PLANNING_ERROR, - "obstacle is moving opposite the reference line"); - } + const auto* obstacle = path_obstacle.Obstacle(); - const auto& point = path_data_.discretized_path().StartPoint(); - const PathPoint curr_point = - reference_line_.get_reference_point(point.x(), point.y()); - const double distance_to_obstacle = ref_point.s() - curr_point.s() - - vehicle_param_.front_edge_to_center() - - st_boundary_config_.follow_buffer(); - - if (distance_to_obstacle > planning_distance_) { - std::string msg = "obstacle is out of range."; - AINFO << msg; - return Status(ErrorCode::PLANNING_SKIP, msg); - } + const auto& speed = obstacle->Perception().velocity(); + const double scalar_speed = std::hypot(speed.x(), speed.y()); + + const auto& perception = obstacle->Perception(); + const PathPoint ref_point = reference_line_.get_reference_point( + perception.position().x(), perception.position().y()); + const double speed_coeff = std::cos(perception.theta() - ref_point.theta()); + if (speed_coeff < 0.0) { + AERROR << "Obstacle is moving opposite to the reference line. Obstacle: " + << perception.DebugString(); + return common::Status(ErrorCode::PLANNING_ERROR, + "obstacle is moving opposite the reference line"); + } - double follow_speed = 0.0; - if (scalar_speed > st_boundary_config_.follow_speed_threshold()) { - follow_speed = st_boundary_config_.follow_speed_threshold() * speed_coeff; - } else { - follow_speed = scalar_speed * speed_coeff * - st_boundary_config_.follow_speed_damping_factor(); - } + const auto& point = path_data_.discretized_path().StartPoint(); + const PathPoint curr_point = + reference_line_.get_reference_point(point.x(), point.y()); + const double distance_to_obstacle = ref_point.s() - curr_point.s() - + vehicle_param_.front_edge_to_center() - + st_boundary_config_.follow_buffer(); + + if (distance_to_obstacle > planning_distance_) { + std::string msg = "obstacle is out of range."; + AINFO << msg; + return Status(ErrorCode::PLANNING_SKIP, msg); + } - const double s_min_lower = distance_to_obstacle; - const double s_min_upper = - std::max(distance_to_obstacle + 1.0, planning_distance_); - const double s_max_upper = std::max( - s_min_upper + planning_time_ * follow_speed, planning_distance_); - const double s_max_lower = s_min_lower + planning_time_ * follow_speed; + double follow_speed = 0.0; + if (scalar_speed > st_boundary_config_.follow_speed_threshold()) { + follow_speed = st_boundary_config_.follow_speed_threshold() * speed_coeff; + } else { + follow_speed = scalar_speed * speed_coeff * + st_boundary_config_.follow_speed_damping_factor(); + } - std::vector boundary_points; - boundary_points.emplace_back(s_min_lower, 0.0); - boundary_points.emplace_back(s_max_lower, planning_time_); - boundary_points.emplace_back(s_max_upper, planning_time_); - boundary_points.emplace_back(s_min_upper, 0.0); + const double s_min_lower = distance_to_obstacle; + const double s_min_upper = + std::max(distance_to_obstacle + 1.0, planning_distance_); + const double s_max_upper = + std::max(s_min_upper + planning_time_ * follow_speed, planning_distance_); + const double s_max_lower = s_min_lower + planning_time_ * follow_speed; - const double area = GetArea(boundary_points); - if (Double::Compare(area, 0.0) <= 0) { - std::string msg = "Do not need to map because area is zero."; - AINFO << msg; - return Status(ErrorCode::PLANNING_SKIP, msg); - } - *boundary = StGraphBoundary(&path_obstacle, boundary_points); - - const double characteristic_length = - std::fmax(scalar_speed * speed_coeff * - st_boundary_config_.minimal_follow_time(), - std::fabs(obj_decision.follow().distance_s())) + - vehicle_param_.front_edge_to_center() + - st_boundary_config_.follow_buffer(); - - boundary->SetCharacteristicLength(characteristic_length * - st_boundary_config_.follow_coeff()); - boundary->SetBoundaryType(StGraphBoundary::BoundaryType::FOLLOW); - boundary->set_id(obstacle->Id()); - return Status::OK(); + std::vector boundary_points; + boundary_points.emplace_back(s_min_lower, 0.0); + boundary_points.emplace_back(s_max_lower, planning_time_); + boundary_points.emplace_back(s_max_upper, planning_time_); + boundary_points.emplace_back(s_min_upper, 0.0); + + const double area = GetArea(boundary_points); + if (Double::Compare(area, 0.0) <= 0) { + std::string msg = "Do not need to map because area is zero."; + AINFO << msg; + return Status(ErrorCode::PLANNING_SKIP, msg); } + *boundary = StGraphBoundary(&path_obstacle, boundary_points); + + const double characteristic_length = + std::fmax(scalar_speed * speed_coeff * + st_boundary_config_.minimal_follow_time(), + std::fabs(obj_decision.follow().distance_s())) + + vehicle_param_.front_edge_to_center() + + st_boundary_config_.follow_buffer(); + + boundary->SetCharacteristicLength(characteristic_length * + st_boundary_config_.follow_coeff()); + boundary->SetBoundaryType(StGraphBoundary::BoundaryType::FOLLOW); + boundary->set_id(obstacle->Id()); + return Status::OK(); +} - double StBoundaryMapper::GetArea(const std::vector& boundary_points) - const { - if (boundary_points.size() < 3) { - return 0.0; - } +double StBoundaryMapper::GetArea( + const std::vector& boundary_points) const { + if (boundary_points.size() < 3) { + return 0.0; + } - double area = 0.0; - for (uint32_t i = 2; i < boundary_points.size(); ++i) { - const double ds1 = boundary_points[i - 1].s() - boundary_points[0].s(); - const double dt1 = boundary_points[i - 1].t() - boundary_points[0].t(); + double area = 0.0; + for (uint32_t i = 2; i < boundary_points.size(); ++i) { + const double ds1 = boundary_points[i - 1].s() - boundary_points[0].s(); + const double dt1 = boundary_points[i - 1].t() - boundary_points[0].t(); - const double ds2 = boundary_points[i].s() - boundary_points[0].s(); - const double dt2 = boundary_points[i].t() - boundary_points[0].t(); - // cross product - area += (ds1 * dt2 - ds2 * dt1); - } - return fabs(area * 0.5); + const double ds2 = boundary_points[i].s() - boundary_points[0].s(); + const double dt2 = boundary_points[i].t() - boundary_points[0].t(); + // cross product + area += (ds1 * dt2 - ds2 * dt1); } + return fabs(area * 0.5); +} - bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point, - const Box2d& obs_box, const double buffer) - const { - const double mid_to_rear_center = - vehicle_param_.length() / 2.0 - vehicle_param_.front_edge_to_center(); - const double x = - path_point.x() - mid_to_rear_center * std::cos(path_point.theta()); - const double y = - path_point.y() - mid_to_rear_center * std::sin(path_point.theta()); - const Box2d adc_box = - Box2d({x, y}, path_point.theta(), vehicle_param_.length() + 2 * buffer, - vehicle_param_.width() + 2 * buffer); - return obs_box.HasOverlap(adc_box); - } +bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point, + const Box2d& obs_box, + const double buffer) const { + const double mid_to_rear_center = + vehicle_param_.length() / 2.0 - vehicle_param_.front_edge_to_center(); + const double x = + path_point.x() - mid_to_rear_center * std::cos(path_point.theta()); + const double y = + path_point.y() - mid_to_rear_center * std::sin(path_point.theta()); + const Box2d adc_box = + Box2d({x, y}, path_point.theta(), vehicle_param_.length() + 2 * buffer, + vehicle_param_.width() + 2 * buffer); + return obs_box.HasOverlap(adc_box); +} - Status StBoundaryMapper::GetSpeedLimits(SpeedLimit * const speed_limit_data) - const { - CHECK_NOTNULL(speed_limit_data); +Status StBoundaryMapper::GetSpeedLimits( + SpeedLimit* const speed_limit_data) const { + CHECK_NOTNULL(speed_limit_data); - std::vector speed_limits; - for (const auto& path_point : path_data_.discretized_path().path_points()) { - if (Double::Compare(path_point.s(), reference_line_.length()) > 0) { - std::string msg = common::util::StrCat( - "path length [", path_data_.discretized_path().Length(), - "] is LARGER than reference_line_ length [", - reference_line_.length(), "]. Please debug before proceeding."); - AWARN << msg; - break; - } + std::vector speed_limits; + for (const auto& path_point : path_data_.discretized_path().path_points()) { + if (Double::Compare(path_point.s(), reference_line_.length()) > 0) { + std::string msg = common::util::StrCat( + "path length [", path_data_.discretized_path().Length(), + "] is LARGER than reference_line_ length [", reference_line_.length(), + "]. Please debug before proceeding."); + AWARN << msg; + break; + } - double speed_limit_on_reference_line = - reference_line_.GetSpeedLimitFromS(path_point.s()); + double speed_limit_on_reference_line = + reference_line_.GetSpeedLimitFromS(path_point.s()); - // speed limit from path curvature - double speed_limit_on_path = - std::sqrt(st_boundary_config_.centric_acceleration_limit() / - std::fmax(std::fabs(path_point.kappa()), - st_boundary_config_.minimal_kappa())); + // speed limit from path curvature + double speed_limit_on_path = + std::sqrt(st_boundary_config_.centric_acceleration_limit() / + std::fmax(std::fabs(path_point.kappa()), + st_boundary_config_.minimal_kappa())); - const double curr_speed_limit = std::fmax( - st_boundary_config_.lowest_speed(), - std::fmin(speed_limit_on_path, speed_limit_on_reference_line)); + const double curr_speed_limit = std::fmax( + st_boundary_config_.lowest_speed(), + std::fmin(speed_limit_on_path, speed_limit_on_reference_line)); - speed_limit_data->AddSpeedLimit(path_point.s(), curr_speed_limit); - } - return Status::OK(); + speed_limit_data->AddSpeedLimit(path_point.s(), curr_speed_limit); } + return Status::OK(); +} } // namespace planning } // namespace apollo diff --git a/modules/planning/optimizer/st_graph/st_graph_boundary.h b/modules/planning/optimizer/st_graph/st_graph_boundary.h index b3be13441904e20b6e4581759954c1616aea011d..58f78f2976b9e9d98d8456f54d6e2f9fd5ee9d3c 100644 --- a/modules/planning/optimizer/st_graph/st_graph_boundary.h +++ b/modules/planning/optimizer/st_graph/st_graph_boundary.h @@ -55,7 +55,8 @@ class StGraphBoundary : public common::math::Polygon2d { ~StGraphBoundary() = default; - bool IsEmpty() const { return points.empty(); } + // TODO: add this function. + // bool IsEmpty() const { return points.empty(); } bool IsPointInBoundary(const StGraphPoint& st_graph_point) const; bool IsPointInBoundary(const STPoint& st_point) const;