apollo::planning::ReferenceLine类 参考

#include <reference_line.h>

apollo::planning::ReferenceLine 的协作图:

Public 成员函数
	ReferenceLine ()=default
	ReferenceLine (const ReferenceLine &reference_line)=default
template<typename Iterator >
	ReferenceLine (const Iterator begin, const Iterator end)
	ReferenceLine (const std::vector< ReferencePoint > &reference_points)
	ReferenceLine (const hdmap::Path &hdmap_path)
bool	Stitch (const ReferenceLine &other)
	Stitch current reference line with the other reference line The stitching strategy is to use current reference points as much as possible.
bool	Segment (const common::math::Vec2d &point, const double distance_backward, const double distance_forward)
bool	Segment (const double s, const double distance_backward, const double distance_forward)
const hdmap::Path &	map_path () const
const std::vector< ReferencePoint > &	reference_points () const
ReferencePoint	GetReferencePoint (const double s) const
common::FrenetFramePoint	GetFrenetPoint (const common::PathPoint &path_point) const
std::pair< std::array< double, 3 >, std::array< double, 3 > >	ToFrenetFrame (const common::TrajectoryPoint &traj_point) const
std::vector< ReferencePoint >	GetReferencePoints (double start_s, double end_s) const
size_t	GetNearestReferenceIndex (const double s) const
ReferencePoint	GetNearestReferencePoint (const common::math::Vec2d &xy) const
std::vector< hdmap::LaneSegment >	GetLaneSegments (const double start_s, const double end_s) const
ReferencePoint	GetNearestReferencePoint (const double s) const
ReferencePoint	GetReferencePoint (const double x, const double y) const
bool	GetApproximateSLBoundary (const common::math::Box2d &box, const double start_s, const double end_s, SLBoundary *const sl_boundary) const
bool	GetSLBoundary (const common::math::Box2d &box, SLBoundary *const sl_boundary, double warm_start_s=-1.0) const
	Get the SL Boundary of the box.
bool	GetSLBoundary (const common::math::Polygon2d &polygon, SLBoundary *const sl_boundary, double warm_start_s=-1.0) const
bool	GetSLBoundary (const std::vector< common::math::Vec2d > &corners, SLBoundary *const sl_boundary, double warm_start_s) const
bool	GetSLBoundary (const hdmap::Polygon &polygon, SLBoundary *const sl_boundary) const
bool	SLToXY (const common::SLPoint &sl_point, common::math::Vec2d *const xy_point) const
bool	XYToSL (const common::math::Vec2d &xy_point, common::SLPoint *const sl_point, double warm_start_s=-1.0) const
	Transvert Cartesian coordinates to Frenet.
bool	XYToSL (const double heading, const common::math::Vec2d &xy_point, common::SLPoint *const sl_point, double warm_start_s=-1.0) const
bool	XYToSL (const common::math::Vec2d &xy_point, common::SLPoint *const sl_point, double hueristic_start_s, double hueristic_end_s) const
template<class XYPoint >
bool	XYToSL (const XYPoint &xy, common::SLPoint *const sl_point) const
bool	GetLaneWidth (const double s, double *const lane_left_width, double *const lane_right_width) const
bool	GetOffsetToMap (const double s, double *l_offset) const
bool	GetRoadWidth (const double s, double *const road_left_width, double *const road_right_width) const
hdmap::Road::Type	GetRoadType (const double s) const
void	GetLaneBoundaryType (const double s, hdmap::LaneBoundaryType::Type *const left_boundary_type, hdmap::LaneBoundaryType::Type *const right_boundary_type) const
void	GetLaneFromS (const double s, std::vector< hdmap::LaneInfoConstPtr > *lanes) const
double	GetDrivingWidth (const SLBoundary &sl_boundary) const
bool	IsOnLane (const common::SLPoint &sl_point) const
	: check if a box/point is on lane along reference line
bool	IsOnLane (const common::math::Vec2d &vec2d_point) const
template<class XYPoint >
bool	IsOnLane (const XYPoint &xy) const
bool	IsOnLane (const SLBoundary &sl_boundary) const
bool	IsOnRoad (const common::SLPoint &sl_point) const
	: check if a box/point is on road (not on sideways/medians) along reference line
bool	IsOnRoad (const common::math::Vec2d &vec2d_point) const
bool	IsOnRoad (const SLBoundary &sl_boundary) const
bool	IsBlockRoad (const common::math::Box2d &box2d, double gap) const
	Check if a box is blocking the road surface.
bool	HasOverlap (const common::math::Box2d &box) const
	check if any part of the box has overlap with the road.
double	Length () const
std::string	DebugString () const
double	GetSpeedLimitFromS (const double s) const
void	AddSpeedLimit (double start_s, double end_s, double speed_limit)
uint32_t	GetPriority () const
void	SetPriority (uint32_t priority)
const hdmap::Path &	GetMapPath () const
void	SetEgoPosition (common::math::Vec2d ego_pos)

详细描述

在文件 reference_line.h 第 40 行定义.

构造及析构函数说明

ReferenceLine() [1/5]

apollo::planning::ReferenceLine::ReferenceLine ( )

default

ReferenceLine() [2/5]

apollo::planning::ReferenceLine::ReferenceLine ( const ReferenceLine &  reference_line )

explicitdefault

ReferenceLine() [3/5]

template<typename Iterator >

apollo::planning::ReferenceLine::ReferenceLine ( const Iterator  begin, const Iterator  end  )

inline

在文件 reference_line.h 第 45 行定义.

      : reference_points_(begin, end),
        map_path_(std::move(std::vector<hdmap::MapPathPoint>(begin, end))) {}

ReferenceLine() [4/5]

apollo::planning::ReferenceLine::ReferenceLine ( const std::vector< ReferencePoint > &  reference_points )

explicit

在文件 reference_line.cc 第 51 行定义.

    : reference_points_(reference_points),
      map_path_(std::move(std::vector<hdmap::MapPathPoint>(
          reference_points.begin(), reference_points.end()))) {
  CHECK_EQ(static_cast<size_t>(map_path_.num_points()),
           reference_points_.size());
}

ReferenceLine() [5/5]

apollo::planning::ReferenceLine::ReferenceLine ( const hdmap::Path &  hdmap_path )

explicit

在文件 reference_line.cc 第 60 行定义.

    : map_path_(hdmap_path) {
  for (const auto& point : hdmap_path.path_points()) {
    DCHECK(!point.lane_waypoints().empty());
    const auto& lane_waypoint = point.lane_waypoints()[0];
    reference_points_.emplace_back(
        hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0);
  }
  CHECK_EQ(static_cast<size_t>(map_path_.num_points()),
           reference_points_.size());
}

成员函数说明

AddSpeedLimit()

void apollo::planning::ReferenceLine::AddSpeedLimit	(	double	start_s,
		double	end_s,
		double	speed_limit
	)

在文件 reference_line.cc 第 951 行定义.

                                                      {
  std::vector<SpeedLimit> new_speed_limit;
  for (const auto& limit : speed_limit_) {
    if (start_s >= limit.end_s || end_s <= limit.start_s) {
      new_speed_limit.emplace_back(limit);
    } else {
      // start_s < speed_limit.end_s && end_s > speed_limit.start_s
      double min_speed = std::min(limit.speed_limit, speed_limit);
      if (start_s >= limit.start_s) {
        new_speed_limit.emplace_back(limit.start_s, start_s, min_speed);
        if (end_s <= limit.end_s) {
          new_speed_limit.emplace_back(start_s, end_s, min_speed);
          new_speed_limit.emplace_back(end_s, limit.end_s, limit.speed_limit);
        } else {
          new_speed_limit.emplace_back(start_s, limit.end_s, min_speed);
        }
      } else {
        new_speed_limit.emplace_back(start_s, limit.start_s, speed_limit);
        if (end_s <= limit.end_s) {
          new_speed_limit.emplace_back(limit.start_s, end_s, min_speed);
          new_speed_limit.emplace_back(end_s, limit.end_s, limit.speed_limit);
        } else {
          new_speed_limit.emplace_back(limit.start_s, limit.end_s, min_speed);
        }
      }
      start_s = limit.end_s;
      end_s = std::max(end_s, limit.end_s);
    }
  }
  speed_limit_.clear();
  if (end_s > start_s) {
    new_speed_limit.emplace_back(start_s, end_s, speed_limit);
  }
  for (const auto& limit : new_speed_limit) {
    if (limit.start_s < limit.end_s) {
      speed_limit_.emplace_back(limit);
    }
  }
  std::sort(speed_limit_.begin(), speed_limit_.end(),
            [](const SpeedLimit& a, const SpeedLimit& b) {
              if (a.start_s != b.start_s) {
                return a.start_s < b.start_s;
              }
              if (a.end_s != b.end_s) {
                return a.end_s < b.end_s;
              }
              return a.speed_limit < b.speed_limit;
            });
}

DebugString()

std::string apollo::planning::ReferenceLine::DebugString

(

)

const

在文件 reference_line.cc 第 908 行定义.

                                           {
  const auto limit =
      std::min(reference_points_.size(),
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
  return absl::StrCat(
      "point num:", reference_points_.size(),
      absl::StrJoin(reference_points_.begin(),
                    reference_points_.begin() + limit, "",
                    apollo::common::util::DebugStringFormatter()));
}

GetApproximateSLBoundary()

bool apollo::planning::ReferenceLine::GetApproximateSLBoundary	(	const common::math::Box2d &	box,
		const double	start_s,
		const double	end_s,
		SLBoundary *const	sl_boundary
	)		const

在文件 reference_line.cc 第 695 行定义.

                                         {
  double s = 0.0;
  double l = 0.0;
  double distance = 0.0;
  if (!map_path_.GetProjectionWithHueristicParams(box.center(), start_s, end_s,
                                                  &s, &l, &distance)) {
    AERROR << "Cannot get projection point from path.";
    return false;
  }
 
  auto projected_point = map_path_.GetSmoothPoint(s);
  auto rotated_box = box;
  rotated_box.RotateFromCenter(-projected_point.heading());
 
  std::vector<common::math::Vec2d> corners;
  rotated_box.GetAllCorners(&corners);
 
  double min_s(std::numeric_limits<double>::max());
  double max_s(std::numeric_limits<double>::lowest());
  double min_l(std::numeric_limits<double>::max());
  double max_l(std::numeric_limits<double>::lowest());
 
  for (const auto& point : corners) {
    // x <--> s, y <--> l
    // Because the box is rotated to align the reference line
    min_s = std::fmin(min_s, point.x() - rotated_box.center().x() + s);
    max_s = std::fmax(max_s, point.x() - rotated_box.center().x() + s);
    min_l = std::fmin(min_l, point.y() - rotated_box.center().y() + l);
    max_l = std::fmax(max_l, point.y() - rotated_box.center().y() + l);
  }
  sl_boundary->set_start_s(min_s);
  sl_boundary->set_end_s(max_s);
  sl_boundary->set_start_l(min_l);
  sl_boundary->set_end_l(max_l);
  return true;
}

GetDrivingWidth()

double apollo::planning::ReferenceLine::GetDrivingWidth

(

const SLBoundary &

sl_boundary

)

const

在文件 reference_line.cc 第 611 行定义.

                                                                         {
  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;
}

GetFrenetPoint()

common::FrenetFramePoint apollo::planning::ReferenceLine::GetFrenetPoint

(

const common::PathPoint &

path_point

)

const

在文件 reference_line.cc 第 185 行定义.

                                             {
  if (reference_points_.empty()) {
    return common::FrenetFramePoint();
  }
 
  common::SLPoint sl_point;
  XYToSL(path_point, &sl_point);
  common::FrenetFramePoint frenet_frame_point;
  frenet_frame_point.set_s(sl_point.s());
  frenet_frame_point.set_l(sl_point.l());
 
  const double theta = path_point.theta();
  const double kappa = path_point.kappa();
  const double l = frenet_frame_point.l();
 
  ReferencePoint ref_point = GetReferencePoint(frenet_frame_point.s());
 
  const double theta_ref = ref_point.heading();
  const double kappa_ref = ref_point.kappa();
  const double dkappa_ref = ref_point.dkappa();
 
  const double dl = CartesianFrenetConverter::CalculateLateralDerivative(
      theta_ref, theta, l, kappa_ref);
  const double ddl =
      CartesianFrenetConverter::CalculateSecondOrderLateralDerivative(
          theta_ref, theta, kappa_ref, kappa, dkappa_ref, l);
  frenet_frame_point.set_dl(dl);
  frenet_frame_point.set_ddl(ddl);
  return frenet_frame_point;
}

GetLaneBoundaryType()

void apollo::planning::ReferenceLine::GetLaneBoundaryType	(	const double	s,
		hdmap::LaneBoundaryType::Type *const	left_boundary_type,
		hdmap::LaneBoundaryType::Type *const	right_boundary_type
	)		const

在文件 reference_line.cc 第 590 行定义.

                                                                {
  auto ref_point = GetReferencePoint(s);
  const auto waypoint = ref_point.lane_waypoints().front();
  *left_boundary_type = hdmap::LeftBoundaryType(waypoint);
  *right_boundary_type = hdmap::RightBoundaryType(waypoint);
}

GetLaneFromS()

void apollo::planning::ReferenceLine::GetLaneFromS	(	const double	s,
		std::vector< hdmap::LaneInfoConstPtr > *	lanes
	)		const

在文件 reference_line.cc 第 599 行定义.

                                                                   {
  CHECK_NOTNULL(lanes);
  auto ref_point = GetReferencePoint(s);
  std::unordered_set<hdmap::LaneInfoConstPtr> lane_set;
  for (const auto& lane_waypoint : ref_point.lane_waypoints()) {
    if (common::util::InsertIfNotPresent(&lane_set, lane_waypoint.lane)) {
      lanes->push_back(lane_waypoint.lane);
    }
  }
}

GetLaneSegments()

std::vector< hdmap::LaneSegment > apollo::planning::ReferenceLine::GetLaneSegments	(	const double	start_s,
		const double	end_s
	)		const

在文件 reference_line.cc 第 847 行定义.

                                                    {
  return map_path_.GetLaneSegments(start_s, end_s);
}

GetLaneWidth()

bool apollo::planning::ReferenceLine::GetLaneWidth	(	const double	s,
		double *const	lane_left_width,
		double *const	lane_right_width
	)		const

在文件 reference_line.cc 第 530 行定义.

                                                                       {
  if (map_path_.path_points().empty()) {
    return false;
  }
 
  if (!map_path_.GetLaneWidth(s, lane_left_width, lane_right_width)) {
    return false;
  }
  return true;
}

GetMapPath()

const hdmap::Path & apollo::planning::ReferenceLine::GetMapPath ( ) const

inline

在文件 reference_line.h 第 221 行定义.

{ return map_path_; }

GetNearestReferenceIndex()

size_t apollo::planning::ReferenceLine::GetNearestReferenceIndex

(

const double

s

)

const

在文件 reference_line.cc 第 269 行定义.

                                                                   {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front() - 1e-2) {
    AWARN << "The requested s: " << s << " < 0.";
    return 0;
  }
  if (s > accumulated_s.back() + 1e-2) {
    AWARN << "The requested s: " << s << " > reference line length "
          << accumulated_s.back();
    return reference_points_.size() - 1;
  }
  auto it_lower =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  return std::distance(accumulated_s.begin(), it_lower);
}

GetNearestReferencePoint() [1/2]

ReferencePoint apollo::planning::ReferenceLine::GetNearestReferencePoint

(

const common::math::Vec2d &

xy

)

const

在文件 reference_line.cc 第 130 行定义.

                                     {
  double min_dist = std::numeric_limits<double>::max();
  size_t min_index = 0;
  for (size_t i = 0; i < reference_points_.size(); ++i) {
    const double distance = DistanceXY(xy, reference_points_[i]);
    if (distance < min_dist) {
      min_dist = distance;
      min_index = i;
    }
  }
  return reference_points_[min_index];
}

GetNearestReferencePoint() [2/2]

ReferencePoint apollo::planning::ReferenceLine::GetNearestReferencePoint

(

const double

s

)

const

在文件 reference_line.cc 第 245 行定义.

                                                                           {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front() - 1e-2) {
    AWARN << "The requested s: " << s << " < 0.";
    return reference_points_.front();
  }
  if (s > accumulated_s.back() + 1e-2) {
    AWARN << "The requested s: " << s
          << " > reference line length: " << accumulated_s.back();
    return reference_points_.back();
  }
  auto it_lower =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  if (it_lower == accumulated_s.begin()) {
    return reference_points_.front();
  }
  auto index = std::distance(accumulated_s.begin(), it_lower);
  if (std::fabs(accumulated_s[index - 1] - s) <
      std::fabs(accumulated_s[index] - s)) {
    return reference_points_[index - 1];
  }
  return reference_points_[index];
}

GetOffsetToMap()

bool apollo::planning::ReferenceLine::GetOffsetToMap	(	const double	s,
		double *	l_offset
	)		const

在文件 reference_line.cc 第 542 行定义.

                                                                         {
  if (map_path_.path_points().empty()) {
    return false;
  }
 
  auto ref_point = GetNearestReferencePoint(s);
  if (ref_point.lane_waypoints().empty()) {
    return false;
  }
  *l_offset = ref_point.lane_waypoints().front().l;
  return true;
}

GetPriority()

uint32_t apollo::planning::ReferenceLine::GetPriority ( ) const

inline

在文件 reference_line.h 第 217 行定义.

{ return priority_; }

GetReferencePoint() [1/2]

ReferencePoint apollo::planning::ReferenceLine::GetReferencePoint

(

const double

s

)

const

在文件 reference_line.cc 第 303 行定义.

                                                                    {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front() - 1e-2) {
    AWARN << "The requested s: " << s << " < 0.";
    return reference_points_.front();
  }
  if (s > accumulated_s.back() + 1e-2) {
    AWARN << "The requested s: " << s
          << " > reference line length: " << accumulated_s.back();
    return reference_points_.back();
  }
 
  auto interpolate_index = map_path_.GetIndexFromS(s);
 
  size_t index = interpolate_index.id;
  size_t next_index = index + 1;
  if (next_index >= reference_points_.size()) {
    next_index = reference_points_.size() - 1;
  }
 
  const auto& p0 = reference_points_[index];
  const auto& p1 = reference_points_[next_index];
 
  const double s0 = accumulated_s[index];
  const double s1 = accumulated_s[next_index];
  return InterpolateWithMatchedIndex(p0, s0, p1, s1, interpolate_index);
}

GetReferencePoint() [2/2]

ReferencePoint apollo::planning::ReferenceLine::GetReferencePoint	(	const double	x,
		const double	y
	)		const

在文件 reference_line.cc 第 347 行定义.

                                                                      {
  CHECK_GE(reference_points_.size(), 0U);
 
  auto func_distance_square = [](const ReferencePoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };
 
  double d_min = func_distance_square(reference_points_.front(), x, y);
  size_t index_min = 0;
 
  for (size_t i = 1; i < reference_points_.size(); ++i) {
    double d_temp = func_distance_square(reference_points_[i], x, y);
    if (d_temp < d_min) {
      d_min = d_temp;
      index_min = i;
    }
  }
 
  size_t index_start = index_min == 0 ? index_min : index_min - 1;
  size_t index_end =
      index_min + 1 == reference_points_.size() ? index_min : index_min + 1;
 
  if (index_start == index_end) {
    return reference_points_[index_start];
  }
 
  double s0 = map_path_.accumulated_s()[index_start];
  double s1 = map_path_.accumulated_s()[index_end];
 
  double s = ReferenceLine::FindMinDistancePoint(
      reference_points_[index_start], s0, reference_points_[index_end], s1, x,
      y);
 
  return Interpolate(reference_points_[index_start], s0,
                     reference_points_[index_end], s1, s);
}

GetReferencePoints()

std::vector< ReferencePoint > apollo::planning::ReferenceLine::GetReferencePoints	(	double	start_s,
		double	end_s
	)		const

在文件 reference_line.cc 第 285 行定义.

                                        {
  if (start_s < 0.0) {
    start_s = 0.0;
  }
  if (end_s > Length()) {
    end_s = Length();
  }
  std::vector<ReferencePoint> ref_points;
  auto start_index = GetNearestReferenceIndex(start_s);
  auto end_index = GetNearestReferenceIndex(end_s);
  if (start_index < end_index) {
    ref_points.assign(reference_points_.begin() + start_index,
                      reference_points_.begin() + end_index);
  }
  return ref_points;
}

GetRoadType()

hdmap::Road::Type apollo::planning::ReferenceLine::GetRoadType

(

const double

s

)

const

在文件 reference_line.cc 第 563 行定义.

                                                             {
  const hdmap::HDMap* hdmap = hdmap::HDMapUtil::BaseMapPtr();
  CHECK_NOTNULL(hdmap);
 
  hdmap::Road::Type road_type = hdmap::Road::UNKNOWN;
 
  SLPoint sl_point;
  sl_point.set_s(s);
  sl_point.set_l(0.0);
  common::math::Vec2d pt;
  SLToXY(sl_point, &pt);
 
  common::PointENU point;
  point.set_x(pt.x());
  point.set_y(pt.y());
  point.set_z(0.0);
  std::vector<hdmap::RoadInfoConstPtr> roads;
  hdmap->GetRoads(point, 4.0, &roads);
  for (auto road : roads) {
    if (road->type() != hdmap::Road::UNKNOWN) {
      road_type = road->type();
      break;
    }
  }
  return road_type;
}

GetRoadWidth()

bool apollo::planning::ReferenceLine::GetRoadWidth	(	const double	s,
		double *const	road_left_width,
		double *const	road_right_width
	)		const

在文件 reference_line.cc 第 555 行定义.

                                                                       {
  if (map_path_.path_points().empty()) {
    return false;
  }
  return map_path_.GetRoadWidth(s, road_left_width, road_right_width);
}

GetSLBoundary() [1/4]

bool apollo::planning::ReferenceLine::GetSLBoundary	(	const common::math::Box2d &	box,
		SLBoundary *const	sl_boundary,
		double	warm_start_s = -1.0
	)		const

Get the SL Boundary of the box.

参数

box	The box to calculate.
sl_boundary	Output of the SLBoundary.
warm_start_s	The initial s for searching mapping point on reference line to accelerate computation time.

返回

True if success.

在文件 reference_line.cc 第 734 行定义.

                                                             {
  std::vector<common::math::Vec2d> corners;
  box.GetAllCorners(&corners);
  return GetSLBoundary(corners, sl_boundary, warm_start_s);
}

GetSLBoundary() [2/4]

bool apollo::planning::ReferenceLine::GetSLBoundary	(	const common::math::Polygon2d &	polygon,
		SLBoundary *const	sl_boundary,
		double	warm_start_s = -1.0
	)		const

在文件 reference_line.cc 第 742 行定义.

                                                             {
  std::vector<common::math::Vec2d> corners = polygon.points();
  return GetSLBoundary(corners, sl_boundary, warm_start_s);
}

GetSLBoundary() [3/4]

bool apollo::planning::ReferenceLine::GetSLBoundary	(	const hdmap::Polygon &	polygon,
		SLBoundary *const	sl_boundary
	)		const

在文件 reference_line.cc 第 852 行定义.

                                                                       {
  double start_s(std::numeric_limits<double>::max());
  double end_s(std::numeric_limits<double>::lowest());
  double start_l(std::numeric_limits<double>::max());
  double end_l(std::numeric_limits<double>::lowest());
  for (const auto& point : polygon.point()) {
    SLPoint sl_point;
    if (!XYToSL(point, &sl_point)) {
      AERROR << "Failed to get projection for point: " << point.DebugString()
             << " on reference line.";
      return false;
    }
    start_s = std::fmin(start_s, sl_point.s());
    end_s = std::fmax(end_s, sl_point.s());
    start_l = std::fmin(start_l, sl_point.l());
    end_l = std::fmax(end_l, sl_point.l());
  }
  sl_boundary->set_start_s(start_s);
  sl_boundary->set_end_s(end_s);
  sl_boundary->set_start_l(start_l);
  sl_boundary->set_end_l(end_l);
  return true;
}

GetSLBoundary() [4/4]

bool apollo::planning::ReferenceLine::GetSLBoundary	(	const std::vector< common::math::Vec2d > &	corners,
		SLBoundary *const	sl_boundary,
		double	warm_start_s
	)		const

在文件 reference_line.cc 第 749 行定义.

                                                              {
  double start_s(std::numeric_limits<double>::max());
  double end_s(std::numeric_limits<double>::lowest());
  double start_l(std::numeric_limits<double>::max());
  double end_l(std::numeric_limits<double>::lowest());
 
  // The order must be counter-clockwise
  std::vector<SLPoint> sl_corners;
  std::vector<common::math::Vec2d> obs_corners = corners;
  // get first point which is closest to ego position
  {
    int first_index = 0;
    double min_dist = std::numeric_limits<double>::max();
    for (int i = 0; i < obs_corners.size(); ++i) {
      double ego_dist = ego_position_.DistanceTo(obs_corners[i]);
      if (ego_dist < min_dist) {
        min_dist = ego_dist;
        first_index = i;
      }
    }
    std::rotate(obs_corners.begin(), obs_corners.begin() + first_index,
                obs_corners.end());
    const common::math::Vec2d& first_point = obs_corners.front();
    ADEBUG << "first_point: " << std::setprecision(9) << first_point.x() << ", "
           << first_point.y();
    SLPoint first_sl_point;
    if (!XYToSL(first_point, &first_sl_point, warm_start_s)) {
      AERROR << "Failed to get projection for point: "
             << first_point.DebugString() << " on reference line.";
      return false;
    }
    sl_corners.push_back(std::move(first_sl_point));
  }
 
  double hueristic_start_s = 0.0;
  double hueristic_end_s = 0.0;
  double distance = 0.0;
  SLPoint sl_point;
  for (size_t i = 1; i < obs_corners.size(); ++i) {
    distance = obs_corners[i].DistanceTo(obs_corners[i - 1]);
    hueristic_start_s = sl_corners.back().s() - 2.0 * distance;
    hueristic_end_s = sl_corners.back().s() + 2.0 * distance;
    if (!XYToSL(obs_corners[i], &sl_point, hueristic_start_s,
                hueristic_end_s)) {
      AERROR << "Failed to get projection for point: "
             << obs_corners[i].DebugString() << " on reference line.";
      return false;
    }
    sl_corners.push_back(std::move(sl_point));
  }
 
  for (size_t i = 0; i < obs_corners.size(); ++i) {
    auto index0 = i;
    auto index1 = (i + 1) % obs_corners.size();
    const auto& p0 = obs_corners[index0];
    const auto& p1 = obs_corners[index1];
 
    const auto p_mid = (p0 + p1) * 0.5;
    distance = obs_corners[index0].DistanceTo(p_mid);
    hueristic_start_s = sl_corners[index0].s() - 2.0 * distance;
    hueristic_end_s = sl_corners[index0].s() + 2.0 * distance;
    SLPoint sl_point_mid;
    if (!XYToSL(p_mid, &sl_point_mid, hueristic_start_s, hueristic_end_s)) {
      AERROR << "Failed to get projection for point: " << p_mid.DebugString()
             << " on reference line.";
      return false;
    }
 
    Vec2d v0(sl_corners[index1].s() - sl_corners[index0].s(),
             sl_corners[index1].l() - sl_corners[index0].l());
 
    Vec2d v1(sl_point_mid.s() - sl_corners[index0].s(),
             sl_point_mid.l() - sl_corners[index0].l());
 
    *sl_boundary->add_boundary_point() = sl_corners[index0];
 
    // sl_point is outside of polygon; add to the vertex list
    if (v0.CrossProd(v1) < 0.0) {
      *sl_boundary->add_boundary_point() = sl_point_mid;
    }
  }
 
  for (const auto& sl_point : sl_boundary->boundary_point()) {
    start_s = std::fmin(start_s, sl_point.s());
    end_s = std::fmax(end_s, sl_point.s());
    start_l = std::fmin(start_l, sl_point.l());
    end_l = std::fmax(end_l, sl_point.l());
  }
 
  sl_boundary->set_start_s(start_s);
  sl_boundary->set_end_s(end_s);
  sl_boundary->set_start_l(start_l);
  sl_boundary->set_end_l(end_l);
  return true;
}

GetSpeedLimitFromS()

double apollo::planning::ReferenceLine::GetSpeedLimitFromS

(

const double

s

)

const

在文件 reference_line.cc 第 919 行定义.

                                                             {
  for (const auto& speed_limit : speed_limit_) {
    if (s >= speed_limit.start_s && s <= speed_limit.end_s) {
      return speed_limit.speed_limit;
    }
  }
  const auto& map_path_point = GetReferencePoint(s);
 
  double speed_limit = FLAGS_planning_upper_speed_limit;
  bool speed_limit_found = false;
  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
    if (lane_waypoint.lane == nullptr) {
      AWARN << "lane_waypoint.lane is nullptr.";
      continue;
    }
    speed_limit_found = true;
    speed_limit =
        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
  }
 
  if (!speed_limit_found) {
    // use default speed limit based on road_type
    speed_limit = FLAGS_default_city_road_speed_limit;
    hdmap::Road::Type road_type = GetRoadType(s);
    if (road_type == hdmap::Road::HIGHWAY) {
      speed_limit = FLAGS_default_highway_speed_limit;
    }
  }
 
  return speed_limit;
}

HasOverlap()

bool apollo::planning::ReferenceLine::HasOverlap

(

const common::math::Box2d &

box

)

const

check if any part of the box has overlap with the road.

在文件 reference_line.cc 第 877 行定义.

                                                               {
  SLBoundary sl_boundary;
  if (!GetSLBoundary(box, &sl_boundary)) {
    AERROR << "Failed to get sl boundary for box: " << box.DebugString();
    return false;
  }
  if (sl_boundary.end_s() < 0 || sl_boundary.start_s() > Length()) {
    return false;
  }
  if (sl_boundary.start_l() * sl_boundary.end_l() < 0) {
    return false;
  }
 
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  const double mid_s = (sl_boundary.start_s() + sl_boundary.end_s()) / 2.0;
  if (mid_s < 0 || mid_s > Length()) {
    ADEBUG << "ref_s is out of range: " << mid_s;
    return false;
  }
  if (!map_path_.GetLaneWidth(mid_s, &lane_left_width, &lane_right_width)) {
    AERROR << "Failed to get width at s = " << mid_s;
    return false;
  }
  if (sl_boundary.start_l() > 0) {
    return sl_boundary.start_l() < lane_left_width;
  } else {
    return sl_boundary.end_l() > -lane_right_width;
  }
}

IsBlockRoad()

bool apollo::planning::ReferenceLine::IsBlockRoad	(	const common::math::Box2d &	box2d,
		double	gap
	)		const

Check if a box is blocking the road surface.

The criteria is to check whether the remaining space on the road surface is larger than the provided gap space.

参数

boxed	the provided box
gap	check the gap of the space

返回

true if the box blocks the road.

在文件 reference_line.cc 第 657 行定义.

                                                  {
  return map_path_.OverlapWith(box2d, gap);
}

IsOnLane() [1/4]

bool apollo::planning::ReferenceLine::IsOnLane

(

const common::math::Vec2d &

vec2d_point

)

const

在文件 reference_line.cc 第 623 行定义.

                                                                     {
  common::SLPoint sl_point;
  if (!XYToSL(vec2d_point, &sl_point)) {
    return false;
  }
  return IsOnLane(sl_point);
}

IsOnLane() [2/4]

bool apollo::planning::ReferenceLine::IsOnLane

(

const common::SLPoint &

sl_point

)

const

: check if a box/point is on lane along reference line

在文件 reference_line.cc 第 643 行定义.

                                                          {
  if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
    return false;
  }
  double left_width = 0.0;
  double right_width = 0.0;
 
  if (!GetLaneWidth(sl_point.s(), &left_width, &right_width)) {
    return false;
  }
 
  return sl_point.l() >= -right_width && sl_point.l() <= left_width;
}

IsOnLane() [3/4]

bool apollo::planning::ReferenceLine::IsOnLane

(

const SLBoundary &

sl_boundary

)

const

在文件 reference_line.cc 第 631 行定义.

                                                                {
  if (sl_boundary.end_s() < 0 || sl_boundary.start_s() > Length()) {
    return false;
  }
  double middle_s = (sl_boundary.start_s() + sl_boundary.end_s()) / 2.0;
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  map_path_.GetLaneWidth(middle_s, &lane_left_width, &lane_right_width);
  return sl_boundary.start_l() <= lane_left_width &&
         sl_boundary.end_l() >= -lane_right_width;
}

IsOnLane() [4/4]

template<class XYPoint >

bool apollo::planning::ReferenceLine::IsOnLane ( const XYPoint &  xy ) const

inline

在文件 reference_line.h 第 181 行定义.

                                         {
    return IsOnLane(common::math::Vec2d(xy.x(), xy.y()));
  }

IsOnRoad() [1/3]

bool apollo::planning::ReferenceLine::IsOnRoad

(

const common::math::Vec2d &

vec2d_point

)

const

在文件 reference_line.cc 第 662 行定义.

                                                                     {
  common::SLPoint sl_point;
  return XYToSL(vec2d_point, &sl_point) && IsOnRoad(sl_point);
}

IsOnRoad() [2/3]

bool apollo::planning::ReferenceLine::IsOnRoad

(

const common::SLPoint &

sl_point

)

const

: check if a box/point is on road (not on sideways/medians) along reference line

在文件 reference_line.cc 第 679 行定义.

                                                          {
  if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
    return false;
  }
  double road_left_width = 0.0;
  double road_right_width = 0.0;
 
  if (!GetRoadWidth(sl_point.s(), &road_left_width, &road_right_width)) {
    return false;
  }
 
  return sl_point.l() >= -road_right_width && sl_point.l() <= road_left_width;
}

IsOnRoad() [3/3]

bool apollo::planning::ReferenceLine::IsOnRoad

(

const SLBoundary &

sl_boundary

)

const

在文件 reference_line.cc 第 667 行定义.

                                                                {
  if (sl_boundary.end_s() < 0 || sl_boundary.start_s() > Length()) {
    return false;
  }
  double middle_s = (sl_boundary.start_s() + sl_boundary.end_s()) / 2.0;
  double road_left_width = 0.0;
  double road_right_width = 0.0;
  map_path_.GetRoadWidth(middle_s, &road_left_width, &road_right_width);
  return sl_boundary.start_l() <= road_left_width &&
         sl_boundary.end_l() >= -road_right_width;
}

Length()

double apollo::planning::ReferenceLine::Length ( ) const

inline

在文件 reference_line.h 第 209 行定义.

{ return map_path_.length(); }

map_path()

const MapPath & apollo::planning::ReferenceLine::map_path

(

)

const

在文件 reference_line.cc 第 528 行定义.

{ return map_path_; }

reference_points()

const std::vector< ReferencePoint > & apollo::planning::ReferenceLine::reference_points

(

)

const

在文件 reference_line.cc 第 524 行定义.

                                                                       {
  return reference_points_;
}

Segment() [1/2]

bool apollo::planning::ReferenceLine::Segment	(	const common::math::Vec2d &	point,
		const double	distance_backward,
		const double	distance_forward
	)

在文件 reference_line.cc 第 144 行定义.

                                                       {
  common::SLPoint sl;
  if (!XYToSL(point, &sl)) {
    AERROR << "Failed to project point: " << point.DebugString();
    return false;
  }
  return Segment(sl.s(), look_backward, look_forward);
}

Segment() [2/2]

bool apollo::planning::ReferenceLine::Segment	(	const double	s,
		const double	distance_backward,
		const double	distance_forward
	)

在文件 reference_line.cc 第 155 行定义.

                                                       {
  const auto& accumulated_s = map_path_.accumulated_s();
 
  // inclusive
  auto start_index =
      std::distance(accumulated_s.begin(),
                    std::lower_bound(accumulated_s.begin(), accumulated_s.end(),
                                     s - look_backward));
 
  // exclusive
  auto end_index =
      std::distance(accumulated_s.begin(),
                    std::upper_bound(accumulated_s.begin(), accumulated_s.end(),
                                     s + look_forward));
 
  if (end_index - start_index < 2) {
    AERROR << "Too few reference points after shrinking.";
    return false;
  }
 
  reference_points_ =
      std::vector<ReferencePoint>(reference_points_.begin() + start_index,
                                  reference_points_.begin() + end_index);
 
  map_path_ = MapPath(std::vector<hdmap::MapPathPoint>(
      reference_points_.begin(), reference_points_.end()));
  return true;
}

SetEgoPosition()

void apollo::planning::ReferenceLine::SetEgoPosition ( common::math::Vec2d  ego_pos )

inline

在文件 reference_line.h 第 223 行定义.

{ ego_position_ = ego_pos; }

SetPriority()

void apollo::planning::ReferenceLine::SetPriority ( uint32_t  priority )

inline

在文件 reference_line.h 第 219 行定义.

{ priority_ = priority; }

SLToXY()

bool apollo::planning::ReferenceLine::SLToXY	(	const common::SLPoint &	sl_point,
		common::math::Vec2d *const	xy_point
	)		const

在文件 reference_line.cc 第 388 行定义.

                                                                  {
  if (map_path_.num_points() < 2) {
    AERROR << "The reference line has too few points.";
    return false;
  }
 
  const auto matched_point = GetReferencePoint(sl_point.s());
  const auto angle = common::math::Angle16::from_rad(matched_point.heading());
  xy_point->set_x(matched_point.x() - common::math::sin(angle) * sl_point.l());
  xy_point->set_y(matched_point.y() + common::math::cos(angle) * sl_point.l());
  return true;
}

Stitch()

bool apollo::planning::ReferenceLine::Stitch

(

const ReferenceLine &

other

)

Stitch current reference line with the other reference line The stitching strategy is to use current reference points as much as possible.

The following two examples show two successful stitch cases.

Example 1 this: |-----—A--—x--—B---—| other: |--—C---—x-----—D----—| Result: |---—A--—x--—B---—x-----—D----—| In the above example, A-B is current reference line, and C-D is the other reference line. If part B and part C matches, we update current reference line to A-B-D.

Example 2 this: |--—A---—x-----—B----—| other: |-----—C--—x--—D---—| Result: |-----—C--—x--—A---—x-----—B----—| In the above example, A-B is current reference line, and C-D is the other reference line. If part A and part D matches, we update current reference line to C-A-B.

返回

false if these two reference line cannot be stitched

在文件 reference_line.cc 第 72 行定义.

                                                     {
  if (other.reference_points().empty()) {
    AWARN << "The other reference line is empty.";
    return true;
  }
  auto first_point = reference_points_.front();
  common::SLPoint first_sl;
  if (!other.XYToSL(first_point, &first_sl)) {
    AWARN << "Failed to project the first point to the other reference line.";
    return false;
  }
  bool first_join = first_sl.s() > 0 && first_sl.s() < other.Length();
 
  auto last_point = reference_points_.back();
  common::SLPoint last_sl;
  if (!other.XYToSL(last_point, &last_sl)) {
    AWARN << "Failed to project the last point to the other reference line.";
    return false;
  }
  bool last_join = last_sl.s() > 0 && last_sl.s() < other.Length();
 
  if (!first_join && !last_join) {
    AERROR << "These reference lines are not connected.";
    return false;
  }
 
  const auto& accumulated_s = other.map_path().accumulated_s();
  const auto& other_points = other.reference_points();
  auto lower = accumulated_s.begin();
  static constexpr double kStitchingError = 1e-1;
  if (first_join) {
    if (first_sl.l() > kStitchingError) {
      AERROR << "lateral stitching error on first join of reference line too "
                "big, stitching fails";
      return false;
    }
    lower = std::lower_bound(accumulated_s.begin(), accumulated_s.end(),
                             first_sl.s());
    size_t start_i = std::distance(accumulated_s.begin(), lower);
    reference_points_.insert(reference_points_.begin(), other_points.begin(),
                             other_points.begin() + start_i);
  }
  if (last_join) {
    if (last_sl.l() > kStitchingError) {
      AERROR << "lateral stitching error on first join of reference line too "
                "big, stitching fails";
      return false;
    }
    auto upper = std::upper_bound(lower, accumulated_s.end(), last_sl.s());
    auto end_i = std::distance(accumulated_s.begin(), upper);
    reference_points_.insert(reference_points_.end(),
                             other_points.begin() + end_i, other_points.end());
  }
  map_path_ = MapPath(std::move(std::vector<hdmap::MapPathPoint>(
      reference_points_.begin(), reference_points_.end())));
  return true;
}

ToFrenetFrame()

std::pair< std::array< double, 3 >, std::array< double, 3 > > apollo::planning::ReferenceLine::ToFrenetFrame

(

const common::TrajectoryPoint &

traj_point

)

const

在文件 reference_line.cc 第 218 行定义.

                                                                          {
  ACHECK(!reference_points_.empty());
 
  common::SLPoint sl_point;
  XYToSL(traj_point.path_point().theta(),
         {traj_point.path_point().x(), traj_point.path_point().y()}, &sl_point);
 
  std::array<double, 3> s_condition;
  std::array<double, 3> l_condition;
  ReferencePoint ref_point = GetReferencePoint(sl_point.s());
  CartesianFrenetConverter::cartesian_to_frenet(
      sl_point.s(), ref_point.x(), ref_point.y(), ref_point.heading(),
      ref_point.kappa(), ref_point.dkappa(), traj_point.path_point().x(),
      traj_point.path_point().y(), traj_point.v(), traj_point.a(),
      traj_point.path_point().theta(), traj_point.path_point().kappa(),
      &s_condition, &l_condition);
  AINFO << "planning_start_point x,y,the,k: " << std::fixed
        << traj_point.path_point().x() << ", y: " << traj_point.path_point().y()
        << "," << traj_point.path_point().theta() << ","
        << traj_point.path_point().kappa() << "," << traj_point.v() << ","
        << traj_point.a();
  AINFO << "ref point x y the ka dka" << std::fixed << ref_point.x() << ","
        << ref_point.y() << "," << ref_point.heading() << ","
        << ref_point.kappa() << "," << ref_point.dkappa();
  return std::make_pair(s_condition, l_condition);
}

XYToSL() [1/4]

bool apollo::planning::ReferenceLine::XYToSL	(	const common::math::Vec2d &	xy_point,
		common::SLPoint *const	sl_point,
		double	hueristic_start_s,
		double	hueristic_end_s
	)		const

在文件 reference_line.cc 第 449 行定义.

                                                         {
  double s = 0.0;
  double l = 0.0;
  double min_distance = 0.0;
  if (!map_path_.GetProjectionWithHueristicParams(xy_point, hueristic_start_s,
                                                  hueristic_end_s, &s, &l,
                                                  &min_distance)) {
    AERROR << "Cannot get nearest point from path with hueristic_start_s: "
           << hueristic_start_s << " hueristic_end_s: " << hueristic_end_s;
    return false;
  }
  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

XYToSL() [2/4]

bool apollo::planning::ReferenceLine::XYToSL	(	const common::math::Vec2d &	xy_point,
		common::SLPoint *const	sl_point,
		double	warm_start_s = -1.0
	)		const

Transvert Cartesian coordinates to Frenet.

参数

xy_point	The Cartesian coordinates.
sl_point	The output Frenet coordinates.
warm_start_s	The initial s for searching mapping point on reference line to accelerate computation time. If not given, search begin at the start of the reference line.

返回

True if success.

在文件 reference_line.cc 第 402 行定义.

                                                      {
  double s = warm_start_s;
  double l = 0.0;
  if (warm_start_s < 0.0) {
    if (!map_path_.GetProjection(xy_point, &s, &l)) {
      AERROR << "Cannot get nearest point from path.";
      return false;
    }
  } else {
    if (!map_path_.GetProjectionWithWarmStartS(xy_point, &s, &l)) {
      AERROR << "Cannot get nearest point from path with warm_start_s: "
             << warm_start_s;
      return false;
    }
  }
 
  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

XYToSL() [3/4]

bool apollo::planning::ReferenceLine::XYToSL	(	const double	heading,
		const common::math::Vec2d &	xy_point,
		common::SLPoint *const	sl_point,
		double	warm_start_s = -1.0
	)		const

在文件 reference_line.cc 第 425 行定义.

                                                      {
  double s = warm_start_s;
  double l = 0.0;
  if (warm_start_s < 0.0) {
    if (!map_path_.GetProjection(heading, xy_point, &s, &l)) {
      AERROR << "Cannot get nearest point from path.";
      return false;
    }
  } else {
    if (!map_path_.GetProjectionWithWarmStartS(xy_point, &s, &l)) {
      AERROR << "Cannot get nearest point from path with warm_start_s: "
             << warm_start_s;
      return false;
    }
  }
 
  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

XYToSL() [4/4]

template<class XYPoint >

bool apollo::planning::ReferenceLine::XYToSL ( const XYPoint &  xy, common::SLPoint *const  sl_point  ) const

inline

在文件 reference_line.h 第 153 行定义.

                                                                      {
    return XYToSL(common::math::Vec2d(xy.x(), xy.y()), sl_point);
  }

---

该类的文档由以下文件生成:

• modules/planning/planning_base/reference_line/reference_line.h
• modules/planning/planning_base/reference_line/reference_line.cc