@@ -1154,103 +1154,70 @@ bool StartPlannerModule::hasFinishedCurrentPath()
1154
1154
return is_near_target && isStopped ();
1155
1155
}
1156
1156
1157
- TurnSignalInfo StartPlannerModule::calcTurnSignalInfo () const
1157
+ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo ()
1158
1158
{
1159
- TurnSignalInfo turn_signal{}; // output
1159
+ const auto path = getFullPath ();
1160
+ if (path.points .empty ()) return getPreviousModuleOutput ().turn_signal_info ;
1160
1161
1161
1162
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
+ };
1182
1175
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 ;
1185
1185
}
1186
1186
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 ;
1206
1189
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 ;
1231
1192
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 ;
1248
1200
}
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;
1251
1206
});
1252
1207
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;
1254
1221
}
1255
1222
1256
1223
bool StartPlannerModule::isSafePath () const
0 commit comments