refactor(mission_planner): add comment functions/remove unused stuff/add comment

planning/mission_planner/package.xml

@@ -9,6 +9,7 @@
   <maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer>
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</author>

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -195,7 +195,6 @@
 {
   lanelet::ConstLanelets route_lanelets;
   lanelet::ConstLanelets end_lanelets;
-  lanelet::ConstLanelets normal_lanelets;
   lanelet::ConstLanelets goal_lanelets;
 
   for (const auto & route_section : route.segments) {
@@ -210,16 +209,14 @@
     }
   }
 
-  std_msgs::msg::ColorRGBA cl_route;
-  std_msgs::msg::ColorRGBA cl_ll_borders;
-  std_msgs::msg::ColorRGBA cl_end;
-  std_msgs::msg::ColorRGBA cl_normal;
-  std_msgs::msg::ColorRGBA cl_goal;
-  set_color(&cl_route, 0.8, 0.99, 0.8, 0.15);
-  set_color(&cl_goal, 0.2, 0.4, 0.4, 0.05);
-  set_color(&cl_end, 0.2, 0.2, 0.4, 0.05);
-  set_color(&cl_normal, 0.2, 0.4, 0.2, 0.05);
-  set_color(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999);
+  const std_msgs::msg::ColorRGBA cl_route =
+    tier4_autoware_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15);
+  const std_msgs::msg::ColorRGBA cl_ll_borders =
+    tier4_autoware_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05);
+  const std_msgs::msg::ColorRGBA cl_end =
+    tier4_autoware_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05);
+  const std_msgs::msg::ColorRGBA cl_goal =
+    tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999);
 
   visualization_msgs::msg::MarkerArray route_marker_array;
   insert_marker_array(
@@ -231,9 +228,6 @@
   insert_marker_array(
     &route_marker_array,
     lanelet::visualization::laneletsAsTriangleMarkerArray("end_lanelets", end_lanelets, cl_end));
-  insert_marker_array(
-    &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
-                           "normal_lanelets", normal_lanelets, cl_normal));
   insert_marker_array(
     &route_marker_array,
     lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal));
@@ -270,42 +264,40 @@
   return msg;
 }
 
-bool DefaultPlanner::check_goal_footprint(
+bool DefaultPlanner::check_goal_footprint_inside_lanes(
   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()) {
+  if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
     return true;
   }
-  points_intersection.clear();
 
-  // check if goal footprint is in between many lanelets
+  // check if goal footprint is in between many lanelets in depth-first search manner
   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_with_shoulder(lanelets, shoulder_lanelets_);
 
-    // if next lanelet length longer than vehicle longitudinal offset
+    // 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);
-      boost::geometry::intersection(
-        goal_footprint, combined_lanelets.polygon2d().basicPolygon(), points_intersection);
-      if (points_intersection.empty()) {
+      // 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;
       }
-      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)) {
+      // 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 {
@@ -352,13 +344,13 @@
 
   double next_lane_length = 0.0;
   // combine calculated route lanelets
-  lanelet::ConstLanelet combined_prev_lanelet =
+  const lanelet::ConstLanelet combined_prev_lanelet =
     combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_);
 
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
   if (
     param_.check_footprint_inside_lanes &&
-    !check_goal_footprint(
+    !check_goal_footprint_inside_lanes(
       closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
     !is_in_parking_lot(
       lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),

planning/mission_planner/src/lanelet2_plugins/default_planner.hpp

@@ -71,13 +71,38 @@
 
   void initialize_common(rclcpp::Node * node);
   void map_callback(const HADMapBin::ConstSharedPtr msg);
-  bool check_goal_footprint(
+
+  /**
+   * @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the
+   * succeeding lanelets around the goal
+   * @attetion this function will terminate when the accumulated search length from the initial
+   * current_lanelet execeeds max_longitudinal_offset_m + search_margin, so under normal assumptions
+   * (i.e. the map is composed of finite elements of practically normal sized lanelets), it is
+   * assured to terminate
+   * @param current_lanelet the start lanelet to begin recursive query
+   * @param combined_prev_lanelet initial entire route_lanelets plus the small consecutive lanelets
+   * around the goal during the query
+   * @param next_lane_length the accumulated total legth 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 tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
     const double search_margin = 2.0);
+
+  /**
+   * @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);
+
+  /**
+   * @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of
+   * route_sections) and return the z-aligned goal position
+   */
   Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections);
+
   void updateRoute(const PlannerPlugin::LaneletRoute & route);
   void clearRoute();
 };

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -21,11 +21,6 @@
 #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)
 {
@@ -39,15 +34,7 @@
   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;
-  cl->g = g;
-  cl->b = b;
-  cl->a = a;
-}
-
 void insert_marker_array(
   visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2)
 {
@@ -64,7 +51,7 @@
   std::vector<uint64_t> right_bound_ids;
 
   for (const auto & llt : lanelets) {
-    if (llt.id() != 0) {
+    if (llt.id() != lanelet::InvalId) {
       left_bound_ids.push_back(llt.leftBound().id());
       right_bound_ids.push_back(llt.rightBound().id());
     }
@@ -96,6 +83,8 @@
       add_bound(left_shared_shoulder_it->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 consitutes 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);
     }
@@ -108,6 +97,8 @@
       add_bound(right_shared_shoulder_it->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 consitutes the right bondary of the entire merged lanelet
       std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
       add_bound(llt.rightBound(), rights);
     }

planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp

@@ -30,8 +30,6 @@
 #include <unordered_set>
 #include <vector>
 
-bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id);
-
 template <typename T>
 bool exists(const std::vector<T> & vectors, const T & item)
 {
@@ -45,12 +43,18 @@ bool exists(const std::vector<T> & vectors, const T & item)
 
 tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
   tier4_autoware_utils::LinearRing2d footprint);
-void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a);
 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 shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets
+ */
 lanelet::ConstLanelet combine_lanelets_with_shoulder(
   const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets);
+
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
 geometry_msgs::msg::Pose convertBasicPoint3dToPose(
   const lanelet::BasicPoint3d & point, const double lane_yaw);