@@ -65,6 +65,15 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
65
65
motion_utils::findNearestSegmentIndex (ego_data.path .points , ego_data.pose .position );
66
66
ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment (
67
67
ego_data.path .points , ego_data.first_path_idx , ego_data.pose .position );
68
+ const auto min_stop_distance =
69
+ motion_utils::calcDecelDistWithJerkAndAccConstraints (
70
+ planner_data_->current_velocity ->twist .linear .x , 0.0 ,
71
+ planner_data_->current_acceleration ->accel .accel .linear .x ,
72
+ planner_data_->max_stop_acceleration_threshold , -planner_data_->max_stop_jerk_threshold ,
73
+ planner_data_->max_stop_jerk_threshold )
74
+ .value_or (0.0 );
75
+ ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose (
76
+ ego_data.path .points , ego_data.pose .position , min_stop_distance);
68
77
69
78
make_ego_footprint_rtree (ego_data, params_);
70
79
const auto has_decided_to_stop =
@@ -80,13 +89,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
80
89
const auto obstacle_forward_footprints =
81
90
make_forward_footprints (dynamic_obstacles, params_, hysteresis);
82
91
const auto footprints_duration_us = stopwatch.toc (" footprints" );
83
- const auto min_stop_distance =
84
- motion_utils::calcDecelDistWithJerkAndAccConstraints (
85
- planner_data_->current_velocity ->twist .linear .x , 0.0 ,
86
- planner_data_->current_acceleration ->accel .accel .linear .x ,
87
- planner_data_->max_stop_acceleration_threshold , -planner_data_->max_stop_jerk_threshold ,
88
- planner_data_->max_stop_jerk_threshold )
89
- .value_or (0.0 );
90
92
stopwatch.tic (" collisions" );
91
93
const auto collision =
92
94
find_earliest_collision (ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_);
@@ -101,8 +103,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
101
103
? motion_utils::calcLongitudinalOffsetPose (
102
104
ego_data.path .points , *collision,
103
105
-params_.stop_distance_buffer - params_.ego_longitudinal_offset )
104
- : motion_utils::calcLongitudinalOffsetPose (
105
- ego_data.path .points , ego_data.pose .position , min_stop_distance);
106
+ : ego_data.earliest_stop_pose ;
106
107
if (stop_pose) {
107
108
const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength (
108
109
path->points , stop_pose->position ,
0 commit comments