From 7c0b3c3e1c7b7e43a92b87ab38cd3907918ac1b8 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 6 Dec 2024 13:12:49 +0900 Subject: [PATCH] fix and code cleanup (working) Signed-off-by: Maxime CLEMENT --- .../src/calculate_slowdown_points.cpp | 13 +++++++++++++ .../src/debug.cpp | 9 +++++---- .../src/out_of_lane_module.cpp | 15 +-------------- .../src/planner_data.cpp | 4 +++- 4 files changed, 22 insertions(+), 19 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 352e5767d6baa..30ba1dbd97e88 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -125,6 +125,19 @@ std::optional 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; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 583d325b59cf6..e019934b06294 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 547faf499d760..1593d9c667382 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/planner_data.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/planner_data.cpp index 4888d31b755a7..a4d0e627c2a19 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/planner_data.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/planner_data.cpp @@ -72,10 +72,12 @@ std::vector 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);