提交 8333ac3a 编写于 作者: J jmtao 提交者: Jiangtao Hu

planning: add GetDrivingWidth() to ReferenceLine

上级 566fff40
......@@ -475,6 +475,18 @@ void ReferenceLine::GetLaneFromS(
}
}
double ReferenceLine::GetDrivingWidth(const SLBoundary& sl_boundary) const {
double lane_left_width = 0.0;
double lane_right_width = 0.0;
GetLaneWidth(sl_boundary.start_s(), &lane_left_width, &lane_right_width);
double driving_width = std::max(lane_left_width - sl_boundary.end_l(),
lane_right_width + sl_boundary.start_l());
driving_width = std::min(lane_left_width + lane_right_width, driving_width);
ADEBUG << "driving_width[" << driving_width << "]";
return driving_width;
}
bool ReferenceLine::IsOnLane(const common::math::Vec2d& vec2d_point) const {
common::SLPoint sl_point;
if (!XYToSL(vec2d_point, &sl_point)) {
......
......@@ -123,6 +123,8 @@ class ReferenceLine {
void GetLaneFromS(const double s,
std::vector<hdmap::LaneInfoConstPtr>* lanes) const;
double GetDrivingWidth(const SLBoundary& sl_boundary) const;
/**
* @brief: check if a box/point is on lane along reference line
*/
......
......@@ -188,19 +188,11 @@ bool SidePassScenario::HasBlockingObstacle(const Frame& frame) {
continue;
}
// check l
// check driving_width
constexpr double kLBufferThreshold = 0.3; // unit: m
const auto& reference_line = reference_line_info.reference_line();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line.GetLaneWidth(obstacle->PerceptionSLBoundary().start_s(),
&lane_left_width, &lane_right_width);
double driving_width =
std::max(lane_left_width - obstacle->PerceptionSLBoundary().end_l(),
lane_right_width + obstacle->PerceptionSLBoundary().start_l());
driving_width =
std::min(lane_left_width + lane_right_width, driving_width);
ADEBUG << "driving_width[" << driving_width << "]";
const double driving_width = reference_line.GetDrivingWidth(
obstacle->PerceptionSLBoundary());
const double adc_width =
VehicleConfigHelper::GetConfig().vehicle_param().width();
if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer >
......
......@@ -73,20 +73,12 @@ Stage::StageStatus SidePassBackup::Process(
continue;
}
// check l
// check driving_width
constexpr double kLBufferThreshold = 0.3; // unit: m
const auto& reference_line =
frame->reference_line_info().front().reference_line();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line.GetLaneWidth(obstacle->PerceptionSLBoundary().start_s(),
&lane_left_width, &lane_right_width);
double driving_width =
std::max(lane_left_width - obstacle->PerceptionSLBoundary().end_l(),
lane_right_width + obstacle->PerceptionSLBoundary().start_l());
driving_width =
std::min(lane_left_width + lane_right_width, driving_width);
ADEBUG << "driving_width[" << driving_width << "]";
const double driving_width = reference_line.GetDrivingWidth(
obstacle->PerceptionSLBoundary());
const double adc_width =
VehicleConfigHelper::GetConfig().vehicle_param().width();
if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer >
......@@ -146,20 +138,12 @@ Stage::StageStatus SidePassApproachObstacle::Process(
continue;
}
// check l
// check driving_width
constexpr double kLBufferThreshold = 0.3; // unit: m
const auto& reference_line =
frame->reference_line_info().front().reference_line();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line.GetLaneWidth(obstacle->PerceptionSLBoundary().start_s(),
&lane_left_width, &lane_right_width);
double driving_width =
std::max(lane_left_width - obstacle->PerceptionSLBoundary().end_l(),
lane_right_width + obstacle->PerceptionSLBoundary().start_l());
driving_width =
std::min(lane_left_width + lane_right_width, driving_width);
ADEBUG << "driving_width[" << driving_width << "]";
const double driving_width = reference_line.GetDrivingWidth(
obstacle->PerceptionSLBoundary());
const double adc_width =
VehicleConfigHelper::GetConfig().vehicle_param().width();
if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer >
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册