Skip to content

Commit

Permalink
feat(behavior_path_planner_common): add general method for calculatin…
Browse files Browse the repository at this point in the history 
…g turn signal for bpp modules v0.19.1 (#1313)

* feat(behavior_path_planner_common): add general method for calculating turn signal for bpp modules (autowarefoundation#6625)

add general method for calculating turn signal for bpp modules

Signed-off-by: Daniel Sanchez <[email protected]>

* feat(behavior_path_avoidance_module): use the general calc turn signal method (autowarefoundation#6626)

use the general calc turn signal method

Signed-off-by: Daniel Sanchez <[email protected]>

* feat(lane_change_module): add general method to get turn signal for LC module (autowarefoundation#6627)

add general method to get turn signal for LC module

Signed-off-by: Daniel Sanchez <[email protected]>

* feat(start_planner): add general turn signal method to start planner (autowarefoundation#6628)

* Add general turnSignal method to start planner

Signed-off-by: Daniel Sanchez <[email protected]>

* add description to ignore signal

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>

* feat(goal_planner): add general turnsignalinfo method for goal planner (autowarefoundation#6629)

add general turnsignalinfo method for goal planner

Signed-off-by: Daniel Sanchez <[email protected]>

* resolve merge conflict

* add turn_signal_remaining_shift_length_threshold

Signed-off-by: Daniel Sanchez <[email protected]>

* include utility header to use std::pair

---------

Signed-off-by: Daniel Sanchez <[email protected]>
Co-authored-by: danielsanchezaran <[email protected]>
Co-authored-by: Daniel Sanchez <[email protected]>
  • Loading branch information
@saka1-s @danielsanchezaran
3 people authored Jun 4, 2024
1 parent 4b1f18e commit 6f956eb
Show file tree
Hide file tree
Showing 17 changed files with 577 additions and 425 deletions.
3 changes: 0 additions & 3 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 @@ -172,9 +172,6 @@ double calcDistanceToAvoidStartLine(
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_
10 changes: 7 additions & 3 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -950,9 +950,13 @@ BehaviorModuleOutput AvoidanceModule::plan()
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else {
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
const auto new_signal = 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);

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
192 changes: 0 additions & 192 deletions planning/behavior_path_avoidance_module/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,98 +251,6 @@ 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 @@ -2363,104 +2271,4 @@ 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_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)) {
return {};
}

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

return turn_signal_info;
}
} // namespace behavior_path_planner::utils::avoidance
10 changes: 7 additions & 3 deletions ...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@
#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>
#include <memory>
Expand Down Expand Up @@ -452,7 +455,7 @@ class GoalPlannerModule : public SceneModuleInterface
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;

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

Expand All @@ -467,10 +470,11 @@ class GoalPlannerModule : public SceneModuleInterface
*/
void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
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

0 comments on commit 6f956eb

Please sign in to comment.