refactor(start_planner): visualize shifting section (autowarefoundati…

…on#6103) * Visualize the footprint from start_pose to end_pose along the path Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
Interplai · May 28, 2024 · 2877cbf · 2877cbf
1 parent 319990d
commit 2877cbf
Showing 1 changed file with 33 additions and 0 deletions.
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1415,6 +1415,39 @@ void StartPlannerModule::setDebugData()
     add(start_pose_text_marker_array);
   }
 
+  // visualize the footprint from pull_out_start pose to pull_out_end pose along the path
+  {
+    MarkerArray pull_out_path_footprint_marker_array{};
+    const auto pink = createMarkerColor(1.0, 0.0, 1.0, 0.99);
+    Marker pull_out_path_footprint_marker = createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shift_path_footprint", 0, Marker::LINE_STRIP,
+      createMarkerScale(0.2, 0.2, 0.2), pink);
+    pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5);
+    PathWithLaneId path_shift_start_to_end{};
+    const auto shift_path = status_.pull_out_path.partial_paths.front();
+    {
+      const size_t pull_out_start_idx = motion_utils::findNearestIndex(
+        shift_path.points, status_.pull_out_path.start_pose.position);
+      const size_t pull_out_end_idx =
+        motion_utils::findNearestIndex(shift_path.points, status_.pull_out_path.end_pose.position);
+
+      path_shift_start_to_end.points.insert(
+        path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
+        shift_path.points.begin() + pull_out_end_idx + 1);
+    }
+
+    for (size_t i = 0; i < path_shift_start_to_end.points.size(); ++i) {
+      pull_out_path_footprint_marker.id = i;
+      pull_out_path_footprint_marker.points.clear();
+      addFootprintMarker(
+        pull_out_path_footprint_marker, path_shift_start_to_end.points.at(i).point.pose,
+        vehicle_info_);
+      pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker);
+    }
+
+    add(pull_out_path_footprint_marker_array);
+  }
+
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {
     if (start_planner_data_.ego_predicted_path.size() > 0) {