@@ -444,6 +444,14 @@ bool isWithinIntersection(
444
444
object_polygon, utils::toPolygon2d (lanelet::utils::to2D (polygon.basicPolygon ())));
445
445
}
446
446
447
+ bool isOnEgoLane (const ObjectData & object)
448
+ {
449
+ const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
450
+ return boost::geometry::within (
451
+ lanelet::utils::to2D (lanelet::utils::conversion::toLaneletPoint (object_pos)).basicPoint (),
452
+ object.overhang_lanelet .polygon2d ().basicPolygon ());
453
+ }
454
+
447
455
bool isParallelToEgoLane (const ObjectData & object, const double threshold)
448
456
{
449
457
const auto & object_pose = object.object .kinematics .initial_pose_with_covariance .pose ;
@@ -491,6 +499,10 @@ bool isParkedVehicle(
491
499
using lanelet::utils::to2D;
492
500
using lanelet::utils::conversion::toLaneletPoint;
493
501
502
+ if (object.is_within_intersection ) {
503
+ return false ;
504
+ }
505
+
494
506
const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
495
507
const auto centerline_pos =
496
508
lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pos).position ;
@@ -579,15 +591,11 @@ bool isParkedVehicle(
579
591
return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
580
592
}
581
593
582
- bool isForceAvoidanceTarget (
594
+ bool isAmbiguousStoppedVehicle (
583
595
ObjectData & object, const AvoidancePlanningData & data,
584
596
const std::shared_ptr<const PlannerData> & planner_data,
585
597
const std::shared_ptr<AvoidanceParameters> & parameters)
586
598
{
587
- if (!parameters->enable_force_avoidance_for_stopped_vehicle ) {
588
- return false ;
589
- }
590
-
591
599
const auto stop_time_longer_than_threshold =
592
600
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle ;
593
601
@@ -618,6 +626,10 @@ bool isForceAvoidanceTarget(
618
626
return false ;
619
627
}
620
628
629
+ if (!object.is_on_ego_lane ) {
630
+ return true ;
631
+ }
632
+
621
633
const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
622
634
623
635
// force avoidance for stopped vehicle
@@ -744,24 +756,17 @@ bool isSatisfiedWithVehicleCondition(
744
756
const std::shared_ptr<const PlannerData> & planner_data,
745
757
const std::shared_ptr<AvoidanceParameters> & parameters)
746
758
{
747
- using boost::geometry::within;
748
- using lanelet::utils::to2D;
749
- using lanelet::utils::conversion::toLaneletPoint;
750
-
751
759
object.behavior = getObjectBehavior (object, parameters);
752
- object.is_within_intersection = isWithinIntersection (object, planner_data->route_handler );
760
+ object.is_on_ego_lane = isOnEgoLane (object);
761
+ object.is_ambiguous = isAmbiguousStoppedVehicle (object, data, planner_data, parameters);
753
762
754
763
// from here condition check for vehicle type objects.
755
- if (isForceAvoidanceTarget ( object, data, planner_data, parameters) ) {
764
+ if (object. is_ambiguous && parameters-> enable_force_avoidance_for_stopped_vehicle ) {
756
765
return true ;
757
766
}
758
767
759
768
// check vehicle shift ratio
760
- const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
761
- const auto on_ego_driving_lane = within (
762
- to2D (toLaneletPoint (object_pos)).basicPoint (),
763
- object.overhang_lanelet .polygon2d ().basicPolygon ());
764
- if (on_ego_driving_lane) {
769
+ if (object.is_on_ego_lane ) {
765
770
if (object.is_parked ) {
766
771
return true ;
767
772
} else {
@@ -1721,6 +1726,8 @@ void filterTargetObjects(
1721
1726
}
1722
1727
1723
1728
if (filtering_utils::isVehicleTypeObject (o)) {
1729
+ o.is_within_intersection =
1730
+ filtering_utils::isWithinIntersection (o, planner_data->route_handler );
1724
1731
o.is_parked = filtering_utils::isParkedVehicle (o, planner_data->route_handler , parameters);
1725
1732
o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1726
1733
0 commit comments