Skip to content

Commit

Permalink
feat(avoidance): suppress unnatural turn signal (autowarefoundation#5905
Browse files Browse the repository at this point in the history 
)

* refactor(avoidance): clean up

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): suppress unnecessary turn signal

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(avoidance): move utils

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Mar 8, 2024
1 parent 580dc84 commit 67f5983
Show file tree
Hide file tree
Showing 4 changed files with 200 additions and 123 deletions.
7 changes: 0 additions & 7 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -325,13 +325,6 @@ class AvoidanceModule : public SceneModuleInterface

// generate output data

/**
* @brief calculate turn signal infomation.
* @param avoidance path.
* @return turn signal command.
*/
TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const;

/**
* @brief fill debug markers.
*/
Expand Down
4 changes: 4 additions & 0 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,10 @@ double calcDistanceToAvoidStartLine(
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

TurnSignalInfo 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_
123 changes: 7 additions & 116 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
#include "behavior_path_avoidance_module/debug.hpp"
#include "behavior_path_avoidance_module/utils.hpp"
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp"
#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
Expand Down Expand Up @@ -874,7 +873,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
if (success_spline_path_generation && success_linear_path_generation) {
helper_->setPreviousLinearShiftPath(linear_shift_path);
helper_->setPreviousSplineShiftPath(spline_shift_path);
helper_->setPreviousReferencePath(data.reference_path);
helper_->setPreviousReferencePath(path_shifter_.getReferencePath());
} else {
spline_shift_path = helper_->getPreviousSplineShiftPath();
}
Expand All @@ -887,9 +886,13 @@ BehaviorModuleOutput AvoidanceModule::plan()
BehaviorModuleOutput output;

// turn signal info
{
if (path_shifter_.getShiftLines().empty()) {
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else {
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
const auto new_signal = calcTurnSignalInfo(spline_shift_path);
const auto new_signal = utils::avoidance::calcTurnSignalInfo(
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
planner_data_);
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 Expand Up @@ -1285,118 +1288,6 @@ void AvoidanceModule::updateRTCData()
updateCandidateRTCStatus(output);
}

TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const
{
const auto shift_lines = path_shifter_.getShiftLines();
if (shift_lines.empty()) {
return {};
}

const auto front_shift_line = shift_lines.front();
const size_t start_idx = front_shift_line.start_idx;
const size_t end_idx = front_shift_line.end_idx;

const auto current_shift_length = helper_->getEgoShift();
const double start_shift_length = path.shift_length.at(start_idx);
const double end_shift_length = path.shift_length.at(end_idx);
const double segment_shift_length = end_shift_length - start_shift_length;

const double turn_signal_shift_length_threshold =
planner_data_->parameters.turn_signal_shift_length_threshold;
const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time;
const double turn_signal_minimum_search_distance =
planner_data_->parameters.turn_signal_minimum_search_distance;

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

// If the vehicle does not shift anymore, we turn off the blinker
if (std::fabs(end_shift_length - current_shift_length) < 0.1) {
return {};
}

// compute blinker start idx and end idx
size_t blinker_start_idx = [&]() {
for (size_t idx = start_idx; idx <= end_idx; ++idx) {
const double current_shift_length = path.shift_length.at(idx);
if (current_shift_length > 0.1) {
return idx;
}
}
return start_idx;
}();
size_t blinker_end_idx = end_idx;

// prevent invalid access for out-of-range
blinker_start_idx =
std::min(std::max(std::size_t(0), blinker_start_idx), path.path.points.size() - 1);
blinker_end_idx =
std::min(std::max(blinker_start_idx, blinker_end_idx), path.path.points.size() - 1);

const auto blinker_start_pose = path.path.points.at(blinker_start_idx).point.pose;
const auto blinker_end_pose = path.path.points.at(blinker_end_idx).point.pose;

const double ego_vehicle_offset =
planner_data_->parameters.vehicle_info.max_longitudinal_offset_m;
const auto signal_prepare_distance =
std::max(getEgoSpeed() * turn_signal_search_time, turn_signal_minimum_search_distance);
const auto ego_front_to_shift_start =
calcSignedArcLength(path.path.points, getEgoPosition(), blinker_start_pose.position) -
ego_vehicle_offset;

if (signal_prepare_distance < ego_front_to_shift_start) {
return {};
}

bool turn_signal_on_swerving = planner_data_->parameters.turn_signal_on_swerving;

TurnSignalInfo turn_signal_info{};
if (turn_signal_on_swerving) {
if (segment_shift_length > 0.0) {
turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
} else {
turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
}
} else {
const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
const auto local_vehicle_footprint =
createVehicleFootprint(planner_data_->parameters.vehicle_info);
boost::geometry::model::ring<tier4_autoware_utils::Point2d> shifted_vehicle_footprint;
for (const auto & cl : current_lanes) {
// get left and right bounds of current lane
const auto lane_left_bound = cl.leftBound2d().basicLineString();
const auto lane_right_bound = cl.rightBound2d().basicLineString();
for (size_t i = start_idx; i < end_idx; ++i) {
// transform vehicle footprint onto path points
shifted_vehicle_footprint = transformVector(
local_vehicle_footprint,
tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose));
if (
boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) ||
boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) {
if (segment_shift_length > 0.0) {
turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
} else {
turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
}
}
}
}
}
if (ego_front_to_shift_start > 0.0) {
turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose;
} else {
turn_signal_info.desired_start_point = blinker_start_pose;
}
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;

return turn_signal_info;
}

void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
{
using marker_utils::avoidance_marker::createTargetObjectsMarkerArray;
Expand Down
189 changes: 189 additions & 0 deletions planning/behavior_path_avoidance_module/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include "behavior_path_avoidance_module/data_structs.hpp"
#include "behavior_path_avoidance_module/utils.hpp"
#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/path_utils.hpp"
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
Expand Down Expand Up @@ -225,6 +226,98 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
}

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

const auto avoid_shift =
std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
if (avoid_shift) {
// 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;
}
}

const auto return_shift =
std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
if (return_shift) {
// 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;
}
}

const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
if (left_middle_shift) {
// 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;
}
}

const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
if (right_middle_shift) {
// 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 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 @@ -2055,4 +2148,100 @@ double calcDistanceToReturnDeadLine(

return distance_to_return_dead_line;
}

TurnSignalInfo 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 {};
}

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

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

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

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 {};
}

// 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 {};
}

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 {};
}

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 turn_signal_info;
}

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

const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true);
const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true);

if (!existShiftSideLane(
start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) {
return {};
}

if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
return {};
}

return turn_signal_info;
}
} // namespace behavior_path_planner::utils::avoidance

0 comments on commit 67f5983

Please sign in to comment.