fix and code cleanup (working)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp

@@ -125,6 +125,19 @@ std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
     slowdown_point = calculate_pose_ahead_of_collision(
       ego_data, *point_to_avoid_it, expanded_footprint, params.precision);
   }
+  if (slowdown_point && params.use_map_stop_lines) {
+    // try to use a map stop line ahead of the stop pose
+    auto stop_arc_length =
+      motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0LU, slowdown_point->position);
+    for (const auto & stop_point : ego_data.map_stop_points) {
+      if (
+        stop_point.ego_trajectory_arc_length < stop_arc_length &&
+        stop_point.ego_trajectory_arc_length >= ego_data.min_stop_arc_length) {
+        slowdown_point = stop_point.ego_stop_pose;
+        stop_arc_length = stop_point.ego_trajectory_arc_length;
+      }
+    }
+  }
   return slowdown_point;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp

@@ -129,7 +129,7 @@ size_t add_stop_line_markers(
     for (const auto & ll : lanelets) {
       debug_marker.points.clear();
       for (const auto & p : ll.polygon2d().basicPolygon()) {
-        debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5));
+        debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z));
       }
       debug_marker.points.push_back(debug_marker.points.front());
       debug_marker_array.markers.push_back(debug_marker);
@@ -140,7 +140,7 @@ size_t add_stop_line_markers(
     debug_marker.points.clear();
     debug_marker.color.r = 1.0;
     for (const auto & p : stop_line.stop_line) {
-      debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5));
+      debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z));
     }
     debug_marker_array.markers.push_back(debug_marker);
     ++debug_marker.id;
@@ -162,6 +162,7 @@ void add_stop_line_markers(
   auto debug_marker = get_base_marker();
   debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
   debug_marker.ns = "ego_stop_lines";
+  debug_marker.scale = universe_utils::createMarkerScale(0.2, 0.2, 0.2);
   debug_marker.color = universe_utils::createMarkerColor(0.5, 0.0, 0.7, 1.0);
   geometry_msgs::msg::Point p1;
   geometry_msgs::msg::Point p2;
@@ -246,11 +247,11 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(
   const EgoData & ego_data, const OutOfLaneData & out_of_lane_data,
   const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data)
 {
-  const auto z = ego_data.pose.position.z;
+  const auto z = ego_data.pose.position.z + 0.5;
   visualization_msgs::msg::MarkerArray debug_marker_array;
 
   auto base_marker = get_base_marker();
-  base_marker.pose.position.z = z + 0.5;
+  base_marker.pose.position.z = z;
   base_marker.ns = "footprints";
   base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0);
   // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -225,6 +225,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
   out_of_lane::calculate_min_stop_and_slowdown_distances(
     ego_data, *planner_data, previous_slowdown_pose_);
   prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length);
+  ego_data.map_stop_points = planner_data->calculate_map_stop_points(ego_data.trajectory_points);
   const auto preprocessing_us = stopwatch.toc("preprocessing");
 
   stopwatch.tic("calculate_trajectory_footprints");
@@ -274,20 +275,6 @@ VelocityPlanningResult OutOfLaneModule::plan(
 
   stopwatch.tic("calculate_slowdown_point");
   auto slowdown_pose = out_of_lane::calculate_slowdown_point(ego_data, out_of_lane_data, params_);
-  if (slowdown_pose && params_.use_map_stop_lines) {
-    // try to use a map stop line ahead of the stop pose
-    ego_data.map_stop_points = planner_data->calculate_map_stop_points(ego_data.trajectory_points);
-    auto stop_arc_length =
-      motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0LU, slowdown_pose->position);
-    for (const auto & stop_point : ego_data.map_stop_points) {
-      if (
-        stop_point.ego_trajectory_arc_length < stop_arc_length &&
-        stop_point.ego_trajectory_arc_length >= ego_data.min_stop_arc_length) {
-        slowdown_pose = stop_point.ego_stop_pose;
-        stop_arc_length = stop_point.ego_trajectory_arc_length;
-      }
-    }
-  }
   const auto calculate_slowdown_point_us = stopwatch.toc("calculate_slowdown_point");
 
   // reuse previous stop pose if there is no new one or if its velocity is not higher than the new

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/planner_data.cpp

@@ -72,10 +72,12 @@ std::vector<StopPoint> PlannerData::calculate_map_stop_points(
           geometry_msgs::msg::Point().set__x(intersection.x()).set__y(intersection.y());
         const auto stop_line_arc_length = motion_utils::calcSignedArcLength(trajectory, 0UL, p);
         StopPoint sp;
-        sp.ego_trajectory_arc_length = stop_line_arc_length - vehicle_info_.front_overhang_m;
+        sp.ego_trajectory_arc_length =
+          stop_line_arc_length - vehicle_info_.max_longitudinal_offset_m;
         if (sp.ego_trajectory_arc_length < 0.0) {
           continue;
         }
+        sp.stop_line = stop_line_2d;
         sp.ego_stop_pose =
           motion_utils::calcInterpolatedPose(trajectory, sp.ego_trajectory_arc_length);
         stop_points.push_back(sp);