feat(obstacle_avoidance_planner): fix bounds search (tier4#965)

* feat(obstacle_avoidance_planner): fix bounds search

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix ci error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -167,6 +167,8 @@ struct DebugData
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> mpt_traj;
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> extended_fixed_traj;
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> extended_non_fixed_traj;
+
+  SequentialBoundsCandidates sequential_bounds_candidates;
 };
 
 struct Trajectories

planning/obstacle_avoidance_planner/src/debug_visualization.cpp

@@ -436,51 +436,49 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray(
   return msg;
 }
 
-/*
 visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray(
   const std::vector<ReferencePoint> & ref_points,
-  const SequentialBoundsCandidates & sequential_bounds_candidates, const double r, const double g,
-  const double b, const double vehicle_circle_radius, const size_t sampling_num)
+  std::vector<std::vector<Bounds>> & bounds_candidates, const double r, const double g,
+  const double b, [[maybe_unused]] const double vehicle_width, const size_t sampling_num)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
+  const std::string ns = "bounds_candidates";
 
-  int unique_id = 0;
+  if (ref_points.empty()) return msg;
 
   auto marker = createDefaultMarker(
-    "map", rclcpp::Clock().now(), "bounds_candidates", 0,
-    visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.05, 0, 0),
-    createMarkerColor(r + 0.5, g, b, 0.3));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+    "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST,
+    createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3));
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
-  for (size_t p_idx = 0; p_idx < sequential_bounds_candidates.size(); ++p_idx) {
-    if (p_idx % sampling_num != 0) {
+  for (size_t i = 0; i < ref_points.size(); i++) {
+    if (i % sampling_num != 0) {
       continue;
     }
 
-    const auto & bounds_candidates = sequential_bounds_candidates.at(p_idx);
-    for (size_t b_idx = 0; b_idx < bounds_candidates.size(); ++b_idx) {
-      const auto & bounds_candidate = bounds_candidates.at(b_idx);
-
-      marker.id = unique_id++;
+    const auto & bound_candidates = bounds_candidates.at(i);
+    for (size_t j = 0; j < bound_candidates.size(); ++j) {
       geometry_msgs::msg::Pose pose;
-      pose.position = ref_points.at(p_idx).p;
-      pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(p_idx).yaw);
+      pose.position = ref_points.at(i).p;
+      pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw);
 
-      const double lb_y = bounds_candidate.lower_bound - vehicle_circle_radius;
+      // lower bound
+      const double lb_y = bound_candidates.at(j).lower_bound;
       const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position;
       marker.points.push_back(lb);
 
-      const double ub_y = bounds_candidate.upper_bound + vehicle_circle_radius;
+      // upper bound
+      const double ub_y = bound_candidates.at(j).upper_bound;
       const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position;
       marker.points.push_back(ub);
+
+      msg.markers.push_back(marker);
     }
   }
 
-  msg.markers.push_back(marker);
   return msg;
 }
-*/
 
 visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
   const std::vector<ReferencePoint> & ref_points, const double r, const double g, const double b,
@@ -762,14 +760,12 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
       debug_data_ptr->mpt_visualize_sampling_num),
     &vis_marker_array);
 
-  /*
   // bounds candidates
   appendMarkerArray(
     getBoundsCandidatesLineMarkerArray(
       debug_data_ptr->ref_points, debug_data_ptr->sequential_bounds_candidates, 0.2, 0.99, 0.99,
-      debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num),
+      vehicle_param.width, debug_data_ptr->mpt_visualize_sampling_num),
     &vis_marker_array);
-  */
 
   // vehicle circle line
   appendMarkerArray(

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -1372,6 +1372,7 @@ void MPTOptimizer::calcBounds(
       enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data_ptr);
     sequential_bounds_candidates.push_back(bounds_candidates);
   }
+  debug_data_ptr->sequential_bounds_candidates = sequential_bounds_candidates;
 
   // search continuous and widest bounds only for front point
   for (size_t i = 0; i < sequential_bounds_candidates.size(); ++i) {
@@ -1408,6 +1409,11 @@ void MPTOptimizer::calcBounds(
           RCLCPP_INFO_EXPRESSION(
             rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_,
             "non-overlapped length bounds is ignored.");
+          RCLCPP_INFO_EXPRESSION(
+            rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_,
+            "In detail, prev: lower=%f, upper=%f, candidate: lower=%f, upper=%f",
+            prev_continuous_bounds.lower_bound, prev_continuous_bounds.upper_bound,
+            bounds_candidate.lower_bound, bounds_candidate.upper_bound);
           continue;
         }
 
@@ -1429,8 +1435,8 @@ void MPTOptimizer::calcBounds(
         const auto nearest_bounds = std::min_element(
           filtered_bounds_candidates.begin(), filtered_bounds_candidates.end(),
           [](const auto & a, const auto & b) {
-            return std::abs(a.lower_bound + a.upper_bound) <
-                   std::abs(b.lower_bound + b.upper_bound);
+            return std::min(std::abs(a.lower_bound), std::abs(a.upper_bound)) <
+                   std::min(std::abs(b.lower_bound), std::abs(b.upper_bound));
           });
         if (
           filtered_bounds_candidates.begin() <= nearest_bounds &&
@@ -1442,7 +1448,7 @@ void MPTOptimizer::calcBounds(
 
       // invalid bounds
       RCLCPP_WARN_EXPRESSION(
-        rclcpp::get_logger("getBounds: front"), is_showing_debug_info_, "invalid bounds");
+        rclcpp::get_logger("getBounds: not front"), is_showing_debug_info_, "invalid bounds");
       const auto invalid_bounds =
         Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD};
       ref_points.at(i).bounds = invalid_bounds;