From 4f6bb1422f91e9fc4f1a759f5dc59ee4f55da4dc Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 9 Sep 2022 16:58:26 +0900 Subject: [PATCH] fix(behavior_path_planner): fix pull_over dies when occupancy_grid_map is not ready (#1824) * fix(behavior_path_planner): fix pull_over dies when occupancy_grid_map is not ready Signed-off-by: kosuke55 * Update planning/freespace_planning_algorithms/src/abstract_algorithm.cpp Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Signed-off-by: kosuke55 Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- .../src/scene_module/pull_over/pull_over_module.cpp | 4 ++++ .../utils/occupancy_grid_based_collision_detector.cpp | 5 +++++ .../freespace_planning_algorithms/src/abstract_algorithm.cpp | 4 ++++ 3 files changed, 13 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index de7143f989ab8..b43dc54de37a2 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -83,6 +83,10 @@ void PullOverModule::resetStatus() // This function is needed for waiting for planner_data_ void PullOverModule::updateOccupancyGrid() { + if (!planner_data_->occupancy_grid) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "occupancy_grid is not ready"); + return; + } occupancy_grid_map_.setMap(*(planner_data_->occupancy_grid)); } diff --git a/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp index 1e59014c4cc28..275c10c1e48ec 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp @@ -155,6 +155,11 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( bool OccupancyGridBasedCollisionDetector::detectCollision( const IndexXYT & base_index, const bool check_out_of_range) const { + if (coll_indexes_table_.empty()) { + std::cerr << "[occupancy_grid_based_collision_detector] setMap has not yet been done." + << std::endl; + return false; + } const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { int idx_theta = 0; // whatever. Yaw is nothing to do with collision detection between grids. diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 7083e74b94bf7..37e46165de17f 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -160,6 +160,10 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) { + if (coll_indexes_table_.empty()) { + std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; + return false; + } const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { int idx_theta = 0; // whatever. Yaw is nothing to do with collision detection between grids.