From c44cd027c9f0314bcdc582962e97225b3fd32b15 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Sun, 24 Sep 2023 09:07:03 +0900 Subject: [PATCH] feat(out_of_lane): more stable decisions Signed-off-by: Maxime CLEMENT --- .../config/out_of_lane.param.yaml | 2 + .../src/calculate_slowdown_points.hpp | 103 ++++++++++++++++++ .../src/debug.cpp | 87 ++++++++++++++- .../src/debug.hpp | 20 +++- .../src/decisions.cpp | 36 ++---- .../src/decisions.hpp | 22 +--- .../src/manager.cpp | 4 +- .../src/overlapping_range.cpp | 11 +- .../src/overlapping_range.hpp | 10 -- .../src/scene_out_of_lane.cpp | 78 ++++++++++--- .../src/scene_out_of_lane.hpp | 7 +- .../src/types.hpp | 71 ++++++++++-- 12 files changed, 363 insertions(+), 88 deletions(-) create mode 100644 planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp 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 510dc86ef651d..28aa58e66df64 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 @@ -17,6 +17,7 @@ use_predicted_paths: true # if true, use the predicted paths to estimate future positions. # 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 overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered @@ -27,6 +28,7 @@ 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. distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap velocity: 2.0 # [m/s] slowdown velocity 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..b9d9f6f98d53a --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,103 @@ +// 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 +{ + +/// @brief estimate whether ego can decelerate without breaking the deceleration limit +/// @details assume ego wants to reach the target point at the target velocity +/// @param [in] ego_data ego data +/// @param [in] point target point +/// @param [in] target_vel target_velocity +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); +} + +/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid +/// @param [in] ego_data ego data +/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) +/// @param [in] footprint the ego footprint +/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits +/// @param [in] params parameters +/// @return the last pose that is not out of lane (if found) +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, + const std::optional & prev_slowdown_point, 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 || prev_slowdown_point || + can_decelerate(ego_data, interpolated_point, decision.velocity); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose); + const auto is_overlap_lane = boost::geometry::overlaps( + interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); + const auto is_overlap_extra_lane = + prev_slowdown_point && + boost::geometry::overlaps( + interpolated_footprint, + prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); + if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) + 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 prev_slowdown_point previously calculated slowdown point +/// @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, + const std::optional prev_slowdown_point, 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, prev_slowdown_point, 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/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 0574a5226dc4a..489bb3f5abc78 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -34,13 +34,12 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); - base_marker.lifetime = rclcpp::Duration::from_seconds(0.5); return base_marker; } } // namespace void add_footprint_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z) + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "footprints"; @@ -54,12 +53,14 @@ void add_footprint_markers( debug_marker.id++; debug_marker.points.clear(); } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); } void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z) + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "current_overlap"; @@ -82,12 +83,14 @@ void add_current_overlap_marker( debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); } void add_lanelet_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color) + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = ns; @@ -103,6 +106,82 @@ void add_lanelet_markers( debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, + const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "ranges"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + for (const auto & range : ranges) { + debug_marker.points.clear(); + debug_marker.points.push_back( + path.points[first_ego_idx + range.entering_path_idx].point.pose.position); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + for (const auto & overlap : range.debug.overlaps) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + } + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.exiting_point.x(), range.exiting_point.y(), z)); + debug_marker.points.push_back( + path.points[first_ego_idx + range.exiting_path_idx].point.pose.position); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); + debug_marker.action = debug_marker.ADD; + debug_marker.id = 0; + debug_marker.ns = "decisions"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.points.clear(); + for (const auto & range : ranges) { + debug_marker.type = debug_marker.LINE_STRIP; + if (range.debug.decision) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + debug_marker.points.push_back( + range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + } + debug_marker_array.markers.push_back(debug_marker); + debug_marker.points.clear(); + debug_marker.id++; + + debug_marker.type = debug_marker.TEXT_VIEW_FACING; + debug_marker.pose.position.x = range.entering_point.x(); + debug_marker.pose.position.y = range.entering_point.y(); + debug_marker.pose.position.z = z; + std::stringstream ss; + ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time + << "\n"; + if (range.debug.object) { + debug_marker.pose.position.x += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; + debug_marker.pose.position.y += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; + debug_marker.pose.position.x /= 2; + debug_marker.pose.position.y /= 2; + ss << "Obj: " << range.debug.times.object.enter_time << " - " + << range.debug.times.object.exit_time << "\n"; + } + debug_marker.scale.z = 1.0; + debug_marker.text = ss.str(); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); } } // namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 06bc6d935f310..2b6b65638ec40 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -29,27 +29,41 @@ namespace behavior_velocity_planner::out_of_lane::debug /// @param [inout] debug_marker_array marker array /// @param [in] footprints footprints to turn into markers /// @param [in] z z value to use for the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_footprint_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z); + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb); /// @brief add footprint markers to the given marker array /// @param [inout] debug_marker_array marker array /// @param [in] current_footprint footprint to turn into a marker /// @param [in] current_overlapped_lanelets lanelets to turn into markers /// @param [in] z z value to use for the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z); + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb); /// @brief add footprint markers to the given marker array /// @param [inout] debug_marker_array marker array /// @param [in] lanelets lanelets to turn into markers /// @param [in] ns namespace of the markers /// @param [in] color namespace of the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_lanelet_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color); + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb); +/// @brief add ranges markers to the given marker array +/// @param [inout] debug_marker_array marker array +/// @param [in] ranges ranges to turn into markers +/// @param [in] path modified ego path that was used to calculate the ranges +/// @param [in] first_path_idx first idx of ego on the path +/// @param [in] z z value to use for the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, + const double z, const size_t prev_nb); } // namespace behavior_velocity_planner::out_of_lane::debug #endif // DEBUG_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 da46c1355e735..abeb9afe8aa43 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -26,7 +26,6 @@ #include #include -#include #include #include #include @@ -63,7 +62,8 @@ 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 rclcpp::Logger & logger) + const std::shared_ptr route_handler, const double dist_buffer, + 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 @@ -72,8 +72,7 @@ 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 + range.inside_distance; - + const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); @@ -307,7 +306,8 @@ 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, logger) + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_dist_buffer, logger) : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); @@ -316,27 +316,14 @@ bool should_not_enter( range_times.object.enter_time = enter_exit_time->first; range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) return true; - } - return false; -} - -OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs) -{ - OverlapRange preceding_range = range; - bool found_preceding_range = true; - while (found_preceding_range) { - found_preceding_range = false; - for (const auto & other_range : inputs.ranges) { - if ( - other_range.entering_path_idx < preceding_range.entering_path_idx && - other_range.exiting_path_idx >= preceding_range.entering_path_idx) { - preceding_range = other_range; - found_preceding_range = true; - } + if (will_collide_on_range(range_times, params, logger)) { + range.debug.times = range_times; + range.debug.object = object; + return true; } } - return preceding_range; + range.debug.times = range_times; + return false; } void set_decision_velocity( @@ -375,6 +362,7 @@ std::vector calculate_decisions( for (const auto & range : inputs.ranges) { if (range.entering_path_idx == 0UL) continue; // skip if we already entered the range const auto optional_decision = calculate_decision(range, inputs, params, logger); + range.debug.decision = optional_decision; if (optional_decision) decisions.push_back(*optional_decision); } return decisions; 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 2898595c2e74c..0612cc041c1ef 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -31,16 +31,6 @@ namespace behavior_velocity_planner::out_of_lane { -struct EnterExitTimes -{ - double enter_time; - double exit_time; -}; -struct RangeTimes -{ - EnterExitTimes ego; - EnterExitTimes object; -}; /// @brief calculate the distance along the ego path between ego and some target path index /// @param [in] ego_data data related to the ego vehicle /// @param [in] target_idx target ego path index @@ -64,13 +54,15 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// 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 rclcpp::Logger & logger); + const std::shared_ptr route_handler, const double dist_buffer, + 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 /// @param [in] range overlapping range /// @param [in] inputs information used to take decisions (ranges, ego and objects data, route /// handler, lanelets) +/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range /// @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. @@ -94,14 +86,6 @@ bool will_collide_on_range( bool should_not_enter( const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief find the most preceding range -/// @details the most preceding range is the first range in a succession of ranges with overlapping -/// enter/exit indexes. -/// @param [in] range range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @return most preceding range -OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs); /// @brief set the velocity of a decision (or unset it) based on the distance away from the range /// @param [out] decision decision to update (either set its velocity or unset the decision) /// @param [in] distance distance between ego and the range corresponding to the decision 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 5765363024aed..23de809e429ec 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -50,13 +50,15 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".objects.use_predicted_paths"); pp.objects_min_confidence = getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); + pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); 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.min_decision_duration = getOrDeclareParameter(node, ns + ".action.min_duration"); pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index 98a9e4404e013..6f74f42af2056 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -23,7 +23,6 @@ #include #include -#include namespace behavior_velocity_planner::out_of_lane { @@ -76,10 +75,12 @@ OverlapRanges calculate_overlapping_ranges( { OverlapRanges ranges; OtherLane other_lane(lanelet); + std::vector overlaps; for (auto i = 0UL; i < path_footprints.size(); ++i) { const auto overlap = calculate_overlap(path_footprints[i], path_lanelets, lanelet); const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; if (has_overlap) { // open/update the range + overlaps.push_back(overlap); if (!other_lane.range_is_open) { other_lane.first_range_bound.index = i; other_lane.first_range_bound.point = overlap.min_overlap_point; @@ -94,10 +95,16 @@ OverlapRanges calculate_overlapping_ranges( other_lane.last_range_bound.inside_distance = overlap.inside_distance; } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); } } // close the range if it is still open - if (other_lane.range_is_open) ranges.push_back(other_lane.close_range()); + if (other_lane.range_is_open) { + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } return ranges; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index ab4d31480e1cb..24bd6b3f16eb9 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -21,21 +21,11 @@ #include -#include #include namespace behavior_velocity_planner::out_of_lane { -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; /// @brief calculate the overlap between the given footprint and lanelet /// @param [in] path_footprint footprint used to calculate the overlap /// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path 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 013643b6bd6d4..8c20d3a826ed6 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 @@ -52,8 +52,7 @@ OutOfLaneModule::OutOfLaneModule( velocity_factor_.init(VelocityFactor::UNKNOWN); } -bool OutOfLaneModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_.reset_data(); *stop_reason = planning_utils::initializeStopReason(StopReason::OUT_OF_LANE); @@ -64,7 +63,24 @@ bool OutOfLaneModule::modifyPathVelocity( ego_data.pose = planner_data_->current_odometry->pose; ego_data.path = path; ego_data.first_path_idx = +<<<<<<< HEAD motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); +======= + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + for (const auto & p : prev_overlapping_path_points_) { + const auto nearest_idx = + motion_utils::findNearestIndex(ego_data.path.points, p.point.pose, 1.0); + const auto insert_idx = + motion_utils::findNearestSegmentIndex(ego_data.path.points, p.point.pose, 1.0); + if (nearest_idx && insert_idx && *insert_idx > ego_data.first_path_idx) { + if ( + tier4_autoware_utils::calcDistance2d( + ego_data.path.points[*nearest_idx].point.pose, p.point.pose) > 0.5) + ego_data.path.points.insert(std::next(ego_data.path.points.begin(), *insert_idx), p); + } + } + motion_utils::removeOverlapPoints(ego_data.path.points); +>>>>>>> 4f53c938b7 (feat(out_of_lane): more stable decisions) 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"); @@ -84,6 +100,8 @@ bool OutOfLaneModule::modifyPathVelocity( debug_data_.path_lanelets = path_lanelets; debug_data_.ignored_lanelets = ignored_lanelets; debug_data_.other_lanelets = other_lanelets; + debug_data_.path = ego_data.path; + debug_data_.first_path_idx = ego_data.first_path_idx; if (params_.skip_if_already_overlapping) { debug_data_.current_footprint = current_ego_footprint; @@ -101,6 +119,10 @@ bool OutOfLaneModule::modifyPathVelocity( stopwatch.tic("calculate_overlapping_ranges"); const auto ranges = calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); + prev_overlapping_path_points_.clear(); + for (const auto & range : ranges) + prev_overlapping_path_points_.push_back( + ego_data.path.points[ego_data.first_path_idx + range.entering_path_idx]); const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); // Calculate stop and slowdown points stopwatch.tic("calculate_decisions"); @@ -110,28 +132,49 @@ bool OutOfLaneModule::modifyPathVelocity( 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 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; + if ( // reset the previous inserted point if the timer expired + prev_inserted_point_ && + (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) + prev_inserted_point_.reset(); + auto point_to_insert = + calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, 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 ( // reset the timer if there is no previous inserted point or if we avoid the same lane + point_to_insert && + (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == + point_to_insert->slowdown.lane_to_avoid.id())) + prev_inserted_point_time_ = clock_->now(); + if (!point_to_insert && prev_inserted_point_) point_to_insert = prev_inserted_point_; + if (point_to_insert) { + prev_inserted_point_ = point_to_insert; + RCLCPP_INFO(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) + + 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.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, point.point.point.pose.position); + 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, VelocityFactor::UNKNOWN); + } else if (!decisions.empty()) { + RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); + debug_data_.ranges = inputs.ranges; const auto total_time_us = stopwatch.toc(); RCLCPP_DEBUG( @@ -152,18 +195,23 @@ MarkerArray OutOfLaneModule::createDebugMarkerArray() constexpr auto z = 0.0; MarkerArray debug_marker_array; - debug::add_footprint_markers(debug_marker_array, debug_data_.footprints, z); + debug::add_footprint_markers( + debug_marker_array, debug_data_.footprints, z, debug_data_.prev_footprints); debug::add_current_overlap_marker( - debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z); + debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z, + debug_data_.prev_current_overlapped_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data_.path_lanelets, "path_lanelets", - tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5)); + tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data_.prev_path_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data_.ignored_lanelets, "ignored_lanelets", - tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5)); + tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data_.prev_ignored_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data_.other_lanelets, "other_lanelets", - tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5)); + tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data_.prev_other_lanelets); + debug::add_range_markers( + debug_marker_array, debug_data_.ranges, debug_data_.path, debug_data_.first_path_idx, z, + debug_data_.prev_ranges); return debug_marker_array; } 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 1a64c11a3c921..3d3beaf58b421 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -48,9 +49,13 @@ class OutOfLaneModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; private: - // Parameter PlannerParam params_; + std::vector + prev_overlapping_path_points_{}; + std::optional prev_inserted_point_{}; + rclcpp::Time prev_inserted_point_time_{}; + protected: int64_t module_id_{}; 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 b81fa09884819..0054de018274a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -47,6 +48,8 @@ struct PlannerParam 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 objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in + // the other lane 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 @@ -57,6 +60,8 @@ struct PlannerParam double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the path + double min_decision_duration; // [s] minimum duration needed a decision can be canceled // 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) @@ -68,17 +73,16 @@ struct PlannerParam double extra_left_offset; // [m] extra left distance }; -/// @brief range along the path where ego overlaps another lane -struct OverlapRange +struct EnterExitTimes { - lanelet::ConstLanelet lane; - size_t entering_path_idx{}; - size_t exiting_path_idx{}; - lanelet::BasicPoint2d entering_point; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane + double enter_time{}; + double exit_time{}; +}; +struct RangeTimes +{ + EnterExitTimes ego{}; + EnterExitTimes object{}; }; -using OverlapRanges = std::vector; /// @brief action taken by the "out of lane" module struct Slowdown @@ -102,6 +106,35 @@ struct RangeBound double arc_length; double inside_distance; }; + +/// @brief representation of an overlap between the ego footprint and some other lane +struct Overlap +{ + double inside_distance = 0.0; ///!< distance inside the overlap + double min_arc_length = std::numeric_limits::infinity(); + double max_arc_length = 0.0; + lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length + lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length +}; + +/// @brief range along the path where ego overlaps another lane +struct OverlapRange +{ + lanelet::ConstLanelet lane; + size_t entering_path_idx{}; + size_t exiting_path_idx{}; + lanelet::BasicPoint2d entering_point; // pose of the overlapping point closest along the lane + lanelet::BasicPoint2d exiting_point; // pose of the overlapping point furthest along the lane + double inside_distance{}; // [m] how much ego footprint enters the lane + mutable struct + { + std::vector overlaps; + std::optional decision; + RangeTimes times; + std::optional object{}; + } debug; +}; +using OverlapRanges = std::vector; /// @brief representation of a lane and its current overlap range struct OtherLane { @@ -164,12 +197,32 @@ struct DebugData lanelet::ConstLanelets path_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; + autoware_auto_planning_msgs::msg::PathWithLaneId path; + size_t first_path_idx; + + size_t prev_footprints = 0; + size_t prev_slowdowns = 0; + size_t prev_ranges = 0; + size_t prev_current_overlapped_lanelets = 0; + size_t prev_ignored_lanelets = 0; + size_t prev_path_lanelets = 0; + size_t prev_other_lanelets = 0; void reset_data() { + prev_footprints = footprints.size(); footprints.clear(); + prev_slowdowns = slowdowns.size(); slowdowns.clear(); + prev_ranges = ranges.size(); ranges.clear(); + prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); current_overlapped_lanelets.clear(); + prev_ignored_lanelets = ignored_lanelets.size(); + ignored_lanelets.clear(); + prev_path_lanelets = path_lanelets.size(); + path_lanelets.clear(); + prev_other_lanelets = other_lanelets.size(); + other_lanelets.clear(); } };