22
22
#include " behavior_path_planner_common/utils/traffic_light_utils.hpp"
23
23
#include " tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
24
24
25
+ #include < lanelet2_extension/utility/message_conversion.hpp>
25
26
#include < tier4_autoware_utils/geometry/geometry.hpp>
26
27
#include < tier4_autoware_utils/ros/uuid_helper.hpp>
27
28
@@ -485,11 +486,11 @@ bool isObjectOnRoadShoulder(
485
486
ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler,
486
487
const std::shared_ptr<AvoidanceParameters> & parameters)
487
488
{
488
- using boost::geometry::return_centroid;
489
489
using boost::geometry::within;
490
490
using lanelet::geometry::distance2d;
491
491
using lanelet::geometry::toArcCoordinates;
492
492
using lanelet::utils::to2D;
493
+ using lanelet::utils::conversion::toLaneletPoint;
493
494
494
495
// assume that there are no parked vehicles in intersection.
495
496
std::string turn_direction = object.overhang_lanelet .attributeOr (" turn_direction" , " else" );
@@ -507,13 +508,9 @@ bool isObjectOnRoadShoulder(
507
508
// +: object position
508
509
// o: nearest point on centerline
509
510
510
- lanelet::BasicPoint2d object_centroid (object.centroid .x (), object.centroid .y ());
511
-
512
511
const auto & object_pose = object.object .kinematics .initial_pose_with_covariance .pose ;
513
- const auto centerline_pose =
514
- lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pose.position );
515
- lanelet::BasicPoint3d centerline_point (
516
- centerline_pose.position .x , centerline_pose.position .y , centerline_pose.position .z );
512
+ const auto centerline_pos =
513
+ lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pose.position ).position ;
517
514
518
515
bool is_left_side_parked_vehicle = false ;
519
516
if (!isOnRight (object)) {
@@ -535,8 +532,9 @@ bool isObjectOnRoadShoulder(
535
532
}
536
533
}
537
534
538
- const auto center_to_left_boundary =
539
- distance2d (to2D (most_left_lanelet.leftBound ().basicLineString ()), to2D (centerline_point));
535
+ const auto center_to_left_boundary = distance2d (
536
+ to2D (most_left_lanelet.leftBound ().basicLineString ()),
537
+ to2D (toLaneletPoint (centerline_pos)).basicPoint ());
540
538
541
539
return std::make_pair (
542
540
center_to_left_boundary - 0.5 * object.object .shape .dimensions .y , sub_type);
@@ -547,7 +545,8 @@ bool isObjectOnRoadShoulder(
547
545
}
548
546
549
547
const auto arc_coordinates = toArcCoordinates (
550
- to2D (object.overhang_lanelet .centerline ().basicLineString ()), object_centroid);
548
+ to2D (object.overhang_lanelet .centerline ().basicLineString ()),
549
+ to2D (toLaneletPoint (object_pose.position )).basicPoint ());
551
550
object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;
552
551
553
552
is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio ;
@@ -573,8 +572,9 @@ bool isObjectOnRoadShoulder(
573
572
}
574
573
}
575
574
576
- const auto center_to_right_boundary =
577
- distance2d (to2D (most_right_lanelet.rightBound ().basicLineString ()), to2D (centerline_point));
575
+ const auto center_to_right_boundary = distance2d (
576
+ to2D (most_right_lanelet.rightBound ().basicLineString ()),
577
+ to2D (toLaneletPoint (centerline_pos)).basicPoint ());
578
578
579
579
return std::make_pair (
580
580
center_to_right_boundary - 0.5 * object.object .shape .dimensions .y , sub_type);
@@ -585,7 +585,8 @@ bool isObjectOnRoadShoulder(
585
585
}
586
586
587
587
const auto arc_coordinates = toArcCoordinates (
588
- to2D (object.overhang_lanelet .centerline ().basicLineString ()), object_centroid);
588
+ to2D (object.overhang_lanelet .centerline ().basicLineString ()),
589
+ to2D (toLaneletPoint (object_pose.position )).basicPoint ());
589
590
object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;
590
591
591
592
is_right_side_parked_vehicle =
@@ -759,6 +760,8 @@ bool isSatisfiedWithVehicleCondition(
759
760
const std::shared_ptr<AvoidanceParameters> & parameters)
760
761
{
761
762
using boost::geometry::within;
763
+ using lanelet::utils::to2D;
764
+ using lanelet::utils::conversion::toLaneletPoint;
762
765
763
766
object.behavior = getObjectBehavior (object, parameters);
764
767
object.is_within_intersection = isWithinIntersection (object, planner_data->route_handler );
@@ -769,9 +772,10 @@ bool isSatisfiedWithVehicleCondition(
769
772
}
770
773
771
774
// check vehicle shift ratio
772
- lanelet::BasicPoint2d object_centroid (object.centroid .x (), object.centroid .y ());
773
- const auto on_ego_driving_lane =
774
- within (object_centroid, object.overhang_lanelet .polygon2d ().basicPolygon ());
775
+ const auto & object_pos = object.object .kinematics .initial_pose_with_covariance .pose .position ;
776
+ const auto on_ego_driving_lane = within (
777
+ to2D (toLaneletPoint (object_pos)).basicPoint (),
778
+ object.overhang_lanelet .polygon2d ().basicPolygon ());
775
779
if (on_ego_driving_lane) {
776
780
if (isObjectOnRoadShoulder (object, planner_data->route_handler , parameters)) {
777
781
return true ;
0 commit comments