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) { 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..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) @@ -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> 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(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(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"); } } } 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..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 @@ -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 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..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, 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,