apollo::relative_map::NavigationLane类 参考

NavigationLane generates a real-time relative map based on navagation lines. 更多...

#include <navigation_lane.h>

apollo::relative_map::NavigationLane 的协作图:

Public 成员函数
	NavigationLane ()=default
	NavigationLane (const NavigationLaneConfig &config)
	~NavigationLane ()=default
void	SetConfig (const NavigationLaneConfig &config)
	Set the configuration information required by the NavigationLane.
void	SetVehicleStateProvider (common::VehicleStateProvider *vehicle_state_provider)
void	UpdateNavigationInfo (const NavigationInfo &navigation_info)
	Update navigation line information.
void	SetDefaultWidth (const double left_width, const double right_width)
	Set the default width of a lane.
bool	GeneratePath ()
	Generate a suitable path (i.e.
void	UpdatePerception (const perception::PerceptionObstacles &perception_obstacles)
	Update perceived lane line information.
NavigationPath	Path () const
	Get the generated lane segment where the vehicle is currently located.
bool	CreateMap (const MapGenerationParam &map_config, MapMsg *const map_msg) const
	Generate a real-time relative map of approximately 250 m in length based on several navigation line segments and map generation configuration information.

详细描述

NavigationLane generates a real-time relative map based on navagation lines.

First, several navigation lines are received from the NavigationInfo object;

Second, several navigation line segments with the length of about 250 m are cut from the whole navigation lines and the UTM coordinates are converted into local coordinates with the current position of the vehicle as the origin;

Third, the navigation line segment of the vehicle's current lane is merged with the perceived lane centerline.

Fourth, a real-time relative map is dynamically created based on navigation line segments and perceived lane width;

Fifth, the relative map is output as a MapMsg object pointer.

在文件 navigation_lane.h 第 97 行定义.

构造及析构函数说明

NavigationLane() [1/2]

apollo::relative_map::NavigationLane::NavigationLane ( )

default

NavigationLane() [2/2]

apollo::relative_map::NavigationLane::NavigationLane ( const NavigationLaneConfig &  config )

explicit

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

    : config_(config) {}

~NavigationLane()

apollo::relative_map::NavigationLane::~NavigationLane ( )

default

成员函数说明

CreateMap()

bool apollo::relative_map::NavigationLane::CreateMap	(	const MapGenerationParam &	map_config,
		MapMsg *const	map_msg
	)		const

Generate a real-time relative map of approximately 250 m in length based on several navigation line segments and map generation configuration information.

参数

map_config	Map generation configuration information.
map_msg	A pointer which outputs the real-time relative map.

返回

True if the real-time relative map is created; false otherwise.

在文件 navigation_lane.cc 第 732 行定义.

                                                            {
  auto *navigation_path = map_msg->mutable_navigation_path();
  auto *hdmap = map_msg->mutable_hdmap();
  auto *lane_marker = map_msg->mutable_lane_marker();
 
  lane_marker->CopyFrom(perception_obstacles_.lane_marker());
 
  // If no navigation path is generated based on navigation lines, we try to
  // create map with "current_navi_path_tuple_" which is generated based on
  // perceived lane markers.
  if (navigation_path_list_.empty()) {
    if (std::get<3>(current_navi_path_tuple_) != nullptr) {
      FLAGS_relative_map_generate_left_boundray = true;
      return CreateSingleLaneMap(current_navi_path_tuple_, map_config,
                                 perception_obstacles_, hdmap, navigation_path);
    } else {
      return false;
    }
  }
 
  int fail_num = 0;
  FLAGS_relative_map_generate_left_boundray = true;
  for (auto iter = navigation_path_list_.cbegin();
       iter != navigation_path_list_.cend(); ++iter) {
    std::size_t index = std::distance(navigation_path_list_.cbegin(), iter);
    if (!CreateSingleLaneMap(*iter, map_config, perception_obstacles_, hdmap,
                             navigation_path)) {
      AWARN << "Failed to generate lane: " << index;
      ++fail_num;
      FLAGS_relative_map_generate_left_boundray = true;
      continue;
    }
    FLAGS_relative_map_generate_left_boundray = false;
 
    // The left border of the middle lane uses the right border of the left
    // lane.
    int lane_index = static_cast<int>(index) - fail_num;
    if (lane_index > 0) {
      auto *left_boundary =
          hdmap->mutable_lane(lane_index)->mutable_left_boundary();
      left_boundary->CopyFrom(hdmap->lane(lane_index - 1).right_boundary());
      auto *left_sample =
          hdmap->mutable_lane(lane_index)->mutable_left_sample();
      left_sample->CopyFrom(hdmap->lane(lane_index - 1).right_sample());
    }
  }
 
  int lane_num = hdmap->lane_size();
  ADEBUG << "The Lane number is: " << lane_num;
 
  // Set road boundary
  auto *road = hdmap->add_road();
  road->mutable_id()->set_id("road_" + hdmap->lane(0).id().id());
  auto *section = road->add_section();
  for (int i = 0; i < lane_num; ++i) {
    auto *lane_id = section->add_lane_id();
    lane_id->CopyFrom(hdmap->lane(0).id());
  }
  auto *outer_polygon = section->mutable_boundary()->mutable_outer_polygon();
  auto *left_edge = outer_polygon->add_edge();
  left_edge->set_type(apollo::hdmap::BoundaryEdge::LEFT_BOUNDARY);
  left_edge->mutable_curve()->CopyFrom(hdmap->lane(0).left_boundary().curve());
 
  auto *right_edge = outer_polygon->add_edge();
  right_edge->set_type(apollo::hdmap::BoundaryEdge::RIGHT_BOUNDARY);
  right_edge->mutable_curve()->CopyFrom(
      hdmap->lane(lane_num - 1).right_boundary().curve());
 
  // Set neighbor information for each lane
  if (lane_num < 2) {
    return true;
  }
  for (int i = 0; i < lane_num; ++i) {
    auto *lane = hdmap->mutable_lane(i);
    if (i > 0) {
      lane->add_left_neighbor_forward_lane_id()->CopyFrom(
          hdmap->lane(i - 1).id());
      ADEBUG << "Left neighbor is: " << hdmap->lane(i - 1).id().id();
    }
    if (i < lane_num - 1) {
      lane->add_right_neighbor_forward_lane_id()->CopyFrom(
          hdmap->lane(i + 1).id());
      ADEBUG << "Right neighbor is: " << hdmap->lane(i + 1).id().id();
    }
  }
  return true;
}

GeneratePath()

bool apollo::relative_map::NavigationLane::GeneratePath

(

)

Generate a suitable path (i.e.

a navigation line segment).

参数

return True if a suitable path is created; false otherwise.

在文件 navigation_lane.cc 第 162 行定义.

                                  {
  navigation_path_list_.clear();
  current_navi_path_tuple_ = std::make_tuple(-1, -1.0, -1.0, nullptr);
 
  // original_pose is in world coordination: ENU
  original_pose_ = vehicle_state_provider_->original_pose();
 
  int navigation_line_num = navigation_info_.navigation_path_size();
  const auto &lane_marker = perception_obstacles_.lane_marker();
 
  auto generate_path_on_perception_func = [this, &lane_marker]() {
    auto current_navi_path = std::make_shared<NavigationPath>();
    auto *path = current_navi_path->mutable_path();
    ConvertLaneMarkerToPath(lane_marker, path);
    current_navi_path->set_path_priority(0);
    double left_width = perceived_left_width_ > 0.0 ? perceived_left_width_
                                                    : default_left_width_;
    double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
                                                      : default_right_width_;
    current_navi_path_tuple_ =
        std::make_tuple(0, left_width, right_width, current_navi_path);
  };
 
  ADEBUG << "Beginning of NavigationLane::GeneratePath().";
  ADEBUG << "navigation_line_num: " << navigation_line_num;
 
  // priority: merge > navigation line > perception lane marker
  if (config_.lane_source() == NavigationLaneConfig::OFFLINE_GENERATED &&
      navigation_line_num > 0) {
    // Generate multiple navigation paths based on navigation lines.
    // Don't worry about efficiency because the total number of navigation lines
    // will not exceed 10 at most.
    for (int i = 0; i < navigation_line_num; ++i) {
      auto current_navi_path = std::make_shared<NavigationPath>();
      auto *path = current_navi_path->mutable_path();
      if (ConvertNavigationLineToPath(i, path)) {
        current_navi_path->set_path_priority(
            navigation_info_.navigation_path(i).path_priority());
        navigation_path_list_.emplace_back(
            i, default_left_width_, default_right_width_, current_navi_path);
      }
    }
 
    // If no navigation path is generated based on navigation lines, we generate
    // one where the vehicle is located based on perceived lane markers.
    if (navigation_path_list_.empty()) {
      generate_path_on_perception_func();
      return true;
    }
 
    // Sort navigation paths from left to right according to the vehicle's
    // direction.
    // In the FLU vehicle coordinate system, the y-coordinate on the left side
    // of the vehicle is positive, and the right value is negative. Therefore,
    // the navigation paths can be sorted from left to right according to its
    // y-coordinate.
    navigation_path_list_.sort(
        [](const NaviPathTuple &left, const NaviPathTuple &right) {
          double left_y = std::get<3>(left)->path().path_point(0).y();
          double right_y = std::get<3>(right)->path().path_point(0).y();
          return left_y > right_y;
        });
 
    // Get which navigation path the vehicle is currently on.
    double min_d = std::numeric_limits<double>::max();
    for (const auto &navi_path_tuple : navigation_path_list_) {
      int current_line_index = std::get<0>(navi_path_tuple);
      ADEBUG << "Current navigation path index is: " << current_line_index;
      double current_d = std::numeric_limits<double>::max();
      auto item_iter = last_project_index_map_.find(current_line_index);
      if (item_iter != last_project_index_map_.end()) {
        current_d = item_iter->second.second;
      }
      if (current_d < min_d) {
        min_d = current_d;
        current_navi_path_tuple_ = navi_path_tuple;
      }
    }
 
    // Merge current navigation path where the vehicle is located with perceived
    // lane markers.
    auto *path = std::get<3>(current_navi_path_tuple_)->mutable_path();
    MergeNavigationLineAndLaneMarker(std::get<0>(current_navi_path_tuple_),
                                     path);
 
    // Set the width for the navigation path which the vehicle is currently on.
    double left_width = perceived_left_width_ > 0.0 ? perceived_left_width_
                                                    : default_left_width_;
    double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
                                                      : default_right_width_;
    if (!IsFloatEqual(left_width, default_left_width_) &&
        !IsFloatEqual(right_width, default_right_width_)) {
      left_width = left_width > default_left_width_ ? left_width - min_d
                                                    : left_width + min_d;
      right_width = right_width > default_right_width_ ? right_width - min_d
                                                       : right_width + min_d;
    }
 
    ADEBUG << "The left width of current lane is: " << left_width
           << " and the right width of current lane is: " << right_width;
 
    std::get<1>(current_navi_path_tuple_) = left_width;
    std::get<2>(current_navi_path_tuple_) = right_width;
    auto curr_navi_path_iter = std::find_if(
        std::begin(navigation_path_list_), std::end(navigation_path_list_),
        [this](const NaviPathTuple &item) {
          return std::get<0>(item) == std::get<0>(current_navi_path_tuple_);
        });
    if (curr_navi_path_iter != std::end(navigation_path_list_)) {
      std::get<1>(*curr_navi_path_iter) = left_width;
      std::get<2>(*curr_navi_path_iter) = right_width;
    }
 
    // Set the width between each navigation path and its adjacent path.
    // The reason for using average of multiple points is to prevent too much
    // interference from a singularity.
    // If current navigation path is the path which the vehicle is currently
    // on, the current lane width uses the perceived width.
    int average_point_size = 5;
    for (auto iter = navigation_path_list_.begin();
         iter != navigation_path_list_.end(); ++iter) {
      const auto &curr_path = std::get<3>(*iter)->path();
 
      // Left neighbor
      auto prev_iter = std::prev(iter);
      if (prev_iter != navigation_path_list_.end()) {
        const auto &prev_path = std::get<3>(*prev_iter)->path();
        average_point_size = std::min(
            average_point_size,
            std::min(curr_path.path_point_size(), prev_path.path_point_size()));
        double lateral_distance_sum = 0.0;
        for (int i = 0; i < average_point_size; ++i) {
          lateral_distance_sum +=
              fabs(curr_path.path_point(i).y() - prev_path.path_point(i).y());
        }
        double width = lateral_distance_sum /
                       static_cast<double>(average_point_size) / 2.0;
        width = common::math::Clamp(width, config_.min_lane_half_width(),
                                    config_.max_lane_half_width());
 
        auto &curr_left_width = std::get<1>(*iter);
        auto &prev_right_width = std::get<2>(*prev_iter);
        if (std::get<0>(*iter) == std::get<0>(current_navi_path_tuple_)) {
          prev_right_width = 2.0 * width - curr_left_width;
        } else {
          curr_left_width = width;
          prev_right_width = width;
        }
      }
      // Right neighbor
      auto next_iter = std::next(iter);
      if (next_iter != navigation_path_list_.end()) {
        const auto &next_path = std::get<3>(*next_iter)->path();
        average_point_size = std::min(
            average_point_size,
            std::min(curr_path.path_point_size(), next_path.path_point_size()));
        double lateral_distance_sum = 0.0;
        for (int i = 0; i < average_point_size; ++i) {
          lateral_distance_sum +=
              fabs(curr_path.path_point(i).y() - next_path.path_point(i).y());
        }
        double width = lateral_distance_sum /
                       static_cast<double>(average_point_size) / 2.0;
        width = common::math::Clamp(width, config_.min_lane_half_width(),
                                    config_.max_lane_half_width());
 
        auto &curr_right_width = std::get<2>(*iter);
        auto &next_left_width = std::get<1>(*next_iter);
        if (std::get<0>(*iter) == std::get<0>(current_navi_path_tuple_)) {
          next_left_width = 2.0 * width - curr_right_width;
        } else {
          next_left_width = width;
          curr_right_width = width;
        }
      }
    }
 
    return true;
  }
 
  // Generate a navigation path where the vehicle is located based on perceived
  // lane markers.
  generate_path_on_perception_func();
  return true;
}

Path()

NavigationPath apollo::relative_map::NavigationLane::Path ( ) const

inline

Get the generated lane segment where the vehicle is currently located.

参数

return The generated lane segment where the vehicle is currently located.

在文件 navigation_lane.h 第 154 行定义.

                              {
    const auto& current_navi_path = std::get<3>(current_navi_path_tuple_);
    if (current_navi_path) {
      return *current_navi_path;
    }
    return NavigationPath();
  }

SetConfig()

void apollo::relative_map::NavigationLane::SetConfig

(

const NavigationLaneConfig &

config

)

Set the configuration information required by the NavigationLane.

参数

config

Configuration object.

返回

None.

在文件 navigation_lane.cc 第 142 行定义.

                                                                 {
  config_ = config;
}

SetDefaultWidth()

void apollo::relative_map::NavigationLane::SetDefaultWidth ( const double  left_width, const double  right_width  )

inline

Set the default width of a lane.

参数

left_width	Left half width of a lane.
right_width	Right half width of a lane.

返回

None.

在文件 navigation_lane.h 第 126 行定义.

                                                                          {
    default_left_width_ = left_width;
    default_right_width_ = right_width;
  }

SetVehicleStateProvider()

void apollo::relative_map::NavigationLane::SetVehicleStateProvider

(

common::VehicleStateProvider *

vehicle_state_provider

)

在文件 navigation_lane.cc 第 146 行定义.

                                                        {
  vehicle_state_provider_ = vehicle_state_provider;
}

UpdateNavigationInfo()

void apollo::relative_map::NavigationLane::UpdateNavigationInfo

(

const NavigationInfo &

navigation_info

)

Update navigation line information.

参数

navigation_info

Navigation line information to be updated.

返回

None.

在文件 navigation_lane.cc 第 151 行定义.

                                           {
  navigation_info_ = navigation_path;
  last_project_index_map_.clear();
  navigation_path_list_.clear();
  current_navi_path_tuple_ = std::make_tuple(-1, -1.0, -1.0, nullptr);
  if (FLAGS_enable_cyclic_rerouting) {
    UpdateStitchIndexInfo();
  }
}

UpdatePerception()

void apollo::relative_map::NavigationLane::UpdatePerception ( const perception::PerceptionObstacles &  perception_obstacles )

inline

Update perceived lane line information.

参数

perception_obstacles

Perceived lane line information to be updated.

返回

None.

在文件 navigation_lane.h 第 143 行定义.

                                                                 {
    perception_obstacles_ = perception_obstacles;
  }

---

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

• modules/map/relative_map/navigation_lane.h
• modules/map/relative_map/navigation_lane.cc