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(out_of_lane): reduce "Same Points are given" messages #893

Merged
merged 1 commit into from
Sep 29, 2023
Merged

fix(out_of_lane): reduce "Same Points are given" messages #893

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
19 changes: 10 additions & 9 deletions planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,31 +70,32 @@ std::optional<std::pair<double, double>> object_time_to_range(
auto worst_exit_time = std::optional<double>();

for (const auto & predicted_path : object.kinematics.predicted_paths) {
const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
const auto enter_point =
geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
const auto enter_segment_idx =
motion_utils::findNearestSegmentIndex(predicted_path.path, enter_point);
motion_utils::findNearestSegmentIndex(unique_path_points, enter_point);
const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment(
predicted_path.path, enter_segment_idx, enter_point);
const auto enter_lat_dist = std::abs(
motion_utils::calcLateralOffset(predicted_path.path, enter_point, enter_segment_idx));
unique_path_points, enter_segment_idx, enter_point);
const auto enter_lat_dist =
std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx));
const auto enter_segment_length = tier4_autoware_utils::calcDistance2d(
predicted_path.path[enter_segment_idx], predicted_path.path[enter_segment_idx + 1]);
unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]);
const auto enter_offset_ratio = enter_offset / enter_segment_length;
const auto enter_time =
static_cast<double>(enter_segment_idx) * time_step + enter_offset_ratio * time_step;

const auto exit_point =
geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y());
const auto exit_segment_idx =
motion_utils::findNearestSegmentIndex(predicted_path.path, exit_point);
motion_utils::findNearestSegmentIndex(unique_path_points, exit_point);
const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment(
predicted_path.path, exit_segment_idx, exit_point);
unique_path_points, exit_segment_idx, exit_point);
const auto exit_lat_dist =
std::abs(motion_utils::calcLateralOffset(predicted_path.path, exit_point, exit_segment_idx));
std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx));
const auto exit_segment_length = tier4_autoware_utils::calcDistance2d(
predicted_path.path[exit_segment_idx], predicted_path.path[exit_segment_idx + 1]);
unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]);
const auto exit_offset_ratio = exit_offset / static_cast<double>(exit_segment_length);
const auto exit_time =
static_cast<double>(exit_segment_idx) * time_step + exit_offset_ratio * time_step;
Expand Down
10 changes: 9 additions & 1 deletion planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,9 @@

namespace behavior_velocity_planner::out_of_lane
{
using motion_utils::calcLateralOffset;
using tier4_autoware_utils::calcDistance2d;

/// @brief filter predicted objects and their predicted paths
/// @param [in] objects predicted objects to filter
/// @param [in] ego_data ego data
Expand All @@ -42,8 +45,13 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
auto filtered_object = object;
const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);

// Note: lateral offset can not be calculated for path with less than 2 unique points
const auto lat_offset_to_current_ego =
std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position));
unique_path_points.size() > 1
? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position))
: calcDistance2d(unique_path_points.front(), ego_data.pose.position);
const auto is_crossing_ego =
lat_offset_to_current_ego <=
object.shape.dimensions.y / 2.0 + std::max(
Expand Down