apollo::planning::LaneFollowMap类 参考

#include <lane_follow_map.h>

类 apollo::planning::LaneFollowMap 继承关系图:

apollo::planning::LaneFollowMap 的协作图:

Public 成员函数
	LaneFollowMap ()
virtual	~LaneFollowMap ()=default
bool	CanProcess (const planning::PlanningCommand &command) const override
bool	UpdatePlanningCommand (const planning::PlanningCommand &command) override
bool	GetRouteSegments (const common::VehicleState &vehicle_state, std::list< apollo::hdmap::RouteSegments > *const route_segments) override
	use heuristic forward length and backward length
bool	ExtendSegments (const apollo::hdmap::RouteSegments &segments, double start_s, double end_s, apollo::hdmap::RouteSegments *const truncated_segments) const override
std::vector< routing::LaneWaypoint >	FutureRouteWaypoints () const override
void	GetEndLaneWayPoint (std::shared_ptr< routing::LaneWaypoint > &end_point) const override
	Get the end point of PlanningCommand.
hdmap::LaneInfoConstPtr	GetLaneById (const hdmap::Id &id) const override
	Get the Lane with the given id.
bool	GetNearestPointFromRouting (const common::VehicleState &state, apollo::hdmap::LaneWaypoint *waypoint) const override
double	GetDistanceToDestination () const override
apollo::hdmap::LaneWaypoint	GetAdcWaypoint () const override
Public 成员函数 继承自 apollo::planning::PncMapBase
virtual	~PncMapBase ()=default
bool	IsNewPlanningCommand (const planning::PlanningCommand &command) const
	Check if the routing is the same as existing one in Map

额外继承的成员函数
静态 Public 成员函数 继承自 apollo::planning::PncMapBase
static double	LookForwardDistance (const double velocity)
static bool	IsNewPlanningCommand (const planning::PlanningCommand &prev_command, const planning::PlanningCommand &new_command)
Protected 属性 继承自 apollo::planning::PncMapBase
planning::PlanningCommand	last_command_
apollo::hdmap::LaneWaypoint	adc_waypoint_
	The waypoint of the autonomous driving car

详细描述

在文件 lane_follow_map.h 第 41 行定义.

构造及析构函数说明

LaneFollowMap()

apollo::planning::LaneFollowMap::LaneFollowMap

(

)

在文件 lane_follow_map.cc 第 53 行定义.

: hdmap_(hdmap::HDMapUtil::BaseMapPtr()) {}

~LaneFollowMap()

virtual apollo::planning::LaneFollowMap::~LaneFollowMap ( )

virtualdefault

成员函数说明

CanProcess()

bool apollo::planning::LaneFollowMap::CanProcess ( const planning::PlanningCommand &  command ) const

overridevirtual

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 55 行定义.

                                                                           {
  return command.has_lane_follow_command();
}

ExtendSegments()

bool apollo::planning::LaneFollowMap::ExtendSegments ( const apollo::hdmap::RouteSegments &  segments, double  start_s, double  end_s, apollo::hdmap::RouteSegments *const  truncated_segments  ) const

overridevirtual

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 636 行定义.

                                                        {
  if (segments.empty()) {
    AERROR << "The input segments is empty";
    return false;
  }
  CHECK_NOTNULL(truncated_segments);
  truncated_segments->SetProperties(segments);
 
  if (start_s >= end_s) {
    AERROR << "start_s(" << start_s << " >= end_s(" << end_s << ")";
    return false;
  }
  std::unordered_set<std::string> unique_lanes;
  static constexpr double kRouteEpsilon = 1e-3;
  // Extend the trajectory towards the start of the trajectory.
  if (start_s < 0) {
    const auto &first_segment = *segments.begin();
    auto lane = first_segment.lane;
    double s = first_segment.start_s;
    double extend_s = -start_s;
    std::vector<hdmap::LaneSegment> extended_lane_segments;
    while (extend_s > kRouteEpsilon) {
      if (s <= kRouteEpsilon) {
        lane = GetRoutePredecessor(lane);
        if (lane == nullptr ||
            unique_lanes.find(lane->id().id()) != unique_lanes.end()) {
          break;
        }
        s = lane->total_length();
      } else {
        const double length = std::min(s, extend_s);
        extended_lane_segments.emplace_back(lane, s - length, s);
        extend_s -= length;
        s -= length;
        unique_lanes.insert(lane->id().id());
      }
    }
    truncated_segments->insert(truncated_segments->begin(),
                               extended_lane_segments.rbegin(),
                               extended_lane_segments.rend());
  }
  bool found_loop = false;
  double router_s = 0;
  for (const auto &lane_segment : segments) {
    const double adjusted_start_s = std::max(
        start_s - router_s + lane_segment.start_s, lane_segment.start_s);
    const double adjusted_end_s =
        std::min(end_s - router_s + lane_segment.start_s, lane_segment.end_s);
    if (adjusted_start_s < adjusted_end_s) {
      if (!truncated_segments->empty() &&
          truncated_segments->back().lane->id().id() ==
              lane_segment.lane->id().id()) {
        truncated_segments->back().end_s = adjusted_end_s;
      } else if (unique_lanes.find(lane_segment.lane->id().id()) ==
                 unique_lanes.end()) {
        truncated_segments->emplace_back(lane_segment.lane, adjusted_start_s,
                                         adjusted_end_s);
        unique_lanes.insert(lane_segment.lane->id().id());
      } else {
        found_loop = true;
        break;
      }
    }
    router_s += (lane_segment.end_s - lane_segment.start_s);
    if (router_s > end_s) {
      break;
    }
  }
  if (found_loop) {
    return true;
  }
  // Extend the trajectory towards the end of the trajectory.
  if (router_s < end_s && !truncated_segments->empty()) {
    auto &back = truncated_segments->back();
    if (back.lane->total_length() > back.end_s) {
      double origin_end_s = back.end_s;
      back.end_s =
          std::min(back.end_s + end_s - router_s, back.lane->total_length());
      router_s += back.end_s - origin_end_s;
    }
  }
  auto last_lane = segments.back().lane;
  while (router_s < end_s - kRouteEpsilon) {
    last_lane = GetRouteSuccessor(last_lane);
    if (last_lane == nullptr ||
        unique_lanes.find(last_lane->id().id()) != unique_lanes.end()) {
      break;
    }
    const double length = std::min(end_s - router_s, last_lane->total_length());
    truncated_segments->emplace_back(last_lane, 0, length);
    unique_lanes.insert(last_lane->id().id());
    router_s += length;
  }
  return true;
}

FutureRouteWaypoints()

std::vector< routing::LaneWaypoint > apollo::planning::LaneFollowMap::FutureRouteWaypoints ( ) const

overridevirtual

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 115 行定义.

                                                                         {
  const auto &waypoints =
      last_command_.lane_follow_command().routing_request().waypoint();
  return std::vector<routing::LaneWaypoint>(
      waypoints.begin() + next_routing_waypoint_index_, waypoints.end());
}

GetAdcWaypoint()

apollo::hdmap::LaneWaypoint apollo::planning::LaneFollowMap::GetAdcWaypoint ( ) const

overridevirtual

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 781 行定义.

                                                            {
  return adc_waypoint_;
}

GetDistanceToDestination()

double apollo::planning::LaneFollowMap::GetDistanceToDestination ( ) const

overridevirtual

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 785 行定义.

                                                     {
  if (adc_route_index_ < 0 || adc_route_index_ >= route_indices_.size()) {
    AERROR << "adc_route_index error, can not get distance to destination, "
              "return 0.";
    return 0.0;
  }
  const auto &routing = last_command_.lane_follow_command();
  int adc_road_index = route_indices_[adc_route_index_].index[0];
  int adc_passage_index = route_indices_[adc_route_index_].index[1];
  int adc_lane_index = route_indices_[adc_route_index_].index[2];
 
  bool get_adc_exit_waypoint =
      routing.road(adc_road_index).passage(adc_passage_index).can_exit();
  int start_passage_index = adc_passage_index;
  int start_lane_index = adc_lane_index;
  double start_lane_s = adc_waypoint_.s;
 
  double dis_to_destination = 0.0;
 
  for (int road_index = adc_road_index; road_index < routing.road_size();
       ++road_index) {
    const auto &road_segment = routing.road(road_index);
    for (int passage_index = 0; passage_index < road_segment.passage_size();
         ++passage_index) {
      const auto &passage = road_segment.passage(passage_index);
      if (!passage.can_exit()) {
        continue;
      }
      // check this passsage is can_exit
      if (!get_adc_exit_waypoint) {
        get_adc_exit_waypoint = true;
        // find adc waypoint in passage which can exit
        for (int index = 0; index < passage.segment_size(); ++index) {
          auto lane = hdmap_->GetLaneById(
              hdmap::MakeMapId(passage.segment(index).id()));
          double s = 0.0;
          double l = 0.0;
          if (!lane->GetProjection({adc_state_.x(), adc_state_.y()}, &s, &l)) {
            continue;
          }
          static constexpr double kEpsilon = 0.5;
          if (s > (lane->total_length() + kEpsilon) || (s + kEpsilon) < 0.0) {
            continue;
          }
          start_passage_index = passage_index;
          start_lane_index = index;
          start_lane_s = s;
        }
      }
 
      // add distance by lane in can_exit passage
      if (get_adc_exit_waypoint) {
        if (road_index == adc_road_index &&
            passage_index == start_passage_index) {
          for (int lane_index = start_lane_index;
               lane_index < passage.segment_size(); ++lane_index) {
            if (lane_index == start_lane_index) {
              dis_to_destination +=
                  passage.segment(lane_index).end_s() - start_lane_s;
            } else {
              dis_to_destination += passage.segment(lane_index).end_s() -
                                    passage.segment(lane_index).start_s();
            }
          }
        } else {
          for (int lane_index = 0; lane_index < passage.segment_size();
               ++lane_index) {
            dis_to_destination += passage.segment(lane_index).end_s() -
                                  passage.segment(lane_index).start_s();
          }
        }
        break;
      }
    }
  }
  return dis_to_destination;
}

GetEndLaneWayPoint()

void apollo::planning::LaneFollowMap::GetEndLaneWayPoint ( std::shared_ptr< routing::LaneWaypoint > &  end_point ) const

overridevirtual

Get the end point of PlanningCommand.

参数

end_point

The end point of PlanningCommand.

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 122 行定义.

                                                         {
  if (!last_command_.has_lane_follow_command() ||
      !last_command_.lane_follow_command().has_routing_request()) {
    end_point = nullptr;
    return;
  }
  const auto &routing_request =
      last_command_.lane_follow_command().routing_request();
  if (routing_request.waypoint().size() < 1) {
    end_point = nullptr;
    return;
  }
  end_point = std::make_shared<routing::LaneWaypoint>();
  end_point->CopyFrom(*(routing_request.waypoint().rbegin()));
}

GetLaneById()

hdmap::LaneInfoConstPtr apollo::planning::LaneFollowMap::GetLaneById ( const hdmap::Id &  id ) const

overridevirtual

Get the Lane with the given id.

参数

id	The id of the lane.

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 139 行定义.

                                                                        {
  if (nullptr == hdmap_) {
    return nullptr;
  }
  return hdmap_->GetLaneById(id);
}

GetNearestPointFromRouting()

bool apollo::planning::LaneFollowMap::GetNearestPointFromRouting ( const common::VehicleState &  state, apollo::hdmap::LaneWaypoint *  waypoint  ) const

overridevirtual

实现了 apollo::planning::PncMapBase.

在文件 lane_follow_map.cc 第 501 行定义.

                                                                        {
  waypoint->lane = nullptr;
  std::vector<hdmap::LaneInfoConstPtr> lanes;
  const auto point = PointFactory::ToPointENU(state);
  std::vector<hdmap::LaneInfoConstPtr> valid_lanes;
  for (auto lane_id : all_lane_ids_) {
    hdmap::Id id = hdmap::MakeMapId(lane_id);
    auto lane = hdmap_->GetLaneById(id);
    if (nullptr != lane) {
      valid_lanes.emplace_back(lane);
    }
  }
 
  // Get nearest_waypoints for current position
  std::vector<hdmap::LaneWaypoint> valid_way_points;
  for (const auto &lane : valid_lanes) {
    if (range_lane_ids_.count(lane->id().id()) == 0) {
      ADEBUG << "not in range" << lane->id().id();
      continue;
    }
    if (route_segments_lane_ids_.size() > 0 &&
        route_segments_lane_ids_.count(lane->id().id()) == 0) {
      ADEBUG << "not in last frame route_segments: " << lane->id().id();
      continue;
    }
    double s = 0.0;
    double l = 0.0;
    {
      if (!lane->GetProjection({point.x(), point.y()}, &s, &l)) {
        continue;
      }
      ADEBUG << lane->id().id() << "," << s << "," << l;
      // Use large epsilon to allow projection diff
      static constexpr double kEpsilon = 0.5;
      if (s > (lane->total_length() + kEpsilon) || (s + kEpsilon) < 0.0) {
        continue;
      }
      double lane_heading = lane->Heading(s);
      if (std::fabs(common::math::AngleDiff(lane_heading, state.heading())) >
          M_PI_2 * 1.5) {
        continue;
      }
    }
 
    valid_way_points.emplace_back();
    auto &last = valid_way_points.back();
    last.lane = lane;
    last.s = s;
    last.l = l;
    ADEBUG << "distance:" << std::fabs(l);
  }
  if (valid_way_points.empty()) {
    AERROR << "Failed to find nearest point: " << point.ShortDebugString();
    return false;
  }
 
  // find closest lane that satisfy vehicle heading
  int closest_index = -1;
  double distance = std::numeric_limits<double>::max();
  double lane_heading = 0.0;
  double vehicle_heading = state.heading();
  for (size_t i = 0; i < valid_way_points.size(); i++) {
    lane_heading = valid_way_points[i].lane->Heading(valid_way_points[i].s);
    if (std::abs(common::math::AngleDiff(lane_heading, vehicle_heading)) >
        M_PI_2 * 1.5) {
      continue;
    }
    if (std::fabs(valid_way_points[i].l) < distance) {
      distance = std::fabs(valid_way_points[i].l);
      closest_index = i;
    }
  }
  if (closest_index == -1) {
    AERROR << "Can not find nearest waypoint. vehicle heading:"
           << vehicle_heading << "lane heading:" << lane_heading;
    return false;
  }
  waypoint->lane = valid_way_points[closest_index].lane;
  waypoint->s = valid_way_points[closest_index].s;
  waypoint->l = valid_way_points[closest_index].l;
  return true;
}

GetRouteSegments()

bool apollo::planning::LaneFollowMap::GetRouteSegments ( const common::VehicleState &  vehicle_state, std::list< apollo::hdmap::RouteSegments > *const  route_segments  )

overridevirtual

use heuristic forward length and backward length

实现了 apollo::planning::PncMapBase.

UpdatePlanningCommand()

bool apollo::planning::LaneFollowMap::UpdatePlanningCommand ( const planning::PlanningCommand &  command )

overridevirtual

重载 apollo::planning::PncMapBase .

在文件 lane_follow_map.cc 第 232 行定义.

                                            {
  if (!CanProcess(command)) {
    AERROR << "Command cannot be processed by LaneFollowMap!";
    return false;
  }
  if (!PncMapBase::UpdatePlanningCommand(command)) {
    return false;
  }
  const auto &routing = command.lane_follow_command();
  range_lane_ids_.clear();
  route_indices_.clear();
  all_lane_ids_.clear();
  for (int road_index = 0; road_index < routing.road_size(); ++road_index) {
    const auto &road_segment = routing.road(road_index);
    for (int passage_index = 0; passage_index < road_segment.passage_size();
         ++passage_index) {
      const auto &passage = road_segment.passage(passage_index);
      for (int lane_index = 0; lane_index < passage.segment_size();
           ++lane_index) {
        all_lane_ids_.insert(passage.segment(lane_index).id());
        route_indices_.emplace_back();
        route_indices_.back().segment =
            ToLaneSegment(passage.segment(lane_index));
        if (route_indices_.back().segment.lane == nullptr) {
          AERROR << "Failed to get lane segment from passage.";
          return false;
        }
        route_indices_.back().index = {road_index, passage_index, lane_index};
      }
    }
  }
 
  range_start_ = 0;
  range_end_ = 0;
  adc_route_index_ = -1;
  next_routing_waypoint_index_ = 0;
  UpdateRoutingRange(adc_route_index_);
 
  routing_waypoint_index_.clear();
  const auto &request_waypoints = routing.routing_request().waypoint();
  if (request_waypoints.empty()) {
    AERROR << "Invalid routing: no request waypoints.";
    return false;
  }
  int i = 0;
  for (size_t j = 0; j < route_indices_.size(); ++j) {
    while (i < request_waypoints.size() &&
           hdmap::RouteSegments::WithinLaneSegment(route_indices_[j].segment,
                                                   request_waypoints.Get(i))) {
      routing_waypoint_index_.emplace_back(
          hdmap::LaneWaypoint(route_indices_[j].segment.lane,
                              request_waypoints.Get(i).s()),
          j);
      ++i;
    }
  }
  adc_waypoint_ = hdmap::LaneWaypoint();
  stop_for_destination_ = false;
  return true;
}

---

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

• modules/planning/pnc_map/lane_follow_map/lane_follow_map.h
• modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc