Skip to content

Commit 546df44

Browse files
feat(start_planner): add general turn signal method to start planner (#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>
1 parent c24c3b0 commit 546df44

File tree

2 files changed

+59
-87
lines changed

2 files changed

+59
-87
lines changed

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+6-1
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333

3434
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
3535

36+
#include <lanelet2_core/Forward.h>
3637
#include <tf2/utils.h>
3738

3839
#include <atomic>
@@ -240,6 +241,10 @@ class StartPlannerModule : public SceneModuleInterface
240241
PullOutStatus status_;
241242
mutable StartPlannerDebugData debug_data_;
242243

244+
// Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this
245+
// module's output. If the ego vehicle is in this lanelet, the calculation is skipped.
246+
std::optional<lanelet::Id> ignore_signal_{std::nullopt};
247+
243248
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
244249

245250
std::unique_ptr<rclcpp::Time> last_route_received_time_;
@@ -264,7 +269,7 @@ class StartPlannerModule : public SceneModuleInterface
264269
std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
265270

266271
// turn signal
267-
TurnSignalInfo calcTurnSignalInfo() const;
272+
TurnSignalInfo calcTurnSignalInfo();
268273

269274
void incrementPathIndex();
270275
PathWithLaneId getCurrentPath() const;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+53-86
Original file line numberDiff line numberDiff line change
@@ -1154,103 +1154,70 @@ bool StartPlannerModule::hasFinishedCurrentPath()
11541154
return is_near_target && isStopped();
11551155
}
11561156

1157-
TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
1157+
TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
11581158
{
1159-
TurnSignalInfo turn_signal{}; // output
1159+
const auto path = getFullPath();
1160+
if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info;
11601161

11611162
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
1162-
const Pose & start_pose = status_.pull_out_path.start_pose;
1163-
const Pose & end_pose = status_.pull_out_path.end_pose;
1164-
1165-
// turn on hazard light when backward driving
1166-
if (!status_.driving_forward) {
1167-
turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE;
1168-
const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose();
1169-
turn_signal.desired_start_point = back_start_pose;
1170-
turn_signal.required_start_point = back_start_pose;
1171-
// pull_out start_pose is same to backward driving end_pose
1172-
turn_signal.required_end_point = start_pose;
1173-
turn_signal.desired_end_point = start_pose;
1174-
return turn_signal;
1175-
}
1176-
1177-
// turn on right signal until passing pull_out end point
1178-
const auto path = getFullPath();
1179-
// pull out path does not overlap
1180-
const double distance_from_end =
1181-
motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
1163+
const auto shift_start_idx =
1164+
motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position);
1165+
const auto shift_end_idx =
1166+
motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position);
1167+
const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
1168+
1169+
const auto is_ignore_signal = [this](const lanelet::Id & id) {
1170+
if (!ignore_signal_.has_value()) {
1171+
return false;
1172+
}
1173+
return ignore_signal_.value() == id;
1174+
};
11821175

1183-
if (path.points.empty()) {
1184-
return {};
1176+
const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
1177+
return is_ignore ? std::make_optional(id) : std::nullopt;
1178+
};
1179+
1180+
lanelet::Lanelet closest_lanelet;
1181+
lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet);
1182+
1183+
if (is_ignore_signal(closest_lanelet.id())) {
1184+
return getPreviousModuleOutput().turn_signal_info;
11851185
}
11861186

1187-
// calculate lateral offset from pull out target lane center line
1188-
lanelet::ConstLanelet closest_road_lane;
1189-
const double backward_path_length =
1190-
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
1191-
const auto road_lanes = utils::getExtendedCurrentLanes(
1192-
planner_data_, backward_path_length, std::numeric_limits<double>::max(),
1193-
/*forward_only_in_route*/ true);
1194-
lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane);
1195-
const double lateral_offset =
1196-
lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose);
1197-
1198-
turn_signal.turn_signal.command = std::invoke([&]() {
1199-
if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE;
1200-
if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset)
1201-
return TurnIndicatorsCommand::ENABLE_RIGHT;
1202-
if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset)
1203-
return TurnIndicatorsCommand::ENABLE_LEFT;
1204-
return TurnIndicatorsCommand::DISABLE;
1205-
});
1187+
const double current_shift_length =
1188+
lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
12061189

1207-
turn_signal.desired_start_point = start_pose;
1208-
turn_signal.required_start_point = start_pose;
1209-
turn_signal.desired_end_point = end_pose;
1210-
1211-
// check if intersection exists within search length
1212-
const bool is_near_intersection = std::invoke([&]() {
1213-
const double check_length = parameters_->intersection_search_length;
1214-
double accumulated_length = 0.0;
1215-
const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position);
1216-
for (size_t i = current_idx; i < path.points.size() - 1; ++i) {
1217-
const auto & p = path.points.at(i);
1218-
for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) {
1219-
const std::string turn_direction = lane.attributeOr("turn_direction", "else");
1220-
if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
1221-
return true;
1222-
}
1223-
}
1224-
accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1));
1225-
if (accumulated_length > check_length) {
1226-
return false;
1227-
}
1228-
}
1229-
return false;
1230-
});
1190+
constexpr bool egos_lane_is_shifted = true;
1191+
constexpr bool is_pull_out = true;
12311192

1232-
turn_signal.required_end_point = std::invoke([&]() {
1233-
if (is_near_intersection) return end_pose;
1234-
const double length_start_to_end =
1235-
motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
1236-
const auto ratio = std::clamp(
1237-
parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
1238-
1239-
const double required_end_length = length_start_to_end * ratio;
1240-
double accumulated_length = 0.0;
1241-
const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
1242-
for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
1243-
accumulated_length +=
1244-
tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
1245-
if (accumulated_length > required_end_length) {
1246-
return path.points.at(i).point.pose;
1247-
}
1193+
// In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction.
1194+
// This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and
1195+
// close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid
1196+
// this issue.
1197+
const bool override_ego_stopped_check = std::invoke([&]() {
1198+
if (status_.planner_type != PlannerType::GEOMETRIC) {
1199+
return false;
12481200
}
1249-
// not found required end point
1250-
return end_pose;
1201+
constexpr double distance_threshold = 1.0;
1202+
const auto stop_point = status_.pull_out_path.partial_paths.front().points.back();
1203+
const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength(
1204+
path.points, stop_point.point.pose.position, current_pose.position));
1205+
return distance_from_ego_to_stop_point < distance_threshold;
12511206
});
12521207

1253-
return turn_signal;
1208+
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
1209+
path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length,
1210+
status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
1211+
ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore);
1212+
1213+
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
1214+
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points);
1215+
const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
1216+
path, current_pose, current_seg_idx, original_signal, new_signal,
1217+
planner_data_->parameters.ego_nearest_dist_threshold,
1218+
planner_data_->parameters.ego_nearest_yaw_threshold);
1219+
1220+
return output_turn_signal_info;
12541221
}
12551222

12561223
bool StartPlannerModule::isSafePath() const

0 commit comments

Comments
 (0)