提交 c6f0c000 编写于 作者: A Aaron Xiao 提交者: Calvin Miao

Prediction: Simplify PredictionMap interface to accept map_id string only.

上级 9aa0bce2
......@@ -80,13 +80,10 @@ double PredictionMap::LaneTotalWidth(
return left + right;
}
std::shared_ptr<const LaneInfo> PredictionMap::LaneById(const Id& id) {
return HDMapUtil::instance()->BaseMapRef().get_lane_by_id(id);
}
std::shared_ptr<const LaneInfo> PredictionMap::LaneById(
const std::string& str_id) {
return LaneById(apollo::hdmap::MakeMapId(str_id));
return HDMapUtil::instance()->BaseMapRef().get_lane_by_id(
apollo::hdmap::MakeMapId(str_id));
}
void PredictionMap::GetProjection(const Eigen::Vector2d& position,
......@@ -162,16 +159,14 @@ void PredictionMap::OnLane(
double PredictionMap::PathHeading(std::shared_ptr<const LaneInfo> lane_info,
const apollo::common::PointENU& point) {
apollo::common::math::Vec2d vec_point;
vec_point.set_x(point.x());
vec_point.set_y(point.y());
apollo::common::math::Vec2d vec_point = {point.x(), point.y()};
double s = -1.0;
double l = 0.0;
lane_info->get_projection(vec_point, &s, &l);
return HeadingOnLane(lane_info, s);
}
int PredictionMap::SmoothPointFromLane(const apollo::hdmap::Id& id,
int PredictionMap::SmoothPointFromLane(const std::string& id,
const double s, const double l,
Eigen::Vector2d* point,
double* heading) {
......@@ -203,10 +198,11 @@ void PredictionMap::NearbyLanesByCurrentLanes(
continue;
}
for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
const std::string& id = lane_id.id();
if (lane_ids.find(id) != lane_ids.end()) {
continue;
}
std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
double s = -1.0;
double l = 0.0;
GetProjection(point, nearby_lane, &s, &l);
......@@ -214,14 +210,15 @@ void PredictionMap::NearbyLanesByCurrentLanes(
::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
continue;
}
lane_ids.insert(lane_id.id());
lane_ids.insert(id);
nearby_lanes->push_back(nearby_lane);
}
for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
const std::string& id = lane_id.id();
if (lane_ids.find(id) != lane_ids.end()) {
continue;
}
std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
double s = -1.0;
double l = 0.0;
GetProjection(point, nearby_lane, &s, &l);
......@@ -229,7 +226,7 @@ void PredictionMap::NearbyLanesByCurrentLanes(
::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
continue;
}
lane_ids.insert(lane_id.id());
lane_ids.insert(id);
nearby_lanes->push_back(nearby_lane);
}
}
......@@ -246,7 +243,7 @@ bool PredictionMap::IsLeftNeighborLane(
return false;
}
for (auto& left_lane_id : curr_lane->lane().left_neighbor_forward_lane_id()) {
if (id_string(left_lane) == left_lane_id.id()) {
if (left_lane->id().id() == left_lane_id.id()) {
return true;
}
}
......@@ -278,7 +275,7 @@ bool PredictionMap::IsRightNeighborLane(
}
for (auto& right_lane_id :
curr_lane->lane().right_neighbor_forward_lane_id()) {
if (id_string(right_lane) == right_lane_id.id()) {
if (right_lane->id().id() == right_lane_id.id()) {
return true;
}
}
......@@ -309,7 +306,7 @@ bool PredictionMap::IsSuccessorLane(
return false;
}
for (auto& successor_lane_id : curr_lane->lane().successor_id()) {
if (id_string(succ_lane) == successor_lane_id.id()) {
if (succ_lane->id().id() == successor_lane_id.id()) {
return true;
}
}
......@@ -340,7 +337,7 @@ bool PredictionMap::IsPredecessorLane(
return false;
}
for (auto& predecessor_lane_id : curr_lane->lane().predecessor_id()) {
if (id_string(pred_lane) == predecessor_lane_id.id()) {
if (pred_lane->id().id() == predecessor_lane_id.id()) {
return true;
}
}
......@@ -367,7 +364,7 @@ bool PredictionMap::IsIdenticalLane(
if (curr_lane == nullptr || other_lane == nullptr) {
return true;
}
return id_string(other_lane) == id_string(curr_lane);
return other_lane->id().id() == curr_lane->id().id();
}
bool PredictionMap::IsIdenticalLane(
......@@ -384,17 +381,13 @@ bool PredictionMap::IsIdenticalLane(
return false;
}
int PredictionMap::LaneTurnType(const Id& id) {
std::shared_ptr<const LaneInfo> lane = LaneById(id);
int PredictionMap::LaneTurnType(const std::string& lane_id) {
std::shared_ptr<const LaneInfo> lane = LaneById(lane_id);
if (lane != nullptr) {
return static_cast<int>(lane->lane().turn());
}
return 1;
}
int PredictionMap::LaneTurnType(const std::string& lane_id) {
return LaneTurnType(apollo::hdmap::MakeMapId(lane_id));
}
} // namespace prediction
} // namespace apollo
......@@ -63,21 +63,13 @@ class PredictionMap {
std::shared_ptr<const apollo::hdmap::LaneInfo> lane_info,
const double s);
/**
* @brief Get a shared pointer to a lane by lane ID.
* @param id The ID of the target lane ID.
* @return A shared pointer to the lane with the input lane ID.
*/
std::shared_ptr<const apollo::hdmap::LaneInfo> LaneById(
const apollo::hdmap::Id& id);
/**
* @brief Get a shared pointer to a lane by lane ID.
* @param id The ID of the target lane ID in the form of string.
* @return A shared pointer to the lane with the input lane ID.
*/
std::shared_ptr<const apollo::hdmap::LaneInfo> LaneById(
const std::string& str_id);
const std::string& id);
/**
* @brief Get the frenet coordinates (s, l) on a lane by a position.
......@@ -139,7 +131,7 @@ class PredictionMap {
* @return If the process is successful. 0: success, -1: failure.
*/
int SmoothPointFromLane(
const apollo::hdmap::Id& id, const double s,
const std::string& id, const double s,
const double l, Eigen::Vector2d* point,
double* heading);
......@@ -259,13 +251,6 @@ class PredictionMap {
std::shared_ptr<const apollo::hdmap::LaneInfo> other_lane,
const std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>>& lanes);
/**
* @brief Get lane turn type.
* @param id The lane ID.
* @return Integer corresponding to the lane turn type.
*/
int LaneTurnType(const apollo::hdmap::Id& id);
/**
* @brief Get lane turn type.
* @param lane_id The lane ID.
......@@ -273,16 +258,6 @@ class PredictionMap {
*/
int LaneTurnType(const std::string& lane_id);
/**
* @brief Get the ID in the form of string.
* @param info Map information.
* @return String of ID.
*/
template <class MapInfo>
static std::string id_string(std::shared_ptr<const MapInfo> info) {
return info->id().id();
}
private:
DECLARE_SINGLETON(PredictionMap);
};
......
......@@ -159,8 +159,7 @@ TEST_F(PredictionMapTest, get_path_heading) {
}
TEST_F(PredictionMapTest, get_smooth_point_from_lane) {
Id id;
id.set_id("l20");
const std::string id = "l20";
double s = 10.0;
double l = 0.0;
double heading = M_PI;
......
......@@ -71,7 +71,7 @@ void RoadGraph::ComputeLaneSequence(
LaneSegment lane_segment;
lane_segment.set_lane_id(lane_info_ptr->id().id());
lane_segment.set_start_s(start_s);
lane_segment.set_lane_turn_type(map->LaneTurnType(lane_info_ptr->id()));
lane_segment.set_lane_turn_type(map->LaneTurnType(lane_info_ptr->id().id()));
if (accumulated_s + lane_info_ptr->total_length() - start_s >= length_) {
lane_segment.set_end_s(length_ - accumulated_s + start_s);
} else {
......@@ -90,8 +90,7 @@ void RoadGraph::ComputeLaneSequence(
const double successor_accumulated_s =
accumulated_s + lane_info_ptr->total_length() - start_s;
for (const auto& successor_lane_id : lane_info_ptr->lane().successor_id()) {
std::shared_ptr<const LaneInfo> successor_lane =
map->LaneById(successor_lane_id);
auto successor_lane = map->LaneById(successor_lane_id.id());
ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lane,
lane_segments, lane_graph_ptr);
}
......
......@@ -772,7 +772,7 @@ void Obstacle::SetCurrentLanes(Feature* feature) {
}
double min_heading_diff = std::numeric_limits<double>::infinity();
for (std::shared_ptr<const LaneInfo> current_lane : current_lanes) {
int turn_type = map->LaneTurnType(current_lane->id());
int turn_type = map->LaneTurnType(current_lane->id().id());
std::string lane_id = current_lane->id().id();
double s = 0.0;
double l = 0.0;
......@@ -843,7 +843,7 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
if (s < 0.0 || s >= nearby_lane->total_length()) {
continue;
}
int turn_type = map->LaneTurnType(nearby_lane->id());
int turn_type = map->LaneTurnType(nearby_lane->id().id());
double heading = feature->theta();
if (FLAGS_enable_kf_tracking) {
heading = feature->t_velocity_heading();
......
......@@ -180,7 +180,7 @@ void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints(
for (size_t i = 0; i < static_cast<size_t>(total_time / freq); ++i) {
Eigen::Vector2d point;
double theta = M_PI;
if (map->SmoothPointFromLane(apollo::hdmap::MakeMapId(lane_id),
if (map->SmoothPointFromLane(lane_id,
lane_s, lane_l, &point, &theta) != 0) {
AERROR << "Unable to get smooth point from lane [" << lane_id
<< "] with s [" << lane_s << "] and l [" << lane_l
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册