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 dd4c1c610261c..510dc86ef651d 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 @@ -34,6 +34,7 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego extra_front_offset: 0.0 # [m] extra front distance extra_rear_offset: 0.0 # [m] extra rear distance extra_right_offset: 0.0 # [m] extra right distance 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 163c8632e40c7..46db7a53b5c81 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -30,11 +30,11 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx) ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } -double time_along_path(const EgoData & ego_data, const size_t target_idx) +double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) { const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( - ego_data.velocity, + std::max(ego_data.velocity, min_velocity), ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; @@ -55,8 +55,7 @@ bool object_is_incoming( std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger) + const std::shared_ptr route_handler, const rclcpp::Logger & logger) { // skip the dynamic object if it is not in a lane preceding the overlapped lane // lane changes are intentionally not considered @@ -65,13 +64,12 @@ std::optional> object_time_to_range( object.kinematics.initial_pose_with_covariance.pose.position.y); if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - const auto max_deviation = object.shape.dimensions.y * 2.0; + const auto max_deviation = object.shape.dimensions.y + range.inside_distance; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { - if (predicted_path.confidence < min_confidence) 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()); @@ -281,8 +279,10 @@ bool should_not_enter( const rclcpp::Logger & logger) { RangeTimes range_times{}; - range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx); - range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx); + range_times.ego.enter_time = + time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); RCLCPP_DEBUG( logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); @@ -298,8 +298,7 @@ bool should_not_enter( // skip objects that are already on the interval const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_min_confidence, logger) + ? object_time_to_range(object, range, inputs.route_handler, logger) : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index bf6bf81e506cf..1d1fe8bea94a6 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -48,6 +48,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief estimate the time when ego will reach some target path index /// @param [in] ego_data data related to the ego vehicle /// @param [in] target_idx target ego path index +/// @param [in] min_velocity minimum ego velocity used to estimate the time /// @return time taken by ego to reach the target [s] double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief use an object's predicted paths to estimate the times it will reach the enter and exit @@ -57,14 +58,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @param [in] object dynamic object /// @param [in] range overlapping range /// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] min_confidence minimum confidence to consider a predicted path /// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger); + const std::shared_ptr route_handler, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object 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 new file mode 100644 index 0000000000000..48869e5e3926d --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 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. + +#ifndef FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +/// @brief filter predicted objects and their predicted paths +/// @param [in] objects predicted objects to filter +/// @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 lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(predicted_path.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; +} + +} // namespace behavior_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ 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 22fb29110466a..8245f7ffb2cd0 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -58,6 +58,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".action.slowdown.distance_threshold"); pp.stop_dist_threshold = node.declare_parameter(ns + ".action.stop.distance_threshold"); + pp.ego_min_velocity = node.declare_parameter(ns + ".ego.min_assumed_velocity"); pp.extra_front_offset = node.declare_parameter(ns + ".ego.extra_front_offset"); pp.extra_rear_offset = node.declare_parameter(ns + ".ego.extra_rear_offset"); pp.extra_left_offset = node.declare_parameter(ns + ".ego.extra_left_offset"); 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 5922a5b9b4488..430cec2d3d4ab 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 @@ -16,6 +16,7 @@ #include "debug.hpp" #include "decisions.hpp" +#include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" #include "overlapping_range.hpp" @@ -103,7 +104,7 @@ bool OutOfLaneModule::modifyPathVelocity( DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = *planner_data_->predicted_objects; + inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; auto decisions = calculate_decisions(inputs, params_, logger_); 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 85266d779f34e..b81fa09884819 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -42,10 +42,11 @@ struct PlannerParam double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + 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_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 double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap