Skip to content

Commit f03f7f2

Browse files
feat(behavior_path_avoidance_module): use the general calc turn signal method (#6626)
use the general calc turn signal method Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent f82f420 commit f03f7f2

File tree

3 files changed

+7
-243
lines changed

3 files changed

+7
-243
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -176,9 +176,6 @@ double calcDistanceToAvoidStartLine(
176176
const std::shared_ptr<const PlannerData> & planner_data,
177177
const std::shared_ptr<AvoidanceParameters> & parameters);
178178

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

184181
#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_

planning/behavior_path_avoidance_module/src/scene.cpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -942,9 +942,13 @@ BehaviorModuleOutput AvoidanceModule::plan()
942942
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
943943
} else {
944944
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
945-
const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
946-
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
947-
planner_data_);
945+
946+
constexpr bool is_driving_forward = true;
947+
constexpr bool egos_lane_is_shifted = true;
948+
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
949+
linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
950+
helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted);
951+
948952
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
949953
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
950954
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,

planning/behavior_path_avoidance_module/src/utils.cpp

-237
Original file line numberDiff line numberDiff line change
@@ -250,132 +250,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
250250
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
251251
}
252252

253-
bool isAvoidShift(
254-
const double start_shift_length, const double end_shift_length, const double threshold)
255-
{
256-
return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold;
257-
}
258-
259-
bool isReturnShift(
260-
const double start_shift_length, const double end_shift_length, const double threshold)
261-
{
262-
return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold;
263-
}
264-
265-
bool isLeftMiddleShift(
266-
const double start_shift_length, const double end_shift_length, const double threshold)
267-
{
268-
return start_shift_length > threshold && end_shift_length > threshold;
269-
}
270-
271-
bool isRightMiddleShift(
272-
const double start_shift_length, const double end_shift_length, const double threshold)
273-
{
274-
return start_shift_length < threshold && end_shift_length < threshold;
275-
}
276-
277-
bool existShiftSideLane(
278-
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
279-
const bool no_right_lanes, const double threshold)
280-
{
281-
const auto relative_shift_length = end_shift_length - start_shift_length;
282-
283-
if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
284-
// Left avoid. But there is no adjacent lane. No need blinker.
285-
if (relative_shift_length > 0.0 && no_left_lanes) {
286-
return false;
287-
}
288-
289-
// Right avoid. But there is no adjacent lane. No need blinker.
290-
if (relative_shift_length < 0.0 && no_right_lanes) {
291-
return false;
292-
}
293-
}
294-
295-
if (isReturnShift(start_shift_length, end_shift_length, threshold)) {
296-
// Right return. But there is no adjacent lane. No need blinker.
297-
if (relative_shift_length > 0.0 && no_right_lanes) {
298-
return false;
299-
}
300-
301-
// Left return. But there is no adjacent lane. No need blinker.
302-
if (relative_shift_length < 0.0 && no_left_lanes) {
303-
return false;
304-
}
305-
}
306-
307-
if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) {
308-
// Left avoid. But there is no adjacent lane. No need blinker.
309-
if (relative_shift_length > 0.0 && no_left_lanes) {
310-
return false;
311-
}
312-
313-
// Left return. But there is no adjacent lane. No need blinker.
314-
if (relative_shift_length < 0.0 && no_left_lanes) {
315-
return false;
316-
}
317-
}
318-
319-
if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) {
320-
// Right avoid. But there is no adjacent lane. No need blinker.
321-
if (relative_shift_length < 0.0 && no_right_lanes) {
322-
return false;
323-
}
324-
325-
// Left avoid. But there is no adjacent lane. No need blinker.
326-
if (relative_shift_length > 0.0 && no_right_lanes) {
327-
return false;
328-
}
329-
}
330-
331-
return true;
332-
}
333-
334-
bool isNearEndOfShift(
335-
const double start_shift_length, const double end_shift_length, const Point & ego_pos,
336-
const lanelet::ConstLanelets & original_lanes, const double threshold)
337-
{
338-
using boost::geometry::within;
339-
using lanelet::utils::to2D;
340-
using lanelet::utils::conversion::toLaneletPoint;
341-
342-
if (!isReturnShift(start_shift_length, end_shift_length, threshold)) {
343-
return false;
344-
}
345-
346-
return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) {
347-
return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon());
348-
});
349-
}
350-
351-
bool straddleRoadBound(
352-
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
353-
const vehicle_info_util::VehicleInfo & vehicle_info)
354-
{
355-
using boost::geometry::intersects;
356-
using tier4_autoware_utils::pose2transform;
357-
using tier4_autoware_utils::transformVector;
358-
359-
const auto footprint = vehicle_info.createFootprint();
360-
361-
for (const auto & lane : lanes) {
362-
for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
363-
const auto transform = pose2transform(path.path.points.at(i).point.pose);
364-
const auto shifted_vehicle_footprint = transformVector(footprint, transform);
365-
366-
if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
367-
return true;
368-
}
369-
370-
if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
371-
return true;
372-
}
373-
}
374-
}
375-
376-
return false;
377-
}
378-
379253
} // namespace
380254

381255
namespace filtering_utils
@@ -2367,115 +2241,4 @@ double calcDistanceToReturnDeadLine(
23672241

23682242
return distance_to_return_dead_line;
23692243
}
2370-
2371-
std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
2372-
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
2373-
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
2374-
{
2375-
constexpr double THRESHOLD = 0.1;
2376-
const auto & p = planner_data->parameters;
2377-
const auto & rh = planner_data->route_handler;
2378-
const auto & ego_pose = planner_data->self_odometry->pose.pose;
2379-
const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x;
2380-
2381-
if (shift_line.start_idx + 1 > path.shift_length.size()) {
2382-
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2383-
return std::make_pair(TurnSignalInfo{}, true);
2384-
}
2385-
2386-
if (shift_line.start_idx + 1 > path.path.points.size()) {
2387-
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2388-
return std::make_pair(TurnSignalInfo{}, true);
2389-
}
2390-
2391-
if (shift_line.end_idx + 1 > path.shift_length.size()) {
2392-
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2393-
return std::make_pair(TurnSignalInfo{}, true);
2394-
}
2395-
2396-
if (shift_line.end_idx + 1 > path.path.points.size()) {
2397-
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2398-
return std::make_pair(TurnSignalInfo{}, true);
2399-
}
2400-
2401-
const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
2402-
const auto end_shift_length = path.shift_length.at(shift_line.end_idx);
2403-
const auto relative_shift_length = end_shift_length - start_shift_length;
2404-
2405-
// If shift length is shorter than the threshold, it does not need to turn on blinkers
2406-
if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
2407-
return std::make_pair(TurnSignalInfo{}, true);
2408-
}
2409-
2410-
// If the vehicle does not shift anymore, we turn off the blinker
2411-
if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) {
2412-
return std::make_pair(TurnSignalInfo{}, true);
2413-
}
2414-
2415-
const auto get_command = [](const auto & shift_length) {
2416-
return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT
2417-
: TurnIndicatorsCommand::ENABLE_RIGHT;
2418-
};
2419-
2420-
const auto signal_prepare_distance =
2421-
std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance);
2422-
const auto ego_front_to_shift_start =
2423-
calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
2424-
p.vehicle_info.max_longitudinal_offset_m;
2425-
2426-
if (signal_prepare_distance < ego_front_to_shift_start) {
2427-
return std::make_pair(TurnSignalInfo{}, false);
2428-
}
2429-
2430-
const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
2431-
const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose;
2432-
const auto get_start_pose = [&](const auto & ego_to_shift_start) {
2433-
return ego_to_shift_start ? ego_pose : blinker_start_pose;
2434-
};
2435-
2436-
TurnSignalInfo turn_signal_info{};
2437-
turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start);
2438-
turn_signal_info.desired_end_point = blinker_end_pose;
2439-
turn_signal_info.required_start_point = blinker_start_pose;
2440-
turn_signal_info.required_end_point = blinker_end_pose;
2441-
turn_signal_info.turn_signal.command = get_command(relative_shift_length);
2442-
2443-
if (!p.turn_signal_on_swerving) {
2444-
return std::make_pair(turn_signal_info, false);
2445-
}
2446-
2447-
lanelet::ConstLanelet lanelet;
2448-
if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
2449-
return std::make_pair(TurnSignalInfo{}, true);
2450-
}
2451-
2452-
const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
2453-
const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet);
2454-
const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true);
2455-
const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet);
2456-
const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty();
2457-
const auto has_right_lane =
2458-
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
2459-
2460-
if (!existShiftSideLane(
2461-
start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
2462-
p.turn_signal_shift_length_threshold)) {
2463-
return std::make_pair(TurnSignalInfo{}, true);
2464-
}
2465-
2466-
if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
2467-
return std::make_pair(TurnSignalInfo{}, true);
2468-
}
2469-
2470-
constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
2471-
if (ego_speed < STOPPED_THRESHOLD) {
2472-
if (isNearEndOfShift(
2473-
start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets,
2474-
p.turn_signal_shift_length_threshold)) {
2475-
return std::make_pair(TurnSignalInfo{}, true);
2476-
}
2477-
}
2478-
2479-
return std::make_pair(turn_signal_info, false);
2480-
}
24812244
} // namespace behavior_path_planner::utils::avoidance

0 commit comments

Comments
 (0)