apollo::external_command::LaneWayTool类 参考

#include <lane_way_tool.h>

apollo::external_command::LaneWayTool 的协作图:

Public 成员函数
	LaneWayTool (const std::shared_ptr< cyber::Node > &node)
bool	ConvertToLaneWayPoint (const apollo::external_command::Pose &pose, apollo::routing::LaneWaypoint *lane_way_point) const
	Convert the pose with (x,y) and heading(or not) to lane_way_point.
bool	GetVehicleLaneWayPoint (apollo::routing::LaneWaypoint *lane_way_point) const
	Get the current vehicle pose and find the mapping point to the nearest lane.
bool	GetVehicleLaneWayPoints (std::vector< apollo::routing::LaneWaypoint > *lane_way_points) const
bool	GetParkingLaneWayPoint (const std::string &parking_id, std::vector< apollo::routing::LaneWaypoint > *lane_way_points) const
	Get the parking space center point projected to lane way point
bool	IsParkandgoScenario () const
bool	GetPreciseParkingLaneWayPoint (hdmap::AreaInfoConstPtr &area_info, std::vector< apollo::routing::LaneWaypoint > *lane_way_points) const

详细描述

在文件 lane_way_tool.h 第 49 行定义.

构造及析构函数说明

LaneWayTool()

apollo::external_command::LaneWayTool::LaneWayTool ( const std::shared_ptr< cyber::Node > &  node )

explicit

在文件 lane_way_tool.cc 第 37 行定义.

    : hdmap_(hdmap::HDMapUtil::BaseMapPtr()),
      message_reader_(MessageReader::Instance()) {
  message_reader_->RegisterMessage<apollo::localization::LocalizationEstimate>(
      FLAGS_localization_topic);
}

成员函数说明

ConvertToLaneWayPoint()

bool apollo::external_command::LaneWayTool::ConvertToLaneWayPoint	(	const apollo::external_command::Pose &	pose,
		apollo::routing::LaneWaypoint *	lane_way_point
	)		const

Convert the pose with (x,y) and heading(or not) to lane_way_point.

参数

pose	The pose to be converted.
lane_way_point	convert result of "lane_way_point".

返回

True if no error occurs.

在文件 lane_way_tool.cc 第 44 行定义.

                                                                       {
  hdmap::LaneInfoConstPtr nearest_lane;
  double nearest_s;
  double nearest_l;
  common::PointENU point;
  point.set_x(pose.x());
  point.set_y(pose.y());
  if (pose.has_heading()) {
    static constexpr double kSearchRadius = 3.0;
    static constexpr double kMaxHeadingDiff = 1.0;
    // Get the lane nearest to the pose with heading check and update the lane
    // info in LaneWayPoint.
    if (nullptr == hdmap_ ||
        hdmap_->GetNearestLaneWithHeading(point, kSearchRadius, pose.heading(),
                                          kMaxHeadingDiff, &nearest_lane,
                                          &nearest_s, &nearest_l) < 0) {
      AERROR << "Failed to get nearest lane with heading of pose "
             << pose.DebugString();
      return false;
    }
  } else {
    // Get the lane nearest to the pose without heading check and update the
    // lane info in LaneWayPoint.
    if (nullptr == hdmap_ ||
        hdmap_->GetNearestLane(point, &nearest_lane, &nearest_s, &nearest_l) <
            0) {
      AERROR << "Failed to get nearest lane of " << pose.DebugString();
      return false;
    }
  }
  // Check the lane type.
  if (nearest_lane->lane().type() != hdmap::Lane::CITY_DRIVING) {
    AERROR << pose.DebugString() << " Lane type is not correct "
           << apollo::hdmap::Lane::LaneType_Name(nearest_lane->lane().type());
    return false;
  }
  // Update the LaneWayPoint info.
  lane_way_point->set_id(nearest_lane->id().id());
  lane_way_point->set_s(nearest_s);
  auto *lane_way_pose = lane_way_point->mutable_pose();
  lane_way_pose->set_x(pose.x());
  lane_way_pose->set_y(pose.y());
  return true;
}

GetParkingLaneWayPoint()

bool apollo::external_command::LaneWayTool::GetParkingLaneWayPoint	(	const std::string &	parking_id,
		std::vector< apollo::routing::LaneWaypoint > *	lane_way_points
	)		const

Get the parking space center point projected to lane way point

参数

parking_id	parking space id
lane_way_point	output lane way point near the parking space

返回

在文件 lane_way_tool.cc 第 149 行定义.

                                                                 {
  hdmap::Id id;
  id.set_id(parking_id);
  auto parking_space_info = hdmap_->GetParkingSpaceById(id);
  auto points = parking_space_info->polygon().points();
  common::math::Vec2d center_point(0, 0);
  for (size_t i = 0; i < points.size(); i++) {
    center_point += points[i];
  }
  center_point /= 4.0;
  apollo::common::PointENU center_enu;
  center_enu.set_x(center_point.x());
  center_enu.set_y(center_point.y());
  for (const auto &parking_space_overlap_id :
       parking_space_info->parking_space().overlap_id()) {
    hdmap::OverlapInfoConstPtr overlap_info_ptr =
        hdmap_->GetOverlapById(parking_space_overlap_id);
    if (nullptr == overlap_info_ptr) {
      AERROR << "Cannot get overlap info!";
      continue;
    }
    for (const auto &object : overlap_info_ptr->overlap().object()) {
      hdmap::LaneInfoConstPtr lane_ptr = hdmap_->GetLaneById(object.id());
      if (nullptr == lane_ptr) {
        AERROR << "Cannot get lane info!";
        continue;
      }
      double s, l;
      if (!lane_ptr->GetProjection(center_point, &s, &l)) {
        AERROR << "get projection failed";
        continue;
      }
      lane_way_points->emplace_back();
      auto &lane_way_point = lane_way_points->back();
      lane_way_point.mutable_pose()->set_x(center_point.x());
      lane_way_point.mutable_pose()->set_y(center_point.y());
      lane_way_point.set_id(lane_ptr->id().id());
      lane_way_point.set_s(s);
      AINFO << "GET" << lane_way_point.DebugString();
    }
  }
  if (lane_way_points->empty()) {
    AERROR << "Cannot get lane way point!";
    return false;
  }
  return true;
}

GetPreciseParkingLaneWayPoint()

bool apollo::external_command::LaneWayTool::GetPreciseParkingLaneWayPoint	(	hdmap::AreaInfoConstPtr &	area_info,
		std::vector< apollo::routing::LaneWaypoint > *	lane_way_points
	)		const

在文件 lane_way_tool.cc 第 199 行定义.

                                                                 {
  if (nullptr == area_info) {
    AERROR << "Cannot get junction info!";
    return false;
  }
 
  for (const auto &junction_overlap_id : area_info->area().overlap_id()) {
    hdmap::OverlapInfoConstPtr overlap_info_ptr =
        hdmap_->GetOverlapById(junction_overlap_id);
    if (nullptr == overlap_info_ptr) {
      AERROR << "Cannot get overlap info!";
      continue;
    }
 
    for (const auto &object : overlap_info_ptr->overlap().object()) {
      hdmap::LaneInfoConstPtr lane_ptr = hdmap_->GetLaneById(object.id());
      if (nullptr == lane_ptr) {
        AERROR << "Cannot get lane info!";
        continue;
      }
      double traget_s = object.lane_overlap_info().end_s();
      apollo::common::PointENU traget_waypoint =
          lane_ptr->GetSmoothPoint(traget_s);
      lane_way_points->emplace_back();
      auto &lane_way_point = lane_way_points->back();
      lane_way_point.mutable_pose()->set_x(traget_waypoint.x());
      lane_way_point.mutable_pose()->set_y(traget_waypoint.y());
      lane_way_point.set_id(lane_ptr->id().id());
      lane_way_point.set_s(traget_s);
      AINFO << "GET" << lane_way_point.DebugString();
    }
  }
  if (lane_way_points->empty()) {
    AERROR << "Cannot get lane way point!";
    return false;
  }
  return true;
}

GetVehicleLaneWayPoint()

bool apollo::external_command::LaneWayTool::GetVehicleLaneWayPoint

(

apollo::routing::LaneWaypoint *

lane_way_point

)

const

Get the current vehicle pose and find the mapping point to the nearest lane.

参数

lane_way_point

The raw command data.

返回

True if no error occurs.

在文件 lane_way_tool.cc 第 90 行定义.

                                                     {
  CHECK_NOTNULL(lane_way_point);
  // Get the current localization pose
  auto *localization =
      message_reader_->GetMessage<apollo::localization::LocalizationEstimate>(
          FLAGS_localization_topic);
  if (nullptr == localization) {
    AERROR << "Cannot get vehicle location!";
    return false;
  }
  external_command::Pose pose;
  pose.set_x(localization->pose().position().x());
  pose.set_y(localization->pose().position().y());
  pose.set_heading(localization->pose().heading());
  return ConvertToLaneWayPoint(pose, lane_way_point);
}

GetVehicleLaneWayPoints()

bool apollo::external_command::LaneWayTool::GetVehicleLaneWayPoints

(

std::vector< apollo::routing::LaneWaypoint > *

lane_way_points

)

const

在文件 lane_way_tool.cc 第 108 行定义.

                                                                 {
  CHECK_NOTNULL(lane_way_points);
  // Get the current localization pose
  auto *localization =
      message_reader_->GetMessage<apollo::localization::LocalizationEstimate>(
          FLAGS_localization_topic);
  if (nullptr == localization) {
    AERROR << "Cannot get vehicle location!";
    return false;
  }
  apollo::common::PointENU center_enu;
  center_enu.set_x(localization->pose().position().x());
  center_enu.set_y(localization->pose().position().y());
  common::math::Vec2d center_point(center_enu.x(), center_enu.y());
  std::vector<hdmap::LaneInfoConstPtr> lanes;
  static constexpr double kNearbyLaneDistance = 20.0;
  if (hdmap_->GetLanes(center_enu, kNearbyLaneDistance, &lanes) < 0) {
    AERROR << "get lanes failed" << center_enu.DebugString();
    return false;
  }
  for (const auto lane : lanes) {
    double s, l;
    if (!lane->GetProjection(center_point, &s, &l)) {
      AERROR << "get projection failed";
      return false;
    }
    if (s < 0.0 || s > lane->total_length()) {
      continue;
    }
    lane_way_points->emplace_back();
    auto &lane_way_point = lane_way_points->back();
    lane_way_point.mutable_pose()->set_x(center_point.x());
    lane_way_point.mutable_pose()->set_y(center_point.y());
    lane_way_point.set_id(lane->id().id());
    lane_way_point.set_s(s);
    AINFO << "GET" << lane_way_point.DebugString();
  }
  return true;
}

IsParkandgoScenario()

bool apollo::external_command::LaneWayTool::IsParkandgoScenario

(

)

const

在文件 lane_way_tool.cc 第 240 行定义.

                                                                  {
  auto *localization =
      message_reader_->GetMessage<apollo::localization::LocalizationEstimate>(
          FLAGS_localization_topic);
  if (nullptr == localization) {
    AERROR << "Cannot get vehicle location!";
    return false;
  }
  common::PointENU adc;
  adc.set_x(localization->pose().position().x());
  adc.set_y(localization->pose().position().y());
  external_command::Pose pose;
  double heading = localization->pose().heading();
  hdmap::LaneInfoConstPtr lane;
  double s = 0.0;
  double l = 0.0;
  if (hdmap_->GetNearestLaneWithHeading(adc, 2.0, heading, M_PI / 3.0, &lane,
                                        &s, &l) != 0 ||
      lane->lane().type() != hdmap::Lane::CITY_DRIVING) {
    return true;
  }
  return false;
}

---

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

• modules/external_command/command_processor/command_processor_base/util/lane_way_tool.h
• modules/external_command/command_processor/command_processor_base/util/lane_way_tool.cc