diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index b205838896038..be7a3dfd195d9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -53,7 +53,8 @@ MarkerArray createAmbiguousObjectsMarkerArray( MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data); MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const BehaviorModuleOutput & output, const AvoidancePlanningData & data, + const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); } // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index a2dbdcc1dadb9..558ac557b66eb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -346,7 +346,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface * @brief fill debug markers. */ void updateDebugMarker( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const; + const BehaviorModuleOutput & output, const AvoidancePlanningData & data, + const PathShifter & shifter, const DebugData & debug) const; /** * @brief fill information markers that are shown in Rviz by default. @@ -368,6 +369,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface */ bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; + auto getTurnSignal(const ShiftedPath & spline_shift_path, const ShiftedPath & linear_shift_path) + -> TurnSignalInfo; + // post process /** diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 9840743867e78..b4063f7824f9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -229,6 +229,67 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: return msg; } +MarkerArray createTurnSignalMarkerArray(const TurnSignalInfo & turn_signal_info, std::string && ns) +{ + MarkerArray msg; + + if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) { + return msg; + } + + const auto yaw_offset = [&turn_signal_info]() { + if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return -1.0 * M_PI_2; + } + + if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return M_PI_2; + } + + return 0.0; + }(); + + size_t i = 0; + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, + createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + marker.pose = turn_signal_info.desired_start_point; + marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + + msg.markers.push_back(marker); + } + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, + createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + marker.pose = turn_signal_info.desired_end_point; + marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + + msg.markers.push_back(marker); + } + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, + createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose = turn_signal_info.required_start_point; + marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + + msg.markers.push_back(marker); + } + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, + createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose = turn_signal_info.required_end_point; + marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + + msg.markers.push_back(marker); + } + + return msg; +} + } // namespace MarkerArray createEgoStatusMarkerArray( @@ -591,7 +652,8 @@ MarkerArray createDrivableBounds( } MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const BehaviorModuleOutput & output, const AvoidancePlanningData & data, + const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters) { using autoware::behavior_path_planner::utils::transformToLanelets; @@ -737,6 +799,7 @@ MarkerArray createDebugMarkerArray( // misc if (parameters->enable_misc_marker) { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + add(createTurnSignalMarkerArray(output.turn_signal_info, "turn_signal_info")); } return msg; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index c7d80dd872500..1eca32b1f8785 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -92,7 +92,7 @@ bool StaticObstacleAvoidanceModule::isExecutionRequested() const // Check ego is in preferred lane updateInfoMarker(avoid_data_); - updateDebugMarker(avoid_data_, path_shifter_, debug_data_); + updateDebugMarker(BehaviorModuleOutput{}, avoid_data_, path_shifter_, debug_data_); // there is object that should be avoid. return true. if (!!avoid_data_.stop_target_object) { @@ -947,6 +947,112 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( return extended_path; } +auto StaticObstacleAvoidanceModule::getTurnSignal( + const ShiftedPath & spline_shift_path, const ShiftedPath & linear_shift_path) -> TurnSignalInfo +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto is_ignore_signal = [this](const UUID & uuid) { + if (!ignore_signal_.has_value()) { + return false; + } + + return ignore_signal_.value() == uuid; + }; + + const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) { + ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; + }; + + const auto is_large_deviation = [this](const auto & path) { + constexpr double threshold = 1.0; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto lateral_deviation = + autoware::motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + return std::abs(lateral_deviation) > threshold; + }; + + auto shift_lines = path_shifter_.getShiftLines(); + if (shift_lines.empty()) { + return getPreviousModuleOutput().turn_signal_info; + } + + if (is_large_deviation(spline_shift_path.path)) { + return getPreviousModuleOutput().turn_signal_info; + } + + const auto itr = + std::remove_if(shift_lines.begin(), shift_lines.end(), [&, this](const auto & s) { + const auto threshold = planner_data_->parameters.turn_signal_shift_length_threshold; + return std::abs(s.start_shift_length - s.end_shift_length) < threshold || + is_ignore_signal(s.id); + }); + shift_lines.erase(itr, shift_lines.end()); + + if (shift_lines.empty()) { + return getPreviousModuleOutput().turn_signal_info; + } + + const auto target_shift_line = [&]() { + const auto & s1 = shift_lines.front(); + + for (size_t i = 1; i < shift_lines.size(); i++) { + const auto & s2 = shift_lines.at(i); + + const auto s1_relative_length = s1.start_shift_length - s1.end_shift_length; + const auto s2_relative_length = s2.start_shift_length - s2.end_shift_length; + + // same side shift + if (s1_relative_length > 0.0 && s2_relative_length > 0.0) { + continue; + } + + // same side shift + if (s1_relative_length < 0.0 && s2_relative_length < 0.0) { + continue; + } + + // different side shift + const auto & points = path_shifter_.getReferencePath().points; + const size_t idx = planner_data_->findEgoIndex(points); + + // output turn signal for near shift line. + if (calcSignedArcLength(points, idx, s1.start_idx) > 0.0) { + return s1; + } + + // output turn signal for far shift line. + if ( + calcSignedArcLength(points, idx, s2.start_idx) < + getEgoSpeed() * parameters_->max_prepare_time) { + return s2; + } + + // output turn signal for near shift line. + return s1; + } + + return s1; + }(); + + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + + constexpr bool is_driving_forward = true; + constexpr bool egos_lane_is_shifted = true; + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + linear_shift_path, target_shift_line, avoid_data_.current_lanelets, helper_->getEgoShift(), + is_driving_forward, egos_lane_is_shifted); + + update_ignore_signal(target_shift_line.id, is_ignore); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); + return planner_data_->turn_signal_decider.overwrite_turn_signal( + spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); +} + BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -992,48 +1098,9 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() BehaviorModuleOutput output; - const auto is_ignore_signal = [this](const UUID & uuid) { - if (!ignore_signal_.has_value()) { - return false; - } - - return ignore_signal_.value() == uuid; - }; - - const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) { - ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; - }; - - const auto is_large_deviation = [this](const auto & path) { - constexpr double threshold = 1.0; - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); - const auto lateral_deviation = - autoware::motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); - return std::abs(lateral_deviation) > threshold; - }; - - // turn signal info - if (path_shifter_.getShiftLines().empty()) { - output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { - output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - } else if (is_large_deviation(spline_shift_path.path)) { - output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - } else { - const auto original_signal = getPreviousModuleOutput().turn_signal_info; - - constexpr bool is_driving_forward = true; - constexpr bool egos_lane_is_shifted = true; - const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( - linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets, - helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); - - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, - planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore); + // turn signal + { + output.turn_signal_info = getTurnSignal(spline_shift_path, linear_shift_path); } // sparse resampling for computational cost @@ -1046,7 +1113,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { updateEgoBehavior(data, spline_shift_path); updateInfoMarker(avoid_data_); - updateDebugMarker(avoid_data_, path_shifter_, debug_data_); + updateDebugMarker(output, avoid_data_, path_shifter_, debug_data_); } if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -1492,12 +1559,13 @@ void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData } void StaticObstacleAvoidanceModule::updateDebugMarker( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const + const BehaviorModuleOutput & output, const AvoidancePlanningData & data, + const PathShifter & shifter, const DebugData & debug) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); debug_marker_.markers.clear(); - debug_marker_ = - utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); + debug_marker_ = utils::static_obstacle_avoidance::createDebugMarkerArray( + output, data, shifter, debug, parameters_); } void StaticObstacleAvoidanceModule::updateAvoidanceDebugData(