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

feat(behavior_path_planner, start_planner,goal_planner,lane_change,avoidance): implement a general behavior turn signal algorithm #6622

Closed
wants to merge 13 commits into from
Closed

feat(behavior_path_planner, start_planner,goal_planner,lane_change,avoidance): implement a general behavior turn signal algorithm #6622

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
d6124fa
Move avoidance calc turn signal to common
danielsanchezaran Mar 12, 2024
c21847a
Add backwards drive support
danielsanchezaran Mar 12, 2024
e4a6bf9
Add general turn signal to lane change
danielsanchezaran Mar 13, 2024
e50bb9d
WIP return avoid not working
danielsanchezaran Mar 13, 2024
ada03b7
add option for modules that shift the target lane and not current lane
danielsanchezaran Mar 14, 2024
24fe4aa
add options for start planner getTurnSignalInfo
danielsanchezaran Mar 14, 2024
7502947
add ignore id func to ignore prev explored lanes
danielsanchezaran Mar 14, 2024
8378472
add use prior turn signal
danielsanchezaran Mar 15, 2024
9b119ff
implement general behaviorturnsignal on goal planner
danielsanchezaran Mar 15, 2024
2fcaacc
delete comments
danielsanchezaran Mar 15, 2024
e2e8e18
delete prints
danielsanchezaran Mar 15, 2024
d1baad8
add comment on override
danielsanchezaran Mar 15, 2024
168409a
comments
danielsanchezaran Mar 15, 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
3 changes: 0 additions & 3 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,9 +176,6 @@ double calcDistanceToAvoidStartLine(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data);
} // namespace behavior_path_planner::utils::avoidance

#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_
10 changes: 7 additions & 3 deletions planning/behavior_path_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_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 1192 to 1194, 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.
Expand Down Expand Up @@ -947,9 +947,13 @@
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else {
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
planner_data_);

constexpr bool is_driving_forward = true;
constexpr bool egos_lane_is_shifted = true;
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted);

Check warning on line 956 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

AvoidanceModule::plan already has high cyclomatic complexity, and now it increases in Lines of Code from 104 to 106. 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.
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
Expand Down
237 changes: 0 additions & 237 deletions planning/behavior_path_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_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Lines of Code in a Single File 

The lines of code decreases from 1895 to 1711, 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_avoidance_module/src/utils.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 4.96 to 4.76, 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.

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Number of Functions in a Single Module 

The number of functions in this module is no longer above the threshold
//
// 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 @@ -249,133 +249,7 @@
{
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
}

Check notice on line 252 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

existShiftSideLane is no longer above the threshold for cyclomatic complexity. 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 252 in planning/behavior_path_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 

existShiftSideLane 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.

Check notice on line 252 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

existShiftSideLane is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check notice on line 252 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

isNearEndOfShift is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
bool isAvoidShift(
const double start_shift_length, const double end_shift_length, const double threshold)
{
return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold;
}

bool isReturnShift(
const double start_shift_length, const double end_shift_length, const double threshold)
{
return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold;
}

bool isLeftMiddleShift(
const double start_shift_length, const double end_shift_length, const double threshold)
{
return start_shift_length > threshold && end_shift_length > threshold;
}

bool isRightMiddleShift(
const double start_shift_length, const double end_shift_length, const double threshold)
{
return start_shift_length < threshold && end_shift_length < threshold;
}

bool existShiftSideLane(
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
const bool no_right_lanes, const double threshold)
{
const auto relative_shift_length = end_shift_length - start_shift_length;

if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
}

// Right avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length < 0.0 && no_right_lanes) {
return false;
}
}

if (isReturnShift(start_shift_length, end_shift_length, threshold)) {
// Right return. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_right_lanes) {
return false;
}

// Left return. But there is no adjacent lane. No need blinker.
if (relative_shift_length < 0.0 && no_left_lanes) {
return false;
}
}

if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
}

// Left return. But there is no adjacent lane. No need blinker.
if (relative_shift_length < 0.0 && no_left_lanes) {
return false;
}
}

if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Right avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length < 0.0 && no_right_lanes) {
return false;
}

// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_right_lanes) {
return false;
}
}

return true;
}

bool isNearEndOfShift(
const double start_shift_length, const double end_shift_length, const Point & ego_pos,
const lanelet::ConstLanelets & original_lanes, const double threshold)
{
using boost::geometry::within;
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;

if (!isReturnShift(start_shift_length, end_shift_length, threshold)) {
return false;
}

return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) {
return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon());
});
}

bool straddleRoadBound(
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
const vehicle_info_util::VehicleInfo & vehicle_info)
{
using boost::geometry::intersects;
using tier4_autoware_utils::pose2transform;
using tier4_autoware_utils::transformVector;

const auto footprint = vehicle_info.createFootprint();

for (const auto & lane : lanes) {
for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
const auto transform = pose2transform(path.path.points.at(i).point.pose);
const auto shifted_vehicle_footprint = transformVector(footprint, transform);

if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
return true;
}

if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
return true;
}
}
}

return false;
}

} // namespace

namespace filtering_utils
Expand Down Expand Up @@ -2359,116 +2233,5 @@
}

return distance_to_return_dead_line;
}

Check notice on line 2236 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

calcTurnSignalInfo is no longer above the threshold for cyclomatic complexity. 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 2236 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

calcTurnSignalInfo is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
{
constexpr double THRESHOLD = 0.1;
const auto & p = planner_data->parameters;
const auto & rh = planner_data->route_handler;
const auto & ego_pose = planner_data->self_odometry->pose.pose;
const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x;

if (shift_line.start_idx + 1 > path.shift_length.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
return std::make_pair(TurnSignalInfo{}, true);
}

if (shift_line.start_idx + 1 > path.path.points.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
return std::make_pair(TurnSignalInfo{}, true);
}

if (shift_line.end_idx + 1 > path.shift_length.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
return std::make_pair(TurnSignalInfo{}, true);
}

if (shift_line.end_idx + 1 > path.path.points.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
return std::make_pair(TurnSignalInfo{}, true);
}

const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
const auto end_shift_length = path.shift_length.at(shift_line.end_idx);
const auto relative_shift_length = end_shift_length - start_shift_length;

// If shift length is shorter than the threshold, it does not need to turn on blinkers
if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
return std::make_pair(TurnSignalInfo{}, true);
}

// If the vehicle does not shift anymore, we turn off the blinker
if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) {
return std::make_pair(TurnSignalInfo{}, true);
}

const auto get_command = [](const auto & shift_length) {
return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT
: TurnIndicatorsCommand::ENABLE_RIGHT;
};

const auto signal_prepare_distance =
std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance);
const auto ego_front_to_shift_start =
calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
p.vehicle_info.max_longitudinal_offset_m;

if (signal_prepare_distance < ego_front_to_shift_start) {
return std::make_pair(TurnSignalInfo{}, false);
}

const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose;
const auto get_start_pose = [&](const auto & ego_to_shift_start) {
return ego_to_shift_start ? ego_pose : blinker_start_pose;
};

TurnSignalInfo turn_signal_info{};
turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start);
turn_signal_info.desired_end_point = blinker_end_pose;
turn_signal_info.required_start_point = blinker_start_pose;
turn_signal_info.required_end_point = blinker_end_pose;
turn_signal_info.turn_signal.command = get_command(relative_shift_length);

if (!p.turn_signal_on_swerving) {
return std::make_pair(turn_signal_info, false);
}

lanelet::ConstLanelet lanelet;
if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
return std::make_pair(TurnSignalInfo{}, true);
}

const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet);
const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true);
const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet);
const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty();
const auto has_right_lane =
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();

if (!existShiftSideLane(
start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
p.turn_signal_shift_length_threshold)) {
return std::make_pair(TurnSignalInfo{}, true);
}

if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
return std::make_pair(TurnSignalInfo{}, true);
}

constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
if (ego_speed < STOPPED_THRESHOLD) {
if (isNearEndOfShift(
start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets,
p.turn_signal_shift_length_threshold)) {
return std::make_pair(TurnSignalInfo{}, true);
}
}

return std::make_pair(turn_signal_info, false);
}
} // namespace behavior_path_planner::utils::avoidance
9 changes: 6 additions & 3 deletions ...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>

#include <lanelet2_core/Forward.h>

#include <atomic>
#include <deque>
#include <limits>
Expand Down Expand Up @@ -525,14 +527,15 @@ class GoalPlannerModule : public SceneModuleInterface
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;

// output setter
void setOutput(BehaviorModuleOutput & output) const;
void setOutput(BehaviorModuleOutput & output);
void updatePreviousData();

void setModifiedGoal(BehaviorModuleOutput & output) const;
void setTurnSignalInfo(BehaviorModuleOutput & output) const;
void setTurnSignalInfo(BehaviorModuleOutput & output);

// new turn signal
TurnSignalInfo calcTurnSignalInfo() const;
TurnSignalInfo calcTurnSignalInfo();
std::optional<lanelet::Id> ignore_signal_{std::nullopt};

std::optional<BehaviorModuleOutput> last_previous_module_output_{};
bool hasPreviousModulePathShapeChanged() const;
Expand Down
Loading
Loading