Skip to content

Commit

Permalink
revert part of as before
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Jul 8, 2024
1 parent 5c07333 commit bf03fb4
Showing 1 changed file with 34 additions and 29 deletions.
63 changes: 34 additions & 29 deletions autoware_lanelet2_extension/lib/query.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,11 @@

using lanelet::utils::to2D;

namespace lanelet::utils::query
namespace lanelet::utils
{

namespace query
{
inline namespace format_v1
{
lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls)
Expand Down Expand Up @@ -682,10 +684,11 @@ std::vector<lanelet::ConstLineString3d> stopSignStopLines(
return stoplines;
}
} // namespace format_v1
} // namespace query

// returns all lanelets in laneletLayer - don't know how to convert
// PrimitiveLayer<Lanelets> -> std::vector<Lanelets>
lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_map)
lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map)
{
lanelet::ConstLanelets lanelets;
if (!ll_map) {
Expand All @@ -700,7 +703,8 @@ lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_map)
return lanelets;
}

lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets & lls, const char subtype[])
lanelet::ConstLanelets query::subtypeLanelets(
const lanelet::ConstLanelets & lls, const char subtype[])
{
lanelet::ConstLanelets subtype_lanelets;

Expand All @@ -716,12 +720,12 @@ lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets & lls, const
return subtype_lanelets;
}

lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets & lls)
lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets & lls)
{
return subtypeLanelets(lls, lanelet::AttributeValueString::Road);
return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road);
}

lanelet::ConstPolygons3d getAllPolygonsByType(
lanelet::ConstPolygons3d query::getAllPolygonsByType(

Check warning on line 728 in autoware_lanelet2_extension/lib/query.cpp

View check run for this annotation

Codecov / codecov/patch

autoware_lanelet2_extension/lib/query.cpp#L728 

Added line #L728 was not covered by tests
const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type)
{
lanelet::ConstPolygons3d polygons;
Expand All @@ -734,7 +738,7 @@ lanelet::ConstPolygons3d getAllPolygonsByType(
return polygons;

Check warning on line 738 in autoware_lanelet2_extension/lib/query.cpp

View check run for this annotation

Codecov / codecov/patch

autoware_lanelet2_extension/lib/query.cpp#L738 

Added line #L738 was not covered by tests
}

ConstLanelets getLaneletsWithinRange(
ConstLanelets query::getLaneletsWithinRange(
const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point,
const double range)
{
Expand All @@ -749,40 +753,41 @@ ConstLanelets getLaneletsWithinRange(
return near_lanelets;
}

ConstLanelets getLaneletsWithinRange(
ConstLanelets query::getLaneletsWithinRange(
const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point,
const double range)
{
return getLaneletsWithinRange(
return query::getLaneletsWithinRange(
lanelets, lanelet::BasicPoint2d(search_point.x, search_point.y), range);
}

ConstLanelets getLaneChangeableNeighbors(
ConstLanelets query::getLaneChangeableNeighbors(
const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
{
return graph->besides(lanelet);
}

ConstLanelets getLaneChangeableNeighbors(
ConstLanelets query::getLaneChangeableNeighbors(
const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets,
const geometry_msgs::msg::Point & search_point)
{
const auto lanelets =
getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits<double>::epsilon());
const auto lanelets = query::getLaneletsWithinRange(
road_lanelets, search_point, std::numeric_limits<double>::epsilon());

Check warning on line 775 in autoware_lanelet2_extension/lib/query.cpp

View check run for this annotation

Codecov / codecov/patch

autoware_lanelet2_extension/lib/query.cpp#L775 

Added line #L775 was not covered by tests
ConstLanelets road_slices;
for (const auto & llt : lanelets) {
const auto tmp_road_slice = getLaneChangeableNeighbors(graph, llt);
const auto tmp_road_slice = query::getLaneChangeableNeighbors(graph, llt);
road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end());
}
return road_slices;
}

ConstLanelets getAllNeighbors(const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
ConstLanelets query::getAllNeighbors(
const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
{
ConstLanelets lanelets;

ConstLanelets left_lanelets = getAllNeighborsLeft(graph, lanelet);
ConstLanelets right_lanelets = getAllNeighborsRight(graph, lanelet);
ConstLanelets left_lanelets = query::getAllNeighborsLeft(graph, lanelet);
ConstLanelets right_lanelets = query::getAllNeighborsRight(graph, lanelet);

std::reverse(left_lanelets.begin(), left_lanelets.end());
lanelets.insert(lanelets.end(), left_lanelets.begin(), left_lanelets.end());
Expand All @@ -792,7 +797,7 @@ ConstLanelets getAllNeighbors(const routing::RoutingGraphPtr & graph, const Cons
return lanelets;
}

ConstLanelets getAllNeighborsRight(
ConstLanelets query::getAllNeighborsRight(
const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
{
ConstLanelets lanelets;
Expand All @@ -806,7 +811,7 @@ ConstLanelets getAllNeighborsRight(
return lanelets;
}

ConstLanelets getAllNeighborsLeft(
ConstLanelets query::getAllNeighborsLeft(
const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
{
ConstLanelets lanelets;
Expand All @@ -819,12 +824,12 @@ ConstLanelets getAllNeighborsLeft(
return lanelets;
}

ConstLanelets getAllNeighbors(
ConstLanelets query::getAllNeighbors(
const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets,
const geometry_msgs::msg::Point & search_point)
{
const auto lanelets =
getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits<double>::epsilon());
const auto lanelets = query::getLaneletsWithinRange(
road_lanelets, search_point, std::numeric_limits<double>::epsilon());

Check warning on line 832 in autoware_lanelet2_extension/lib/query.cpp

View check run for this annotation

Codecov / codecov/patch

autoware_lanelet2_extension/lib/query.cpp#L832 

Added line #L832 was not covered by tests
ConstLanelets road_slices;
for (const auto & llt : lanelets) {
const auto tmp_road_slice = getAllNeighbors(graph, llt);
Expand All @@ -833,7 +838,7 @@ ConstLanelets getAllNeighbors(
return road_slices;
}

bool getClosestLanelet(
bool query::getClosestLanelet(
const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
ConstLanelet * closest_lanelet_ptr)
{
Expand Down Expand Up @@ -897,7 +902,7 @@ bool getClosestLanelet(
return found;
}

bool getClosestLaneletWithConstrains(
bool query::getClosestLaneletWithConstrains(
const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
ConstLanelet * closest_lanelet_ptr, const double dist_threshold, const double yaw_threshold)
{
Expand Down Expand Up @@ -964,7 +969,7 @@ bool getClosestLaneletWithConstrains(
return found;
}

bool getCurrentLanelets(
bool query::getCurrentLanelets(
const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point,
ConstLanelets * current_lanelets_ptr)
{
Expand All @@ -988,7 +993,7 @@ bool getCurrentLanelets(
return !current_lanelets_ptr->empty(); // return found
}

bool getCurrentLanelets(
bool query::getCurrentLanelets(
const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
ConstLanelets * current_lanelets_ptr)
{
Expand Down Expand Up @@ -1076,7 +1081,7 @@ std::vector<std::deque<lanelet::ConstLanelet>> getPrecedingLaneletSequencesRecur
return preceding_lanelet_sequences;
}

std::vector<lanelet::ConstLanelets> getSucceedingLaneletSequences(
std::vector<lanelet::ConstLanelets> query::getSucceedingLaneletSequences(
const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
const double length)
{
Expand All @@ -1092,7 +1097,7 @@ std::vector<lanelet::ConstLanelets> getSucceedingLaneletSequences(
return lanelet_sequences_vec;
}

std::vector<lanelet::ConstLanelets> getPrecedingLaneletSequences(
std::vector<lanelet::ConstLanelets> query::getPrecedingLaneletSequences(
const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
const double length, const lanelet::ConstLanelets & exclude_lanelets)
{
Expand All @@ -1113,6 +1118,6 @@ std::vector<lanelet::ConstLanelets> getPrecedingLaneletSequences(
}
return lanelet_sequences_vec;
}
} // namespace lanelet::utils::query
} // namespace lanelet::utils

// NOLINTEND(readability-identifier-naming)

0 comments on commit bf03fb4

Please sign in to comment.