diff --git a/modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc b/modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc index aa3b56f513b9fa715c597537de04083d9f70af45..25c080b2aaa06f6b2d171a1e787838ffa31b989b 100644 --- a/modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc +++ b/modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc @@ -128,8 +128,7 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle( GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle); - if (planning_target.has_stop_point() - && planning_target.stop_point().s() > 0) { + if (planning_target.has_stop_point()) { GenerateSpeedProfileForStopping(planning_target.stop_point().s(), ptr_lon_trajectory_bundle); } diff --git a/modules/planning/planner/lattice/lattice_planner.cc b/modules/planning/planner/lattice/lattice_planner.cc index 5fc6004b35270afc9490ad8cbccd71193dfce4bc..25ec76f74e2197134660b2b901ed8e1776b16582 100644 --- a/modules/planning/planner/lattice/lattice_planner.cc +++ b/modules/planning/planner/lattice/lattice_planner.cc @@ -298,9 +298,12 @@ Status LatticePlanner::PlanOnReferenceLine( if (!lattice_traj_ptr) { ADEBUG << "Dynamically casting trajectory1d ptr. failed."; } - ADEBUG << "Ending Lon. State s = " << lattice_traj_ptr->target_position() - << " ds = " << lattice_traj_ptr->target_velocity() - << " t = " << lattice_traj_ptr->target_time(); + + if (lattice_traj_ptr->has_target_position()) { + ADEBUG << "Ending Lon. State s = " << lattice_traj_ptr->target_position() + << " ds = " << lattice_traj_ptr->target_velocity() + << " t = " << lattice_traj_ptr->target_time(); + } ADEBUG << "InputPose"; ADEBUG << "XY: " << planning_init_point.ShortDebugString(); diff --git a/modules/planning/tasks/traffic_decider/traffic_decider.cc b/modules/planning/tasks/traffic_decider/traffic_decider.cc index 0b8ea53584c5d9cd9e920e8707c74e6f8fe61e0a..89109b193d2d458f3bcc5299e3fa8854f7d44817 100644 --- a/modules/planning/tasks/traffic_decider/traffic_decider.cc +++ b/modules/planning/tasks/traffic_decider/traffic_decider.cc @@ -125,17 +125,18 @@ void TrafficDecider::BuildPlanningTarget( stop_point.set_type(StopPoint::SOFT); ADEBUG << "Soft stop at: " << min_s << " STOP_REASON_YELLOW_SIGNAL"; } else { - min_s = -1.0; ADEBUG << "No planning target found at reference line."; } } } - const auto& vehicle_config = + if (min_s != std::numeric_limits::infinity()) { + const auto& vehicle_config = common::VehicleConfigHelper::instance()->GetConfig(); - double front_edge_to_center = + double front_edge_to_center = vehicle_config.vehicle_param().front_edge_to_center(); - stop_point.set_s(min_s - front_edge_to_center); - reference_line_info->SetStopPoint(stop_point); + stop_point.set_s(min_s - front_edge_to_center); + reference_line_info->SetStopPoint(stop_point); + } } Status TrafficDecider::Execute(Frame *frame,