From e4b0bccfb0132816acac44a7bf5cead5488945a4 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Mon, 17 Jan 2022 11:51:49 +0900 Subject: [PATCH] fix(behavior velocity planner): occlusion spot slice (#237) * fix: occlusion spot slice Signed-off-by: tanaka3 * chore: use to path lanelet Signed-off-by: tanaka3 * fix: pre-commit Signed-off-by: tanaka3 * chore: minor change for debug and test Signed-off-by: tanaka3 * fix(gtest): update to latest and minor fix of edge case Signed-off-by: tanaka3 * chore(typo): fix typo Signed-off-by: tanaka3 --- .../scene_module/occlusion_spot/geometry.hpp | 14 +- .../occlusion_spot/occlusion_spot_utils.hpp | 1 + .../src/scene_module/occlusion_spot/debug.cpp | 1 + .../scene_module/occlusion_spot/geometry.cpp | 231 ++++++------------ .../occlusion_spot/occlusion_spot_utils.cpp | 23 +- .../test/src/test_geometry.cpp | 148 ++--------- .../test/src/test_performances.cpp | 31 +-- .../test/src/utils.hpp | 30 +++ 8 files changed, 159 insertions(+), 320 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp index 4d9d6a55f6cbd..1f7fde9acd103 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp @@ -49,16 +49,12 @@ struct Slice // using the given range and desired slice length and width void buildSlices( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, const double slice_width); -//!< @brief build an interpolated polygon between the given bounds -void buildInterpolatedPolygon( - lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & from_bound, - const lanelet::BasicLineString2d & to_bound, const double from_length, const double to_length, - const double from_ratio_dist, const double to_ratio_dist); + const double slice_length, const double slice_width, const double resolution); //!< @brief build sidewalk slice from path -std::vector buildSidewalkSlices( - const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset, - const double lateral_offset, const double min_size, const double lateral_max_dist); +void buildSidewalkSlices( + std::vector & slice, const lanelet::ConstLanelet & path_lanelet, + const double longitudinal_offset, const double lateral_offset, const double min_size, + const double lateral_max_dist); //!< @brief calculate interpolation between a and b at distance ratio t template T lerp(T a, T b, double t) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index d609d7836ec2c..2e57dc7f40236 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -36,6 +36,7 @@ namespace behavior_velocity_planner { +using autoware_auto_planning_msgs::msg::PathWithLaneId; namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 5884aee1e9ae8..ad7a7f7ffd784 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -194,6 +194,7 @@ visualization_msgs::msg::MarkerArray createMarkers( occlusion_spot_slowdown_markers.markers.insert( occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), collision_markers.end()); + break; } // draw slice if having sidewalk polygon diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp index af314be864051..e88274458e229 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp @@ -24,76 +24,80 @@ namespace behavior_velocity_planner { namespace geometry { +using lanelet::BasicLineString2d; +using lanelet::BasicPoint2d; +using lanelet::BasicPolygon2d; +namespace bg = boost::geometry; +namespace lg = lanelet::geometry; + +void createOffsetLineString( + const BasicLineString2d & in, const double offset, BasicLineString2d & offset_line_string) +{ + for (size_t i = 0; i < in.size() - 1; i++) { + const auto & p0 = in.at(i); + const auto & p1 = in.at(i + 1); + // translation + const double dy = p1[1] - p0[1]; + const double dx = p1[0] - p0[0]; + // rotation (use inverse matrix of rotation) + const double yaw = std::atan2(dy, dx); + // translation + const double offset_x = p0[0] - std::sin(yaw) * offset; + const double offset_y = p0[1] + std::cos(yaw) * offset; + offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y}); + //! insert final offset linestring using prev vertical direction + if (i == in.size() - 2) { + const double offset_x = p1[0] - std::sin(yaw) * offset; + const double offset_y = p1[1] + std::cos(yaw) * offset; + offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y}); + } + } + return; +} + void buildSlices( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, const double slice_width) + const double slice_length, const double slice_width, const double resolution) { - lanelet::BasicLineString2d path_boundary = path_lanelet.centerline2d().basicLineString(); - if (path_boundary.size() < 2) { - return; - } const int num_lateral_slice = static_cast(std::abs(range.max_distance - range.min_distance) / slice_width); - // ignore the last point - const int num_longitudinal_slice = - static_cast(std::abs(range.max_length - range.min_length) / slice_length); - slices.reserve(num_lateral_slice * num_longitudinal_slice); - // Note: offsetNoThrow is necessary - // as the sharp turn at the end of the trajectory can "reverse" the linestring /** - * @brief - * build slice from occupancy grid : the first slice to create is ssss - * - * | 0 | 1 | 2 | 3 | 4 | - * 0 | --|---|---|---|---|-- outer bound - * 1 | | ? | ? | | | - * 2 | | ? | ? | | | - * 3 | | ? | ? | | | ^ - * 4 | s | s | ? | | | | dist_ratio - * 5 |-s-|-s-|---|---|---|-- inner bound --> length ratio - * Ego---------------------> total_slice_length + * @brief bounds + * +---------- outer bounds + * | +------ inner bounds(original path) + * | | */ - lanelet::BasicLineString2d inner_bounds = path_boundary; - lanelet::BasicLineString2d outer_bounds; - outer_bounds = lanelet::geometry::offsetNoThrow(path_boundary, range.max_distance); - // correct self crossing with best effort - boost::geometry::correct(outer_bounds); - rclcpp::Clock clock{RCL_ROS_TIME}; - try { - // if correct couldn't solve self crossing skip this area - lanelet::geometry::internal::checkForInversion( - path_boundary, outer_bounds, std::abs(range.max_distance)); - } catch (...) { - RCLCPP_DEBUG_STREAM_THROTTLE( - rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"), clock, 5000, - "self crossing with offset " << range.max_distance); - } - // offset last point is especially messy - inner_bounds.pop_back(); - outer_bounds.pop_back(); - // resize to the same size as slice - inner_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size())); - outer_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size())); - if (inner_bounds.size() < 2 || outer_bounds.size() < 2) { - return; - } + BasicLineString2d inner_bounds = path_lanelet.centerline2d().basicLineString(); + BasicLineString2d outer_bounds; + if (inner_bounds.size() < 2) return; + createOffsetLineString(inner_bounds, range.max_distance, outer_bounds); const double ratio_dist_start = std::abs(range.min_distance / range.max_distance); const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance)); - for (int s = 0; s < num_longitudinal_slice; s++) { - const double length = range.min_length + s * slice_length; - const double next_length = range.min_length + (s + 1.0) * slice_length; - const double min_length = - std::min(lanelet::geometry::length(outer_bounds), lanelet::geometry::length(inner_bounds)); - if (next_length > min_length) { - break; - } + lanelet::BasicPolygon2d poly; + const int num_step = static_cast(slice_length / resolution); + //! max index is the last index of path point + const int max_index = static_cast(inner_bounds.size() - 1); + for (int s = 0; s < max_index; s += num_step) { + const double length = s * slice_length; + const double next_length = (s + num_step) * resolution; for (int d = 0; d < num_lateral_slice; d++) { const double ratio_dist = ratio_dist_start + d * ratio_dist_increment; const double next_ratio_dist = ratio_dist_start + (d + 1.0) * ratio_dist_increment; Slice slice; - buildInterpolatedPolygon( - slice.polygon, inner_bounds, outer_bounds, length, next_length, ratio_dist, - next_ratio_dist); + BasicLineString2d inner_polygons; + BasicLineString2d outer_polygons; + // build interpolated polygon for lateral + for (int i = 0; i <= num_step; i++) { + if (s + i >= max_index) continue; + inner_polygons.emplace_back( + lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), ratio_dist)); + outer_polygons.emplace_back( + lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), next_ratio_dist)); + } + // Build polygon + inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); + slice.polygon = lanelet::BasicPolygon2d(inner_polygons); + // add range info slice.range.min_length = length; slice.range.max_length = next_length; slice.range.min_distance = ratio_dist * range.max_distance; @@ -103,106 +107,29 @@ void buildSlices( } } -void buildInterpolatedPolygon( - lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & inner_bounds, - const lanelet::BasicLineString2d & outer_bounds, const double current_length, - const double next_length, const double from_ratio_dist, const double to_ratio_dist) +void buildSidewalkSlices( + std::vector & slices, const lanelet::ConstLanelet & path_lanelet, + const double longitudinal_offset, const double lateral_offset, const double slice_size, + const double lateral_max_dist) { - if (current_length >= next_length) { - RCLCPP_WARN( - rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"), - "buildInterpolatedPolygon: starting length must be lower than target length"); - } - lanelet::BasicLineString2d inner_polygons; - lanelet::BasicLineString2d outer_polygons; - inner_polygons.reserve(inner_bounds.size()); - outer_polygons.reserve(outer_bounds.size()); - // Find starting point. Interpolate it if necessary - double length = 0.0; - size_t point_index = 0; - lanelet::BasicPoint2d inner_polygon_from; - lanelet::BasicPoint2d inner_polygon_to; - lanelet::BasicPoint2d outer_polygon_from; - lanelet::BasicPoint2d outer_polygon_to; - // Search first points of polygon - for (; length < current_length && point_index < inner_bounds.size() - 1; ++point_index) { - length += - lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]); - } - // if find current bound point index - if (length > current_length) { - const double length_between_points = - lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]); - const double length_ratio = - (length_between_points - (length - current_length)) / length_between_points; - inner_polygon_from = - lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio); - outer_polygon_from = - lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio); - } else { - inner_polygon_from = inner_bounds[point_index]; - outer_polygon_from = outer_bounds[point_index]; - if (length < next_length && point_index < inner_bounds.size() - 1) { - length += - lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]); - ++point_index; - } - } - inner_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, from_ratio_dist)); - outer_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, to_ratio_dist)); - - // Intermediate points - for (; length < next_length && point_index < inner_bounds.size() - 1; ++point_index) { - inner_polygons.emplace_back( - lerp(inner_bounds[point_index], outer_bounds[point_index], from_ratio_dist)); - outer_polygons.emplace_back( - lerp(inner_bounds[point_index], outer_bounds[point_index], to_ratio_dist)); - length += - lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]); - } - // Last points - if (length > next_length) { - const double length_between_points = - lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]); - const double length_ratio = - (length_between_points - (length - next_length)) / length_between_points; - inner_polygon_to = lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio); - outer_polygon_to = lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio); - } else { - inner_polygon_to = inner_bounds[point_index]; - outer_polygon_to = outer_bounds[point_index]; - } - inner_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, from_ratio_dist)); - outer_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, to_ratio_dist)); - // Build polygon - inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); - polygons = lanelet::BasicPolygon2d(inner_polygons); -} - -std::vector buildSidewalkSlices( - const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset, - const double lateral_offset, const double slice_size, const double lateral_max_dist) -{ - std::vector slices; - std::vector left_slices; - std::vector right_slices; - const double longitudinal_max_dist = lanelet::geometry::length2d(path_lanelet); - geometry::SliceRange left_slice_range = { + std::vector left_slices; + std::vector right_slices; + const double longitudinal_max_dist = lg::length2d(path_lanelet); + SliceRange left_slice_range = { longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist}; - geometry::buildSlices(left_slices, path_lanelet, left_slice_range, slice_size, slice_size); - geometry::SliceRange right_slice_range = { + // in most case lateral distance is much more effective for velocity planning + const double slice_length = 4.0 * slice_size; + const double slice_width = slice_size; + const double resolution = 1.0; + buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution); + SliceRange right_slice_range = { longitudinal_offset, longitudinal_max_dist, -lateral_offset, -lateral_offset - lateral_max_dist}; - geometry::buildSlices(right_slices, path_lanelet, right_slice_range, slice_size, slice_size); + buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution); // Properly order lanelets from closest to furthest - for (size_t i = 0; i < right_slices.size(); ++i) { - slices.emplace_back(right_slices[i]); - } - for (size_t i = 0; i < left_slices.size(); ++i) { - slices.emplace_back(left_slices[i]); - } - - return slices; + slices = left_slices; + slices.insert(slices.end(), right_slices.begin(), right_slices.end()); + return; } } // namespace geometry } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index cad0afc42d852..5a24c5bd4e9d5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -32,6 +32,20 @@ namespace behavior_velocity_planner { namespace occlusion_spot_utils { +lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path) +{ + lanelet::Points3d path_points; + path_points.reserve(path.points.size()); + for (const auto & path_point : path.points) { + const auto & p = path_point.point.pose.position; + path_points.emplace_back(lanelet::InvalId, p.x, p.y, p.z); + } + lanelet::LineString3d centerline(lanelet::InvalId, path_points); + lanelet::Lanelet path_lanelet(lanelet::InvalId); + path_lanelet.setCenterline(centerline); + return lanelet::ConstLanelet(path_lanelet); +} + bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger) @@ -419,7 +433,7 @@ void generatePossibleCollisions( const PlannerParam & param, std::vector & debug) { // NOTE : buildPathLanelet first index should always be zero because path is already limited - lanelet::ConstLanelet path_lanelet = buildPathLanelet(path); + lanelet::ConstLanelet path_lanelet = toPathLanelet(path); if (path_lanelet.centerline2d().empty()) { return; } @@ -446,9 +460,10 @@ void generateSidewalkPossibleCollisions( } else { min_range = param.vehicle_info.baselink_to_front - offset_form_ego_to_target; } - std::vector sidewalk_slices = geometry::buildSidewalkSlices( - path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5, param.sidewalk.slice_size, - param.sidewalk.focus_range); + std::vector sidewalk_slices; + geometry::buildSidewalkSlices( + sidewalk_slices, path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5, + param.sidewalk.slice_size, param.sidewalk.focus_range); double length_lower_bound = std::numeric_limits::max(); double distance_lower_bound = std::numeric_limits::max(); std::sort( diff --git a/planning/behavior_velocity_planner/test/src/test_geometry.cpp b/planning/behavior_velocity_planner/test/src/test_geometry.cpp index 84a7477387c75..cc1e631e91c58 100644 --- a/planning/behavior_velocity_planner/test/src/test_geometry.cpp +++ b/planning/behavior_velocity_planner/test/src/test_geometry.cpp @@ -20,92 +20,6 @@ #include -TEST(lerp, interpolation) -{ - using behavior_velocity_planner::geometry::lerp; - using V = Eigen::Vector2d; - // 1D - EXPECT_EQ(lerp(0.0, 1.0, 0.5), 0.5); - EXPECT_EQ(lerp(0.0, 1.0, 0.0), 0.0); - EXPECT_EQ(lerp(0.0, 1.0, 1.0), 1.0); - EXPECT_EQ(lerp(0.0, 1.0, 4.5), 4.5); - EXPECT_EQ(lerp(1.0, 2.0, 0.5), 1.5); - EXPECT_EQ(lerp(1.0, 2.0, 0.75), 1.75); - EXPECT_EQ(lerp(0.0, -2.0, 0.75), -1.5); - EXPECT_EQ(lerp(-10.0, -5.0, 0.5), -7.5); - // 2D - auto expect_eq = [](V v1, V v2) { - EXPECT_EQ(v1.x(), v2.x()); - EXPECT_EQ(v1.y(), v2.y()); - }; - expect_eq(lerp(V(0.0, 0.0), V(0.0, 1.0), 0.5), V(0.0, 0.5)); - expect_eq(lerp(V(0.0, 1.0), V(0.0, 2.0), 0.5), V(0.0, 1.5)); - expect_eq(lerp(V(0.0, 0.0), V(2.0, 2.0), 0.5), V(1.0, 1.0)); - expect_eq(lerp(V(-100.0, 10.0), V(0.0, 0.0), 0.1), V(-90.0, 9.0)); -} - -TEST(buildInterpolatedPolygon, straight_polygon) -{ - using behavior_velocity_planner::geometry::buildInterpolatedPolygon; - using V = Eigen::Vector2d; - auto expect_eq = [](V v1, V v2) { - EXPECT_EQ(v1.x(), v2.x()); - EXPECT_EQ(v1.y(), v2.y()); - }; - - lanelet::BasicLineString2d from; - lanelet::BasicLineString2d to; - double from_length; - double to_length; - double from_ratio_dist; - double to_ratio_dist; - lanelet::BasicPolygon2d polygon; - - from = {{0.0, 0.0}, {0.0, 10.0}}; - to = {{10.0, 0.0}, {10.0, 10.0}}; - from_length = 0.0; - to_length = 10.0; - from_ratio_dist = 0.0; - to_ratio_dist = 1.0; - buildInterpolatedPolygon( - polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); - ASSERT_EQ(polygon.size(), static_cast(4)); - expect_eq(polygon[0], from[0]); - expect_eq(polygon[1], from[1]); - expect_eq(polygon[2], to[1]); - expect_eq(polygon[3], to[0]); - - from_ratio_dist = 0.0; - to_ratio_dist = 0.5; - buildInterpolatedPolygon( - polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); - ASSERT_EQ(polygon.size(), static_cast(4)); - expect_eq(polygon[0], V(0.0, 0.0)); - expect_eq(polygon[1], V(0.0, 10.0)); - expect_eq(polygon[2], V(5.0, 10.0)); - expect_eq(polygon[3], V(5.0, 0.0)); - - from_ratio_dist = 0.25; - to_ratio_dist = 0.5; - buildInterpolatedPolygon( - polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); - ASSERT_EQ(polygon.size(), static_cast(4)); - expect_eq(polygon[0], V(2.5, 0.0)); - expect_eq(polygon[1], V(2.5, 10.0)); - expect_eq(polygon[2], V(5.0, 10.0)); - expect_eq(polygon[3], V(5.0, 0.0)); - - from_length = 1.5; - to_length = 7.5; - buildInterpolatedPolygon( - polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); - ASSERT_EQ(polygon.size(), static_cast(4)); - expect_eq(polygon[0], V(2.5, 1.5)); - expect_eq(polygon[1], V(2.5, 7.5)); - expect_eq(polygon[2], V(5.0, 7.5)); - expect_eq(polygon[3], V(5.0, 1.5)); -} - TEST(buildSlices, one_full_slice) { using behavior_velocity_planner::geometry::buildSlices; @@ -125,19 +39,17 @@ TEST(buildSlices, one_full_slice) const int nb_points = 6; lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, 5.0, nb_points); SliceRange range; - range.min_length = 0.0; - range.max_length = 3.0; range.min_distance = 0.0; range.max_distance = -3.0; const double slice_length = 3.0; - const double slice_width = 3.0; + const double slice_width = 1.5; std::vector slices; - buildSlices(slices, traj_lanelet, range, slice_length, slice_width); - ASSERT_EQ(slices.size(), static_cast(1)); + buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5); + ASSERT_EQ(slices.size(), static_cast(4)); EXPECT_EQ(slices[0].range.min_length, 0.0); EXPECT_EQ(slices[0].range.min_distance, 0.0); EXPECT_EQ(slices[0].range.max_length, 3.0); - EXPECT_EQ(slices[0].range.max_distance, -3.0); + EXPECT_EQ(slices[0].range.max_distance, -1.5); } TEST(buildSlices, 3x3square_slice) @@ -153,57 +65,29 @@ TEST(buildSlices, 3x3square_slice) 1 | | | 2 | |SLICE| 3 | |-----| - 4 | | - 5 | | */ - const int nb_points = 6; - lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, 5.0, nb_points); + const int nb_points = 4; + const double len = 3.0; + lanelet::ConstLanelet lanelet = test::createLanelet({0.0, 0.0}, {0.0, len}, nb_points); SliceRange range; - range.min_length = 0.0; - range.max_length = 3.0; - range.min_distance = 0.0; + range.min_distance = -1.0; range.max_distance = -3.0; const double slice_length = 1.0; const double slice_width = 1.0; + const int num_right_slice = + (len / slice_length) * (std::abs(range.max_distance - range.min_distance) / slice_width); + // { std::vector slices; - buildSlices(slices, traj_lanelet, range, slice_length, slice_width); - SliceRange ref; - auto equal_range = [&ref](const Slice & s) { - return s.range.min_distance == ref.min_distance && s.range.max_distance == ref.max_distance && - s.range.min_length == ref.min_length && s.range.max_length == ref.max_length; - }; - ASSERT_EQ(slices.size(), static_cast(9)); - for (double l = range.min_length; l < range.max_length; l += slice_length) { - for (double d = range.min_distance; d > range.max_distance; d -= slice_width) { - ref.min_length = l; - ref.max_length = l + slice_length; - ref.min_distance = d; - ref.max_distance = d - slice_width; - EXPECT_NE(std::find_if(slices.begin(), slices.end(), equal_range), slices.end()); - } - } + buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0); + ASSERT_EQ(slices.size(), static_cast(num_right_slice)); } - // change to the left side (positive distance) + range.min_distance = 1.0; range.max_distance = 3.0; { std::vector slices; - buildSlices(slices, traj_lanelet, range, slice_length, slice_width); - SliceRange ref; - auto equal_range = [&ref](const Slice & s) { - return s.range.min_distance == ref.min_distance && s.range.max_distance == ref.max_distance && - s.range.min_length == ref.min_length && s.range.max_length == ref.max_length; - }; - ASSERT_EQ(slices.size(), static_cast(9)); - for (double l = range.min_length; l < range.max_length; l += slice_length) { - for (double d = range.min_distance; d < range.max_distance; d += slice_width) { - ref.min_length = l; - ref.max_length = l + slice_length; - ref.min_distance = d; - ref.max_distance = d + slice_width; - EXPECT_NE(std::find_if(slices.begin(), slices.end(), equal_range), slices.end()); - } - } + buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0); + ASSERT_EQ(slices.size(), static_cast(num_right_slice)); } } diff --git a/planning/behavior_velocity_planner/test/src/test_performances.cpp b/planning/behavior_velocity_planner/test/src/test_performances.cpp index 7a19116889bc3..8a1c096cb1255 100644 --- a/planning/behavior_velocity_planner/test/src/test_performances.cpp +++ b/planning/behavior_velocity_planner/test/src/test_performances.cpp @@ -28,19 +28,18 @@ using test::grid_param; TEST(performances, many_sidewalk_occlusion_spots) { + using autoware_auto_planning_msgs::msg::PathWithLaneId; + using behavior_velocity_planner::occlusion_spot_utils::generatePossibleCollisions; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; using std::chrono::microseconds; - using behavior_velocity_planner::occlusion_spot_utils::generatePossibleCollisions; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; - std::vector possible_collisions; - autoware_auto_planning_msgs::msg::PathWithLaneId trajectory = - test::generatePath(0.5, 0.5, 300.0, 0.5, 3000); - grid_map::GridMap grid = test::generateGrid(3000, 3000, 0.1); + PathWithLaneId path = test::generatePath(0.5, 0.5, 300.0, 0.5, 300); + grid_map::GridMap grid = test::generateGrid(300, 300, 0.1); for (int x = 0; x < grid.getSize().x(); ++x) { for (int y = 0; y < grid.getSize().x(); ++y) { grid.at("layer", grid_map::Index(x, y)) = @@ -48,20 +47,6 @@ TEST(performances, many_sidewalk_occlusion_spots) } } - // empty lanelet map - lanelet::LaneletMapPtr map = std::make_shared(); - - // Make routing graph - lanelet::traffic_rules::TrafficRulesPtr traffic_rules{ - lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::Vehicle, - lanelet::traffic_rules::TrafficRules::Configuration())}; - lanelet::routing::RoutingCostPtrs cost_ptrs{ - std::make_shared( - lanelet::routing::RoutingCostDistance{2.})}; - lanelet::routing::RoutingGraphPtr routing_graph = - lanelet::routing::RoutingGraph::build(*map, *traffic_rules, cost_ptrs); - // Set parameters: enable sidewalk obstacles behavior_velocity_planner::occlusion_spot_utils::PlannerParam parameters; parameters.vehicle_info.baselink_to_front = 0.0; @@ -71,11 +56,11 @@ TEST(performances, many_sidewalk_occlusion_spots) parameters.grid = grid_param; const double offset_from_ego_to_closest = 0; const double offset_from_closest_to_target = 0; - std::vector to_remove; + std::vector debug; auto start_closest = high_resolution_clock::now(); generatePossibleCollisions( - possible_collisions, trajectory, grid, offset_from_ego_to_closest, - offset_from_closest_to_target, parameters, to_remove); + possible_collisions, path, grid, offset_from_ego_to_closest, offset_from_closest_to_target, + parameters, debug); auto end_closest = high_resolution_clock::now(); std::string ms = " (micro seconds) "; std::cout << "| only_closest : runtime (microseconds) \n" diff --git a/planning/behavior_velocity_planner/test/src/utils.hpp b/planning/behavior_velocity_planner/test/src/utils.hpp index 7afaff9d4a755..bec213595f9b3 100644 --- a/planning/behavior_velocity_planner/test/src/utils.hpp +++ b/planning/behavior_velocity_planner/test/src/utils.hpp @@ -33,6 +33,36 @@ namespace test { // Default grid parameter such that UNKNOWN cells have a value of 50 const behavior_velocity_planner::grid_utils::GridParam grid_param = {10, 51}; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using ConstPair = const std::pair; + +/* lanelet + 0 1 2 3 4 5 + 0 x + 1 x + 2 x + 3 x + 4 x + 5 x + */ +inline lanelet::ConstLanelet createLanelet( + ConstPair & start = {0, 0}, ConstPair & end = {5, 5}, int nb_points = 6) +{ + const double interval = + std::hypot(end.first - start.first, end.second - start.second) / (nb_points - 1); + const double norm_x = (end.first - start.first) / interval; + const double norm_y = (end.second - start.second) / interval; + lanelet::Points3d path_points; + for (int i = 0; i < nb_points; i++) { + const double x = start.first + norm_x * i; + const double y = start.second + norm_y * i; + path_points.emplace_back(lanelet::InvalId, x, y, 0); + } + lanelet::LineString3d centerline(lanelet::InvalId, path_points); + lanelet::Lanelet path_lanelet(lanelet::InvalId); + path_lanelet.setCenterline(centerline); + return lanelet::ConstLanelet(path_lanelet); +} /* Horizontal lanelet 0 1 2 3 4 5 6