refactor(behavior_velocity_occlusion_spot_module): reduce cppcheck wa…

…rning (autowarefoundation#6987) * remove unnecessary if branch Signed-off-by: Ryuta Kambe <[email protected]> * add const for const variable Signed-off-by: Ryuta Kambe <[email protected]> * remove redundant assignment Signed-off-by: Ryuta Kambe <[email protected]> * remove redundant if branch Signed-off-by: Ryuta Kambe <[email protected]> * underflow check Signed-off-by: Ryuta Kambe <[email protected]> * use const for const parameter Signed-off-by: Ryuta Kambe <[email protected]> * style(pre-commit): autofix * delete unnecessary pointsToRays function Signed-off-by: Ryuta Kambe <[email protected]> --------- Signed-off-by: Ryuta Kambe <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: vividf <[email protected]>
vividf · May 16, 2024 · bc93a8e · bc93a8e
1 parent a3af5cc
commit bc93a8e
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 53 deletions.
diff --git 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) {

diff --git 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<std::pair<grid_map::Position, grid_map::Position>> pointsToRays(
-  const grid_map::Position p0, const grid_map::Position p1, const double radius)
-{
-  using grid_map::Position;
-  std::vector<std::pair<Position, Position>> 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<grid_map::Position> & occlusion_spot_positions, const grid_map::GridMap & grid,
   const Polygon2d & polygon, [[maybe_unused]] double min_size)
@@ -86,36 +69,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<std::pair<grid_map::Position, grid_map::Position>> 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;
@@ -324,7 +292,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<unsigned char>(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) {
@@ -348,14 +316,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<unsigned char>(y, x) = intensity;
-      } else if (param.occupied_min <= intensity) {
+      } else {
         intensity = grid_utils::occlusion_cost_value::OCCUPIED_IMAGE;
         border_image->at<unsigned char>(y, x) = intensity;
-      } else {
-        throw std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause");
       }
     }
   }

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
@@ -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;
@@ -426,7 +429,8 @@ bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d
 std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
   const grid_map::GridMap & grid, const std::vector<grid_map::Position> & 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
@@ -241,7 +241,8 @@ void calcSlowDownPointsForPossibleCollision(
 std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
   const grid_map::GridMap & grid, const std::vector<grid_map::Position> & 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<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,