fix(behavior_path_planner): fix pull_over dies when occupancy_grid_ma…

…p is not ready (autowarefoundation#1824) * fix(behavior_path_planner): fix pull_over dies when occupancy_grid_map is not ready Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * 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 <kosuke.tnp@gmail.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
boyali · Oct 3, 2022 · 4f6bb14 · 4f6bb14
1 parent f27798e
commit 4f6bb14
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 0 deletions.
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
@@ -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/.../behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp b/.../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
@@ -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.