@@ -221,7 +221,7 @@ bool StartPlannerModule::receivedNewRoute() const
221
221
bool StartPlannerModule::requiresDynamicObjectsCollisionDetection () const
222
222
{
223
223
return parameters_->safety_check_params .enable_safety_check && status_.driving_forward &&
224
- !isPreventingRearVehicleFromCrossing ();
224
+ !isPreventingRearVehicleFromPassingThrough ();
225
225
}
226
226
227
227
bool StartPlannerModule::noMovingObjectsAround () const
@@ -288,18 +288,20 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const
288
288
return std::abs (lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road ;
289
289
}
290
290
291
- bool StartPlannerModule::isPreventingRearVehicleFromCrossing () const
291
+ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough () const
292
292
{
293
293
const auto & current_pose = planner_data_->self_odometry ->pose .pose ;
294
294
const auto & dynamic_object = planner_data_->dynamic_object ;
295
295
const auto & route_handler = planner_data_->route_handler ;
296
+ const Pose start_pose = planner_data_->route_handler ->getOriginalStartPose ();
297
+
296
298
const auto target_lanes = utils::getCurrentLanes (planner_data_);
297
299
if (target_lanes.empty ()) return false ;
298
300
299
301
// Define functions to get distance between a point and a lane's boundaries.
300
- auto calc_right_lateral_offset = [&](
301
- const lanelet::ConstLineString2d & boundary_line,
302
- const geometry_msgs::msg::Pose & search_pose) {
302
+ auto calc_absolute_lateral_offset = [&](
303
+ const lanelet::ConstLineString2d & boundary_line,
304
+ const geometry_msgs::msg::Pose & search_pose) {
303
305
std::vector<geometry_msgs::msg::Point > boundary_path;
304
306
std::for_each (
305
307
boundary_line.begin (), boundary_line.end (), [&boundary_path](const auto & boundary_point) {
@@ -311,60 +313,63 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
311
313
return std::fabs (calcLateralOffset (boundary_path, search_pose.position ));
312
314
};
313
315
314
- auto calc_left_lateral_offset = [&](
315
- const lanelet::ConstLineString2d & boundary_line,
316
- const geometry_msgs::msg::Pose & search_pose) {
317
- return -calc_right_lateral_offset (boundary_line, search_pose);
318
- };
316
+ // Check from what side of the road the ego is merging
317
+ const auto centerline_path =
318
+ route_handler->getCenterLinePath (target_lanes, 0.0 , std::numeric_limits<double >::max ());
319
+ const auto start_pose_nearest_segment_index =
320
+ motion_utils::findNearestSegmentIndex (centerline_path.points , start_pose);
321
+ if (!start_pose_nearest_segment_index) return false ;
322
+
323
+ const auto start_pose_point_msg = tier4_autoware_utils::createPoint (
324
+ start_pose.position .x , start_pose.position .y , start_pose.position .z );
325
+ const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset (
326
+ centerline_path.points , start_pose_point_msg, start_pose_nearest_segment_index.value ());
327
+ if (std::isnan (starting_pose_lateral_offset)) return false ;
328
+
329
+ const bool ego_is_merging_from_the_left = (starting_pose_lateral_offset > 0.0 );
319
330
320
331
// Get the ego's overhang point closest to the centerline path and the gap between said point and
321
332
// the lane's border.
322
- const auto local_vehicle_footprint = vehicle_info_.createFootprint ();
323
- const auto vehicle_footprint =
324
- transformVector (local_vehicle_footprint, tier4_autoware_utils::pose2transform (current_pose));
325
- const auto centerline_path =
326
- route_handler->getCenterLinePath (target_lanes, 0.0 , std::numeric_limits<double >::max ());
327
- double smallest_lateral_offset = std::numeric_limits<double >::max ();
328
- double left_dist_to_lane_border = 0.0 ;
329
- double right_dist_to_lane_border = 0.0 ;
330
- geometry_msgs::msg::Pose ego_overhang_point_as_pose;
331
- for (const auto & point : vehicle_footprint) {
332
- geometry_msgs::msg::Pose point_pose;
333
- point_pose.position .x = point.x ();
334
- point_pose.position .y = point.y ();
335
- point_pose.position .z = 0.0 ;
336
-
337
- const auto nearest_segment_index =
338
- motion_utils::findNearestSegmentIndex (centerline_path.points , point_pose);
339
- if (!nearest_segment_index) continue ;
340
-
341
- const auto point_msg = tier4_autoware_utils::createPoint (point.x (), point.y (), 0.0 );
342
- const auto lateral_offset = std::abs (motion_utils::calcLateralOffset (
343
- centerline_path.points , point_msg, nearest_segment_index.value ()));
344
- if (std::isnan (lateral_offset)) continue ;
345
-
346
- if (lateral_offset < smallest_lateral_offset) {
347
- smallest_lateral_offset = lateral_offset;
348
- ego_overhang_point_as_pose.position .x = point.x ();
349
- ego_overhang_point_as_pose.position .y = point.y ();
350
- ego_overhang_point_as_pose.position .z = 0.0 ;
333
+ auto get_gap_between_ego_and_lane_border =
334
+ [&](
335
+ geometry_msgs::msg::Pose & ego_overhang_point_as_pose,
336
+ const bool ego_is_merging_from_the_left) -> std::optional<double > {
337
+ const auto local_vehicle_footprint = vehicle_info_.createFootprint ();
338
+ const auto vehicle_footprint =
339
+ transformVector (local_vehicle_footprint, tier4_autoware_utils::pose2transform (current_pose));
340
+ double smallest_lateral_gap_between_ego_and_border = std::numeric_limits<double >::max ();
341
+ for (const auto & point : vehicle_footprint) {
342
+ geometry_msgs::msg::Pose point_pose;
343
+ point_pose.position .x = point.x ();
344
+ point_pose.position .y = point.y ();
345
+ point_pose.position .z = 0.0 ;
351
346
352
347
lanelet::Lanelet closest_lanelet;
353
348
lanelet::utils::query::getClosestLanelet (target_lanes, point_pose, &closest_lanelet);
354
349
lanelet::ConstLanelet closest_lanelet_const (closest_lanelet.constData ());
355
350
356
- const lanelet::ConstLineString2d current_left_bound = closest_lanelet_const.leftBound2d ();
357
- const lanelet::ConstLineString2d current_right_bound = closest_lanelet_const.rightBound2d ();
358
- left_dist_to_lane_border = calc_left_lateral_offset (current_left_bound, point_pose);
359
- right_dist_to_lane_border = calc_right_lateral_offset (current_right_bound, point_pose);
351
+ const lanelet::ConstLineString2d current_lane_bound = (ego_is_merging_from_the_left)
352
+ ? closest_lanelet_const.rightBound2d ()
353
+ : closest_lanelet_const.leftBound2d ();
354
+ const double current_point_lateral_gap =
355
+ calc_absolute_lateral_offset (current_lane_bound, point_pose);
356
+ if (current_point_lateral_gap < smallest_lateral_gap_between_ego_and_border) {
357
+ smallest_lateral_gap_between_ego_and_border = current_point_lateral_gap;
358
+ ego_overhang_point_as_pose.position .x = point.x ();
359
+ ego_overhang_point_as_pose.position .y = point.y ();
360
+ ego_overhang_point_as_pose.position .z = 0.0 ;
361
+ }
360
362
}
361
- }
362
- if (smallest_lateral_offset == std::numeric_limits<double >::max ()) return false ;
363
+ if (smallest_lateral_gap_between_ego_and_border == std::numeric_limits<double >::max ()) {
364
+ return std::nullopt;
365
+ }
366
+ return smallest_lateral_gap_between_ego_and_border;
367
+ };
363
368
364
- const double gap_between_ego_and_lane_border =
365
- ( std::abs (left_dist_to_lane_border) > std::abs (right_dist_to_lane_border))
366
- ? std::abs (left_dist_to_lane_border)
367
- : std::abs (right_dist_to_lane_border) ;
369
+ geometry_msgs::msg::Pose ego_overhang_point_as_pose;
370
+ const auto gap_between_ego_and_lane_border =
371
+ get_gap_between_ego_and_lane_border (ego_overhang_point_as_pose, ego_is_merging_from_the_left);
372
+ if (!gap_between_ego_and_lane_border) return false ;
368
373
369
374
// Get the lanelets that will be queried for target objects
370
375
const auto relevant_lanelets = std::invoke ([&]() -> std::optional<lanelet::ConstLanelets> {
@@ -395,7 +400,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
395
400
relevant_lanelets.value (), route_handler, filtered_objects, objects_filtering_params_);
396
401
if (target_objects_on_lane.on_current_lane .empty ()) return false ;
397
402
398
- // Get the closest target obj in the relevant lanes
403
+ // Get the closest target obj width in the relevant lanes
399
404
const auto closest_object_width = std::invoke ([&]() -> std::optional<double > {
400
405
double arc_length_to_closet_object = std::numeric_limits<double >::max ();
401
406
double closest_object_width = -1.0 ;
@@ -415,7 +420,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
415
420
if (!closest_object_width) return false ;
416
421
// Decide if the closest object does not fit in the gap left by the ego vehicle.
417
422
return closest_object_width.value () + parameters_->extra_width_margin_for_rear_obstacle >
418
- gap_between_ego_and_lane_border;
423
+ gap_between_ego_and_lane_border. value () ;
419
424
}
420
425
421
426
bool StartPlannerModule::isOverlapWithCenterLane () const
0 commit comments