Costmap out of bounds access exception handling (#14)

* add exception for costmap get cost && change planner_server launch file

* added path planning failure behaviour when static layer costmap wasn't initialized

* added out of bounds error message printing

nav2_bringup/bringup/launch/planner_server_launch.py

@@ -108,7 +108,7 @@ def generate_launch_description():
             condition=IfCondition(use_lifecycle_mgr),
             package='nav2_lifecycle_manager',
             executable='lifecycle_manager',
-            name='lifecycle_manager_navigation',
+            name='lifecycle_manager_planner_server',
             #arguments=['--ros-args', '--log-level', 'DEBUG'],
             output='screen',
             parameters=[{'use_sim_time': use_sim_time},

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -127,8 +127,7 @@ class Costmap2D
 
   /**
    * @brief  Get the cost of a cell in the costmap
-   * @param mx The x coordinate of the cell
-   * @param my The y coordinate of the cell
+   * @param index The cell index
    * @return The cost of the cell
    */
   unsigned char getCost(unsigned int index) const;
@@ -186,11 +185,7 @@ class Costmap2D
    * @param my The y coordinate
    * @return The associated index
    */
-  inline unsigned int getIndex(unsigned int mx, unsigned int my) const
-  {
-    return my * size_x_ + mx;
-  }
-
+  unsigned int getIndex(unsigned int mx, unsigned int my) const;
   /**
    * @brief  Given an index... compute the associated map coordinates
    * @param  index The index

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -256,6 +256,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   bool getUseRadius() {return use_radius_;}
 
+
+  bool is_static_layer_costmap_initialized();
+
 protected:
   rclcpp::Node::SharedPtr client_node_;
 
@@ -313,6 +316,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
+  std::shared_ptr<Layer> static_layer_plugin;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -128,6 +128,8 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
   void undeclareAllParameters();
   std::string getFullName(const std::string & param_name);
 
+  virtual bool map_received() { return false; }
+
 protected:
   LayeredCostmap * layered_costmap_;
   std::string name_;

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -72,6 +72,8 @@ class StaticLayer : public CostmapLayer
 
   virtual void matchSize();
 
+  inline bool map_received() { return map_received_; }
+
 private:
   void getParameters();
   void processMap(const nav_msgs::msg::OccupancyGrid & new_map);

nav2_costmap_2d/src/costmap_2d.cpp

@@ -41,6 +41,10 @@
 #include <cstdio>
 #include <string>
 #include <vector>
+#include <iostream>
+#include "nav2_costmap_2d/cost_values.hpp"
+
+using nav2_costmap_2d::NO_INFORMATION;
 
 namespace nav2_costmap_2d
 {
@@ -207,19 +211,40 @@ unsigned char * Costmap2D::getCharMap() const
   return costmap_;
 }
 
+unsigned int Costmap2D::getIndex(unsigned int mx, unsigned int my) const
+{
+    auto cell_index = my * size_x_ + mx;
+    if (cell_index < size_x_ * size_y_) {
+        return cell_index;
+    } else {
+        std::cout << "ERROR: Costmap2D::getIndex out of bounds x, y cell coordinates" << std::endl;
+        throw std::runtime_error("Access to out of bounds costmap cell");
+    }
+}
+
 unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
 {
-  return costmap_[getIndex(mx, my)];
+  try {
+      return costmap_[getIndex(mx, my)];
+  } catch (const std::runtime_error& e) {
+      return nav2_costmap_2d::NO_INFORMATION;
+  }
 }
 
 unsigned char Costmap2D::getCost(unsigned int undex) const
 {
+  // no need to do bounds checks here
+  // it is already done in smac_planner/node_se2.cpp:373
   return costmap_[undex];
 }
 
 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
 {
-  costmap_[getIndex(mx, my)] = cost;
+  try {
+      costmap_[getIndex(mx, my)] = cost;
+  } catch (const std::runtime_error& e) {
+      return;
+  }
 }
 
 void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const
@@ -351,8 +376,12 @@ bool Costmap2D::setConvexPolygonCost(
 
   // set the cost of those cells
   for (unsigned int i = 0; i < polygon_cells.size(); ++i) {
-    unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
-    costmap_[index] = cost_value;
+      try {
+        unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
+        costmap_[index] = cost_value;
+      } catch (const std::runtime_error& e) {
+          continue;
+      }
   }
   return true;
 }

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -154,6 +154,10 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
       layered_costmap_, plugin_names_[i], tf_buffer_.get(),
       shared_from_this(), client_node_, rclcpp_node_);
 
+    if  (plugin_names_[i] == "static_layer") {
+      static_layer_plugin = plugin;
+    }
+
     RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str());
   }
 
@@ -538,4 +542,10 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose)
     global_frame_, robot_base_frame_, transform_tolerance_);
 }
 
+bool
+Costmap2DROS::is_static_layer_costmap_initialized()
+{
+    // return static layer boolean variable map_received_
+    return static_layer_plugin->map_received();
+}
 }  // namespace nav2_costmap_2d

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -145,7 +145,7 @@ class SimpleActionServer
       } catch (std::exception & ex) {
         RCLCPP_ERROR(
           node_logging_interface_->get_logger(),
-          "Action server failed while executing action callback: \"%s\"", ex.what());
+          "Action server %s failed while executing action callback: \"%s\"", action_name_, ex.what());
         terminate_all();
         return;
       }

smac_planner/include/smac_planner/smac_planner.hpp

@@ -124,6 +124,7 @@ class SmacPlanner : public nav2_core::GlobalPlanner
   SmootherParams _smoother_params;
   OptimizerParams _optimizer_params;
   double _max_planning_time;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
 };
 
 }  // namespace smac_planner

smac_planner/src/smac_planner.cpp

@@ -50,6 +50,7 @@ void SmacPlanner::configure(
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
 {
   _node = parent;
+  _costmap_ros = costmap_ros;
   _costmap = costmap_ros->getCostmap();
   _name = name;
   _global_frame = costmap_ros->getGlobalFrameID();
@@ -217,6 +218,19 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
 
   std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
 
+  // check if static layer was initialized
+  if (!_costmap_ros->is_static_layer_costmap_initialized()) {
+       // no map for static layer was published
+       // abort planning
+      RCLCPP_WARN(
+              _node->get_logger(), "static layer costmap haven't received any initial occupancy grid",
+              _name.c_str());
+      nav_msgs::msg::Path plan;
+      plan.header.stamp = _node->now();
+      plan.header.frame_id = _global_frame;
+      // returning empty path equals to planning failure
+      return plan;
+  }
   // Downsample costmap, if required
   nav2_costmap_2d::Costmap2D * costmap = _costmap;
   if (_costmap_downsampler) {