diff --git a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp b/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp index 78db6a5072a50..c992aae1e77d9 100644 --- a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp +++ b/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp @@ -13,22 +13,33 @@ // limitations under the License. #include <../src/lanelet2_plugins/default_planner.hpp> +#include #include +#include #include #include #include #include +#include #include #include #include #include +#include -struct DefaultPlanner : protected autoware::mission_planner::lanelet2::DefaultPlanner +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::Pose; +using RoutePoints = std::vector; + +// inherit DefaultPlanner to access protected methods and make wrapper to private methods +struct DefaultPlanner : public autoware::mission_planner::lanelet2::DefaultPlanner { - void set_front_offset(const double offset) { vehicle_info_.max_longitudinal_offset_m = offset; } - void set_dummy_map() { route_handler_.setMap(autoware::test_utils::makeMapBinMsg()); } + // todo(someone): create tests with various kinds of maps + void set_default_test_map() { route_handler_.setMap(autoware::test_utils::makeMapBinMsg()); } [[nodiscard]] bool check_goal_inside_lanes( const lanelet::ConstLanelet & closest_lanelet_to_goal, const lanelet::ConstLanelets & path_lanelets, @@ -37,15 +48,61 @@ struct DefaultPlanner : protected autoware::mission_planner::lanelet2::DefaultPl return check_goal_footprint_inside_lanes( closest_lanelet_to_goal, path_lanelets, goal_footprint); } + bool is_goal_valid_wrapper( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets) + { + return is_goal_valid(goal, path_lanelets); + } + + lanelet::ConstLanelets get_lanelets_from_ids(const std::vector & ids) + { + const auto lanelet_map_ptr = route_handler_.getLaneletMapPtr(); + + lanelet::ConstLanelets lanelets; + + for (const auto & id : ids) { + lanelets.push_back(lanelet_map_ptr->laneletLayer.get(id)); + } + return lanelets; + } +}; + +class DefaultPlannerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto mission_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_mission_planner"); + options.arguments( + {"--ros-args", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + mission_planner_dir + "/config/mission_planner.param.yaml"}); + // NOTE: vehicle width and length set by test_vehicle_info.param.yaml are as follows + // vehicle_width: 1.83, vehicle_length: 4.77 + + node_ = std::make_shared("test_node", options); + planner_.initialize(node_.get()); + } + + ~DefaultPlannerTest() override { rclcpp::shutdown(); } + + std::shared_ptr node_; + + DefaultPlanner planner_; }; -TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) +TEST_F(DefaultPlannerTest, checkGoalInsideLane) { // Test with dummy map such that only the lanelets provided as inputs are used for the ego lane - DefaultPlanner planner; - planner.set_dummy_map(); + planner_.set_default_test_map(); // vehicle max longitudinal offset is used to retrieve additional lanelets from the map - planner.set_front_offset(0.0); lanelet::LineString3d left_bound; lanelet::LineString3d right_bound; left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); @@ -63,7 +120,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(0.5, 0.5); goal_footprint.outer().emplace_back(0.5, 0); goal_footprint.outer().emplace_back(0, 0); - EXPECT_TRUE(planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); + EXPECT_TRUE(planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); // the footprint touches the border of the lanelet goal_footprint.clear(); @@ -72,7 +129,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1, 1); goal_footprint.outer().emplace_back(1, 0); goal_footprint.outer().emplace_back(0, 0); - EXPECT_TRUE(planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); + EXPECT_TRUE(planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); // add lanelets such that the footprint touches the linestring shared by the combined lanelets lanelet::LineString3d next_left_bound; @@ -83,7 +140,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) next_right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 2, 1}); lanelet::ConstLanelet next_lanelet{lanelet::InvalId, next_left_bound, next_right_bound}; EXPECT_TRUE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is inside the other lanelet goal_footprint.clear(); @@ -93,7 +150,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.6, -0.5); goal_footprint.outer().emplace_back(1.1, -0.5); EXPECT_TRUE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is completely outside of the lanelets goal_footprint.clear(); @@ -103,7 +160,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.6, 1.5); goal_footprint.outer().emplace_back(1.1, 1.5); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is outside of the lanelets but touches an edge goal_footprint.clear(); @@ -113,7 +170,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.6, 1.0); goal_footprint.outer().emplace_back(1.1, 1.0); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is outside of the lanelets but share a point goal_footprint.clear(); @@ -123,7 +180,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(3.0, 1.0); goal_footprint.outer().emplace_back(2.0, 1.0); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // ego footprint that overlaps both lanelets goal_footprint.clear(); @@ -133,7 +190,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.5, -0.5); goal_footprint.outer().emplace_back(-0.5, -0.5); EXPECT_TRUE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // ego footprint that goes further than the next lanelet goal_footprint.clear(); @@ -143,5 +200,215 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(2.5, -0.5); goal_footprint.outer().emplace_back(-0.5, -0.5); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); +} + +TEST_F(DefaultPlannerTest, isValidGoal) +{ + planner_.set_default_test_map(); + + std::vector path_lanelet_ids = {9102, 9540, 9546, 9178, 52, 124}; + const auto path_lanelets = planner_.get_lanelets_from_ids(path_lanelet_ids); + + const double yaw_threshold = 0.872665; // 50 degrees + + /** + * 1) goal pose within the lanelet + */ + // goal pose within the lanelet + Pose goal_pose; + goal_pose.position.x = 3810.24951171875; + goal_pose.position.y = 73769.2578125; + goal_pose.position.z = 0.0; + goal_pose.orientation.x = 0.0; + goal_pose.orientation.y = 0.0; + goal_pose.orientation.z = 0.23908402523702438; + goal_pose.orientation.w = 0.9709988820160721; + const double yaw = tf2::getYaw(goal_pose.orientation); + EXPECT_TRUE(planner_.is_goal_valid_wrapper(goal_pose, path_lanelets)); + + // move 1m to the right to make the goal outside of the lane + Pose right_offset_goal_pose = calcOffsetPose(goal_pose, 0.0, 1.0, 0.0); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(right_offset_goal_pose, path_lanelets)); + + // move 1m to the left to make the goal outside of the lane + Pose left_offset_goal_pose = calcOffsetPose(goal_pose, 0.0, -1.0, 0.0); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(left_offset_goal_pose, path_lanelets)); + + // rotate to the right + Pose right_rotated_goal_pose = goal_pose; + right_rotated_goal_pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw + yaw_threshold); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(right_rotated_goal_pose, path_lanelets)); + + // rotate to the left + Pose left_rotated_goal_pose = goal_pose; + left_rotated_goal_pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw - yaw_threshold); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(left_rotated_goal_pose, path_lanelets)); + + /** + * 2) goal pose on the road shoulder + */ + std::vector path_lanelet_to_road_shoulder_ids = {9102, 9540, 9546}; + lanelet::Id target_road_shoulder_id = 10304; // left road shoulder of 9546 + const auto path_lanelets_to_road_shoulder = + planner_.get_lanelets_from_ids(path_lanelet_to_road_shoulder_ids); + const auto target_road_shoulder = + planner_.get_lanelets_from_ids({target_road_shoulder_id}).front(); + + Pose goal_pose_on_road_shoulder; + goal_pose_on_road_shoulder.position.x = 3742.3825513144147; + goal_pose_on_road_shoulder.position.y = 73737.75783925217; + goal_pose_on_road_shoulder.position.z = 0.0; + goal_pose_on_road_shoulder.orientation.x = 0.0; + goal_pose_on_road_shoulder.orientation.y = 0.0; + goal_pose_on_road_shoulder.orientation.z = 0.23721495449671745; + goal_pose_on_road_shoulder.orientation.w = 0.9714571865826721; + EXPECT_TRUE( + planner_.is_goal_valid_wrapper(goal_pose_on_road_shoulder, path_lanelets_to_road_shoulder)); + + // put goal on the left bound of the road shoulder + // if the goal is on the road shoulder, the footprint can be outside of the lane, and only the + // angle is checked + const auto left_bound = target_road_shoulder.leftBound(); + Pose goal_pose_on_road_shoulder_left_bound = goal_pose_on_road_shoulder; + goal_pose_on_road_shoulder_left_bound.position.x = left_bound.front().x(); + goal_pose_on_road_shoulder_left_bound.position.y = left_bound.front().y(); + EXPECT_TRUE(planner_.is_goal_valid_wrapper( + goal_pose_on_road_shoulder_left_bound, path_lanelets_to_road_shoulder)); + + // move goal pose outside of the road shoulder + Pose goal_pose_outside_road_shoulder = + calcOffsetPose(goal_pose_on_road_shoulder_left_bound, 0.0, 0.1, 0.0); + EXPECT_FALSE(planner_.is_goal_valid_wrapper( + goal_pose_outside_road_shoulder, path_lanelets_to_road_shoulder)); + + // rotate goal to the right + Pose right_rotated_goal_pose_on_road_shoulder = goal_pose_on_road_shoulder; + right_rotated_goal_pose_on_road_shoulder.orientation = + createQuaternionFromRPY(0.0, 0.0, yaw + yaw_threshold); + EXPECT_FALSE(planner_.is_goal_valid_wrapper( + right_rotated_goal_pose_on_road_shoulder, path_lanelets_to_road_shoulder)); + + /** + * todo(someone): add more test for parking area + */ +} + +TEST_F(DefaultPlannerTest, plan) +{ + planner_.set_default_test_map(); + + /** + * 1) goal pose within the lanelet + */ + RoutePoints route_points; + Pose start_pose; + start_pose.position.x = 3717.239501953125; + start_pose.position.y = 73720.84375; + start_pose.position.z = 0.0; + start_pose.orientation.x = 0.00012620055018808463; + start_pose.orientation.y = -0.0005077247816171834; + start_pose.orientation.z = 0.2412209576008544; + + Pose goal_pose; + goal_pose.position.x = 3810.24951171875; + goal_pose.position.y = 73769.2578125; + goal_pose.position.z = 0.0; + goal_pose.orientation.x = 0.0; + goal_pose.orientation.y = 0.0; + goal_pose.orientation.z = 0.23908402523702438; + goal_pose.orientation.w = 0.9709988820160721; + + route_points.push_back(start_pose); + route_points.push_back(goal_pose); + const auto route = planner_.plan(route_points); + + EXPECT_EQ(route.start_pose.position.x, start_pose.position.x); + EXPECT_EQ(route.start_pose.position.y, start_pose.position.y); + EXPECT_EQ(route.goal_pose.position.x, goal_pose.position.x); + EXPECT_EQ(route.goal_pose.position.y, goal_pose.position.y); + std::vector path_lanelet_ids = {9102, 9540, 9546, 9178, 52, 124}; + EXPECT_EQ(route.segments.size(), path_lanelet_ids.size()); + for (size_t i = 0; i < route.segments.size(); i++) { + EXPECT_EQ(route.segments[i].preferred_primitive.id, path_lanelet_ids[i]); + const auto & primitives = route.segments[i].primitives; + EXPECT_TRUE(std::find_if(primitives.begin(), primitives.end(), [&](const auto & primitive) { + return primitive.id == path_lanelet_ids[i]; + }) != primitives.end()); + } + + /** + * 2) goal pose on the road shoulder + */ + Pose goal_pose_on_road_shoulder; + goal_pose_on_road_shoulder.position.x = 3742.3825513144147; + goal_pose_on_road_shoulder.position.y = 73737.75783925217; + goal_pose_on_road_shoulder.position.z = 0.0; + goal_pose_on_road_shoulder.orientation.x = 0.0; + goal_pose_on_road_shoulder.orientation.y = 0.0; + goal_pose_on_road_shoulder.orientation.z = 0.23721495449671745; + goal_pose_on_road_shoulder.orientation.w = 0.9714571865826721; + + RoutePoints route_points_to_road_shoulder; + route_points_to_road_shoulder.push_back(start_pose); + route_points_to_road_shoulder.push_back(goal_pose_on_road_shoulder); + const auto route_to_road_shoulder = planner_.plan(route_points_to_road_shoulder); + + EXPECT_EQ(route_to_road_shoulder.start_pose.position.x, start_pose.position.x); + EXPECT_EQ(route_to_road_shoulder.start_pose.position.y, start_pose.position.y); + EXPECT_EQ(route_to_road_shoulder.goal_pose.position.x, goal_pose_on_road_shoulder.position.x); + EXPECT_EQ(route_to_road_shoulder.goal_pose.position.y, goal_pose_on_road_shoulder.position.y); + std::vector path_lanelet_to_road_shoulder_ids = {9102, 9540, 9546}; + EXPECT_EQ(route_to_road_shoulder.segments.size(), path_lanelet_to_road_shoulder_ids.size()); + for (size_t i = 0; i < route_to_road_shoulder.segments.size(); i++) { + EXPECT_EQ( + route_to_road_shoulder.segments[i].preferred_primitive.id, + path_lanelet_to_road_shoulder_ids[i]); + const auto & primitives = route_to_road_shoulder.segments[i].primitives; + EXPECT_TRUE(std::find_if(primitives.begin(), primitives.end(), [&](const auto & primitive) { + return primitive.id == path_lanelet_to_road_shoulder_ids[i]; + }) != primitives.end()); + } +} + +// `visualize` function is used for user too, so it is more important than debug functions +TEST_F(DefaultPlannerTest, visualize) +{ + DefaultPlanner planner; + planner_.set_default_test_map(); + const LaneletRoute route = autoware::test_utils::makeBehaviorNormalRoute(); + const auto marker_array = planner_.visualize(route); + // clang-format off + const std::vector expected_ns = { + // 1) lanelet::visualization::laneletsBoundaryAsMarkerArray + // center line is not set in the test lanelets, + // so "center_lane_line" and "center_line_arrows" are not visualized + "left_lane_bound", "right_lane_bound", "lane_start_bound", + // 2) lanelet::visualization::laneletsAsMarkerArray + "route_lanelets", + // 3) lanelet::visualization::laneletsAsMarkerArray + "goal_lanelets"}; + // clang-format on + for (const auto & marker : marker_array.markers) { + EXPECT_TRUE(std::find(expected_ns.begin(), expected_ns.end(), marker.ns) != expected_ns.end()); + } +} + +TEST_F(DefaultPlannerTest, visualizeDebugFootrprint) +{ + DefaultPlanner planner; + planner_.set_default_test_map(); + + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({1.0, 1.0}); + + const auto marker_array = planner_.visualize_debug_footprint(footprint); + EXPECT_EQ(marker_array.markers.size(), 1); + EXPECT_EQ(marker_array.markers[0].points.size(), 7); }