diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 1609cdbc60b7a..160ebdc180020 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -21,12 +21,14 @@ rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint left: 0.5 # [m] extra length to add to the left of the dynamic object footprint right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + path_preprocessing: + max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) expansion: method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. # 'lanelet': add lanelets overlapped by the ego footprints # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index b682d28efeaa5..ee44c4f509963 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -147,6 +148,7 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable std::optional drivable_area_expansion_prev_crop_pose; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 53ef91473fe24..8293e0a1d10a9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" @@ -22,17 +23,17 @@ #include +#include + namespace drivable_area_expansion { /// @brief Expand the drivable area based on the projected ego footprint along the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] params expansion parameters -/// @param[in] dynamic_objects dynamic objects -/// @param[in] route_handler route handler +/// @param[inout] path path whose drivable area will be expanded +/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) /// @param[in] path_lanes lanelets of the path void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes); /// @brief Create a polygon combining the drivable area of a path and some expansion polygons diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index e5e1c76f2521f..8fc0157710dc8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -64,6 +64,6 @@ multi_polygon_t createObjectFootprints( /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); + const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index a4c11c68d13ec..81eab9560b736 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -48,8 +48,10 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto RESAMPLE_INTERVAL_PARAM = + "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = - "dynamic_expansion.expansion.max_path_arc_length"; + "dynamic_expansion.path_preprocessing.max_arc_length"; static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; @@ -78,6 +80,7 @@ struct DrivableAreaExpansionParameters double dynamic_objects_extra_front_offset{}; double max_expansion_distance{}; double max_path_arc_length{}; + double resample_interval{}; double extra_arc_length{}; bool avoid_dynamic_objects{}; std::vector avoid_linestring_types{}; @@ -98,6 +101,7 @@ struct DrivableAreaExpansionParameters enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); + resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index daabfa97598fd..e300a347e47a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -26,6 +26,7 @@ namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 84393848267b9..4a3c582242cf9 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1731,6 +1731,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); + updateParam( + parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, + planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.extra_arc_length); 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 837b8f0dfb77b..8e15848f22382 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 @@ -19,20 +19,71 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "interpolation/linear_interpolation.hpp" #include +#include +#include #include namespace drivable_area_expansion { +std::vector crop_and_resample( + const std::vector & points, + const std::shared_ptr planner_data, + const double resample_interval) +{ + auto lon_offset = 0.0; + auto crop_pose = *planner_data->drivable_area_expansion_prev_crop_pose; + // reuse or update the previous crop point + if (planner_data->drivable_area_expansion_prev_crop_pose) { + const auto lon_offset = motion_utils::calcSignedArcLength( + points, points.front().point.pose.position, crop_pose.position); + if (lon_offset < 0.0) { + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } else { + const auto is_behind_ego = + motion_utils::calcSignedArcLength( + points, crop_pose.position, planner_data->self_odometry->pose.pose.position) > 0.0; + const auto is_too_far = motion_utils::calcLateralOffset(points, crop_pose.position) > 0.1; + if (!is_behind_ego || is_too_far) + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } + } + if (!planner_data->drivable_area_expansion_prev_crop_pose) { + crop_pose = planner_data->drivable_area_expansion_prev_crop_pose.value_or( + motion_utils::calcInterpolatedPose(points, resample_interval - lon_offset)); + } + // crop + const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); + const auto cropped_points = motion_utils::cropPoints( + points, crop_pose.position, crop_seg_idx + 1, + planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); + planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; + // resample + PathWithLaneId cropped_path; + if (tier4_autoware_utils::calcDistance2d(crop_pose, cropped_points.front()) > 1e-3) { + PathPointWithLaneId crop_path_point; + crop_path_point.point.pose = crop_pose; + cropped_path.points.push_back(crop_path_point); + } + cropped_path.points.insert( + cropped_path.points.end(), cropped_points.begin(), cropped_points.end()); + const auto resampled_path = + motion_utils::resamplePath(cropped_path, resample_interval, true, true, false); + + return resampled_path.points; +} + void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes) { + const auto & params = planner_data->drivable_area_expansion_parameters; + const auto & dynamic_objects = *planner_data->dynamic_object; + const auto & route_handler = *planner_data->route_handler; const auto uncrossable_lines = extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); multi_linestring_t uncrossable_lines_in_range; @@ -40,7 +91,8 @@ void expandDrivableArea( for (const auto & line : uncrossable_lines) if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) uncrossable_lines_in_range.push_back(line); - const auto path_footprints = createPathFootprints(path, params); + const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); + const auto path_footprints = createPathFootprints(points, params); const auto predicted_paths = createObjectFootprints(dynamic_objects, params); const auto expansion_polygons = params.expansion_method == "lanelet" diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 035cc579d2a82..828fdc2f17a51 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -25,10 +25,15 @@ double calculateDistanceLimit( const multi_linestring_t & limit_lines) { auto dist_limit = std::numeric_limits::max(); - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); + for (const auto & line : limit_lines) { + multi_point_t intersections; + boost::geometry::intersection(expansion_polygon, limit_lines, intersections); + for (const auto & p : intersections) + dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); + for (const auto & p : line) + if (boost::geometry::within(p, expansion_polygon)) + dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); + } return dist_limit; } @@ -80,7 +85,7 @@ std::array calculate_arc_length_range_and_distance( } for (const auto & p : footprint.outer()) { const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; + if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { expansion_dist = std::abs(projection.distance); from_arc_length = std::min(from_arc_length, projection.arc_length); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 0c31093a83c0e..dd305cdfdb760 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -63,7 +63,7 @@ multi_polygon_t createObjectFootprints( } multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params) + const std::vector & points, const DrivableAreaExpansionParameters & params) { const auto left = params.ego_left_offset + params.ego_extra_left_offset; const auto right = params.ego_right_offset - params.ego_extra_right_offset; @@ -75,9 +75,9 @@ multi_polygon_t createPathFootprints( point_t{front, left}}; multi_polygon_t footprints; // skip the last footprint as its orientation is usually wrong - footprints.reserve(path.points.size() - 1); + footprints.reserve(points.size() - 1); double arc_length = 0.0; - for (auto it = path.points.begin(); std::next(it) != path.points.end(); ++it) { + for (auto it = points.begin(); std::next(it) != points.end(); ++it) { footprints.push_back(createFootprint(it->point.pose, base_footprint)); if (params.max_path_arc_length > 0.0) { arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 36e3014486d74..a2a487e573aec 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1500,9 +1500,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea( - path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, - transformed_lanes); + drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); } // make bound longitudinally monotonic diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 605b3b96ee9b9..6c02609b474c4 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" @@ -257,6 +258,7 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit + params.resample_interval = 1.0; params.extra_arc_length = 1.0; params.expansion_method = "polygon"; // 2m x 4m ego footprint @@ -265,9 +267,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.ego_left_offset = 2.0; params.ego_right_offset = -2.0; } + behavior_path_planner::PlannerData planner_data; + planner_data.drivable_area_expansion_parameters = params; + planner_data.reference_path = std::make_shared(path); + planner_data.dynamic_object = + std::make_shared(dynamic_objects); + planner_data.route_handler = std::make_shared(route_handler); // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expandDrivableArea( - path, params, dynamic_objects, route_handler, path_lanes); + path, std::make_shared(planner_data), path_lanes); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { @@ -304,7 +312,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; const multi_linestring_t uncrossable_lines = {}; const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; const auto limit_distance = calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); @@ -313,7 +321,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; const auto limit_distance = calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); EXPECT_NEAR(limit_distance, 2.0, 1e-9); @@ -323,9 +331,44 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) const multi_linestring_t uncrossable_lines = { {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; const auto limit_distance = calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); EXPECT_NEAR(limit_distance, 1.0, 1e-9); } } + +TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases) +{ + using drivable_area_expansion::calculateDistanceLimit; + using drivable_area_expansion::linestring_t; + using drivable_area_expansion::polygon_t; + + const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; + const polygon_t expansion_polygon = { + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; + { // intersection points further than the line point inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 3.0, 1e-9); + } + { // intersection points further than the line point inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 2.0, 1e-9); + } + { // line completely inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 2.0, 1e-9); + } + { // line completely inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 3.0, 1e-9); + } +} diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index a3089cbc1fde3..da46055460a99 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -101,7 +101,7 @@ at its current velocity or at half the velocity of the path points, whichever is ###### Dynamic objects Two methods are used to estimate the time when a dynamic objects with reach some point. -If `objects.use_predicted_paths` is set to `true`, the predicted path of the dynamic object is used. +If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. #### 5. Path update @@ -138,10 +138,11 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | -------------- | ------ | ------------------------------------------------------------------------------------------------------ | | `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | -| Parameter /objects | Type | Description | -| --------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | -| `use_predicted_paths` | bool | [-] 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 | +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | +| `use_predicted_paths` | bool | [-] 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` | double | [-] minimum confidence required for a predicted path to be considered | | Parameter /overlap | 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 c501599b4ae1c..510dc86ef651d 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -16,6 +16,7 @@ minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored 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. overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered @@ -33,6 +34,7 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego extra_front_offset: 0.0 # [m] extra front distance extra_rear_offset: 0.0 # [m] extra rear distance extra_right_offset: 0.0 # [m] extra right distance diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index a164a03a9aa43..46db7a53b5c81 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -30,21 +30,41 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx) ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } -double time_along_path(const EgoData & ego_data, const size_t target_idx) +double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) { const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( - ego_data.velocity, + std::max(ego_data.velocity, min_velocity), ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; } +bool object_is_incoming( + const lanelet::BasicPoint2d & object_position, + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lane) +{ + const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); + if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; + for (const auto & lls : lanelets) + for (const auto & ll : lls) + if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; + return false; +} + std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const rclcpp::Logger & logger) + const std::shared_ptr route_handler, const rclcpp::Logger & logger) { - const auto max_deviation = object.shape.dimensions.y * 2.0; + // skip the dynamic object if it is not in a lane preceding the overlapped lane + // lane changes are intentionally not considered + const auto object_point = lanelet::BasicPoint2d( + object.kinematics.initial_pose_with_covariance.pose.position.x, + 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; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); @@ -243,7 +263,7 @@ bool ttc_condition( return collision_during_overlap || ttc_is_bellow_threshold; } -bool object_is_incoming( +bool will_collide_on_range( const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) { RCLCPP_DEBUG( @@ -259,8 +279,10 @@ bool should_not_enter( const rclcpp::Logger & logger) { RangeTimes range_times{}; - range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx); - range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx); + range_times.ego.enter_time = + time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); RCLCPP_DEBUG( logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); @@ -274,9 +296,10 @@ bool should_not_enter( continue; // skip objects with velocity bellow a threshold } // skip objects that are already on the interval - const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range(object, range, logger) - : object_time_to_range(object, range, inputs, logger); + 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, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); continue; // skip objects that are not driving towards the overlapping range @@ -284,7 +307,7 @@ bool should_not_enter( range_times.object.enter_time = enter_exit_time->first; range_times.object.exit_time = enter_exit_time->second; - if (object_is_incoming(range_times, params, logger)) return true; + if (will_collide_on_range(range_times, params, logger)) return true; } return false; } 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 6af03bc53ea3c..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 @@ -56,28 +57,31 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// but may not exist (e.g,, predicted path ends before reaching the end of the range) /// @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] 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 autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + 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 /// @param [in] range overlapping range -/// @param [in] lanelets objects to consider -/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @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 lanelet::ConstLanelets & lanelets, - const std::shared_ptr & route_handler); + const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) /// @param [in] range_times times when ego and the object enter/exit the range /// @param [in] params parameters /// @param [in] logger ros logger -bool object_is_incoming( +bool will_collide_on_range( const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); /// @brief check whether we should avoid entering the given range /// @param [in] range the range to check 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/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index a0e2b86caa623..9924319b141f7 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,21 @@ 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) { 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 1eccd8773cc92..8245f7ffb2cd0 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -44,6 +44,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.objects_min_vel = node.declare_parameter(ns + ".objects.minimum_velocity"); pp.objects_use_predicted_paths = node.declare_parameter(ns + ".objects.use_predicted_paths"); + pp.objects_min_confidence = + node.declare_parameter(ns + ".objects.predicted_path_min_confidence"); pp.overlap_min_dist = node.declare_parameter(ns + ".overlap.minimum_distance"); pp.overlap_extra_length = node.declare_parameter(ns + ".overlap.extra_length"); @@ -56,6 +58,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".action.slowdown.distance_threshold"); pp.stop_dist_threshold = node.declare_parameter(ns + ".action.stop.distance_threshold"); + pp.ego_min_velocity = node.declare_parameter(ns + ".ego.min_assumed_velocity"); pp.extra_front_offset = node.declare_parameter(ns + ".ego.extra_front_offset"); pp.extra_rear_offset = node.declare_parameter(ns + ".ego.extra_rear_offset"); pp.extra_left_offset = node.declare_parameter(ns + ".ego.extra_left_offset"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 5922a5b9b4488..430cec2d3d4ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -16,6 +16,7 @@ #include "debug.hpp" #include "decisions.hpp" +#include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" #include "overlapping_range.hpp" @@ -103,7 +104,7 @@ bool OutOfLaneModule::modifyPathVelocity( DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = *planner_data_->predicted_objects; + inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; auto decisions = calculate_decisions(inputs, params_, logger_); diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index e4ba8fbfead81..b81fa09884819 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -42,9 +42,11 @@ struct PlannerParam double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // # whether to use the objects' predicted paths - double objects_min_vel; // # [m/s] objects lower than this velocity will be ignored + 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