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