Skip to content

Commit bc93a8e

Browse files
veqccpre-commit-ci[bot]
authored and
vividf
committed
refactor(behavior_velocity_occlusion_spot_module): reduce cppcheck warning (autowarefoundation#6987)
* remove unnecessary if branch Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * add const for const variable Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * remove redundant assignment Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * remove redundant if branch Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * underflow check Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * use const for const parameter Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * style(pre-commit): autofix * delete unnecessary pointsToRays function Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> --------- Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: vividf <yihsiang.fang@tier4.jp>
1 parent a3af5cc commit bc93a8e

File tree

4 files changed

+24
-53
lines changed

4 files changed

+24
-53
lines changed

‎planning/behavior_velocity_occlusion_spot_module/src/debug.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -118,7 +118,7 @@ MarkerArray makeDebugInfoMarkers(T & debug_data)
118118
{
119119
// add slow down markers for occlusion spot
120120
MarkerArray debug_markers;
121-
auto & possible_collisions = debug_data.possible_collisions;
121+
const auto & possible_collisions = debug_data.possible_collisions;
122122
size_t id = 0;
123123
// draw obstacle collision
124124
for (const auto & pc : possible_collisions) {

‎planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp

+16-50
Original file line numberDiff line numberDiff line change
@@ -43,23 +43,6 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius)
4343
return line_poly;
4444
}
4545

46-
std::vector<std::pair<grid_map::Position, grid_map::Position>> pointsToRays(
47-
const grid_map::Position p0, const grid_map::Position p1, const double radius)
48-
{
49-
using grid_map::Position;
50-
std::vector<std::pair<Position, Position>> lines;
51-
const double angle = atan2(p0.y() - p1.y(), p0.x() - p1.x());
52-
const double r = radius;
53-
lines.emplace_back(std::make_pair(p0, p1));
54-
lines.emplace_back(std::make_pair(
55-
Position(p0.x() + r * sin(angle), p0.y() - r * cos(angle)),
56-
Position(p1.x() + r * sin(angle), p1.y() - r * cos(angle))));
57-
lines.emplace_back(std::make_pair(
58-
Position(p1.x() - r * sin(angle), p1.y() + r * cos(angle)),
59-
Position(p0.x() - r * sin(angle), p0.y() + r * cos(angle))));
60-
return lines;
61-
}
62-
6346
void findOcclusionSpots(
6447
std::vector<grid_map::Position> & occlusion_spot_positions, const grid_map::GridMap & grid,
6548
const Polygon2d & polygon, [[maybe_unused]] double min_size)
@@ -86,36 +69,21 @@ bool isCollisionFree(
8669
const double radius)
8770
{
8871
const grid_map::Matrix & grid_data = grid["layer"];
89-
bool polys = true;
9072
try {
91-
if (polys) {
92-
Point2d occlusion_p = {p1.x(), p1.y()};
93-
Point2d collision_p = {p2.x(), p2.y()};
94-
Polygon2d polygon = pointsToPoly(occlusion_p, collision_p, radius);
95-
grid_map::Polygon grid_polygon;
96-
for (const auto & point : polygon.outer()) {
97-
grid_polygon.addVertex({point.x(), point.y()});
98-
}
99-
for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
100-
++iterator) {
101-
const grid_map::Index & index = *iterator;
102-
if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) {
103-
return false;
104-
}
105-
}
106-
} else {
107-
std::vector<std::pair<grid_map::Position, grid_map::Position>> lines =
108-
pointsToRays(p1, p2, radius);
109-
for (const auto & p : lines) {
110-
for (grid_map::LineIterator iterator(grid, p.first, p.second); !iterator.isPastEnd();
111-
++iterator) {
112-
const grid_map::Index & index = *iterator;
113-
if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) {
114-
return false;
115-
}
116-
}
73+
Point2d occlusion_p = {p1.x(), p1.y()};
74+
Point2d collision_p = {p2.x(), p2.y()};
75+
Polygon2d polygon = pointsToPoly(occlusion_p, collision_p, radius);
76+
grid_map::Polygon grid_polygon;
77+
for (const auto & point : polygon.outer()) {
78+
grid_polygon.addVertex({point.x(), point.y()});
79+
}
80+
for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
81+
++iterator) {
82+
const grid_map::Index & index = *iterator;
83+
if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) {
84+
return false;
11785
}
118-
} // polys or not
86+
}
11987
} catch (const std::invalid_argument & e) {
12088
std::cerr << e.what() << std::endl;
12189
return false;
@@ -324,7 +292,7 @@ void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid
324292
const int idx = (height - 1 - y) + (width - 1 - x) * height;
325293
unsigned char intensity = cv_image.at<unsigned char>(y, x);
326294
if (intensity == grid_utils::occlusion_cost_value::FREE_SPACE) {
327-
intensity = grid_utils::occlusion_cost_value::FREE_SPACE;
295+
// do nothing
328296
} else if (intensity == grid_utils::occlusion_cost_value::UNKNOWN_IMAGE) {
329297
intensity = grid_utils::occlusion_cost_value::UNKNOWN;
330298
} else if (intensity == grid_utils::occlusion_cost_value::OCCUPIED_IMAGE) {
@@ -348,14 +316,12 @@ void toQuantizedImage(
348316
unsigned char intensity = occupancy_grid.data.at(idx);
349317
if (intensity <= param.free_space_max) {
350318
continue;
351-
} else if (param.free_space_max < intensity && intensity < param.occupied_min) {
319+
} else if (intensity < param.occupied_min) {
352320
intensity = grid_utils::occlusion_cost_value::UNKNOWN_IMAGE;
353321
occlusion_image->at<unsigned char>(y, x) = intensity;
354-
} else if (param.occupied_min <= intensity) {
322+
} else {
355323
intensity = grid_utils::occlusion_cost_value::OCCUPIED_IMAGE;
356324
border_image->at<unsigned char>(y, x) = intensity;
357-
} else {
358-
throw std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause");
359325
}
360326
}
361327
}

‎planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,9 @@ void calcSlowDownPointsForPossibleCollision(
117117
size_t collision_index = 0;
118118
double dist_along_path_point = offset;
119119
double dist_along_next_path_point = dist_along_path_point;
120+
if (path.points.size() == 0) {
121+
return;
122+
}
120123
for (size_t idx = closest_idx; idx < path.points.size() - 1; idx++) {
121124
auto p_prev = path.points.at(idx).point;
122125
auto p_next = path.points.at(idx + 1).point;
@@ -426,7 +429,8 @@ bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d
426429
std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
427430
const grid_map::GridMap & grid, const std::vector<grid_map::Position> & occlusion_spot_positions,
428431
const double offset_from_start_to_ego, const Point2d base_point,
429-
const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data)
432+
const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param,
433+
const DebugData & debug_data)
430434
{
431435
const double baselink_to_front = param.baselink_to_front;
432436
const double right_overhang = param.right_overhang;

‎planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -241,7 +241,8 @@ void calcSlowDownPointsForPossibleCollision(
241241
std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
242242
const grid_map::GridMap & grid, const std::vector<grid_map::Position> & occlusion_spot_positions,
243243
const double offset_from_start_to_ego, const Point2d base_point,
244-
const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data);
244+
const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param,
245+
const DebugData & debug_data);
245246
//!< @brief generate possible collisions coming from occlusion spots on the side of the path
246247
bool generatePossibleCollisionsFromGridMap(
247248
std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,

0 commit comments

Comments
 (0)