feat(obstacle_cruise_planner): single slow down virtual wall (#4374)

* feat(obstacle_cruise_planner): single slow down virtual wall Signed-off-by: Takayuki Murooka <[email protected]> * update Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Jul 24, 2023 · b409dd4 · b409dd4
1 parent b212643
commit b409dd4
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 15 deletions.
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/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;

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -1342,6 +1342,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const
     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

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -411,17 +411,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
     }
     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) {
@@ -433,7 +422,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
       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;
@@ -469,19 +458,45 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
         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);