diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 08fc06330b1d8..2ceb22fd6e7b9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -687,6 +687,32 @@ lanelet::Lanelets getLeftOppositeLanelets( return opposite_lanelets; } + +void replaceObjectYawWithLaneletsYaw( + const LaneletsData & current_lanelets, TrackedObject & transformed_object) +{ + // return if no lanelet is found + if (current_lanelets.empty()) return; + auto & pose_with_cov = transformed_object.kinematics.pose_with_covariance; + // for each lanelet, calc lanelet angle and calculate mean angle + double sum_x = 0.0; + double sum_y = 0.0; + for (const auto & current_lanelet : current_lanelets) { + const auto lanelet_angle = + lanelet::utils::getLaneletAngle(current_lanelet.lanelet, pose_with_cov.pose.position); + sum_x += std::cos(lanelet_angle); + sum_y += std::sin(lanelet_angle); + } + const double mean_yaw_angle = std::atan2(sum_y, sum_x); + double roll, pitch, yaw; + tf2::Quaternion original_quaternion; + tf2::fromMsg(pose_with_cov.pose.orientation, original_quaternion); + tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + tf2::Quaternion filtered_quaternion; + filtered_quaternion.setRPY(roll, pitch, mean_yaw_angle); + pose_with_cov.pose.orientation = tf2::toMsg(filtered_quaternion); +} + } // namespace MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) @@ -929,11 +955,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); debug_markers.markers.push_back(debug_marker); + // Fix object angle if its orientation unreliable (e.g. far object by radar sensor) + // This prevent bending predicted path + TrackedObject yaw_fixed_transformed_object = transformed_object; + if ( + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE) { + replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object); + } // Generate Predicted Path std::vector predicted_paths; for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( + yaw_fixed_transformed_object, ref_path.path); if (predicted_path.path.empty()) { continue; } diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index b0f736916ebda..96b6ca452bbcc 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -191,11 +191,18 @@ # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] - remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] nominal_avoidance_speed: 8.33 # [m/s] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 30.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] # For yield maneuver yield: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 7976399a7ed40..c8b87c7b4759f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -112,6 +112,14 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // consider avoidance return dead line + bool enable_dead_line_for_goal{false}; + bool enable_dead_line_for_traffic_light{false}; + + // module try to return original path to keep this distance from edge point of the path. + double dead_line_buffer_for_goal{0.0}; + double dead_line_buffer_for_traffic_light{0.0}; + // max deceleration for double max_deceleration{0.0}; @@ -217,9 +225,6 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed{0.0}; - // module try to return original path to keep this distance from edge point of the path. - double remain_buffer_distance{0.0}; - // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double soft_road_shoulder_margin{1.0}; @@ -517,6 +522,10 @@ struct AvoidancePlanningData bool found_avoidance_path{false}; double to_stop_line{std::numeric_limits::max()}; + + double to_start_point{std::numeric_limits::lowest()}; + + double to_return_point{std::numeric_limits::max()}; }; /* diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index b8b4efb7aed57..2e04935e37336 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -172,6 +172,16 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + +double calcDistanceToReturnDeadLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + +double calcDistanceToAvoidStartLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 5423cff11bfeb..9e4a3a928c4ca 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -288,6 +288,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.reference_path, 0, data.reference_path.points.size(), calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); + data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine( + data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); + + data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine( + data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); + // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -1066,18 +1072,35 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( : 0.0; const auto offset = object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + const auto to_shift_end = o.longitudinal - offset; const auto path_front_to_ego = avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); - al_avoid.start_longitudinal = - std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); + // start point (use previous linear shift length as start shift length.) + al_avoid.start_longitudinal = [&]() { + const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3); + + if (data.to_start_point > to_shift_end) { + return nearest_avoid_distance; + } + + const auto minimum_avoid_distance = + helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); + + return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); + }(); + al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); + // end point al_avoid.end_shift_length = feasible_shift_length.get(); - al_avoid.end_longitudinal = o.longitudinal - offset; + al_avoid.end_longitudinal = to_shift_end; + + // misc al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; al_avoid.object_on_right = utils::avoidance::isOnRight(o); @@ -1086,18 +1109,24 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidLine al_return; { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - // The end_margin also has the purpose of preventing the return path from NOT being - // triggered at the end point. - const auto return_remaining_distance = std::max( - data.arclength_from_ego.back() - o.longitudinal - offset - - parameters_->remain_buffer_distance, - 0.0); + const auto to_shift_start = o.longitudinal + offset; + // start point al_return.start_shift_length = feasible_shift_length.get(); + al_return.start_longitudinal = to_shift_start; + + // end point + al_return.end_longitudinal = [&]() { + if (data.to_return_point > to_shift_start) { + return std::clamp( + data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start); + } + + return to_shift_start + feasible_return_distance; + }(); al_return.end_shift_length = 0.0; - al_return.start_longitudinal = o.longitudinal + offset; - al_return.end_longitudinal = - o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); + + // misc al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; al_return.object_on_right = utils::avoidance::isOnRight(o); @@ -1795,7 +1824,9 @@ AvoidLineArray AvoidanceModule::addReturnShiftLine( return ret; } - const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; + const auto remaining_distance = std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, + avoid_data_.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); @@ -2859,8 +2890,8 @@ void AvoidanceModule::insertReturnDeadLine( { const auto & data = avoid_data_; - if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { - RCLCPP_DEBUG(getLogger(), "goal is far enough."); + if (data.to_return_point > planner_data_->parameters.forward_path_length) { + RCLCPP_DEBUG(getLogger(), "return dead line is far enough."); return; } @@ -2872,10 +2903,7 @@ void AvoidanceModule::insertReturnDeadLine( } const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); - - const auto to_goal = calcSignedArcLength( - shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); - const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + const auto to_stop_line = data.to_return_point - min_return_distance; // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 33c6982f99fd9..6ed67fc817f28 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -223,13 +223,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.remain_buffer_distance = getOrDeclareParameter(*node, ns + "remain_buffer_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); } + // avoidance maneuver (return shift dead line) + { + std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + // yield { std::string ns = "avoidance.yield."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 54a1c0c649916..f1bbb3bc28a36 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -48,6 +48,7 @@ namespace behavior_path_planner::utils::avoidance { +using autoware_perception_msgs::msg::TrafficSignalElement; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -225,6 +226,86 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) { base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } + +bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { + return false; + } + + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + +std::optional calcDistanceToRedTrafficLight( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + continue; + } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + lanelet::ConstLineString3d stop_line = *(element->stopLine()); + const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x()); + const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); + const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); + + return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); + } + } + + return std::nullopt; +} } // namespace bool isOnRight(const ObjectData & obj) @@ -754,26 +835,41 @@ void fillObjectEnvelopePolygon( return; } - const auto envelope_poly = + const auto one_shot_envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); - if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) { + // If the one_shot_envelope_poly is within the registered envelope, use the registered one + if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) { object_data.envelope_poly = same_id_obj->envelope_poly; return; } std::vector unions; - boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions); + boost::geometry::union_(one_shot_envelope_poly, same_id_obj->envelope_poly, unions); + // If union fails, use the current envelope if (unions.empty()) { - object_data.envelope_poly = - createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + object_data.envelope_poly = one_shot_envelope_poly; return; } boost::geometry::correct(unions.front()); - object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); + const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon_area = boost::geometry::area(object_polygon); + const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); + + // keep multi-step envelope polygon. + constexpr double THRESHOLD = 5.0; + if (envelope_polygon_area < object_polygon_area * THRESHOLD) { + object_data.envelope_poly = multi_step_envelope_poly; + return; + } + + // use latest one-shot envelope polygon. + object_data.envelope_poly = one_shot_envelope_poly; } void fillObjectMovingTime( @@ -1887,4 +1983,63 @@ DrivableLanes generateExpandDrivableLanes( return current_drivable_lanes; } + +double calcDistanceToAvoidStartLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (lanelets.empty()) { + return std::numeric_limits::lowest(); + } + + double distance_to_return_dead_line = std::numeric_limits::lowest(); + + // dead line stop factor(traffic light) + if (parameters->enable_dead_line_for_traffic_light) { + const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data); + if (to_traffic_light.has_value()) { + distance_to_return_dead_line = std::max( + distance_to_return_dead_line, + to_traffic_light.value() + parameters->dead_line_buffer_for_traffic_light); + } + } + + return distance_to_return_dead_line; +} + +double calcDistanceToReturnDeadLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (lanelets.empty()) { + return std::numeric_limits::max(); + } + + double distance_to_return_dead_line = std::numeric_limits::max(); + + // dead line stop factor(traffic light) + if (parameters->enable_dead_line_for_traffic_light) { + const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data); + if (to_traffic_light.has_value()) { + distance_to_return_dead_line = std::min( + distance_to_return_dead_line, + to_traffic_light.value() - parameters->dead_line_buffer_for_traffic_light); + } + } + + // dead line for goal + if (parameters->enable_dead_line_for_goal) { + if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + const auto to_goal_distance = + calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); + distance_to_return_dead_line = std::min( + distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); + } + } + + return distance_to_return_dead_line; +} } // namespace behavior_path_planner::utils::avoidance