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.

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

feat(start_planner): keep distance back static objects from start pose #6149

Closed
Changes from 1 commit
Commits
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
Prev Previous commit
Next Next commit
update
Signed-off-by: kyoichi-sugahara <[email protected]>
kyoichi-sugahara committed Feb 11, 2024
commit 2b7d88f1c3abd574710c39a878bd1ef5166d8bb7
Original file line number Diff line number Diff line change
@@ -6,7 +6,7 @@
th_stopped_velocity: 0.01
th_stopped_time: 1.0
objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1]
back_objects_collision_check_margin: 3.0
back_objects_collision_check_margin: 0.5
collision_check_margin_from_front_object: 5.0
th_moving_object_velocity: 1.0
th_distance_to_middle_of_the_road: 0.5
Original file line number Diff line number Diff line change
@@ -67,7 +67,6 @@ Pose getBackedPose(
* lanelets. It compares each corner of the vehicle's transformed footprint with every corner of
* object polygons to find the shortest distance within the lanelet system.
*/

double calcMinArcLengthDistanceFromEgoToObjects(
const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose,
const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects);
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1286 to 1290, 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.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -964,54 +964,54 @@
const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;

const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
backward_path_length);

const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
std::numeric_limits<double>::max(), 0);

// Set the maximum backward distance less than the distance from the vehicle's base_link to
// the lane's rearmost point to prevent lane departure.
const double current_arc_length =
lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
const double allowed_backward_distance = std::clamp(
current_arc_length - planner_data_->parameters.base_link2rear, 0.0,
parameters_->max_back_distance);

for (double back_distance = 0.0; back_distance <= allowed_backward_distance;
back_distance += parameters_->backward_search_resolution) {
const auto backed_pose = calcLongitudinalOffsetPose(
back_path_from_start_pose.points, start_pose.position, -back_distance);
if (!backed_pose) continue;

if (utils::checkCollisionBetweenFootprintAndObjects(
local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes,
parameters_->collision_check_margin_from_front_object))
continue;

const double backed_pose_arc_length =
lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length;
const double length_to_lane_end = std::accumulate(
std::begin(pull_out_lanes), std::end(pull_out_lanes), 0.0,
[](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); });
const double distance_from_lane_end = length_to_lane_end - backed_pose_arc_length;
if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000,
"the ego is too close to the lane end, so needs backward driving");
continue;
}

if (
(start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects(
local_vehicle_footprint, *backed_pose, pull_out_lanes,
back_stop_objects_in_pull_out_lanes) > parameters_->back_objects_collision_check_margin) &&
back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) ||
(utils::checkCollisionBetweenFootprintAndObjects(
local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes,
parameters_->objects_collision_check_margins.back()))) {
break; // poses behind this is too close to back static object, so break.

Check warning on line 1014 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

StartPlannerModule::searchPullOutStartPoseCandidates increases in cyclomatic complexity from 9 to 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
}

pull_out_start_pose_candidates.push_back(*backed_pose);

Unchanged files with check annotations Beta

p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
p.objects_collision_check_margins =
node->declare_parameter<std::vector<double>>(ns + "objects_collision_check_margins");
p.back_objects_collision_check_margin =
node->declare_parameter<double>(ns + "back_objects_collision_check_margin");

Check warning on line 49 in planning/behavior_path_start_planner_module/src/manager.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

StartPlannerModuleManager::init increases from 266 to 268 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
p.collision_check_margin_from_front_object =
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");