diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index 092f1d32b27b3..1b483b95510ed 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -18,6 +18,7 @@ # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index 43dea2f2153ff..b61441132969c 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -27,6 +27,7 @@ tf2 tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index be72109db4548..e948bd74eba45 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -73,11 +73,13 @@ std::optional> object_time_to_range( if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; + const auto max_lon_deviation = object.shape.dimensions.x / 2.0; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + if (unique_path_points.size() < 2) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -121,7 +123,17 @@ std::optional> object_time_to_range( max_deviation); continue; } - // else we rely on the interpolation to estimate beyond the end of the predicted path + const auto is_last_predicted_path_point = + (exit_segment_idx + 2 == unique_path_points.size() || + enter_segment_idx + 2 == unique_path_points.size()); + const auto does_not_reach_overlap = + is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); + if (does_not_reach_overlap) { + RCLCPP_DEBUG( + logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", + std::min(exit_offset, enter_offset), max_lon_deviation); + continue; + } const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..de12334b91a19 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,146 @@ +// Copyright 2023-2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + auto stop_line_idx = 0UL; + bool found = false; + lanelet::BasicSegment2d path_segment; + path_segment.first.x() = predicted_path.path.front().position.x; + path_segment.first.y() = predicted_path.path.front().position.y; + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; + if (boost::geometry::intersects(stop_line, path_segment)) { + found = true; + break; + } + path_segment.first = path_segment.second; + } + if (found) { + auto cut_idx = stop_line_idx; + double arc_length = 0; + while (cut_idx > 0 && arc_length < object_front_overhang) { + arc_length += tier4_autoware_utils::calcDistance2d( + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); + --cut_idx; + } + predicted_path.path.resize(cut_idx); + } +} + +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) +{ + lanelet::ConstLanelets lanelets; + lanelet::BasicLineString2d query_line; + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); + const auto query_results = lanelet::geometry::findWithin2d( + planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line); + for (const auto & r : query_results) lanelets.push_back(r.second); + for (const auto & ll : lanelets) { + for (const auto & element : ll.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id()); + if ( + traffic_signal_stamped.has_value() && element->stopLine().has_value() && + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { + lanelet::BasicLineString2d stop_line; + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); + return stop_line; + } + } + } + return std::nullopt; +} + +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const PlannerData & planner_data, const double object_front_overhang) +{ + const auto stop_line = find_next_stop_line(predicted_path, planner_data); + if (stop_line) { + // we use a longer stop line to also cut predicted paths that slightly go around the stop line + auto longer_stop_line = *stop_line; + const auto diff = stop_line->back() - stop_line->front(); + longer_stop_line.front() -= diff * 0.5; + longer_stop_line.back() += diff * 0.5; + cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang); + } +} + +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = planner_data.predicted_objects->header; + for (const auto & object : planner_data.predicted_objects->objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + if (no_overlap_path.size() <= 1) return true; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, planner_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); + } + + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 7bad4974ea9af..bb6b5f4d00005 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,56 +17,41 @@ #include "types.hpp" -#include +#include -#include +#include namespace behavior_velocity_planner::out_of_lane { +/// @brief cut a predicted path beyond the given stop line +/// @param [inout] predicted_path predicted path to cut +/// @param [in] stop_line stop line used for cutting +/// @param [in] object_front_overhang extra distance to cut ahead of the stop line +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); + +/// @brief find the next red light stop line along the given path +/// @param [in] path predicted path to check for a stop line +/// @param [in] planner_data planner data with stop line information +/// @return the first red light stop line found along the path (if any) +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); + +/// @brief cut predicted path beyond stop lines of red lights +/// @param [inout] predicted_path predicted path to cut +/// @param [in] planner_data planner data to get the map and traffic light information +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const PlannerData & planner_data, const double object_front_overhang); + /// @brief filter predicted objects and their predicted paths -/// @param [in] objects predicted objects to filter +/// @param [in] planner_data planner data /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, - const PlannerParam & params) -{ - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = objects.header; - for (const auto & object : objects.objects) { - const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); - if (is_pedestrian) continue; - - auto filtered_object = object; - const auto is_invalid_predicted_path = [&](const auto & predicted_path) { - const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); - if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = - std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); - const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); - return is_low_confidence || is_crossing_ego; - }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - } - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) - filtered_objects.objects.push_back(filtered_object); - } - return filtered_objects; -} - + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 3c90a8cbf0dce..738ea22106b29 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -24,12 +24,47 @@ namespace behavior_velocity_planner::out_of_lane { + +lanelet::ConstLanelets consecutive_lanelets( + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); + consecutives.insert(consecutives.end(), previous.begin(), previous.end()); + return consecutives; +} + +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler) +{ + lanelet::ConstLanelets missing_lane_change_lanelets; + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelets adjacents; + lanelet::ConstLanelets consecutives; + for (const auto & ll : path_lanelets) { + const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); + std::copy_if( + consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), + [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); + const auto adjacents_of_ll = routing_graph.besides(ll); + std::copy_if( + adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), + [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); + } + std::copy_if( + adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), + [&](const auto & l) { + return !contains_lanelet(missing_lane_change_lanelets, l.id()) && + !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id()); + }); + return missing_lane_change_lanelets; +} + lanelet::ConstLanelets calculate_path_lanelets( const EgoData & ego_data, const route_handler::RouteHandler & route_handler) { const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); - lanelet::ConstLanelets path_lanelets = - planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose); + lanelet::ConstLanelets path_lanelets; lanelet::BasicLineString2d path_ls; for (const auto & p : ego_data.path.points) path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); @@ -38,6 +73,8 @@ lanelet::ConstLanelets calculate_path_lanelets( if (!contains_lanelet(path_lanelets, dist_lanelet.second.id())) path_lanelets.push_back(dist_lanelet.second); } + const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler); + path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); return path_lanelets; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index 9c3aee46c8be5..87757a0cb2230 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -41,6 +41,13 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_path_lanelets( const EgoData & ego_data, const route_handler::RouteHandler & route_handler); +/// @brief calculate lanelets that may not be crossed by the path but may be overlapped during a +/// lane change +/// @param [in] path_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler); /// @brief calculate lanelets that should be ignored /// @param [in] ego_data data about the ego vehicle /// @param [in] path_lanelets lanelets driven by the ego vehicle diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 23de809e429ec..8055f5c9106ef 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -51,6 +51,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.objects_min_confidence = getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); + pp.objects_cut_predicted_paths_beyond_red_lights = + getOrDeclareParameter(node, ns + ".objects.cut_predicted_paths_beyond_red_lights"); pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index ef4434e235244..47020fb1cb623 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -109,13 +109,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); // Calculate stop and slowdown points - stopwatch.tic("calculate_decisions"); DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); + stopwatch.tic("filter_predicted_objects"); + inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_); + const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; + stopwatch.tic("calculate_decisions"); const auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); @@ -157,7 +159,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } if (point_to_insert) { prev_inserted_point_ = point_to_insert; - RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); + RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); debug_data_.slowdowns = {*point_to_insert}; auto path_idx = motion_utils::findNearestSegmentIndex( path->points, point_to_insert->point.point.pose.position) + @@ -187,12 +189,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto "\tcalculate_lanelets = %2.0fus\n" "\tcalculate_path_footprints = %2.0fus\n" "\tcalculate_overlapping_ranges = %2.0fus\n" + "\tfilter_pred_objects = %2.0fus\n" "\tcalculate_decisions = %2.0fus\n" "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", total_time_us, calculate_lanelets_us, calculate_path_footprints_us, - calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us, - insert_slowdown_points_us); + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); return true; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 34bd52634ce7b..d482870e5ce18 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -47,8 +47,10 @@ struct PlannerParam double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range bool objects_use_predicted_paths; // whether to use the objects' predicted paths - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path + bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red + // lights' stop lines + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in // the other lane