edge fix

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -100,6 +100,16 @@ class GridCollisionChecker
     return angles_;
   }
 
+private:
+  /**
+   * @brief Check if value outside the range
+   * @param min Minimum value of the range
+   * @param max Maximum value of the range
+   * @param value the value to check if it is within the range
+   * @return boolean if in range or not
+   */
+  bool outsideRange(const unsigned int & max, const float & value);
+
 protected:
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;

nav2_smac_planner/src/collision_checker.cpp

@@ -86,6 +86,13 @@ bool GridCollisionChecker::inCollision(
   const float & angle_bin,
   const bool & traverse_unknown)
 {
+  // Check to make sure cell is inside the map
+  if (outsideRange(costmap_->getSizeInCellsX(), x) ||
+    outsideRange(costmap_->getSizeInCellsY(), y))
+  {
+    return false;
+  }
+
   // Assumes setFootprint already set
   double wx, wy;
   costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
@@ -164,4 +171,9 @@ float GridCollisionChecker::getCost()
   return static_cast<float>(footprint_cost_);
 }
 
+bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value)
+{
+  return value < 0.0f || value > max;
+}
+
 }  // namespace nav2_smac_planner