Skip to content

Commit 7693f63

Browse files
satoshi-otaHansRobo
andcommitted
fix(avoidance): don't use polygon centroid in shiftable ratio calculation (autowarefoundation#6285)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Co-authored-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent 2c948eb commit 7693f63

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
@@ -22,6 +22,7 @@
2222
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
2323
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
2424

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

@@ -485,11 +486,11 @@ bool isObjectOnRoadShoulder(
485486
ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler,
486487
const std::shared_ptr<AvoidanceParameters> & parameters)
487488
{
488-
using boost::geometry::return_centroid;
489489
using boost::geometry::within;
490490
using lanelet::geometry::distance2d;
491491
using lanelet::geometry::toArcCoordinates;
492492
using lanelet::utils::to2D;
493+
using lanelet::utils::conversion::toLaneletPoint;
493494

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

510-
lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());
511-
512511
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;
517514

518515
bool is_left_side_parked_vehicle = false;
519516
if (!isOnRight(object)) {
@@ -535,8 +532,9 @@ bool isObjectOnRoadShoulder(
535532
}
536533
}
537534

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

541539
return std::make_pair(
542540
center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
@@ -547,7 +545,8 @@ bool isObjectOnRoadShoulder(
547545
}
548546

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

553552
is_left_side_parked_vehicle =
@@ -574,8 +573,9 @@ bool isObjectOnRoadShoulder(
574573
}
575574
}
576575

577-
const auto center_to_right_boundary =
578-
distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point));
576+
const auto center_to_right_boundary = distance2d(
577+
to2D(most_right_lanelet.rightBound().basicLineString()),
578+
to2D(toLaneletPoint(centerline_pos)).basicPoint());
579579

580580
return std::make_pair(
581581
center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
@@ -586,7 +586,8 @@ bool isObjectOnRoadShoulder(
586586
}
587587

588588
const auto arc_coordinates = toArcCoordinates(
589-
to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid);
589+
to2D(object.overhang_lanelet.centerline().basicLineString()),
590+
to2D(toLaneletPoint(object_pose.position)).basicPoint());
590591
object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;
591592

592593
is_right_side_parked_vehicle =
@@ -762,6 +763,8 @@ bool isSatisfiedWithVehicleCondition(
762763
const std::shared_ptr<AvoidanceParameters> & parameters)
763764
{
764765
using boost::geometry::within;
766+
using lanelet::utils::to2D;
767+
using lanelet::utils::conversion::toLaneletPoint;
765768

766769
object.behavior = getObjectBehavior(object, parameters);
767770
object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
@@ -772,9 +775,10 @@ bool isSatisfiedWithVehicleCondition(
772775
}
773776

774777
// check vehicle shift ratio
775-
lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());
776-
const auto on_ego_driving_lane =
777-
within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon());
778+
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
779+
const auto on_ego_driving_lane = within(
780+
to2D(toLaneletPoint(object_pos)).basicPoint(),
781+
object.overhang_lanelet.polygon2d().basicPolygon());
778782
if (on_ego_driving_lane) {
779783
if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) {
780784
return true;

0 commit comments

Comments
 (0)