@@ -1180,12 +1180,12 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
1180
1180
return std::nullopt;
1181
1181
}
1182
1182
1183
- const double lat_margin_for_stop =
1183
+ const double max_lat_margin_for_stop =
1184
1184
(obstacle.classification .label == ObjectClassification::UNKNOWN)
1185
1185
? p.max_lat_margin_for_stop_against_unknown
1186
1186
: p.max_lat_margin_for_stop ;
1187
1187
1188
- if (precise_lat_dist > std::max (lat_margin_for_stop , 1e-3 )) {
1188
+ if (precise_lat_dist > std::max (max_lat_margin_for_stop , 1e-3 )) {
1189
1189
return std::nullopt;
1190
1190
}
1191
1191
@@ -1218,7 +1218,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
1218
1218
calcObstacleMaxLength (obstacle.shape ) + p.decimate_trajectory_step_length +
1219
1219
std::hypot (
1220
1220
vehicle_info_.vehicle_length_m ,
1221
- vehicle_info_.vehicle_width_m * 0.5 + lat_margin_for_stop ));
1221
+ vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop ));
1222
1222
if (collision_points.empty ()) {
1223
1223
RCLCPP_INFO_EXPRESSION (
1224
1224
get_logger (), enable_debug_info_,
@@ -1244,7 +1244,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
1244
1244
1245
1245
// calculate collision points with trajectory with lateral stop margin
1246
1246
const auto traj_polys_with_lat_margin =
1247
- createOneStepPolygons (traj_points, vehicle_info_, odometry.pose .pose , lat_margin_for_stop );
1247
+ createOneStepPolygons (traj_points, vehicle_info_, odometry.pose .pose , max_lat_margin_for_stop );
1248
1248
1249
1249
const auto collision_point = polygon_utils::getCollisionPoint (
1250
1250
traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);
0 commit comments