Skip to content

Commit 35b42a5

Browse files
fix(avoidance): don't use polygon centroid in shiftable ratio calculation (#6285)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Co-authored-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent 69e16f4 commit 35b42a5

File tree

1 file changed

+20
-16
lines changed
  • planning/behavior_path_avoidance_module/src

1 file changed

+20
-16
lines changed

planning/behavior_path_avoidance_module/src/utils.cpp

+20-16
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
2222
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
2323

24+
#include <lanelet2_extension/utility/message_conversion.hpp>
2425
#include <tier4_autoware_utils/geometry/geometry.hpp>
2526
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
2627

@@ -484,11 +485,11 @@ bool isObjectOnRoadShoulder(
484485
ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler,
485486
const std::shared_ptr<AvoidanceParameters> & parameters)
486487
{
487-
using boost::geometry::return_centroid;
488488
using boost::geometry::within;
489489
using lanelet::geometry::distance2d;
490490
using lanelet::geometry::toArcCoordinates;
491491
using lanelet::utils::to2D;
492+
using lanelet::utils::conversion::toLaneletPoint;
492493

493494
// assume that there are no parked vehicles in intersection.
494495
std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else");
@@ -506,13 +507,9 @@ bool isObjectOnRoadShoulder(
506507
// +: object position
507508
// o: nearest point on centerline
508509

509-
lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());
510-
511510
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;
516513

517514
bool is_left_side_parked_vehicle = false;
518515
if (!isOnRight(object)) {
@@ -534,8 +531,9 @@ bool isObjectOnRoadShoulder(
534531
}
535532
}
536533

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());
539537

540538
return std::make_pair(
541539
center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
@@ -546,7 +544,8 @@ bool isObjectOnRoadShoulder(
546544
}
547545

548546
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());
550549
object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;
551550

552551
is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio;
@@ -572,8 +571,9 @@ bool isObjectOnRoadShoulder(
572571
}
573572
}
574573

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());
577577

578578
return std::make_pair(
579579
center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
@@ -584,7 +584,8 @@ bool isObjectOnRoadShoulder(
584584
}
585585

586586
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());
588589
object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;
589590

590591
is_right_side_parked_vehicle =
@@ -760,6 +761,8 @@ bool isSatisfiedWithVehicleCondition(
760761
const std::shared_ptr<AvoidanceParameters> & parameters)
761762
{
762763
using boost::geometry::within;
764+
using lanelet::utils::to2D;
765+
using lanelet::utils::conversion::toLaneletPoint;
763766

764767
object.behavior = getObjectBehavior(object, parameters);
765768
object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
@@ -770,9 +773,10 @@ bool isSatisfiedWithVehicleCondition(
770773
}
771774

772775
// 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());
776780
if (on_ego_driving_lane) {
777781
if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) {
778782
return true;

0 commit comments

Comments
 (0)