Skip to content

Commit

Permalink
feat(goal_planner): use neighboring lane of pull over lane to check g…
Browse files Browse the repository at this point in the history
…oal footprint (autowarefoundation#8716)

move to utils and add tests

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
kosuke55 authored and AkariNakano821 committed Nov 14, 2024
1 parent a82b1cb commit f716783
Show file tree
Hide file tree
Showing 4 changed files with 286 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,29 @@ MarkerArray createLaneletPolygonMarkerArray(
MarkerArray createNumObjectsToAvoidTextsMarkerArray(
const GoalCandidates & goal_candidates, std::string && ns,
const std_msgs::msg::ColorRGBA & color);
std::string makePathPriorityDebugMessage(
const std::vector<size_t> & sorted_path_indices,
const std::vector<PullOverPath> & pull_over_path_candidates,
const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
const std::map<size_t, double> & path_id_to_rough_margin_map,
const std::function<bool(const PullOverPath &)> & isSoftMargin,
const std::function<bool(const PullOverPath &)> & isHighCurvature);
/**
* @brief combine two points
* @param points lane points
* @param points_next next lane points
* @return combined points
*/
lanelet::Points3d combineLanePoints(
const lanelet::Points3d & points, const lanelet::Points3d & points_next);
/** @brief Create a lanelet that represents the departure check area.
* @param [in] pull_over_lanes Lanelets that the vehicle will pull over to.
* @param [in] route_handler RouteHandler object.
* @return Lanelet that goal footprints should be inside.
*/
lanelet::Lanelet createDepartureCheckLanelet(
const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
const bool left_side_parking);
} // namespace autoware::behavior_path_planner::goal_planner_utils

#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -115,11 +115,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
*route_handler, left_side_parking_, parameters_.backward_goal_search_length,
parameters_.forward_goal_search_length);
auto lanes = utils::getExtendedCurrentLanes(
planner_data, backward_length, forward_length,
/*forward_only_in_route*/ false);
lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());

const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
pull_over_lanes, *route_handler, left_side_parking_);
const auto goal_arc_coords =
lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_);
const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
Expand Down Expand Up @@ -177,7 +174,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
break;
}

if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) {
if (!boost::geometry::within(
transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) {
continue;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -576,4 +576,96 @@ std::vector<Polygon2d> createPathFootPrints(
return footprints;
}

std::string makePathPriorityDebugMessage(
const std::vector<size_t> & sorted_path_indices,
const std::vector<PullOverPath> & pull_over_path_candidates,
const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
const std::map<size_t, double> & path_id_to_rough_margin_map,
const std::function<bool(const PullOverPath &)> & isSoftMargin,
const std::function<bool(const PullOverPath &)> & isHighCurvature)
{
std::stringstream ss;

// Unsafe goal and its priority are not visible as debug marker in rviz,
// so exclude unsafe goal from goal_priority
std::map<size_t, int> goal_id_and_priority;
for (size_t i = 0; i < goal_candidates.size(); ++i) {
goal_id_and_priority[goal_candidates[i].id] = goal_candidates[i].is_safe ? i : -1;
}

ss << "\n---------------------- path priority ----------------------\n";
for (size_t i = 0; i < sorted_path_indices.size(); ++i) {
const auto & path = pull_over_path_candidates[sorted_path_indices[i]];
// goal_index is same to goal priority including unsafe goal
const int goal_index = static_cast<int>(goal_id_to_index.at(path.goal_id));
const bool is_safe_goal = goal_candidates[goal_index].is_safe;
const int goal_priority = goal_id_and_priority[path.goal_id];

ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type)
<< ", path_id: " << path.id << ", goal_id: " << path.goal_id
<< ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe")
<< ", margin: " << path_id_to_rough_margin_map.at(path.id)
<< (isSoftMargin(path) ? " (soft)" : " (hard)")
<< ", curvature: " << path.getParkingPathMaxCurvature()
<< (isHighCurvature(path) ? " (high)" : " (low)") << "\n";
}
ss << "-----------------------------------------------------------\n";
return ss.str();
}

lanelet::Points3d combineLanePoints(
const lanelet::Points3d & points, const lanelet::Points3d & points_next)
{
lanelet::Points3d combined_points;
std::unordered_set<lanelet::Id> point_ids;
for (const auto & point : points) {
if (point_ids.insert(point.id()).second) {
combined_points.push_back(point);
}
}
for (const auto & point : points_next) {
if (point_ids.insert(point.id()).second) {
combined_points.push_back(point);
}
}
return combined_points;
}

lanelet::Lanelet createDepartureCheckLanelet(
const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
const bool left_side_parking)
{
const auto getBoundPoints =
[&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d {
lanelet::Points3d points;
const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound())
: (is_outer ? lane.rightBound() : lane.leftBound());
for (const auto & pt : bound) {
points.push_back(lanelet::Point3d(pt));
}
return points;
};

const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet {
return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true)
: route_handler.getMostLeftLanelet(lane, false, true);
};

lanelet::Points3d outer_bound_points{};
lanelet::Points3d inner_bound_points{};
for (const auto & lane : pull_over_lanes) {
const auto current_outer_bound_points = getBoundPoints(lane, true);
const auto most_inner_lane = getMostInnerLane(lane);
const auto current_inner_bound_points = getBoundPoints(most_inner_lane, false);
outer_bound_points = combineLanePoints(outer_bound_points, current_outer_bound_points);
inner_bound_points = combineLanePoints(inner_bound_points, current_inner_bound_points);
}

const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points);
const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points);
return lanelet::Lanelet(
lanelet::InvalId, left_side_parking ? outer_linestring : inner_linestring,
left_side_parking ? inner_linestring : outer_linestring);
}

} // namespace autoware::behavior_path_planner::goal_planner_utils
Original file line number Diff line number Diff line change
@@ -0,0 +1,167 @@
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware/behavior_path_goal_planner_module/util.hpp>
#include <autoware/route_handler/route_handler.hpp>
#include <autoware_lanelet2_extension/utility/query.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>

#include <gtest/gtest.h>

class TestUtilWithMap : public ::testing::Test
{
protected:
void SetUp() override
{
rclcpp::init(0, nullptr);
// parameters
auto node_options = rclcpp::NodeOptions{};
node_options.arguments(std::vector<std::string>{
"--ros-args", "--params-file",
ament_index_cpp::get_package_share_directory("autoware_test_utils") +
"/config/test_vehicle_info.param.yaml"});
auto node = rclcpp::Node::make_shared("test", node_options);
vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo();

// lanelet map
const std::string shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map(
"autoware_behavior_path_planner_common", "road_shoulder/lanelet2_map.osm");
const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5);

// load map
route_handler = std::make_shared<autoware::route_handler::RouteHandler>(map_bin_msg);
}

void TearDown() override { rclcpp::shutdown(); }

public:
std::shared_ptr<autoware::route_handler::RouteHandler> route_handler;
autoware::vehicle_info_utils::VehicleInfo vehicle_info;
};

TEST_F(TestUtilWithMap, getBusStopAreaPolygons)
{
const auto lanes = lanelet::utils::query::laneletLayer(route_handler->getLaneletMapPtr());
const auto shoulder_lanes = lanelet::utils::query::shoulderLanelets(lanes);
const auto bus_stop_area_polygons =
autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons(shoulder_lanes);
EXPECT_EQ(bus_stop_area_polygons.size(), 2);
}

TEST_F(TestUtilWithMap, isWithinAreas)
{
const auto lanes = lanelet::utils::query::laneletLayer(route_handler->getLaneletMapPtr());
const auto shoulder_lanes = lanelet::utils::query::shoulderLanelets(lanes);
const auto bus_stop_area_polygons =
autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons(shoulder_lanes);

const auto footprint = vehicle_info.createFootprint();
const geometry_msgs::msg::Pose baselink_pose =
geometry_msgs::build<geometry_msgs::msg::Pose>()
.position(
geometry_msgs::build<geometry_msgs::msg::Point>().x(273.102814).y(402.194794).z(0.0))
.orientation(
geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0.0).y(0.0).z(0.707390).w(
0.706824));
const auto baselink_footprint = autoware::universe_utils::transformVector(
footprint, autoware::universe_utils::pose2transform(baselink_pose));
EXPECT_EQ(
autoware::behavior_path_planner::goal_planner_utils::isWithinAreas(
baselink_footprint, bus_stop_area_polygons),
true);
}

TEST_F(TestUtilWithMap, combineLanePoints)
{
// 1) combine points with no duplicate IDs
{
lanelet::Points3d points{
{lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}};
lanelet::Points3d points_next{
{lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0), lanelet::Point3d(6, 0, 0)}};

const auto combined_points =
autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next);
EXPECT_EQ(combined_points.size(), 6);
}

// 2) combine points with duplicate IDs
{
lanelet::Points3d points{
{lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}};
lanelet::Points3d points_next{
{lanelet::Point3d(3, 0, 0), lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0)}};

const auto combined_points =
autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next);
EXPECT_EQ(combined_points.size(), 5);
}
}

TEST_F(TestUtilWithMap, createDepartureCheckLanelet)
{
const auto lanelet_map_ptr = route_handler->getLaneletMapPtr();

const geometry_msgs::msg::Pose goal_pose =
geometry_msgs::build<geometry_msgs::msg::Pose>()
.position(geometry_msgs::build<geometry_msgs::msg::Point>()
.x(433.42254638671875)
.y(465.3381652832031)
.z(0.0))
.orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>()
.x(0.0)
.y(0.0)
.z(0.306785474523741)
.w(0.9517786888879384));

// 1) get target shoulder lane and check it's lane id
const auto target_shoulder_lane = route_handler->getPullOverTarget(goal_pose);
EXPECT_EQ(target_shoulder_lane.has_value(), true);
EXPECT_EQ(target_shoulder_lane.value().id(), 18391);

// 2) get shoulder lane sequence
const auto target_shoulder_lanes =
route_handler->getShoulderLaneletSequence(target_shoulder_lane.value(), goal_pose);
EXPECT_EQ(target_shoulder_lanes.size(), 3);

// 3) check if the right bound of the departure check lane extended to the right end matches the
// right bound of the rightmost lanelet
const auto to_points3d = [](const lanelet::ConstLineString3d & bound) {
lanelet::Points3d points;
for (const auto & pt : bound) {
points.push_back(lanelet::Point3d(pt));
}
return points;
};

const auto departure_check_lane =
autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet(
target_shoulder_lanes, *route_handler, true);
const auto departure_check_lane_right_bound_points =
to_points3d(departure_check_lane.rightBound());

const std::vector<lanelet::Id> most_right_lanelet_ids = {18381, 18383, 18388};
lanelet::Points3d right_bound_points;
for (const auto & id : most_right_lanelet_ids) {
const auto lanelet = lanelet_map_ptr->laneletLayer.get(id);
right_bound_points = autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(
right_bound_points, to_points3d(lanelet.rightBound()));
}

EXPECT_EQ(departure_check_lane_right_bound_points.size(), right_bound_points.size());
for (size_t i = 0; i < departure_check_lane_right_bound_points.size(); ++i) {
EXPECT_EQ(departure_check_lane_right_bound_points.at(i).id(), right_bound_points.at(i).id());
}
}

0 comments on commit f716783

Please sign in to comment.