@@ -445,6 +445,14 @@ bool isWithinIntersection(
445
445
object_polygon, utils::toPolygon2d (lanelet::utils::to2D (polygon.basicPolygon ())));
446
446
}
447
447
448
+ bool isOnEgoLane (const ObjectData & object)
449
+ {
450
+ const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
451
+ return boost::geometry::within (
452
+ lanelet::utils::to2D (lanelet::utils::conversion::toLaneletPoint (object_pos)).basicPoint (),
453
+ object.overhang_lanelet .polygon2d ().basicPolygon ());
454
+ }
455
+
448
456
bool isParallelToEgoLane (const ObjectData & object, const double threshold)
449
457
{
450
458
const auto & object_pose = object.object .kinematics .initial_pose_with_covariance .pose ;
@@ -492,6 +500,10 @@ bool isParkedVehicle(
492
500
using lanelet::utils::to2D;
493
501
using lanelet::utils::conversion::toLaneletPoint;
494
502
503
+ if (object.is_within_intersection ) {
504
+ return false ;
505
+ }
506
+
495
507
const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
496
508
const auto centerline_pos =
497
509
lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pos).position ;
@@ -566,15 +578,11 @@ bool isParkedVehicle(
566
578
return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
567
579
}
568
580
569
- bool isForceAvoidanceTarget (
581
+ bool isAmbiguousStoppedVehicle (
570
582
ObjectData & object, const AvoidancePlanningData & data,
571
583
const std::shared_ptr<const PlannerData> & planner_data,
572
584
const std::shared_ptr<AvoidanceParameters> & parameters)
573
585
{
574
- if (!parameters->enable_force_avoidance_for_stopped_vehicle ) {
575
- return false ;
576
- }
577
-
578
586
const auto stop_time_longer_than_threshold =
579
587
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle ;
580
588
@@ -605,6 +613,10 @@ bool isForceAvoidanceTarget(
605
613
return false ;
606
614
}
607
615
616
+ if (!object.is_on_ego_lane ) {
617
+ return true ;
618
+ }
619
+
608
620
const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
609
621
610
622
// force avoidance for stopped vehicle
@@ -731,24 +743,17 @@ bool isSatisfiedWithVehicleCondition(
731
743
const std::shared_ptr<const PlannerData> & planner_data,
732
744
const std::shared_ptr<AvoidanceParameters> & parameters)
733
745
{
734
- using boost::geometry::within;
735
- using lanelet::utils::to2D;
736
- using lanelet::utils::conversion::toLaneletPoint;
737
-
738
746
object.behavior = getObjectBehavior (object, parameters);
739
- object.is_within_intersection = isWithinIntersection (object, planner_data->route_handler );
747
+ object.is_on_ego_lane = isOnEgoLane (object);
748
+ object.is_ambiguous = isAmbiguousStoppedVehicle (object, data, planner_data, parameters);
740
749
741
750
// from here condition check for vehicle type objects.
742
- if (isForceAvoidanceTarget ( object, data, planner_data, parameters) ) {
751
+ if (object. is_ambiguous && parameters-> enable_force_avoidance_for_stopped_vehicle ) {
743
752
return true ;
744
753
}
745
754
746
755
// check vehicle shift ratio
747
- const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
748
- const auto on_ego_driving_lane = within (
749
- to2D (toLaneletPoint (object_pos)).basicPoint (),
750
- object.overhang_lanelet .polygon2d ().basicPolygon ());
751
- if (on_ego_driving_lane) {
756
+ if (object.is_on_ego_lane ) {
752
757
if (object.is_parked ) {
753
758
return true ;
754
759
} else {
@@ -1708,6 +1713,8 @@ void filterTargetObjects(
1708
1713
}
1709
1714
1710
1715
if (filtering_utils::isVehicleTypeObject (o)) {
1716
+ o.is_within_intersection =
1717
+ filtering_utils::isWithinIntersection (o, planner_data->route_handler );
1711
1718
o.is_parked = filtering_utils::isParkedVehicle (o, planner_data->route_handler , parameters);
1712
1719
o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1713
1720
0 commit comments