feat(mission_planner): check goal footprint (autowarefoundation#2088)

Signed-off-by: ismet atabay <ismet@leodrive.ai>

planning/mission_planner/README.md

@@ -14,12 +14,13 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 
 ### Parameters
 
-| Name                      | Type   | Description                       |
-| ------------------------- | ------ | --------------------------------- |
-| `map_frame`               | string | The frame name for map            |
-| `arrival_check_angle_deg` | double | Angle threshold for goal check    |
-| `arrival_check_distance`  | double | Distance threshold for goal check |
-| `arrival_check_duration`  | double | Duration threshold for goal check |
+| Name                      | Type   | Description                          |
+| ------------------------- | ------ | ------------------------------------ |
+| `map_frame`               | string | The frame name for map               |
+| `arrival_check_angle_deg` | double | Angle threshold for goal check       |
+| `arrival_check_distance`  | double | Distance threshold for goal check    |
+| `arrival_check_duration`  | double | Duration threshold for goal check    |
+| `goal_angle_threshold`    | double | Max goal pose angle for goal approve |
 
 ### Services
 
@@ -38,11 +39,12 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 
 ### Publications
 
-| Name                            | Type                                    | Description            |
-| ------------------------------- | --------------------------------------- | ---------------------- |
-| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state            |
-| `/planning/routing/route`       | autoware_planning_msgs/LaneletRoute     | route                  |
-| `debug/route_marker`            | visualization_msgs::msg::MarkerArray    | route marker for debug |
+| Name                            | Type                                    | Description              |
+| ------------------------------- | --------------------------------------- | ------------------------ |
+| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state              |
+| `/planning/routing/route`       | autoware_planning_msgs/LaneletRoute     | route                    |
+| `debug/route_marker`            | visualization_msgs::msg::MarkerArray    | route marker for debug   |
+| `debug/goal_footprint`          | visualization_msgs::msg::MarkerArray    | goal footprint for debug |
 
 ## Route section
 
@@ -57,6 +59,15 @@ The ROS message of route section contains following three elements for each rout
 - `preferred_primitive`: Preferred lane to follow towards the goal.
 - `primitives`: All neighbor lanes in the same direction including the preferred lane.
 
+## Goal Validation
+
+The mission planner has control mechanism to validate the given goal pose and create a route. If goal pose angle between goal pose lanelet and goal pose' yaw is greater than `goal_angle_threshold` parameter, the goal is rejected.
+Another control mechanism is the creation of a footprint of the goal pose according to the dimensions of the vehicle and checking whether this footprint is within the lanelets. If goal footprint exceeds lanelets, then the goal is rejected.
+
+At the image below, there are sample goal pose validation cases.
+
+![goal_footprints](./media/goal_footprints.svg)
+
 ## Implementation
 
 ### Mission Planner

planning/mission_planner/launch/mission_planner.launch.xml

@@ -8,6 +8,7 @@
     <param name="arrival_check_angle_deg" value="45.0"/>
     <param name="arrival_check_distance" value="1.0"/>
     <param name="arrival_check_duration" value="1.0"/>
+    <param name="goal_angle_threshold_deg" value="45.0"/>
     <remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
     <remap from="input/vector_map" to="$(var map_topic_name)"/>
     <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>

planning/mission_planner/package.xml

@@ -31,6 +31,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>vehicle_info_util</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -21,6 +21,7 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <lanelet2_extension/visualization/visualization.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
@@ -113,6 +114,13 @@ void DefaultPlanner::initialize(rclcpp::Node * node)
   map_subscriber_ = node_->create_subscription<HADMapBin>(
     "input/vector_map", rclcpp::QoS{10}.transient_local(),
     std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1));
+
+  const auto durable_qos = rclcpp::QoS(1).transient_local();
+  pub_goal_footprint_marker_ =
+    node_->create_publisher<MarkerArray>("debug/goal_footprint", durable_qos);
+
+  vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
+  param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg", 45.0);
 }
 
 void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg)
@@ -186,21 +194,118 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
   return route_marker_array;
 }
 
-bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const
+visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
+  tier4_autoware_utils::LinearRing2d goal_footprint) const
+{
+  visualization_msgs::msg::MarkerArray msg;
+  auto marker = tier4_autoware_utils::createDefaultMarker(
+    "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP,
+    tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0),
+    tier4_autoware_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0));
+  marker.lifetime = rclcpp::Duration::from_seconds(2.5);
+
+  marker.points.push_back(
+    tier4_autoware_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0));
+  marker.points.push_back(
+    tier4_autoware_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0));
+  marker.points.push_back(
+    tier4_autoware_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0));
+  marker.points.push_back(
+    tier4_autoware_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0));
+  marker.points.push_back(
+    tier4_autoware_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0));
+  marker.points.push_back(
+    tier4_autoware_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0));
+  marker.points.push_back(marker.points.front());
+
+  msg.markers.push_back(marker);
+
+  return msg;
+}
+
+bool DefaultPlanner::check_goal_footprint(
+  const lanelet::ConstLanelet & current_lanelet,
+  const lanelet::ConstLanelet & combined_prev_lanelet,
+  const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
+  const double search_margin)
+{
+  std::vector<tier4_autoware_utils::Point2d> points_intersection;
+
+  // check if goal footprint is in current lane
+  boost::geometry::intersection(
+    goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon(), points_intersection);
+  if (points_intersection.empty()) {
+    return true;
+  }
+  points_intersection.clear();
+
+  // check if goal footprint is in between many lanelets
+  for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) {
+    next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
+    lanelet::ConstLanelets lanelets;
+    lanelets.push_back(combined_prev_lanelet);
+    lanelets.push_back(next_lane);
+    lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets);
+
+    // if next lanelet length 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);
+      boost::geometry::intersection(
+        goal_footprint, combined_lanelets.polygon2d().basicPolygon(), points_intersection);
+      if (points_intersection.empty()) {
+        return true;
+      }
+      points_intersection.clear();
+    } else {  // if next lanelet length shorter than vehicle longitudinal offset -> recursive call
+      if (!check_goal_footprint(next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
+        next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
+        continue;
+      } else {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+bool DefaultPlanner::is_goal_valid(
+  const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets)
 {
+  const auto logger = node_->get_logger();
   lanelet::Lanelet closest_lanelet;
   if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) {
     return false;
   }
+
+  const auto local_vehicle_footprint = vehicle_info_.createFootprint();
+  tier4_autoware_utils::LinearRing2d goal_footprint =
+    transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal));
+  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
+  lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets);
+
+  // check if goal footprint exceeds lane when the goal isn't in parking_lot
+  if (
+    !check_goal_footprint(
+      closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
+    !is_in_parking_lot(
+      lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),
+      lanelet::utils::conversion::toLaneletPoint(goal.position))) {
+    RCLCPP_WARN(logger, "Goal's footprint exceeds lane!");
+    return false;
+  }
+
   const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);
 
   if (is_in_lane(closest_lanelet, goal_lanelet_pt)) {
     const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position);
     const auto goal_yaw = tf2::getYaw(goal.orientation);
     const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
 
-    constexpr double th_angle = M_PI / 4;
-
+    const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
     if (std::abs(angle_diff) < th_angle) {
       return true;
     }
@@ -230,12 +335,11 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const
     const auto goal_yaw = tf2::getYaw(goal.orientation);
     const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
 
-    constexpr double th_angle = M_PI / 4;
+    const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
     if (std::abs(angle_diff) < th_angle) {
       return true;
     }
   }
-
   return false;
 }
 
@@ -255,11 +359,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
   LaneletRoute route_msg;
   RouteSections route_sections;
 
-  if (!is_goal_valid(points.back())) {
-    RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose");
-    return route_msg;
-  }
-
+  lanelet::ConstLanelets all_route_lanelets;
   for (std::size_t i = 1; i < points.size(); i++) {
     const auto start_check_point = points.at(i - 1);
     const auto goal_check_point = points.at(i);
@@ -268,12 +368,20 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
           start_check_point, goal_check_point, &path_lanelets)) {
       return route_msg;
     }
+    for (const auto & lane : path_lanelets) {
+      all_route_lanelets.push_back(lane);
+    }
     // create local route sections
     route_handler_.setRouteLanelets(path_lanelets);
     const auto local_route_sections = route_handler_.createMapSegments(path_lanelets);
     route_sections = combine_consecutive_route_sections(route_sections, local_route_sections);
   }
 
+  if (!is_goal_valid(points.back(), all_route_lanelets)) {
+    RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose");
+    return route_msg;
+  }
+
   if (route_handler_.isRouteLooped(route_sections)) {
     RCLCPP_WARN(logger, "Loop detected within route!");
     return route_msg;

planning/mission_planner/src/lanelet2_plugins/default_planner.hpp

@@ -18,6 +18,7 @@
 #include <mission_planner/mission_planner_plugin.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
@@ -32,6 +33,11 @@
 namespace mission_planner::lanelet2
 {
 
+struct DefaultPlannerParameters
+{
+  double goal_angle_threshold_deg;
+};
+
 class DefaultPlanner : public mission_planner::PlannerPlugin
 {
 public:
@@ -40,6 +46,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
   bool ready() const override;
   LaneletRoute plan(const RoutePoints & points) override;
   MarkerArray visualize(const LaneletRoute & route) const override;
+  MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const;
+  vehicle_info_util::VehicleInfo vehicle_info_;
 
 private:
   using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
@@ -52,11 +60,19 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
   lanelet::ConstLanelets shoulder_lanelets_;
   route_handler::RouteHandler route_handler_;
 
+  DefaultPlannerParameters param_;
+
   rclcpp::Node * node_;
   rclcpp::Subscription<HADMapBin>::SharedPtr map_subscriber_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_goal_footprint_marker_;
 
   void map_callback(const HADMapBin::ConstSharedPtr msg);
-  bool is_goal_valid(const geometry_msgs::msg::Pose & goal) const;
+  bool check_goal_footprint(
+    const lanelet::ConstLanelet & current_lanelet,
+    const lanelet::ConstLanelet & combined_prev_lanelet,
+    const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
+    const double search_margin = 2.0);
+  bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
   Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections);
 };
 

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -14,15 +14,32 @@
 
 #include "utility_functions.hpp"
 
+#include <boost/geometry.hpp>
+
 #include <lanelet2_core/geometry/Lanelet.h>
 
 #include <unordered_set>
+#include <utility>
 
 bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id)
 {
   return set.find(id) != set.end();
 }
 
+tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
+  tier4_autoware_utils::LinearRing2d footprint)
+{
+  tier4_autoware_utils::Polygon2d footprint_polygon;
+  boost::geometry::append(footprint_polygon.outer(), footprint[0]);
+  boost::geometry::append(footprint_polygon.outer(), footprint[1]);
+  boost::geometry::append(footprint_polygon.outer(), footprint[2]);
+  boost::geometry::append(footprint_polygon.outer(), footprint[3]);
+  boost::geometry::append(footprint_polygon.outer(), footprint[4]);
+  boost::geometry::append(footprint_polygon.outer(), footprint[5]);
+  boost::geometry::correct(footprint_polygon);
+  return footprint_polygon;
+}
+
 void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a)
 {
   cl->r = r;
@@ -36,3 +53,35 @@ void insert_marker_array(
 {
   a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
 }
+
+lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets)
+{
+  lanelet::Points3d lefts;
+  lanelet::Points3d rights;
+  lanelet::Points3d centers;
+  std::vector<uint64_t> bound_ids;
+
+  for (const auto & llt : lanelets) {
+    if (llt.id() != 0) {
+      bound_ids.push_back(llt.leftBound().id());
+      bound_ids.push_back(llt.rightBound().id());
+    }
+  }
+
+  for (const auto & llt : lanelets) {
+    if (std::count(bound_ids.begin(), bound_ids.end(), llt.leftBound().id()) < 2) {
+      for (const auto & pt : llt.leftBound()) {
+        lefts.push_back(lanelet::Point3d(pt));
+      }
+    }
+    if (std::count(bound_ids.begin(), bound_ids.end(), llt.rightBound().id()) < 2) {
+      for (const auto & pt : llt.rightBound()) {
+        rights.push_back(lanelet::Point3d(pt));
+      }
+    }
+  }
+  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);
+}