@@ -478,6 +478,14 @@ bool isWithinIntersection(
478
478
object_polygon, utils::toPolygon2d (lanelet::utils::to2D (polygon.basicPolygon ())));
479
479
}
480
480
481
+ bool isOnEgoLane (const ObjectData & object)
482
+ {
483
+ const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
484
+ return boost::geometry::within (
485
+ lanelet::utils::to2D (lanelet::utils::conversion::toLaneletPoint (object_pos)).basicPoint (),
486
+ object.overhang_lanelet .polygon2d ().basicPolygon ());
487
+ }
488
+
481
489
bool isParallelToEgoLane (const ObjectData & object, const double threshold)
482
490
{
483
491
const auto & object_pose = object.object .kinematics .initial_pose_with_covariance .pose ;
@@ -525,6 +533,10 @@ bool isParkedVehicle(
525
533
using lanelet::utils::to2D;
526
534
using lanelet::utils::conversion::toLaneletPoint;
527
535
536
+ if (object.is_within_intersection ) {
537
+ return false ;
538
+ }
539
+
528
540
const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
529
541
const auto centerline_pos =
530
542
lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pos).position ;
@@ -613,15 +625,11 @@ bool isParkedVehicle(
613
625
return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
614
626
}
615
627
616
- bool isForceAvoidanceTarget (
628
+ bool isAmbiguousStoppedVehicle (
617
629
ObjectData & object, const AvoidancePlanningData & data,
618
630
const std::shared_ptr<const PlannerData> & planner_data,
619
631
const std::shared_ptr<AvoidanceParameters> & parameters)
620
632
{
621
- if (!parameters->enable_force_avoidance_for_stopped_vehicle ) {
622
- return false ;
623
- }
624
-
625
633
const auto stop_time_longer_than_threshold =
626
634
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle ;
627
635
@@ -652,6 +660,10 @@ bool isForceAvoidanceTarget(
652
660
return false ;
653
661
}
654
662
663
+ if (!object.is_on_ego_lane ) {
664
+ return true ;
665
+ }
666
+
655
667
const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
656
668
657
669
// force avoidance for stopped vehicle
@@ -778,24 +790,17 @@ bool isSatisfiedWithVehicleCondition(
778
790
const std::shared_ptr<const PlannerData> & planner_data,
779
791
const std::shared_ptr<AvoidanceParameters> & parameters)
780
792
{
781
- using boost::geometry::within;
782
- using lanelet::utils::to2D;
783
- using lanelet::utils::conversion::toLaneletPoint;
784
-
785
793
object.behavior = getObjectBehavior (object, parameters);
786
- object.is_within_intersection = isWithinIntersection (object, planner_data->route_handler );
794
+ object.is_on_ego_lane = isOnEgoLane (object);
795
+ object.is_ambiguous = isAmbiguousStoppedVehicle (object, data, planner_data, parameters);
787
796
788
797
// from here condition check for vehicle type objects.
789
- if (isForceAvoidanceTarget ( object, data, planner_data, parameters) ) {
798
+ if (object. is_ambiguous && parameters-> enable_force_avoidance_for_stopped_vehicle ) {
790
799
return true ;
791
800
}
792
801
793
802
// check vehicle shift ratio
794
- const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
795
- const auto on_ego_driving_lane = within (
796
- to2D (toLaneletPoint (object_pos)).basicPoint (),
797
- object.overhang_lanelet .polygon2d ().basicPolygon ());
798
- if (on_ego_driving_lane) {
803
+ if (object.is_on_ego_lane ) {
799
804
if (object.is_parked ) {
800
805
return true ;
801
806
} else {
@@ -1755,6 +1760,8 @@ void filterTargetObjects(
1755
1760
}
1756
1761
1757
1762
if (filtering_utils::isVehicleTypeObject (o)) {
1763
+ o.is_within_intersection =
1764
+ filtering_utils::isWithinIntersection (o, planner_data->route_handler );
1758
1765
o.is_parked = filtering_utils::isParkedVehicle (o, planner_data->route_handler , parameters);
1759
1766
o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1760
1767
0 commit comments