Skip to content

Commit

Permalink
Merge pull request #1441 from tier4/cp-lane-change
Browse files Browse the repository at this point in the history
feat(lane_change): cherry-pick lane change commits to beta/v0.29.0
  • Loading branch information
Naophis authored Aug 16, 2024
2 parents d25170f + a9f63ec commit 87cdb1e
Show file tree
Hide file tree
Showing 21 changed files with 1,376 additions and 619 deletions.
30 changes: 25 additions & 5 deletions common/autoware_test_utils/src/autoware_test_utils.cpp
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::LaneletMapPtr loadMap(const std::string & lanelet2_filename)
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;
}

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

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;
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, getLaneletSequenceWhenOverlappingRoute)

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 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute)
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

0 comments on commit 87cdb1e

Please sign in to comment.