@@ -42,8 +42,10 @@ std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
42
42
const auto & ego_pose = ego_data.path .points [path_idx].point .pose ;
43
43
const auto angle_diff = tier4_autoware_utils::normalizeRadian (
44
44
tf2::getYaw (ego_pose.orientation ) - tf2::getYaw (object_pose.orientation ));
45
- if ((!params.ignore_objects_behind_ego && std::abs (angle_diff) < params.yaw_threshold_behind_object ) ||
46
- std::abs (angle_diff) > params.yaw_threshold ) {
45
+ if (
46
+ (!params.ignore_objects_behind_ego &&
47
+ std::abs (angle_diff) < params.yaw_threshold_behind_object ) ||
48
+ std::abs (angle_diff) > params.yaw_threshold ) {
47
49
tier4_autoware_utils::MultiPoint2d collision_points;
48
50
boost::geometry::intersection (
49
51
object_footprint.outer (), ego_footprint.outer (), collision_points);
@@ -64,13 +66,15 @@ std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
64
66
std::vector<Collision> find_collisions (
65
67
const EgoData & ego_data,
66
68
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
67
- const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints, const PlannerParam & params)
69
+ const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints,
70
+ const PlannerParam & params)
68
71
{
69
72
std::vector<Collision> collisions;
70
73
for (auto object_idx = 0UL ; object_idx < objects.size (); ++object_idx) {
71
74
const auto & object_pose = objects[object_idx].kinematics .initial_pose_with_covariance .pose ;
72
75
const auto & object_footprint = object_forward_footprints[object_idx];
73
- const auto collision = find_closest_collision_point (ego_data, object_pose, object_footprint, params);
76
+ const auto collision =
77
+ find_closest_collision_point (ego_data, object_pose, object_footprint, params);
74
78
if (collision) {
75
79
Collision c;
76
80
c.object_uuid = tier4_autoware_utils::toHexString (objects[object_idx].object_id );
0 commit comments