From 972dbbc1643ec8f5bca2e2ddf8b588bbb0a82458 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 --- common/autoware_auto_geometry/CMakeLists.txt | 6 -- common/autoware_auto_tf2/CMakeLists.txt | 5 -- .../tf2_autoware_auto_msgs.hpp | 51 ----------- planning/behavior_path_planner/CMakeLists.txt | 6 -- .../drivable_area_expansion.cpp | 34 ++++++++ .../config/out_of_lane.param.yaml | 2 + .../src/calculate_slowdown_points.hpp | 35 ++++++-- .../src/debug.cpp | 87 ++++++++++++++++++- .../src/debug.hpp | 20 ++++- .../src/decisions.cpp | 36 +++----- .../src/decisions.hpp | 22 +---- .../src/manager.cpp | 2 + .../src/overlapping_range.cpp | 11 ++- .../src/overlapping_range.hpp | 10 --- .../src/scene_out_of_lane.cpp | 59 ++++++++++--- .../src/scene_out_of_lane.hpp | 7 +- .../src/types.hpp | 73 +++++++++++++--- .../surround_obstacle_checker/CMakeLists.txt | 6 -- 18 files changed, 307 insertions(+), 165 deletions(-) diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index bf48e8fcaca3f..454e0e7ef044f 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -12,12 +12,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/bounding_box.cpp ) -if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(${PROJECT_NAME} PRIVATE - DEFINE_LEGACY_FUNCTION - ) -endif() - if(BUILD_TESTING) set(GEOMETRY_GTEST geometry_gtest) set(GEOMETRY_SRC test/src/test_geometry.cpp diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index fe4cec1c0fc89..a8ae9ec2d962e 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -17,11 +17,6 @@ if(BUILD_TESTING) "tf2_geometry_msgs" "tf2_ros" ) - if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE - DEFINE_LEGACY_FUNCTION - ) - endif() endif() ament_auto_package() diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index ee8d52fa402c0..b35eb6e93ce6e 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -45,57 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; namespace tf2 { -#ifdef DEFINE_LEGACY_FUNCTION -/*************/ -/** Point32 **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The point to transform, as a Point32 message. - * \param t_out The transformed point, as a Point32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); - t_out.x = static_cast(v_out[0]); - t_out.y = static_cast(v_out[1]); - t_out.z = static_cast(v_out[2]); -} - -/*************/ -/** Polygon **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The polygon to transform. - * \param t_out The transformed polygon. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - // Don't call the Point32 doTransform to avoid doing this conversion every time - const auto kdl_frame = gmTransformToKDL(transform); - // We don't use std::back_inserter to allow aliasing between t_in and t_out - t_out.points.resize(t_in.points.size()); - for (size_t i = 0; i < t_in.points.size(); ++i) { - const KDL::Vector v_out = - kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z); - t_out.points[i].x = static_cast(v_out[0]); - t_out.points[i].y = static_cast(v_out[1]); - t_out.points[i].z = static_cast(v_out[2]); - } -} -#endif - /******************/ /** Quaternion32 **/ /******************/ diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index efae65ae48813..0352096d02b2b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -57,12 +57,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/marker_utils/lane_change/debug.cpp ) -if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(behavior_path_planner_node PRIVATE - DEFINE_LEGACY_FUNCTION - ) -endif() - target_include_directories(behavior_path_planner_node SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 102ed5b8c5fd1..81bcd383f0a49 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -26,6 +26,15 @@ #include +// for writing the svg file +#include +#include +// for the geometry types +#include +// for the svg mapper +#include +#include + namespace drivable_area_expansion { @@ -57,6 +66,22 @@ void expandDrivableArea( { if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return; + // Declare a stream and an SVG mapper + std::ofstream svg1("/home/mclement/Pictures/DA_before.svg"); // /!\ CHANGE PATH + boost::geometry::svg_mapper before(svg1, 400, 400); + // Declare a stream and an SVG mapper + std::ofstream svg2("/home/mclement/Pictures/DA_after.svg"); // /!\ CHANGE PATH + boost::geometry::svg_mapper after(svg2, 400, 400); + + linestring_t left_ls; + linestring_t right_ls; + for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); + for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); + before.add(left_ls); + before.add(right_ls); + before.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2"); + before.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2"); + tier4_autoware_utils::StopWatch stopwatch; const auto & params = planner_data->drivable_area_expansion_parameters; const auto & dynamic_objects = *planner_data->dynamic_object; @@ -126,6 +151,15 @@ void expandDrivableArea( std::printf("\tupdate: %2.2fms\n", update_duration); std::cout << std::endl; } + + left_ls.clear(); + right_ls.clear(); + for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); + for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); + after.add(left_ls); + after.add(right_ls); + after.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2"); + after.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2"); } point_t convert_point(const Point & p) 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 c74c5b3d87c1d..092f1d32b27b3 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 @@ -26,6 +27,7 @@ skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed 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 + 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 index 6f3094aea9682..b9d9f6f98d53a 100644 --- 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 @@ -26,6 +26,11 @@ 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) { @@ -37,9 +42,16 @@ bool can_decelerate( 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 PlannerParam & params) + 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); @@ -50,12 +62,17 @@ std::optional calculate_last_in_lane_pose( // 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 || + !params.skip_if_over_max_decel || prev_slowdown_point || 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())) + 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; @@ -64,10 +81,12 @@ std::optional calculate_last_in_lane_pose( /// @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, PlannerParam params) + 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); @@ -75,7 +94,7 @@ std::optional calculate_slowdown_point( // 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); + 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; 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 475e341d76528..43377ec744eb5 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -32,13 +32,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"; @@ -52,12 +51,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"; @@ -80,12 +81,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; @@ -101,6 +104,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 339ac35eb4444..80ac33849b4f2 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -17,7 +17,6 @@ #include #include -#include #include #include #include @@ -55,7 +54,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 @@ -64,8 +64,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(); @@ -300,7 +299,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"); @@ -309,27 +309,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( @@ -367,6 +354,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 1d1fe8bea94a6..9fdc9f333db65 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -30,16 +30,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 @@ -63,13 +53,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. @@ -93,14 +85,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 2422c6fe84e56..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,6 +50,7 @@ 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"); @@ -57,6 +58,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.skip_if_over_max_decel = getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); 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 74d1f86c51df1..1a73e74a723e2 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 @@ -50,8 +50,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); @@ -63,6 +62,19 @@ bool OutOfLaneModule::modifyPathVelocity( ego_data.path.points = path->points; ego_data.first_path_idx = 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); 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"); @@ -82,6 +94,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; @@ -99,6 +113,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"); @@ -108,14 +126,27 @@ 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 point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); + 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"); 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) + @@ -127,14 +158,17 @@ bool OutOfLaneModule::modifyPathVelocity( tier4_planning_msgs::msg::StopFactor stop_factor; 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_to_insert->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_to_insert->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( @@ -155,18 +189,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 f2ca2ababfef4..ebadf81bee004 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 @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -47,9 +48,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 64d88f15e6966..34bd52634ce7b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -24,7 +24,9 @@ #include #include +#include #include +#include #include #include #include @@ -47,6 +49,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 @@ -56,7 +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 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(); } }; diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index 694bddf421486..0d7b636646783 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -18,12 +18,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp ) -if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(${PROJECT_NAME} PRIVATE - DEFINE_LEGACY_FUNCTION - ) -endif() - target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} )