Skip to content

Commit

Permalink
fix(mission_planner): improve condition to check if the goal is withi…
Browse files Browse the repository at this point in the history 
…n the lane (autowarefoundation#8710)

Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem
maxime-clem authored Sep 3, 2024
1 parent a5cc3ba commit e90a127
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 138 deletions.
108 changes: 54 additions & 54 deletions planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "utility_functions.hpp"

#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/route_handler/route_handler.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/math/normalization.hpp>
#include <autoware/universe_utils/math/unit_conversion.hpp>
Expand All @@ -27,10 +28,13 @@
#include <autoware_lanelet2_extension/visualization/visualization.hpp>
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>

#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>

#include <lanelet2_core/Forward.h>
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingCost.h>
#include <tf2/utils.h>

#include <limits>
Expand Down Expand Up @@ -216,7 +220,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
}

visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
autoware::universe_utils::LinearRing2d goal_footprint) const
autoware::universe_utils::LinearRing2d goal_footprint)
{
visualization_msgs::msg::MarkerArray msg;
auto marker = autoware::universe_utils::createDefaultMarker(
Expand Down Expand Up @@ -244,52 +248,58 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
return msg;
}

bool DefaultPlanner::check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet,
const lanelet::ConstLanelet & combined_prev_lanelet,
const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length,
const double search_margin)
lanelet::ConstLanelets next_lanelets_up_to(
const lanelet::ConstLanelet & start_lanelet, const double up_to_distance,
const route_handler::RouteHandler & route_handler)
{
// check if goal footprint is in current lane
if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
return true;
lanelet::ConstLanelets lanelets;
if (up_to_distance <= 0.0) {
return lanelets;
}
const auto following = route_handler_.getNextLanelets(current_lanelet);
// check if goal footprint is in between many lanelets in depth-first search manner
for (const auto & next_lane : following) {
next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
lanelet::ConstLanelets lanelets;
lanelets.push_back(combined_prev_lanelet);
for (const auto & next_lane : route_handler.getNextLanelets(start_lanelet)) {
lanelets.push_back(next_lane);
lanelet::ConstLanelet combined_lanelets =
combine_lanelets_with_shoulder(lanelets, route_handler_);

// if next lanelet length is longer than vehicle longitudinal offset
if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
// and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the
// query
if (boost::geometry::within(goal_footprint, combined_lanelets.polygon2d().basicPolygon())) {
return true;
}
// if not, iteration continues to next next_lane, and this subtree is terminated
} else { // if next lanelet length is shorter than vehicle longitudinal offset, check the
// overlap with the polygon including the next_lane(s) until the additional lanes get
// longer than ego vehicle length
if (!check_goal_footprint_inside_lanes(
next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
continue;
} else {
return true;
}
const auto next_lanelets = next_lanelets_up_to(
next_lane, up_to_distance - lanelet::geometry::length2d(next_lane), route_handler);
lanelets.insert(lanelets.end(), next_lanelets.begin(), next_lanelets.end());
}
return lanelets;
}

bool DefaultPlanner::check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
const universe_utils::Polygon2d & goal_footprint) const
{
universe_utils::MultiPolygon2d ego_lanes;
universe_utils::Polygon2d poly;
for (const auto & ll : path_lanelets) {
const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll);
if (left_shoulder) {
boost::geometry::convert(left_shoulder->polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}
const auto right_shoulder = route_handler_.getRightShoulderLanelet(ll);
if (right_shoulder) {
boost::geometry::convert(right_shoulder->polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}
boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}
return false;
const auto next_lanelets =
next_lanelets_up_to(current_lanelet, vehicle_info_.max_longitudinal_offset_m, route_handler_);
for (const auto & ll : next_lanelets) {
boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}

// check if goal footprint is in the ego lane
universe_utils::MultiPolygon2d difference;
boost::geometry::difference(goal_footprint, ego_lanes, difference);
return boost::geometry::is_empty(difference);
}

bool DefaultPlanner::is_goal_valid(
const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets)
const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets)
{
const auto logger = node_->get_logger();

Expand Down Expand Up @@ -337,16 +347,10 @@ bool DefaultPlanner::is_goal_valid(
pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint));
const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint);

double next_lane_length = 0.0;
// combine calculated route lanelets
const lanelet::ConstLanelet combined_prev_lanelet =
combine_lanelets_with_shoulder(path_lanelets, route_handler_);

// check if goal footprint exceeds lane when the goal isn't in parking_lot
if (
param_.check_footprint_inside_lanes &&
!check_goal_footprint_inside_lanes(
closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
!check_goal_footprint_inside_lanes(closest_lanelet, path_lanelets, polygon_footprint) &&
!is_in_parking_lot(
lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()),
lanelet::utils::conversion::toLaneletPoint(goal.position))) {
Expand Down Expand Up @@ -375,11 +379,7 @@ bool DefaultPlanner::is_goal_valid(
// check if goal is in parking lot
const auto parking_lots =
lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr());
if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) {
return true;
}

return false;
return is_in_parking_lot(parking_lots, goal_lanelet_pt);
}

PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
Expand Down Expand Up @@ -429,7 +429,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
return route_msg;
}

if (route_handler_.isRouteLooped(route_sections)) {
if (route_handler::RouteHandler::isRouteLooped(route_sections)) {
RCLCPP_WARN(logger, "Loop detected within route!");
return route_msg;
}
Expand Down
22 changes: 10 additions & 12 deletions planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>

#include <memory>
#include <vector>

namespace autoware::mission_planner::lanelet2
Expand All @@ -44,17 +43,17 @@ struct DefaultPlannerParameters
class DefaultPlanner : public mission_planner::PlannerPlugin
{
public:
DefaultPlanner() : is_graph_ready_(false), route_handler_(), param_(), node_(nullptr) {}
DefaultPlanner() : vehicle_info_(), is_graph_ready_(false), param_(), node_(nullptr) {}

void initialize(rclcpp::Node * node) override;
void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override;
bool ready() const override;
[[nodiscard]] bool ready() const override;
LaneletRoute plan(const RoutePoints & points) override;
void updateRoute(const PlannerPlugin::LaneletRoute & route) override;
void clearRoute() override;
MarkerArray visualize(const LaneletRoute & route) const override;
MarkerArray visualize_debug_footprint(
autoware::universe_utils::LinearRing2d goal_footprint) const;
[[nodiscard]] MarkerArray visualize(const LaneletRoute & route) const override;
[[nodiscard]] static MarkerArray visualize_debug_footprint(
autoware::universe_utils::LinearRing2d goal_footprint);
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

private:
Expand Down Expand Up @@ -85,17 +84,16 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
* @param next_lane_length the accumulated total length from the start lanelet of the search to
* the lanelet of current goal query
*/
bool check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet,
const lanelet::ConstLanelet & combined_prev_lanelet,
const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length,
const double search_margin = 2.0);
[[nodiscard]] bool check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
const universe_utils::Polygon2d & goal_footprint) const;

/**
* @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the
* footprint around the goal does not overlap the lanes
*/
bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
bool is_goal_valid(
const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets);

/**
* @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of
Expand Down
60 changes: 0 additions & 60 deletions planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@

#include <lanelet2_core/geometry/Lanelet.h>

#include <unordered_set>
#include <utility>

autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon(
autoware::universe_utils::LinearRing2d footprint)
{
Expand All @@ -41,63 +38,6 @@ void insert_marker_array(
a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
}

lanelet::ConstLanelet combine_lanelets_with_shoulder(
const lanelet::ConstLanelets & lanelets,
const autoware::route_handler::RouteHandler & route_handler)
{
lanelet::Points3d lefts;
lanelet::Points3d rights;
lanelet::Points3d centers;
std::vector<uint64_t> left_bound_ids;
std::vector<uint64_t> right_bound_ids;

for (const auto & llt : lanelets) {
if (llt.id() != lanelet::InvalId) {
left_bound_ids.push_back(llt.leftBound().id());
right_bound_ids.push_back(llt.rightBound().id());
}
}

// lambda to add bound to target_bound
const auto add_bound = [](const auto & bound, auto & target_bound) {
std::transform(
bound.begin(), bound.end(), std::back_inserter(target_bound),
[](const auto & pt) { return lanelet::Point3d(pt); });
};
for (const auto & llt : lanelets) {
// check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist
const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt);
if (left_shared_shoulder) {
// if exist, add left bound of SHOULDER lanelet to lefts
add_bound(left_shared_shoulder->leftBound(), lefts);
} else if (
// if not exist, add left bound of lanelet to lefts
// if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
// then its left bound constitutes the left boundary of the entire merged lanelet
std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
add_bound(llt.leftBound(), lefts);
}

// check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist
const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt);
if (right_shared_shoulder) {
// if exist, add right bound of SHOULDER lanelet to rights
add_bound(right_shared_shoulder->rightBound(), rights);
} else if (
// if not exist, add right bound of lanelet to rights
// if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,
// then its right bound constitutes the right boundary of the entire merged lanelet
std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
add_bound(llt.rightBound(), rights);
}
}

const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
return std::move(combined_lanelet);
}

std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
{
std::vector<geometry_msgs::msg::Point> centerline_points;
Expand Down
12 changes: 0 additions & 12 deletions planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,6 @@
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/primitives/LaneletSequence.h>

#include <string>
#include <unordered_set>
#include <vector>

template <typename T>
Expand All @@ -47,16 +45,6 @@ autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon(
void insert_marker_array(
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);

/**
* @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost
* bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively
* @param lanelets topologically sorted sequence of lanelets
* @param route_handler route handler to query the lanelet map
*/
lanelet::ConstLanelet combine_lanelets_with_shoulder(
const lanelet::ConstLanelets & lanelets,
const autoware::route_handler::RouteHandler & route_handler);

std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
geometry_msgs::msg::Pose convertBasicPoint3dToPose(
const lanelet::BasicPoint3d & point, const double lane_yaw);
Expand Down

0 comments on commit e90a127

Please sign in to comment.