@@ -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 ;
@@ -580,15 +592,11 @@ bool isParkedVehicle(
580
592
return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
581
593
}
582
594
583
- bool isForceAvoidanceTarget (
595
+ bool isAmbiguousStoppedVehicle (
584
596
ObjectData & object, const AvoidancePlanningData & data,
585
597
const std::shared_ptr<const PlannerData> & planner_data,
586
598
const std::shared_ptr<AvoidanceParameters> & parameters)
587
599
{
588
- if (!parameters->enable_force_avoidance_for_stopped_vehicle ) {
589
- return false ;
590
- }
591
-
592
600
const auto stop_time_longer_than_threshold =
593
601
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle ;
594
602
@@ -619,6 +627,10 @@ bool isForceAvoidanceTarget(
619
627
return false ;
620
628
}
621
629
630
+ if (!object.is_on_ego_lane ) {
631
+ return true ;
632
+ }
633
+
622
634
const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
623
635
624
636
// force avoidance for stopped vehicle
@@ -745,24 +757,17 @@ bool isSatisfiedWithVehicleCondition(
745
757
const std::shared_ptr<const PlannerData> & planner_data,
746
758
const std::shared_ptr<AvoidanceParameters> & parameters)
747
759
{
748
- using boost::geometry::within;
749
- using lanelet::utils::to2D;
750
- using lanelet::utils::conversion::toLaneletPoint;
751
-
752
760
object.behavior = getObjectBehavior (object, parameters);
753
- object.is_within_intersection = isWithinIntersection (object, planner_data->route_handler );
761
+ object.is_on_ego_lane = isOnEgoLane (object);
762
+ object.is_ambiguous = isAmbiguousStoppedVehicle (object, data, planner_data, parameters);
754
763
755
764
// from here condition check for vehicle type objects.
756
- if (isForceAvoidanceTarget ( object, data, planner_data, parameters) ) {
765
+ if (object. is_ambiguous && parameters-> enable_force_avoidance_for_stopped_vehicle ) {
757
766
return true ;
758
767
}
759
768
760
769
// check vehicle shift ratio
761
- const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
762
- const auto on_ego_driving_lane = within (
763
- to2D (toLaneletPoint (object_pos)).basicPoint (),
764
- object.overhang_lanelet .polygon2d ().basicPolygon ());
765
- if (on_ego_driving_lane) {
770
+ if (object.is_on_ego_lane ) {
766
771
if (object.is_parked ) {
767
772
return true ;
768
773
} else {
@@ -1722,6 +1727,8 @@ void filterTargetObjects(
1722
1727
}
1723
1728
1724
1729
if (filtering_utils::isVehicleTypeObject (o)) {
1730
+ o.is_within_intersection =
1731
+ filtering_utils::isWithinIntersection (o, planner_data->route_handler );
1725
1732
o.is_parked = filtering_utils::isParkedVehicle (o, planner_data->route_handler , parameters);
1726
1733
o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1727
1734
0 commit comments