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): stop position is unstable #7880

Merged
merged 1 commit into from
Jul 9, 2024
Merged

fix(static_obstacle_avoidance): stop position is unstable #7880

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
6 changes: 2 additions & 4 deletions ...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,10 +113,8 @@ void AvoidanceByLaneChange::updateSpecialData()
: Direction::RIGHT;
}

utils::static_obstacle_avoidance::updateRegisteredObject(
registered_objects_, avoidance_data_.target_objects, p);
utils::static_obstacle_avoidance::compensateDetectionLost(
registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects);
utils::static_obstacle_avoidance::compensateLostTargetObjects(
registered_objects_, avoidance_data_, clock_.now(), planner_data_, p);

std::sort(
avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(),
Expand Down
2 changes: 2 additions & 0 deletions ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
MarkerArray createOtherObjectsMarkerArray(
const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose);

MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data);

MarkerArray createDebugMarkerArray(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
const std::shared_ptr<AvoidanceParameters> & parameters);
Expand Down
6 changes: 6 additions & 0 deletions ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,6 +266,12 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
*/
void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const;

/**
* @brief fill additional data which are necessary to plan avoidance path/velocity.
* @param avoidance target objects.
*/
void fillAvoidanceTargetData(ObjectDataArray & objects) const;

/**
* @brief fill candidate shift lines.
* @param avoidance data.
Expand Down
11 changes: 4 additions & 7 deletions ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,15 +118,12 @@ void fillObjectStoppableJudge(
ObjectData & object_data, const ObjectDataArray & registered_objects,
const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters);

void updateRegisteredObject(
ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
const std::shared_ptr<AvoidanceParameters> & parameters);

void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data);

void compensateDetectionLost(
const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
ObjectDataArray & other_objects);
void compensateLostTargetObjects(
ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

void filterTargetObjects(
ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
Expand Down
17 changes: 17 additions & 0 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
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession 

The ratio of primitive types in function arguments decreases from 44.68% to 43.75%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Code Duplication 

The module no longer contains too many functions with similar structure
//
// 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 @@ -466,6 +466,23 @@
return msg;
}

MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data)
{
MarkerArray msg;

if (!data.stop_target_object.has_value()) {
return msg;
}

appendMarkerArray(
createObjectsCubeMarkerArray(
{data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9),
createMarkerColor(1.0, 0.0, 0.42, 0.5)),
&msg);

return msg;
}

MarkerArray createDrivableBounds(
const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g,
const float & b)
Expand Down
46 changes: 29 additions & 17 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.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/scene.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 1264 to 1268, 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/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 5.29 to 5.19, 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 @@ -286,16 +286,22 @@
data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine(
data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);

// target objects for avoidance
// filter only for the latest detected objects.
fillAvoidanceTargetObjects(data, debug);

// lost object compensation
utils::static_obstacle_avoidance::updateRegisteredObject(
registered_objects_, data.target_objects, parameters_);
utils::static_obstacle_avoidance::compensateDetectionLost(
registered_objects_, data.target_objects, data.other_objects);
// compensate lost object which was avoidance target. if the time hasn't passed more than
// threshold since perception module lost the target yet, this module keeps it as avoidance
// target.
utils::static_obstacle_avoidance::compensateLostTargetObjects(
registered_objects_, data, clock_->now(), planner_data_, parameters_);

// once an object filtered for boundary clipping, this module keeps the information until the end
// of execution.
utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data);

// calculate various data for each target objects.
fillAvoidanceTargetData(data.target_objects);

Check notice on line 304 in planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Large Method 

StaticObstacleAvoidanceModule::fillFundamentalData decreases from 86 to 85 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.
// sort object order by longitudinal distance
std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) {
return a.longitudinal < b.longitudinal;
Expand All @@ -308,8 +314,6 @@
void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
AvoidancePlanningData & data, DebugData & debug) const
{
using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
using utils::static_obstacle_avoidance::fillObjectStoppableJudge;
using utils::static_obstacle_avoidance::filterTargetObjects;
using utils::static_obstacle_avoidance::separateObjectsByPath;
using utils::static_obstacle_avoidance::updateRoadShoulderDistance;
Expand Down Expand Up @@ -345,15 +349,6 @@
filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);
updateRoadShoulderDistance(data, planner_data_, parameters_);

// Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) {
fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
o.to_stop_line = calcDistanceToStopLine(o);
fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
});

// debug
{
std::vector<AvoidanceDebugMsg> debug_info_array;
Expand All @@ -371,6 +366,21 @@
}
}

void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const
{
using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
using utils::static_obstacle_avoidance::fillObjectStoppableJudge;

// Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
std::for_each(objects.begin(), objects.end(), [&, this](auto & o) {
fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
o.to_stop_line = calcDistanceToStopLine(o);
fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
});
}

ObjectData StaticObstacleAvoidanceModule::createObjectData(
const AvoidancePlanningData & data, const PredictedObject & object) const
{
Expand Down Expand Up @@ -1376,11 +1386,13 @@

void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
{
using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray;
using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray;

info_marker_.markers.clear();
appendMarkerArray(
createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_);
appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_);
}

void StaticObstacleAvoidanceModule::updateDebugMarker(
Expand Down
117 changes: 59 additions & 58 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: Overall Code Complexity 

The mean cyclomatic complexity increases from 5.49 to 5.56, 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 @@ -1645,93 +1645,94 @@
object_data.is_stoppable = same_id_obj->is_stoppable;
}

void updateRegisteredObject(
ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
void compensateLostTargetObjects(
ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto updateIfDetectedNow = [&now_objects](auto & registered_object) {
const auto & n = now_objects;
const auto r_id = registered_object.object.object_id;
const auto same_id_obj = std::find_if(
n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
const auto include = [](const auto & objects, const auto & search_id) {
return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) {
return o.object.object_id != search_id;
});
};

// STEP.1 UPDATE STORED OBJECTS.
const auto match = [&data](auto & object) {
const auto & search_id = object.object.object_id;
const auto same_id_object = std::find_if(
data.target_objects.begin(), data.target_objects.end(),
[&search_id](const auto & o) { return o.object.object_id == search_id; });

// same id object is detected. update registered.
if (same_id_obj != n.end()) {
registered_object = *same_id_obj;
if (same_id_object != data.target_objects.end()) {
object = *same_id_object;
return true;
}

constexpr auto POS_THR = 1.5;
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.getPose()) < POS_THR;
});
const auto similar_pos_obj = std::find_if(
data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) {
constexpr auto POS_THR = 1.5;
return calcDistance2d(object.getPose(), o.getPose()) < POS_THR;
});

// same id object is not detected, but object is found around registered. update registered.
if (similar_pos_obj != n.end()) {
registered_object = *similar_pos_obj;
if (similar_pos_obj != data.target_objects.end()) {
object = *similar_pos_obj;
return true;
}

// Same ID nor similar position object does not found.
return false;
};

const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();

// -- check registered_objects, remove if lost_count exceeds limit. --
for (int i = static_cast<int>(registered_objects.size()) - 1; i >= 0; --i) {
auto & r = registered_objects.at(i);

// registered object is not detected this time. lost count up.
if (!updateIfDetectedNow(r)) {
r.lost_time = (now - r.last_seen).seconds();
} else {
r.last_seen = now;
r.lost_time = 0.0;
}
// STEP1-1: REMOVE EXPIRED OBJECTS.
const auto itr = std::remove_if(
stored_objects.begin(), stored_objects.end(), [&now, &match, &parameters](auto & o) {
if (!match(o)) {
o.lost_time = (now - o.last_seen).seconds();
} else {
o.last_seen = now;
o.lost_time = 0.0;
}

// lost count exceeds threshold. remove object from register.
if (r.lost_time > parameters->object_last_seen_threshold) {
registered_objects.erase(registered_objects.begin() + i);
}
}
return o.lost_time > parameters->object_last_seen_threshold;
});

const auto isAlreadyRegistered = [&](const auto & n_id) {
const auto & r = registered_objects;
return std::any_of(
r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; });
};
stored_objects.erase(itr, stored_objects.end());

// -- check now_objects, add it if it has new object id --
for (const auto & now_obj : now_objects) {
if (!isAlreadyRegistered(now_obj.object.object_id)) {
registered_objects.push_back(now_obj);
// STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS.
for (const auto & current_object : data.target_objects) {
if (!include(stored_objects, current_object.object.object_id)) {
stored_objects.push_back(current_object);
}
}

Check notice on line 1708 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)

✅ No longer an issue: Bumpy Road Ahead 

updateRegisteredObject is no longer above the threshold for logical blocks with deeply nested code. 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.
}

void compensateDetectionLost(
const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
ObjectDataArray & other_objects)
{
const auto isDetectedNow = [&](const auto & r_id) {
const auto & n = now_objects;
// STEP2: COMPENSATE CURRENT TARGET OBJECTS
const auto is_detected = [&](const auto & object_id) {
return std::any_of(
n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
data.target_objects.begin(), data.target_objects.end(),
[&object_id](const auto & o) { return o.object.object_id == object_id; });
};

const auto isIgnoreObject = [&](const auto & r_id) {
const auto & n = other_objects;
const auto is_ignored = [&](const auto & object_id) {
return std::any_of(
n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
data.other_objects.begin(), data.other_objects.end(),
[&object_id](const auto & o) { return o.object.object_id == object_id; });
};

for (const auto & registered : registered_objects) {
if (
!isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) {
now_objects.push_back(registered);
for (auto & stored_object : stored_objects) {
if (is_detected(stored_object.object.object_id)) {
continue;
}
if (is_ignored(stored_object.object.object_id)) {
continue;
}

const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
fillLongitudinalAndLengthByClosestEnvelopeFootprint(
data.reference_path_rough, ego_pos, stored_object);

data.target_objects.push_back(stored_object);

Check warning on line 1735 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)

❌ New issue: Complex Method 

compensateLostTargetObjects has a cyclomatic complexity of 11, 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.

Check notice on line 1735 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)

ℹ New issue: Bumpy Road Ahead 

compensateLostTargetObjects has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested 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.

Check notice on line 1735 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)

ℹ New issue: Excess Number of Function Arguments 

compensateLostTargetObjects has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
}
}

Expand Down
Loading