From aaa539c53cdcf86b4af55d063d75fa5e19e48b5f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 19 Dec 2022 20:08:41 +0900 Subject: [PATCH] update Signed-off-by: Takayuki Murooka --- .../src/pid_based_planner/pid_based_planner.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 0cb74755e6dac..8925759ba7e47 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -83,8 +83,8 @@ PIDBasedPlanner::PIDBasedPlanner( node.declare_parameter( "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); - velocity_limit_based_planner_param_.disable_target_acceleration = - node.declare_parameter("pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); } { // velocity insertion based planner @@ -304,10 +304,12 @@ Trajectory PIDBasedPlanner::planCruise( : std::abs(vehicle_info_.min_longitudinal_offset_m); const double dist_to_rss_wall = std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, dist_to_rss_wall, ego_idx); const auto markers = motion_utils::createSlowDownVirtualWallMarker( - planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, - dist_to_rss_wall); + planner_data.traj.points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, + 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); // cruise obstalce @@ -361,9 +363,8 @@ VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( return pid_output_vel; }(); - const double positive_target_vel = - lpf_output_vel_ptr_->filter( - std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel)); + const double positive_target_vel = lpf_output_vel_ptr_->filter( + std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel)); // calculate target acceleration const double target_acc = [&]() {