feat(obstacle_cruise_planner): single slow down virtual wall

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -217,6 +217,7 @@ struct DebugData
   std::vector<StopObstacle> obstacles_to_stop;
   std::vector<CruiseObstacle> obstacles_to_cruise;
   std::vector<SlowDownObstacle> obstacles_to_slow_down;
+  MarkerArray slow_down_debug_wall_marker;
   MarkerArray stop_wall_marker;
   MarkerArray cruise_wall_marker;
   MarkerArray slow_down_wall_marker;

planning/obstacle_cruise_planner/src/node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -995,7 +995,7 @@
       }
       return true;
     }
     // check if entrying slow down
     if (is_lat_dist_low) {
       const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid);
       if (p.successive_num_to_entry_slow_down_condition <= count) {
@@ -1016,7 +1016,7 @@
   const auto obstacle_poly = obstacle.toPolygon();
 
   // calculate collision points with trajectory with lateral stop margin
   // NOTE: For additional margin, hysteresis is not devided by two.
   const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons(
     traj_points, vehicle_info_,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
@@ -1342,6 +1342,10 @@
     debug_marker.markers.push_back(marker);
   }
 
+  // slow down debug wall marker
+  tier4_autoware_utils::appendMarkerArray(
+    debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker);
+
   debug_marker_pub_->publish(debug_marker);
 
   // 2. publish virtual wall for cruise and stop

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -410,78 +410,93 @@
       return inserted_idx.get();
     }
     return std::nullopt;
   };
-  const auto add_slow_down_marker =
-    [&](const size_t obstacle_idx, const auto slow_down_traj_idx, const bool is_start) {
-      if (!slow_down_traj_idx) return;
-
-      const int id = obstacle_idx * 2 + (is_start ? 0 : 1);
-      const auto text = is_start ? "obstacle slow down start" : "obstacle slow down end";
-      const auto pose = slow_down_traj_points.at(*slow_down_traj_idx).pose;
-      const auto markers = motion_utils::createSlowDownVirtualWallMarker(
-        pose, text, planner_data.current_time, id, abs_ego_offset);
-      tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
-    };
 
   std::vector<SlowDownOutput> new_prev_slow_down_output;
   for (size_t i = 0; i < obstacles.size(); ++i) {
     const auto & obstacle = obstacles.at(i);
     const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid);
 
     // calculate slow down start distance, and insert slow down velocity
     const auto dist_vec_to_slow_down = calculateDistanceToSlowDownWithConstraints(
       planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego);
     if (!dist_vec_to_slow_down) {
       RCLCPP_INFO_EXPRESSION(
-        rclcpp::get_logger("ObstacleCruisePlanner::Plannerinterface"), enable_debug_info_,
+        rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_,
         "[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid",
         obstacle.uuid.c_str());
       continue;
     }
     const auto [dist_to_slow_down_start, dist_to_slow_down_end, feasible_slow_down_vel] =
       *dist_vec_to_slow_down;
 
     // calculate slow down end distance, and insert slow down velocity
     // NOTE: slow_down_start_idx will not be wrong since inserted back point is after inserted
     // front point.
     const auto slow_down_start_idx = insert_point_in_trajectory(dist_to_slow_down_start);
     const auto slow_down_end_idx = dist_to_slow_down_start < dist_to_slow_down_end
                                      ? insert_point_in_trajectory(dist_to_slow_down_end)
                                      : std::nullopt;
     if (!slow_down_end_idx) {
       continue;
     }
 
     // calculate slow down velocity
     const double stable_slow_down_vel = [&]() {
       if (prev_output) {
         return signal_processing::lowpassFilter(
           feasible_slow_down_vel, prev_output->target_vel, slow_down_param_.lpf_gain_slow_down_vel);
       }
       return feasible_slow_down_vel;
     }();
 
     // insert slow down velocity between slow start and end
     for (size_t i = (slow_down_start_idx ? *slow_down_start_idx : 0); i <= *slow_down_end_idx;
          ++i) {
       auto & traj_point = slow_down_traj_points.at(i);
       traj_point.longitudinal_velocity_mps =
         std::min(traj_point.longitudinal_velocity_mps, static_cast<float>(stable_slow_down_vel));
     }
 
-    // add debug data and virtual wall
+    // add debug data
     slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist);
     slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start);
     slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end);
     slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel);
     slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel);
     slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0);
     slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0);
+
+    // add virtual wall
+    if (slow_down_start_idx && slow_down_end_idx) {
+      const size_t ego_idx =
+        ego_nearest_param_.findIndex(slow_down_traj_points, planner_data.ego_pose);
+      const size_t slow_down_wall_idx = [&]() {
+        if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx;
+        if (ego_idx < *slow_down_end_idx) return ego_idx;
+        return *slow_down_end_idx;
+      }();
+
+      const auto markers = motion_utils::createSlowDownVirtualWallMarker(
+        slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
+        planner_data.current_time, i, abs_ego_offset);
+      tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
+    }
+
+    // add debug virtual wall
     if (slow_down_start_idx) {
-      add_slow_down_marker(i, slow_down_start_idx, true);
+      const auto markers = motion_utils::createSlowDownVirtualWallMarker(
+        slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start",
+        planner_data.current_time, i * 2, abs_ego_offset);
+      tier4_autoware_utils::appendMarkerArray(
+        markers, &debug_data_ptr_->slow_down_debug_wall_marker);
     }
     if (slow_down_end_idx) {
-      add_slow_down_marker(i, slow_down_end_idx, false);
+      const auto markers = motion_utils::createSlowDownVirtualWallMarker(
+        slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end",
+        planner_data.current_time, i * 2 + 1, abs_ego_offset);
+      tier4_autoware_utils::appendMarkerArray(
+        markers, &debug_data_ptr_->slow_down_debug_wall_marker);
     }
 
     debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle);