From 30b90a3b7fc0f70a1c9851641c56682b5c48dfc3 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 13:54:16 +0900 Subject: [PATCH 1/8] remove unnecessary if branch Signed-off-by: Ryuta Kambe --- .../src/grid_utils.cpp | 41 ++++++------------- 1 file changed, 13 insertions(+), 28 deletions(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 9f580149be7ae..72a052fa0d70f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -86,36 +86,21 @@ bool isCollisionFree( const double radius) { const grid_map::Matrix & grid_data = grid["layer"]; - bool polys = true; try { - if (polys) { - Point2d occlusion_p = {p1.x(), p1.y()}; - Point2d collision_p = {p2.x(), p2.y()}; - Polygon2d polygon = pointsToPoly(occlusion_p, collision_p, radius); - grid_map::Polygon grid_polygon; - for (const auto & point : polygon.outer()) { - grid_polygon.addVertex({point.x(), point.y()}); - } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { - const grid_map::Index & index = *iterator; - if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { - return false; - } - } - } else { - std::vector> lines = - pointsToRays(p1, p2, radius); - for (const auto & p : lines) { - for (grid_map::LineIterator iterator(grid, p.first, p.second); !iterator.isPastEnd(); - ++iterator) { - const grid_map::Index & index = *iterator; - if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { - return false; - } - } + Point2d occlusion_p = {p1.x(), p1.y()}; + Point2d collision_p = {p2.x(), p2.y()}; + Polygon2d polygon = pointsToPoly(occlusion_p, collision_p, radius); + grid_map::Polygon grid_polygon; + for (const auto & point : polygon.outer()) { + grid_polygon.addVertex({point.x(), point.y()}); + } + for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); + ++iterator) { + const grid_map::Index & index = *iterator; + if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { + return false; } - } // polys or not + } } catch (const std::invalid_argument & e) { std::cerr << e.what() << std::endl; return false; From b9fcb38354941e8f1a5c27262e27d927be1094d5 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 14:02:06 +0900 Subject: [PATCH 2/8] add const for const variable Signed-off-by: Ryuta Kambe --- planning/behavior_velocity_occlusion_spot_module/src/debug.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index a6e13d0dfdcdc..f4d3fbf6cc2c1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -118,7 +118,7 @@ MarkerArray makeDebugInfoMarkers(T & debug_data) { // add slow down markers for occlusion spot MarkerArray debug_markers; - auto & possible_collisions = debug_data.possible_collisions; + const auto & possible_collisions = debug_data.possible_collisions; size_t id = 0; // draw obstacle collision for (const auto & pc : possible_collisions) { From 06acd456a7308a2d72ff7b824b07c0e8c7fd2e47 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 14:02:39 +0900 Subject: [PATCH 3/8] remove redundant assignment Signed-off-by: Ryuta Kambe --- .../behavior_velocity_occlusion_spot_module/src/grid_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 72a052fa0d70f..b1d43b54ce931 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -309,7 +309,7 @@ void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid const int idx = (height - 1 - y) + (width - 1 - x) * height; unsigned char intensity = cv_image.at(y, x); if (intensity == grid_utils::occlusion_cost_value::FREE_SPACE) { - intensity = grid_utils::occlusion_cost_value::FREE_SPACE; + // do nothing } else if (intensity == grid_utils::occlusion_cost_value::UNKNOWN_IMAGE) { intensity = grid_utils::occlusion_cost_value::UNKNOWN; } else if (intensity == grid_utils::occlusion_cost_value::OCCUPIED_IMAGE) { From d740ae047d32523f8b3e786f343a8348dcc4f219 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 14:10:31 +0900 Subject: [PATCH 4/8] remove redundant if branch Signed-off-by: Ryuta Kambe --- .../src/grid_utils.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index b1d43b54ce931..f98b9971639c4 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -333,14 +333,12 @@ void toQuantizedImage( unsigned char intensity = occupancy_grid.data.at(idx); if (intensity <= param.free_space_max) { continue; - } else if (param.free_space_max < intensity && intensity < param.occupied_min) { + } else if (intensity < param.occupied_min) { intensity = grid_utils::occlusion_cost_value::UNKNOWN_IMAGE; occlusion_image->at(y, x) = intensity; - } else if (param.occupied_min <= intensity) { + } else { intensity = grid_utils::occlusion_cost_value::OCCUPIED_IMAGE; border_image->at(y, x) = intensity; - } else { - throw std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); } } } From da773258a772eda1663eabf070af5c96a512f7f0 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 14:12:05 +0900 Subject: [PATCH 5/8] underflow check Signed-off-by: Ryuta Kambe --- .../src/occlusion_spot_utils.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index f38768ce26da4..1dbfdeeedd383 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -117,6 +117,9 @@ void calcSlowDownPointsForPossibleCollision( size_t collision_index = 0; double dist_along_path_point = offset; double dist_along_next_path_point = dist_along_path_point; + if (path.points.size() == 0) { + return; + } for (size_t idx = closest_idx; idx < path.points.size() - 1; idx++) { auto p_prev = path.points.at(idx).point; auto p_next = path.points.at(idx + 1).point; From eca35cd3a35b9723bccbe87ef4bd100039ba5028 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 14:13:59 +0900 Subject: [PATCH 6/8] use const for const parameter Signed-off-by: Ryuta Kambe --- .../src/occlusion_spot_utils.cpp | 2 +- .../src/occlusion_spot_utils.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 1dbfdeeedd383..7ed18e955a1b9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -429,7 +429,7 @@ bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data) + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, const DebugData & debug_data) { const double baselink_to_front = param.baselink_to_front; const double right_overhang = param.right_overhang; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index ec58e38dec45a..53c76d53b07a8 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -241,7 +241,7 @@ void calcSlowDownPointsForPossibleCollision( std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, const DebugData & debug_data); //!< @brief generate possible collisions coming from occlusion spots on the side of the path bool generatePossibleCollisionsFromGridMap( std::vector & possible_collisions, const grid_map::GridMap & grid, From 03da9085bb46983217f1667a7cca93800390c979 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 12 May 2024 05:30:24 +0000 Subject: [PATCH 7/8] style(pre-commit): autofix --- .../src/occlusion_spot_utils.cpp | 3 ++- .../src/occlusion_spot_utils.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 7ed18e955a1b9..22b93c2c0236c 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -429,7 +429,8 @@ bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, const DebugData & debug_data) + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, + const DebugData & debug_data) { const double baselink_to_front = param.baselink_to_front; const double right_overhang = param.right_overhang; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 53c76d53b07a8..47242b70f0cbd 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -241,7 +241,8 @@ void calcSlowDownPointsForPossibleCollision( std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, const DebugData & debug_data); + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, + const DebugData & debug_data); //!< @brief generate possible collisions coming from occlusion spots on the side of the path bool generatePossibleCollisionsFromGridMap( std::vector & possible_collisions, const grid_map::GridMap & grid, From cb222a76c32459168a81dd770dbac44effaee3d7 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 13 May 2024 14:58:41 +0900 Subject: [PATCH 8/8] delete unnecessary pointsToRays function Signed-off-by: Ryuta Kambe --- .../src/grid_utils.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index f98b9971639c4..c8adb324b9055 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -43,23 +43,6 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) return line_poly; } -std::vector> pointsToRays( - const grid_map::Position p0, const grid_map::Position p1, const double radius) -{ - using grid_map::Position; - std::vector> lines; - const double angle = atan2(p0.y() - p1.y(), p0.x() - p1.x()); - const double r = radius; - lines.emplace_back(std::make_pair(p0, p1)); - lines.emplace_back(std::make_pair( - Position(p0.x() + r * sin(angle), p0.y() - r * cos(angle)), - Position(p1.x() + r * sin(angle), p1.y() - r * cos(angle)))); - lines.emplace_back(std::make_pair( - Position(p1.x() - r * sin(angle), p1.y() + r * cos(angle)), - Position(p0.x() - r * sin(angle), p0.y() + r * cos(angle)))); - return lines; -} - void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, const Polygon2d & polygon, [[maybe_unused]] double min_size)