From bdbb61661c37108f6e6f75c7f6cc5a20dbcbc25b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 11:30:12 +0900 Subject: [PATCH 1/2] fix(out_of_lane): improve stop decision Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 8 +- .../README.md | 15 ++- .../config/out_of_lane.param.yaml | 4 +- .../src/calculate_slowdown_points.hpp | 84 +++++++++++++++++ .../src/decisions.cpp | 39 ++++---- .../src/decisions.hpp | 5 +- .../src/filter_predicted_objects.hpp | 68 ++++++++++++++ .../src/footprint.cpp | 6 +- .../src/lanelets_selection.cpp | 21 +++-- .../src/manager.cpp | 3 +- .../src/scene_out_of_lane.cpp | 91 ++++--------------- .../src/scene_out_of_lane.hpp | 9 -- .../src/types.hpp | 11 ++- 13 files changed, 228 insertions(+), 136 deletions(-) create mode 100644 planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp create mode 100644 planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 633d5fdd613af..8ce1a334c4b35 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -6,13 +6,7 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/decisions.cpp - src/footprint.cpp - src/lanelets_selection.cpp - src/manager.cpp - src/overlapping_range.cpp - src/scene_out_of_lane.cpp + DIRECTORY src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index da46055460a99..d934df43aa6de 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -149,14 +149,13 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | | `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | 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..c74c5b3d87c1d 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 @@ -24,8 +24,7 @@ action: # action to insert in the path if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego - # if false, ego stops just before entering a lane but may then be overlapping another lane. + precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap @@ -34,6 +33,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/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..6f3094aea9682 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,84 @@ +// 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 CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "footprint.hpp" +#include "types.hpp" + +#include + +#include +#include + +namespace behavior_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) +{ + if (ego_data.velocity < 0.5) return true; + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, point.point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, + const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); + const auto to_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); + PathPointWithLaneId interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || + can_decelerate(ego_data, interpolated_point, decision.velocity); + if ( + respect_decel_limit && !boost::geometry::overlaps( + project_to_pose(footprint, interpolated_point.point.pose), + decision.lane_to_avoid.polygon2d().basicPolygon())) + return interpolated_point; + } + return std::nullopt; +} + +/// @brief calculate the slowdown point to insert in the path +/// @param ego_data ego data (path, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param params parameters +/// @return optional slowdown point to insert in the path +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace behavior_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ 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..3d8c6b7eb663b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -27,15 +27,15 @@ namespace behavior_velocity_planner::out_of_lane double distance_along_path(const EgoData & ego_data, const size_t target_idx) { return motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + 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, - ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * + 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()); @@ -119,11 +117,9 @@ std::optional> object_time_to_range( const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { - RCLCPP_DEBUG(logger, " / SAME DIR \\\n"); worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; } else { - RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n"); worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; } @@ -205,8 +201,11 @@ std::optional> object_time_to_range( bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) { - return std::min(range_times.object.enter_time, range_times.object.exit_time) < - params.time_threshold; + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; } bool intervals_condition( @@ -281,8 +280,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 +299,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"); @@ -349,11 +349,10 @@ std::optional calculate_decision( { std::optional decision; if (should_not_enter(range, inputs, params, logger)) { - const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range; decision.emplace(); - decision->target_path_idx = inputs.ego_data.first_path_idx + - stop_before_range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = stop_before_range.lane; + decision->target_path_idx = + inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); set_decision_velocity(decision, ego_dist_to_range, params); } 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/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 929fbd944af9f..024e28ebec612 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -58,9 +58,9 @@ std::vector calculate_path_footprints( { const auto base_footprint = make_base_footprint(params); std::vector path_footprints; - path_footprints.reserve(ego_data.path->points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) { - const auto & path_pose = ego_data.path->points[i].point.pose; + path_footprints.reserve(ego_data.path.points.size()); + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; 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 a0e2b86caa623..35a28bead33a9 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 @@ -54,16 +54,23 @@ lanelet::ConstLanelets calculate_other_lanelets( const auto lanelets_within_range = lanelet::geometry::findWithin2d( route_handler.getLaneletMapPtr()->laneletLayer, ego_point, std::max(params.slow_dist_threshold, params.stop_dist_threshold)); + const auto is_overlapped_by_path_lanelets = [&](const auto & l) { + for (const auto & path_ll : path_lanelets) { + if ( + boost::geometry::overlaps( + path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - const auto is_overlapped_by_path_lanelets = [&](const auto & l) { - for (const auto & path_ll : path_lanelets) - if (boost::geometry::overlaps( - path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon())) - return true; - return false; - }; if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) other_lanelets.push_back(ll.second); } 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 075738f07d62e..2422c6fe84e56 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -56,7 +56,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.skip_if_over_max_decel = getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); - pp.strict = getOrDeclareParameter(node, ns + ".action.strict"); + pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = @@ -64,6 +64,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.stop_dist_threshold = getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); + pp.ego_min_velocity = getOrDeclareParameter(node, ns + ".ego.min_assumed_velocity"); pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, 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..74d1f86c51df1 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 @@ -14,8 +14,10 @@ #include "scene_out_of_lane.hpp" +#include "calculate_slowdown_points.hpp" #include "debug.hpp" #include "decisions.hpp" +#include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" #include "overlapping_range.hpp" @@ -58,9 +60,9 @@ bool OutOfLaneModule::modifyPathVelocity( stopwatch.tic(); EgoData ego_data; ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path = path; + ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; stopwatch.tic("calculate_path_footprints"); @@ -69,7 +71,7 @@ bool OutOfLaneModule::modifyPathVelocity( const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider const auto path_lanelets = planning_utils::getLaneletsOnPath( - *path, planner_data_->route_handler_->getLaneletMapPtr(), + ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); @@ -103,28 +105,33 @@ 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_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); - const auto points_to_insert = calculate_slowdown_points(ego_data, decisions, params_); - debug_data_.slowdowns = points_to_insert; + const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); stopwatch.tic("insert_slowdown_points"); - for (const auto & point : points_to_insert) { - auto path_idx = point.slowdown.target_path_idx; - planning_utils::insertVelocity(*ego_data.path, point.point, point.slowdown.velocity, path_idx); - if (point.slowdown.velocity == 0.0) { + debug_data_.slowdowns.clear(); + if (point_to_insert) { + debug_data_.slowdowns = {*point_to_insert}; + auto path_idx = motion_utils::findNearestSegmentIndex( + path->points, point_to_insert->point.point.pose.position) + + 1; + planning_utils::insertVelocity( + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, + params_.precision); + if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point.point.point.pose; + stop_factor.stop_pose = point_to_insert->point.point.pose; stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, point.point.point.pose.position); + ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position); planning_utils::appendStopReason(stop_factor, stop_reason); } velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point.point.point.pose, + path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, VelocityFactor::UNKNOWN); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); @@ -177,62 +184,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params) -{ - std::vector to_insert; - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - const auto can_decel = [&](const auto dist_ahead_of_ego, const auto target_vel) { - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); - }; - const auto insert_decision = [&](const auto & path_point, const auto & decision) -> bool { - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, path_point.point.pose.position); - if (!params.skip_if_over_max_decel || can_decel(dist_ahead_of_ego, decision.velocity)) { - to_insert.push_back({decision, path_point}); - return true; - } - return false; - }; - const auto insert_interpolated_decision = - [&](const auto & path_point, const auto & decision) -> bool { - auto interpolated_point = path_point; - const auto & path_pose = path_point.point.pose; - const auto & prev_path_pose = ego_data.path->points[decision.target_path_idx - 1].point.pose; - constexpr auto precision = 0.1; - for (auto ratio = precision; ratio <= 1.0; ratio += precision) { - interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(path_pose, prev_path_pose, ratio, false); - const auto is_overlap = boost::geometry::overlaps( - project_to_pose(base_footprint, interpolated_point.point.pose), - decision.lane_to_avoid.polygon2d().basicPolygon()); - if (!is_overlap) { - return insert_decision(path_point, decision); - } - } - return false; - }; - for (const auto & decision : decisions) { - const auto & path_point = ego_data.path->points[decision.target_path_idx]; - const auto decision_is_at_beginning_of_path = - decision.target_path_idx == ego_data.first_path_idx; - bool inserted = false; - if (decision_is_at_beginning_of_path) { - inserted = insert_decision(path_point, decision); - } else { - inserted = insert_interpolated_decision(path_point, decision); - // if no valid point found, fallback to using the previous index (known to not overlap) - if (!inserted) - inserted = insert_decision(ego_data.path->points[decision.target_path_idx], decision); - } - // only insert the first (i.e., lowest arc length) decision - if (inserted) break; - } - return to_insert; -} - } // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 79c66148926fc..f2ca2ababfef4 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -56,15 +56,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; - -/// @brief calculate points along the path where we want ego to slowdown/stop -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decisions (before which point to stop, what lane to avoid entering, etc) -/// @param params parameters -/// @return precise points to insert in the path -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params); - } // namespace behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ 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..64d88f15e6966 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -42,20 +42,21 @@ 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 // action to insert in the path if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - bool strict; // if true stop before entering *any* other lane, not only the lane to avoid double dist_buffer; double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the path // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -134,7 +135,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId * path{}; + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] From 43b457d7a670b29703f8c89809fd1f7f275477cf Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 19:37:25 +0900 Subject: [PATCH 2/2] apply clang-format Signed-off-by: Takayuki Murooka --- .../src/lanelets_selection.cpp | 1 + 1 file changed, 1 insertion(+) 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 35a28bead33a9..c9ec623477671 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 @@ -19,6 +19,7 @@ #include #include +#include namespace behavior_velocity_planner::out_of_lane {