Skip to content

Commit d98e07a

Browse files
add general method to get turn signal for LC module
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 6ee9627 commit d98e07a

File tree

3 files changed

+14
-65
lines changed

3 files changed

+14
-65
lines changed

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -151,8 +151,6 @@ class NormalLaneChange : public LaneChangeBase
151151
const lanelet::ConstLanelets & current_lanes,
152152
const lanelet::ConstLanelets & target_lanes) const;
153153

154-
TurnSignalInfo calcTurnSignalInfo() const override;
155-
156154
bool isValidPath(const PathWithLaneId & path) const override;
157155

158156
PathSafetyStatus isLaneChangePathSafe(

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -218,8 +218,6 @@ class LaneChangeBase
218218
LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
219219
const bool is_stuck, const bool check_safety) const = 0;
220220

221-
virtual TurnSignalInfo calcTurnSignalInfo() const = 0;
222-
223221
virtual bool isValidPath(const PathWithLaneId & path) const = 0;
224222

225223
virtual bool isAbleToStopSafely() const = 0;

planning/behavior_path_lane_change_module/src/scene.cpp

+14-61
Original file line numberDiff line numberDiff line change
@@ -480,14 +480,20 @@ void NormalLaneChange::resetParameters()
480480

481481
TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
482482
{
483-
TurnSignalInfo turn_signal_info = calcTurnSignalInfo();
484-
const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal(
485-
status_.current_lanes, status_.lane_change_path.shifted_path,
486-
status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x,
487-
planner_data_->parameters);
488-
turn_signal_info.turn_signal.command = turn_signal_command.command;
489-
490-
return turn_signal_info;
483+
const auto & pose = getEgoPose();
484+
const auto & current_lanes = status_.current_lanes;
485+
const auto & shift_line = status_.lane_change_path.info.shift_line;
486+
const auto & shift_path = status_.lane_change_path.shifted_path;
487+
const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance;
488+
constexpr bool is_driving_forward = true;
489+
// The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's
490+
// current lane, lane change is different, so we set this flag to false.
491+
constexpr bool egos_lane_is_shifted = false;
492+
493+
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
494+
shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward,
495+
egos_lane_is_shifted);
496+
return new_signal;
491497
}
492498

493499
lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
@@ -1686,59 +1692,6 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis(
16861692
return {};
16871693
}
16881694

1689-
TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const
1690-
{
1691-
const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) {
1692-
double accumulated_length = 0.0;
1693-
for (size_t i = 0; i < path.points.size() - 1; ++i) {
1694-
accumulated_length +=
1695-
tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
1696-
if (accumulated_length > length) {
1697-
return path.points.at(i).point.pose;
1698-
}
1699-
}
1700-
1701-
return path.points.front().point.pose;
1702-
};
1703-
1704-
const auto & path = status_.lane_change_path;
1705-
const auto & shifted_path = path.shifted_path.path;
1706-
1707-
TurnSignalInfo turn_signal_info{};
1708-
1709-
if (path.path.points.empty()) {
1710-
return turn_signal_info;
1711-
}
1712-
1713-
// desired start pose = prepare start pose
1714-
turn_signal_info.desired_start_point = std::invoke([&]() {
1715-
const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time;
1716-
const auto diff_time = path.info.duration.prepare - blinker_start_duration;
1717-
if (diff_time < 1e-5) {
1718-
return path.path.points.front().point.pose;
1719-
}
1720-
1721-
const auto current_twist = getEgoTwist();
1722-
const auto diff_length = std::abs(current_twist.linear.x) * diff_time;
1723-
return get_blinker_pose(path.path, diff_length);
1724-
});
1725-
1726-
// desired end pose
1727-
const auto length_ratio =
1728-
std::clamp(lane_change_parameters_->length_ratio_for_turn_signal_deactivation, 0.0, 1.0);
1729-
const auto desired_end_length = path.info.length.lane_changing * length_ratio;
1730-
turn_signal_info.desired_end_point = get_blinker_pose(shifted_path, desired_end_length);
1731-
1732-
// required start pose = lane changing start pose
1733-
turn_signal_info.required_start_point = path.info.shift_line.start;
1734-
1735-
// required end pose = in the middle of the lane change
1736-
const auto mid_lane_change_length = path.info.length.lane_changing / 2;
1737-
turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length);
1738-
1739-
return turn_signal_info;
1740-
}
1741-
17421695
bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const
17431696
{
17441697
const auto & route_handler = planner_data_->route_handler;

0 commit comments

Comments
 (0)