Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(route_handler): add unit test for lane change related functions #7504

Merged
merged 3 commits into from
Jun 24, 2024
Merged

feat(route_handler): add unit test for lane change related functions #7504

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
84deaa7
RT1-6230 feat(route_handler): add unit test for lane change related f…
zulfaqar-azmi-t4 Jun 17, 2024
fe2fc62
fix spell check
zulfaqar-azmi-t4 Jun 17, 2024
819f137
fix spellcheck
zulfaqar-azmi-t4 Jun 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 25 additions & 5 deletions common/autoware_test_utils/src/autoware_test_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>

#include <lanelet2_core/geometry/LineString.h>

#include <utility>

namespace autoware::test_utils
Expand Down Expand Up @@ -54,14 +56,32 @@
lanelet::ErrorMessages errors{};
lanelet::projection::MGRSProjector projector{};
lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
if (errors.empty()) {
return map;
if (!errors.empty()) {
for (const auto & error : errors) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
}
return nullptr;
}

for (const auto & error : errors) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
for (lanelet::Point3d point : map->pointLayer) {
if (point.hasAttribute("local_x")) {
point.x() = point.attribute("local_x").asDouble().value();
}
if (point.hasAttribute("local_y")) {
point.y() = point.attribute("local_y").asDouble().value();
}
}
return nullptr;

// realign lanelet borders using updated points
for (lanelet::Lanelet lanelet : map->laneletLayer) {
auto left = lanelet.leftBound();
auto right = lanelet.rightBound();
std::tie(left, right) = lanelet::geometry::align(left, right);
lanelet.setLeftBound(left);
lanelet.setRightBound(right);
}

return map;

Check warning on line 84 in common/autoware_test_utils/src/autoware_test_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

loadMap has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}

LaneletMapBin convertToMapBinMsg(
Expand Down
238 changes: 179 additions & 59 deletions planning/autoware_route_handler/test/test_route_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,11 @@

TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute)
{
set_route_handler("/test_map/overlap_map.osm");
set_route_handler("overlap_map.osm");
ASSERT_FALSE(route_handler_->isHandlerReady());

geometry_msgs::msg::Pose start_pose, goal_pose;
geometry_msgs::msg::Pose start_pose;
geometry_msgs::msg::Pose goal_pose;

Check warning on line 56 in planning/autoware_route_handler/test/test_route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Assertion Blocks 

The test suite contains 6 assertion blocks with at least 4 assertions, threshold = 4. This test file has several blocks of large, consecutive assert statements. Avoid adding more.
start_pose.position = autoware::universe_utils::createPoint(3728.870361, 73739.281250, 0);
start_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, -0.513117, 0.858319);
goal_pose.position = autoware::universe_utils::createPoint(3729.961182, 73727.328125, 0);
Expand All @@ -79,11 +80,12 @@

TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute)
{
set_route_handler("/test_map/overlap_map.osm");
set_test_route("/test_route/overlap_test_route.yaml");
set_route_handler("overlap_map.osm");
set_test_route("overlap_test_route.yaml");
ASSERT_TRUE(route_handler_->isHandlerReady());

geometry_msgs::msg::Pose reference_pose, search_pose;
geometry_msgs::msg::Pose reference_pose;
geometry_msgs::msg::Pose search_pose;

lanelet::ConstLanelet reference_lanelet;
reference_pose.position = autoware::universe_utils::createPoint(3730.88, 73735.3, 0);
Expand All @@ -102,71 +104,189 @@
ASSERT_EQ(closest_lanelet.id(), 345);

found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet(
search_pose, reference_lanelet, &closest_lanelet, 3.0, 1.046);
search_pose, reference_lanelet, &closest_lanelet, dist_threshold, yaw_threshold);
ASSERT_TRUE(found_lanelet);
ASSERT_EQ(closest_lanelet.id(), 277);
}

// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute)
// {
// lanelet::ConstLanelet closest_lane;

// Pose search_pose;

// search_pose.position = autoware::universe_utils::createPoint(-1.0, 1.75, 0);
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained7 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained7);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = autoware::universe_utils::createPoint(-0.5, 1.75, 0);
// const auto closest_lane_obtained =
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = autoware::universe_utils::createPoint(-.01, 1.75, 0);
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained3 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
TEST_F(TestRouteHandler, CheckLaneIsInGoalRouteSection)
{
const auto lane = route_handler_->getLaneletsFromId(4785);
const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane);
ASSERT_TRUE(is_lane_in_goal_route_section);
}

// ASSERT_TRUE(closest_lane_obtained3);
// ASSERT_EQ(closest_lane.id(), 4775);
TEST_F(TestRouteHandler, CheckLaneIsNotInGoalRouteSection)
{
const auto lane = route_handler_->getLaneletsFromId(4780);
const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane);
ASSERT_FALSE(is_lane_in_goal_route_section);
}

// search_pose.position = autoware::universe_utils::createPoint(0.0, 1.75, 0);
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained1 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
TEST_F(TestRouteHandler, checkGetLaneletSequence)
{
const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0);

// ASSERT_TRUE(closest_lane_obtained1);
// ASSERT_EQ(closest_lane.id(), 4775);
lanelet::ConstLanelet closest_lanelet;
const auto found_closest_lanelet = route_handler_->getClosestLaneletWithConstrainsWithinRoute(
current_pose, &closest_lanelet, dist_threshold, yaw_threshold);
ASSERT_TRUE(found_closest_lanelet);
ASSERT_EQ(closest_lanelet.id(), 4765ul);

const auto current_lanes = route_handler_->getLaneletSequence(
closest_lanelet, current_pose, backward_path_length, forward_path_length);

ASSERT_EQ(current_lanes.size(), 6ul);
ASSERT_EQ(current_lanes.at(0).id(), 4765ul);
ASSERT_EQ(current_lanes.at(1).id(), 4770ul);
ASSERT_EQ(current_lanes.at(2).id(), 4775ul);
ASSERT_EQ(current_lanes.at(3).id(), 4424ul);
ASSERT_EQ(current_lanes.at(4).id(), 4780ul);
ASSERT_EQ(current_lanes.at(5).id(), 4785ul);
}

// search_pose.position = autoware::universe_utils::createPoint(0.01, 1.75, 0);
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained2 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneWhenLaneChangeToRight)
{
const auto current_lanes = get_current_lanes();

// The input is within expectation.
// this lane is of preferred lane type
std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) {
const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT);
ASSERT_EQ(result.size(), 0ul);
});

// The input is within expectation.
// this alternative lane is a subset of preferred lane route section
std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) {
const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT);
ASSERT_EQ(result.size(), 1ul);
EXPECT_DOUBLE_EQ(result.at(0), -3.5);
});

// The input is within expectation.
// Although Direction::NONE, the function should still return result similar to
// Direction::RIGHT.
std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) {
const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE);
ASSERT_EQ(result.size(), 0ul);
});

// The input is within expectation.
// Although Direction::NONE is provided, the function should behave similarly to
// Direction::RIGHT.
std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) {
const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE);
ASSERT_EQ(result.size(), 1ul);
EXPECT_DOUBLE_EQ(result.at(0), -3.5);
});
}

// ASSERT_TRUE(closest_lane_obtained2);
// ASSERT_EQ(closest_lane.id(), 4424);
TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneUsingUnexpectedResults)
{
const auto current_lanes = get_current_lanes();

// search_pose.position = autoware::universe_utils::createPoint(0.5, 1.75, 0);
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained4 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) {
const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::LEFT);
ASSERT_EQ(result.size(), 0ul);
});
}

// ASSERT_TRUE(closest_lane_obtained4);
// ASSERT_EQ(closest_lane.id(), 4424);
TEST_F(TestRouteHandler, testGetCenterLinePath)
{
const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785});
{
// The input is within expectation.
const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 50.0);
ASSERT_EQ(center_line_path.points.size(), 51); // 26 + 26 - 1(overlapped)
ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 2);
ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4780);
ASSERT_EQ(center_line_path.points.back().lane_ids.at(1), 4785);
}
{
// The input is broken.
// s_start is negative, and s_end is over the boundary.
const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, -1.0, 200.0);
ASSERT_EQ(center_line_path.points.size(), 76); // 26 + 26 + 26 - 2(overlapped)
ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1);
ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424);
ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1);
ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4785);
}
}
TEST_F(TestRouteHandler, DISABLED_testGetCenterLinePathWhenLanesIsNotConnected)
{
// broken current lanes. 4424 and 4785 are not connected directly.
const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785});

// The input is broken. Test is disabled because it doesn't pass.
const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 75.0);
ASSERT_EQ(center_line_path.points.size(), 26); // 26 + 26 + 26 - 2(overlapped)
ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1);
ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424);
ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1);
ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4424);
}

// search_pose.position = autoware::universe_utils::createPoint(1.0, 1.75, 0);
// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained5 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute)
{
auto get_closest_lanelet_within_route =
[&](double x, double y, double z) -> std::optional<lanelet::Id> {
const auto pose = autoware::test_utils::createPose(x, y, z, 0.0, 0.0, 0.0);
lanelet::ConstLanelet closest_lanelet;
const auto closest_lane_obtained =
route_handler_->getClosestLaneletWithinRoute(pose, &closest_lanelet);
if (!closest_lane_obtained) {
return std::nullopt;
}
return closest_lanelet.id();
};

ASSERT_TRUE(get_closest_lanelet_within_route(-0.5, 1.75, 0).has_value());
ASSERT_EQ(get_closest_lanelet_within_route(-0.5, 1.75, 0).value(), 4775ul);

ASSERT_TRUE(get_closest_lanelet_within_route(-0.01, 1.75, 0).has_value());
ASSERT_EQ(get_closest_lanelet_within_route(-0.01, 1.75, 0).value(), 4775ul);

ASSERT_TRUE(get_closest_lanelet_within_route(0.0, 1.75, 0).has_value());
ASSERT_EQ(get_closest_lanelet_within_route(0.0, 1.75, 0).value(), 4775ul);

ASSERT_TRUE(get_closest_lanelet_within_route(0.01, 1.75, 0).has_value());
ASSERT_EQ(get_closest_lanelet_within_route(0.01, 1.75, 0).value(), 4424ul);

ASSERT_TRUE(get_closest_lanelet_within_route(0.5, 1.75, 0).has_value());
ASSERT_EQ(get_closest_lanelet_within_route(0.5, 1.75, 0).value(), 4424ul);
}

// ASSERT_TRUE(closest_lane_obtained5);
// ASSERT_EQ(closest_lane.id(), 4424);
// }
TEST_F(TestRouteHandler, testGetLaneChangeTargetLanes)
{
{
// The input is within expectation.
// There exist no lane changing lane since both 4770 and 4775 are preferred lane.
const auto current_lanes = route_handler_->getLaneletsFromIds({4770, 4775});
const auto lane_change_lane =
route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT);
ASSERT_FALSE(lane_change_lane.has_value());
}

{
// The input is within expectation.
// There exist lane changing lane since 4424 is subset of preferred lane 9598.
const auto current_lanes = route_handler_->getLaneletsFromIds({4775, 4424});
const auto lane_change_lane =
route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT);
EXPECT_TRUE(lane_change_lane.has_value());
ASSERT_EQ(lane_change_lane.value().id(), 9598ul);
}

{
// The input is within expectation.
// There is a lane-changing lane. Within the maximum current lanes, there is an alternative lane
// to the preferred lane. Therefore, the lane-changing lane exists.
const auto current_lanes = get_current_lanes();
const auto lane_change_lane = route_handler_->getLaneChangeTarget(current_lanes);
ASSERT_TRUE(lane_change_lane.has_value());
ASSERT_EQ(lane_change_lane.value().id(), 9598ul);
}
}
} // namespace autoware::route_handler::test
Loading
Loading