Skip to content

Commit 6f956eb

Browse files
saka1-sdanielsanchezaran
saka1-s
and
danielsanchezaran
authored
feat(behavior_path_planner_common): add general method for calculating 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 <danielsanchezaran@gmail.com> * 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 <danielsanchezaran@gmail.com> * 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 <danielsanchezaran@gmail.com> * 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 <danielsanchezaran@gmail.com> * add description to ignore signal Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * feat(goal_planner): add general turnsignalinfo method for goal planner (autowarefoundation#6629) add general turnsignalinfo method for goal planner Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * resolve merge conflict * add turn_signal_remaining_shift_length_threshold Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * include utility header to use std::pair --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Co-authored-by: danielsanchezaran <daniel.sanchez@tier4.jp> Co-authored-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 4b1f18e commit 6f956eb

File tree

17 files changed

+577
-425
lines changed

17 files changed

+577
-425
lines changed

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -172,9 +172,6 @@ double calcDistanceToAvoidStartLine(
172172
const std::shared_ptr<const PlannerData> & planner_data,
173173
const std::shared_ptr<AvoidanceParameters> & parameters);
174174

175-
TurnSignalInfo calcTurnSignalInfo(
176-
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
177-
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data);
178175
} // namespace behavior_path_planner::utils::avoidance
179176

180177
#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
@@ -950,9 +950,13 @@ BehaviorModuleOutput AvoidanceModule::plan()
950950
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
951951
} else {
952952
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
953-
const auto new_signal = utils::avoidance::calcTurnSignalInfo(
954-
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
955-
planner_data_);
953+
954+
constexpr bool is_driving_forward = true;
955+
constexpr bool egos_lane_is_shifted = true;
956+
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
957+
linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
958+
helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted);
959+
956960
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
957961
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
958962
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,

‎planning/behavior_path_avoidance_module/src/utils.cpp

-192
Original file line numberDiff line numberDiff line change
@@ -251,98 +251,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
251251
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
252252
}
253253

254-
bool existShiftSideLane(
255-
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
256-
const bool no_right_lanes)
257-
{
258-
constexpr double THRESHOLD = 0.1;
259-
const auto relative_shift_length = end_shift_length - start_shift_length;
260-
261-
const auto avoid_shift =
262-
std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
263-
if (avoid_shift) {
264-
// Left avoid. But there is no adjacent lane. No need blinker.
265-
if (relative_shift_length > 0.0 && no_left_lanes) {
266-
return false;
267-
}
268-
269-
// Right avoid. But there is no adjacent lane. No need blinker.
270-
if (relative_shift_length < 0.0 && no_right_lanes) {
271-
return false;
272-
}
273-
}
274-
275-
const auto return_shift =
276-
std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
277-
if (return_shift) {
278-
// Right return. But there is no adjacent lane. No need blinker.
279-
if (relative_shift_length > 0.0 && no_right_lanes) {
280-
return false;
281-
}
282-
283-
// Left return. But there is no adjacent lane. No need blinker.
284-
if (relative_shift_length < 0.0 && no_left_lanes) {
285-
return false;
286-
}
287-
}
288-
289-
const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
290-
if (left_middle_shift) {
291-
// Left avoid. But there is no adjacent lane. No need blinker.
292-
if (relative_shift_length > 0.0 && no_left_lanes) {
293-
return false;
294-
}
295-
296-
// Left return. But there is no adjacent lane. No need blinker.
297-
if (relative_shift_length < 0.0 && no_left_lanes) {
298-
return false;
299-
}
300-
}
301-
302-
const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
303-
if (right_middle_shift) {
304-
// Right avoid. But there is no adjacent lane. No need blinker.
305-
if (relative_shift_length < 0.0 && no_right_lanes) {
306-
return false;
307-
}
308-
309-
// Left avoid. But there is no adjacent lane. No need blinker.
310-
if (relative_shift_length > 0.0 && no_right_lanes) {
311-
return false;
312-
}
313-
}
314-
315-
return true;
316-
}
317-
318-
bool straddleRoadBound(
319-
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
320-
const vehicle_info_util::VehicleInfo & vehicle_info)
321-
{
322-
using boost::geometry::intersects;
323-
using tier4_autoware_utils::pose2transform;
324-
using tier4_autoware_utils::transformVector;
325-
326-
const auto footprint = vehicle_info.createFootprint();
327-
328-
for (const auto & lane : lanes) {
329-
for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
330-
const auto transform = pose2transform(path.path.points.at(i).point.pose);
331-
const auto shifted_vehicle_footprint = transformVector(footprint, transform);
332-
333-
if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
334-
return true;
335-
}
336-
337-
if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
338-
return true;
339-
}
340-
}
341-
}
342-
343-
return false;
344-
}
345-
346254
} // namespace
347255

348256
namespace filtering_utils
@@ -2363,104 +2271,4 @@ double calcDistanceToReturnDeadLine(
23632271

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

‎planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,9 @@
3737
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
3838
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
3939

40+
#include <lanelet2_core/Forward.h>
41+
42+
#include <atomic>
4043
#include <deque>
4144
#include <limits>
4245
#include <memory>
@@ -452,7 +455,7 @@ class GoalPlannerModule : public SceneModuleInterface
452455
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
453456

454457
// output setter
455-
void setOutput(BehaviorModuleOutput & output) const;
458+
void setOutput(BehaviorModuleOutput & output);
456459
void setStopPath(BehaviorModuleOutput & output) const;
457460
void updatePreviousData(const BehaviorModuleOutput & output);
458461

@@ -467,10 +470,11 @@ class GoalPlannerModule : public SceneModuleInterface
467470
*/
468471
void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
469472
void setModifiedGoal(BehaviorModuleOutput & output) const;
470-
void setTurnSignalInfo(BehaviorModuleOutput & output) const;
473+
void setTurnSignalInfo(BehaviorModuleOutput & output);
471474

472475
// new turn signal
473-
TurnSignalInfo calcTurnSignalInfo() const;
476+
TurnSignalInfo calcTurnSignalInfo();
477+
std::optional<lanelet::Id> ignore_signal_{std::nullopt};
474478

475479
std::optional<BehaviorModuleOutput> last_previous_module_output_{};
476480
bool hasPreviousModulePathShapeChanged() const;

0 commit comments

Comments
 (0)