feat(route_handler): prioritize start-goal footprint

Signed-off-by: Mehmet Dogru <[email protected]>

planning/mission_planner/config/mission_planner.param.yaml

@@ -4,9 +4,12 @@
     arrival_check_angle_deg: 45.0
     arrival_check_distance: 1.0
     arrival_check_duration: 1.0
+    start_angle_threshold_deg: 90.0
     goal_angle_threshold_deg: 45.0
     enable_correct_goal_pose: false
     reroute_time_threshold: 10.0
     minimum_reroute_length: 30.0
     consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
     check_footprint_inside_lanes: true
+    prioritize_start_footprint: false # in case start checkpoint is inside multiple lanelet prioritize the one which start checkpoint footprint is within
+    prioritize_goal_footprint: false # in case goal checkpoint is inside multiple lanelet prioritize the one which goal checkpoint footprint is within

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -149,11 +149,14 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node)
     node_->create_publisher<MarkerArray>("debug/goal_footprint", durable_qos);
 
   vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
+  param_.start_angle_threshold_deg = node_->declare_parameter<double>("start_angle_threshold_deg");
   param_.goal_angle_threshold_deg = node_->declare_parameter<double>("goal_angle_threshold_deg");
   param_.enable_correct_goal_pose = node_->declare_parameter<bool>("enable_correct_goal_pose");
   param_.consider_no_drivable_lanes = node_->declare_parameter<bool>("consider_no_drivable_lanes");
   param_.check_footprint_inside_lanes =
     node_->declare_parameter<bool>("check_footprint_inside_lanes");
+  param_.prioritize_start_footprint = node_->declare_parameter<bool>("prioritize_start_footprint");
+  param_.prioritize_goal_footprint = node_->declare_parameter<bool>("prioritize_goal_footprint");
 }
 
 void DefaultPlanner::initialize(rclcpp::Node * node)
@@ -391,6 +394,12 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
 {
   const auto logger = node_->get_logger();
 
+  LaneletRoute route_msg;
+  if (points.size() < 2) {
+    RCLCPP_WARN_STREAM(logger, "Not enough points to plan a route!" << std::endl);
+    return route_msg;
+  }
+
   std::stringstream log_ss;
   for (const auto & point : points) {
     log_ss << "x: " << point.position.x << " "
@@ -400,16 +409,28 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
     logger, "start planning route with check points: " << std::endl
                                                        << log_ss.str());
 
-  LaneletRoute route_msg;
+  auto goal_pose = points.back();
+  if (param_.enable_correct_goal_pose) {
+    goal_pose = get_closest_centerline_pose(
+      lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_);
+  }
+
   RouteSections route_sections;
 
   lanelet::ConstLanelets all_route_lanelets;
   for (std::size_t i = 1; i < points.size(); i++) {
+    bool is_start_pose = i == 0;
+    bool is_goal_pose = i == (points.size() - 1);
+
     const auto start_check_point = points.at(i - 1);
-    const auto goal_check_point = points.at(i);
+    const auto goal_check_point = is_goal_pose ? goal_pose : points.at(i);
+
     lanelet::ConstLanelets path_lanelets;
     if (!route_handler_.planPathLaneletsBetweenCheckpoints(
-          start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) {
+          start_check_point, goal_check_point, &path_lanelets, is_start_pose, is_goal_pose,
+          &vehicle_info_, param_.start_angle_threshold_deg, param_.goal_angle_threshold_deg,
+          param_.prioritize_start_footprint, param_.prioritize_goal_footprint,
+          param_.consider_no_drivable_lanes)) {
       RCLCPP_WARN(logger, "Failed to plan route.");
       return route_msg;
     }
@@ -423,12 +444,6 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
   }
   route_handler_.setRouteLanelets(all_route_lanelets);
 
-  auto goal_pose = points.back();
-  if (param_.enable_correct_goal_pose) {
-    goal_pose = get_closest_centerline_pose(
-      lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_);
-  }
-
   if (!is_goal_valid(goal_pose, all_route_lanelets)) {
     RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose");
     return route_msg;

planning/mission_planner/src/lanelet2_plugins/default_planner.hpp

@@ -35,10 +35,13 @@ namespace mission_planner::lanelet2
 
 struct DefaultPlannerParameters
 {
+  double start_angle_threshold_deg;
   double goal_angle_threshold_deg;
   bool enable_correct_goal_pose;
   bool consider_no_drivable_lanes;
   bool check_footprint_inside_lanes;
+  bool prioritize_start_footprint;
+  bool prioritize_goal_footprint;
 };
 
 class DefaultPlanner : public mission_planner::PlannerPlugin

planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp

@@ -344,7 +344,7 @@ LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int &
 
   // Plan the path between checkpoints (start and goal poses)
   lanelet::ConstLanelets path_lanelets;
-  if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) {
+  if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets, true, true)) {
     return route_msg;
   }
 

planning/route_handler/include/route_handler/route_handler.hpp

@@ -16,6 +16,7 @@
 #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_
 
 #include <rclcpp/logger.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -81,7 +82,12 @@ class RouteHandler
   // for routing
   bool planPathLaneletsBetweenCheckpoints(
     const Pose & start_checkpoint, const Pose & goal_checkpoint,
-    lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const;
+    lanelet::ConstLanelets * path_lanelets, const bool is_start_pose, const bool is_goal_pose,
+    const vehicle_info_util::VehicleInfo * vehicle_info = nullptr,
+    const double start_angle_threshold_deg = std::numeric_limits<double>::max(),
+    const double goal_angle_threshold_deg = std::numeric_limits<double>::max(),
+    const bool prioritize_start_footprint = false, const bool prioritize_goal_footprint = false,
+    const bool consider_no_drivable_lanes = false) const;
   std::vector<LaneletSegment> createMapSegments(const lanelet::ConstLanelets & path_lanelets) const;
   static bool isRouteLooped(const RouteSections & route_sections);
 

planning/route_handler/package.xml

@@ -28,6 +28,7 @@
   <depend>rclcpp_components</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>vehicle_info_util</depend>
 
   <export>
     <build_type>ament_cmake</build_type>