Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(static_obstacle_avoidance): fix issues in target object filtering logic #7830

Merged
merged 4 commits into from
Jul 4, 2024
Merged

fix(static_obstacle_avoidance): fix issues in target object filtering logic #7830

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e27f497
fix(static_obstacle_avoidance): check if object is inside/outside by …
satoshi-ota Jul 1, 2024
81efab8
refactor(static_obstacle_avoidance): add getter functions
satoshi-ota Jul 2, 2024
5da4c47
fix(static_obstacle_avoidance): check next lane without route if the …
satoshi-ota Jul 3, 2024
b6ca523
fix(static_obstacle_avoidance): fix parked vehicle check
satoshi-ota Jul 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions ...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -352,6 +352,10 @@ struct ObjectData // avoidance target
{
}

Pose getPose() const { return object.kinematics.initial_pose_with_covariance.pose; }

Point getPosition() const { return object.kinematics.initial_pose_with_covariance.pose.position; }

PredictedObject object;

// object behavior.
Expand Down
10 changes: 4 additions & 6 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@
}

marker.id = uuidToInt32(object.object.object_id);
marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
marker.pose = object.getPose();

Check warning on line 67 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp#L67 

Added line #L67 was not covered by tests
msg.markers.push_back(marker);
}

Expand All @@ -80,10 +80,8 @@
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));

const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position;

for (const auto & p : object.envelope_poly.outer()) {
marker.points.push_back(createPoint(p.x(), p.y(), pos.z));
marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z));
}

marker.points.push_back(marker.points.front());
Expand Down Expand Up @@ -142,7 +140,7 @@
for (const auto & object : objects) {
if (verbose) {
marker.id = uuidToInt32(object.object.object_id);
marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
marker.pose = object.getPose();

Check warning on line 143 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp#L143 

Added line #L143 was not covered by tests
std::ostringstream string_stream;
string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
string_stream << "ratio:" << object.shiftable_ratio << " [-]\n"
Expand All @@ -162,7 +160,7 @@

{
marker.id = uuidToInt32(object.object.object_id);
marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
marker.pose = object.getPose();

Check warning on line 163 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp#L163 

Added line #L163 was not covered by tests
marker.pose.position.z += 2.0;
std::ostringstream string_stream;
string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : "");
Expand Down
16 changes: 6 additions & 10 deletions ...nner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,9 +165,9 @@
}

// the avoidance path is already approved
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) ||
(helper_->getShift(object_pos) < 0.0 && !is_object_on_right);
const auto is_approved =
(helper_->getShift(object.getPosition()) > 0.0 && is_object_on_right) ||
(helper_->getShift(object.getPosition()) < 0.0 && !is_object_on_right);
if (is_approved) {
return std::make_pair(desire_shift_length, avoidance_distance);
}
Expand Down Expand Up @@ -363,9 +363,8 @@
if (is_return_shift_to_goal) {
return true;
}
const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position;
const bool has_object_near_goal =
autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) <
autoware::universe_utils::calcDistance2d(goal_pose.position, o.getPosition()) <

Check warning on line 367 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp#L367 

Added line #L367 was not covered by tests
parameters_->object_check_goal_distance;
return has_object_near_goal;
}();
Expand Down Expand Up @@ -1027,8 +1026,7 @@
const auto has_object_near_goal =
std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
return autoware::universe_utils::calcDistance2d(
data_->route_handler->getGoalPose().position,
o.object.kinematics.initial_pose_with_covariance.pose.position) <
data_->route_handler->getGoalPose().position, o.getPosition()) <

Check warning on line 1029 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp#L1029 

Added line #L1029 was not covered by tests
parameters_->object_check_goal_distance;
});
if (has_object_near_goal) {
Expand Down Expand Up @@ -1097,9 +1095,7 @@
if (utils::isAllowedGoalModification(data_->route_handler)) {
const bool has_last_shift_near_goal =
std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
return autoware::universe_utils::calcDistance2d(
last_sl.end.position,
o.object.kinematics.initial_pose_with_covariance.pose.position) <
return autoware::universe_utils::calcDistance2d(last_sl.end.position, o.getPosition()) <

Check warning on line 1098 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp#L1098 

Added line #L1098 was not covered by tests
parameters_->object_check_goal_distance;
});
if (has_last_shift_near_goal) {
Expand Down
84 changes: 45 additions & 39 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1902 to 1908, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 5.27 to 5.30, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -293,8 +293,7 @@
{
using Point = boost::geometry::model::d2::point_xy<double>;

const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position;
const Point p_object{p.x, p.y};
const Point p_object{object.getPosition().x, object.getPosition().y};

// get conflicting crosswalk crosswalk
constexpr int PEDESTRIAN_GRAPH_ID = 1;
Expand Down Expand Up @@ -335,57 +334,68 @@
route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str()));

return boost::geometry::within(
object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
.basicPoint(),
lanelet::utils::to2D(polygon.basicPolygon()));
}

bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
{
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
if (boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
.basicPoint(),
object.overhang_lanelet.polygon2d().basicPolygon())) {
return true;
}

// push previous lanelet
lanelet::ConstLanelets prev_lanelet;
if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) {
if (boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
.basicPoint(),
prev_lanelet.front().polygon2d().basicPolygon())) {
return true;
}
}

// push next lanelet
lanelet::ConstLanelet next_lanelet;
if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) {
if (boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
.basicPoint(),
next_lanelet.polygon2d().basicPolygon())) {
return true;
}
} else {
for (const auto & lane : route_handler->getNextLanelets(object.overhang_lanelet)) {
if (boost::geometry::within(

Check warning on line 373 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L373 

Added line #L373 was not covered by tests
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
.basicPoint(),
lane.polygon2d().basicPolygon())) {
return true;
}
}

Check notice on line 379 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead 

isOnEgoLane increases from 2 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}

return false;
}

bool isParallelToEgoLane(const ObjectData & object, const double threshold)
{
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
const auto closest_pose =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose));
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition());
const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object.getPose()));

Check warning on line 389 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L388-L389 

Added lines #L388 - L389 were not covered by tests

return yaw_deviation < threshold || yaw_deviation > M_PI - threshold;
}

bool isMergingToEgoLane(const ObjectData & object)
{
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
const auto closest_pose =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose);
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition());
const auto yaw_deviation = calcYawDeviation(closest_pose, object.getPose());

Check warning on line 398 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L397-L398 

Added lines #L397 - L398 were not covered by tests

if (isOnRight(object)) {
if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) {
Expand Down Expand Up @@ -422,9 +432,8 @@
return false;
}

const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
const auto centerline_pos =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position;
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()).position;

Check warning on line 436 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L436 

Added line #L436 was not covered by tests

bool is_left_side_parked_vehicle = false;
if (!isOnRight(object)) {
Expand All @@ -442,7 +451,8 @@
return same_direction_lane;
}

return static_cast<lanelet::ConstLanelet>(opposite_lanes.front().invert());
return static_cast<lanelet::ConstLanelet>(
route_handler->getMostRightLanelet(opposite_lanes.front()).invert());
}();

const auto center_to_left_boundary = distance2d(
Expand All @@ -457,7 +467,7 @@
if (sub_type == "road_shoulder") {
// assuming it's parked vehicle if its CoG is within road shoulder lanelet.
if (boost::geometry::within(
to2D(toLaneletPoint(object_pos)).basicPoint(),
to2D(toLaneletPoint(object.getPosition())).basicPoint(),
most_left_lanelet.polygon2d().basicPolygon())) {
return true;
}
Expand All @@ -468,7 +478,7 @@

const auto arc_coordinates = toArcCoordinates(
to2D(object.overhang_lanelet.centerline().basicLineString()),
to2D(toLaneletPoint(object_pos)).basicPoint());
to2D(toLaneletPoint(object.getPosition())).basicPoint());
object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;

is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio;
Expand All @@ -490,7 +500,8 @@
return same_direction_lane;
}

return static_cast<lanelet::ConstLanelet>(opposite_lanes.front().invert());
return static_cast<lanelet::ConstLanelet>(
route_handler->getMostLeftLanelet(opposite_lanes.front()).invert());
}();

const auto center_to_right_boundary = distance2d(
Expand All @@ -505,7 +516,7 @@
if (sub_type == "road_shoulder") {
// assuming it's parked vehicle if its CoG is within road shoulder lanelet.
if (boost::geometry::within(
to2D(toLaneletPoint(object_pos)).basicPoint(),
to2D(toLaneletPoint(object.getPosition())).basicPoint(),
most_right_lanelet.polygon2d().basicPolygon())) {
return true;
}
Expand All @@ -516,7 +527,7 @@

const auto arc_coordinates = toArcCoordinates(
to2D(object.overhang_lanelet.centerline().basicLineString()),
to2D(toLaneletPoint(object_pos)).basicPoint());
to2D(toLaneletPoint(object.getPosition())).basicPoint());
object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;

is_right_side_parked_vehicle =
Expand All @@ -527,9 +538,8 @@
return false;
}

const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
object.to_centerline =
lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance;
if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
return false;
}
Expand All @@ -544,13 +554,13 @@
{
const auto & rh = planner_data->route_handler;
const auto & ego_pose = planner_data->self_odometry->pose.pose;
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;

// force avoidance for stopped vehicle
bool is_close_to_stop_factor = false;

// check traffic light
const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets);
const auto to_traffic_light =
getDistanceToNextTrafficLight(object.getPose(), data.extend_lanelets);

Check warning on line 563 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L563 

Added line #L563 was not covered by tests
{
is_close_to_stop_factor =
to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
Expand Down Expand Up @@ -792,9 +802,8 @@
}

// Object is on center line -> ignore.
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
object.to_centerline =
lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance;
if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
object.info = ObjectInfo::TOO_NEAR_TO_CENTERLINE;
return false;
Expand Down Expand Up @@ -844,9 +853,8 @@
return false;
}

const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose;
const auto is_moving_distance_longer_than_threshold =
calcDistance2d(object.init_pose, current_pose) >
calcDistance2d(object.init_pose, object.getPose()) >
parameters->distance_threshold_for_ambiguous_vehicle;
if (is_moving_distance_longer_than_threshold) {
object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
Expand Down Expand Up @@ -944,9 +952,8 @@
using autoware::universe_utils::Point2d;
using lanelet::utils::to2D;

const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
const auto object_closest_index =
autoware::motion_utils::findNearestIndex(data.reference_path.points, object_pose.position);
autoware::motion_utils::findNearestIndex(data.reference_path.points, object.getPosition());
const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose;

const auto rh = planner_data->route_handler;
Expand All @@ -957,7 +964,7 @@
std::vector<std::tuple<double, Point, Point>> intersects;
for (const auto & p1 : object.overhang_points) {
const auto centerline_pose =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition());
const auto p_tmp =
geometry_msgs::build<Pose>().position(p1.second).orientation(centerline_pose.orientation);

Expand Down Expand Up @@ -1326,8 +1333,7 @@
object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0;
const auto obj_poly =
autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer);
obstacles_for_drivable_area.push_back(
{object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)});
obstacles_for_drivable_area.push_back({object.getPose(), obj_poly, !isOnRight(object)});
}
return obstacles_for_drivable_area;
}
Expand Down Expand Up @@ -1495,7 +1501,7 @@
object_data.last_stop = now;
object_data.move_time = 0.0;
if (is_new_object) {
object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
object_data.init_pose = object_data.getPose();

Check warning on line 1504 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L1504 

Added line #L1504 was not covered by tests
object_data.stop_time = 0.0;
object_data.last_move = now;
stopped_objects.push_back(object_data);
Expand All @@ -1510,7 +1516,7 @@
}

if (is_new_object) {
object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
object_data.init_pose = object_data.getPose();

Check warning on line 1519 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L1519 

Added line #L1519 was not covered by tests
object_data.move_time = std::numeric_limits<double>::infinity();
object_data.stop_time = 0.0;
object_data.last_move = now;
Expand All @@ -1520,7 +1526,7 @@
object_data.last_stop = same_id_obj->last_stop;
object_data.move_time = (now - same_id_obj->last_stop).seconds();
object_data.stop_time = 0.0;
object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
object_data.init_pose = object_data.getPose();

Check warning on line 1529 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L1529 

Added line #L1529 was not covered by tests

if (object_data.move_time > object_parameter.moving_time_threshold) {
stopped_objects.erase(same_id_obj);
Expand Down Expand Up @@ -1617,9 +1623,9 @@
}

constexpr auto POS_THR = 1.5;
const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose;
const auto r_pos = registered_object.getPose();
const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) {
return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR;
return calcDistance2d(r_pos, o.getPose()) < POS_THR;

Check warning on line 1628 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp#L1628 

Added line #L1628 was not covered by tests
});

// same id object is not detected, but object is found around registered. update registered.
Expand Down
Loading