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

Prediction: Simplify PredictionMap interface to accept map_id string only.

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