@@ -366,9 +366,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
366
366
? std::abs (left_dist_to_lane_border)
367
367
: std::abs (right_dist_to_lane_border);
368
368
369
- std::cerr << " smallest_lateral_offset " << smallest_lateral_offset << " \n " ;
370
- std::cerr << " remaining gap " << gap_between_ego_and_lane_border << " \n " ;
371
-
372
369
// Get the lanelets that will be queried for target objects
373
370
const auto relevant_lanelets = std::invoke ([&]() -> std::optional<lanelet::ConstLanelets> {
374
371
lanelet::Lanelet closest_lanelet;
@@ -386,7 +383,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
386
383
if (!relevant_lanelets) return false ;
387
384
388
385
// filtering objects with velocity, position and class
389
- std::cerr << " Filter objects \n " ;
390
386
const auto filtered_objects = utils::path_safety_checker::filterObjects (
391
387
dynamic_object, route_handler, relevant_lanelets.value (), current_pose.position ,
392
388
objects_filtering_params_);
@@ -415,12 +411,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
415
411
return closest_object_width;
416
412
});
417
413
if (!closest_object_width) return false ;
418
-
419
- std::cerr << " Dims \n " ;
420
- std::cerr << " target_object_itr->shape.dimensions.y " << closest_object_width.value () << " \n " ;
421
- std::cerr << " gap_between_ego_and_lane_border " << gap_between_ego_and_lane_border << " \n " ;
422
- std::cerr << " parameters_->extra_width_margin_for_rear_obstacle "
423
- << parameters_->extra_width_margin_for_rear_obstacle << " \n " ;
424
414
// Decide if the closest object does not fit in the gap left by the ego vehicle.
425
415
return closest_object_width.value () + parameters_->extra_width_margin_for_rear_obstacle >
426
416
gap_between_ego_and_lane_border;
0 commit comments