adding warning when robot is out of costmap bounds

nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp

@@ -158,6 +158,10 @@ public:
    * This is updated by setFootprint(). */
   double getInscribedRadius() {return inscribed_radius_;}
 
+  /** @brief Checks if the robot is outside the bounds of its costmap in the case
+  * of poorly configured setups. */
+  bool isOutofBounds(double robot_x, double robot_y);
+
 private:
   Costmap2D costmap_;
   std::string global_frame_;

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -428,9 +428,9 @@ Costmap2DROS::updateMap()
     // get global pose
     geometry_msgs::msg::PoseStamped pose;
     if (getRobotPose(pose)) {
-      double x = pose.pose.position.x;
-      double y = pose.pose.position.y;
-      double yaw = tf2::getYaw(pose.pose.orientation);
+      const double & x = pose.pose.position.x;
+      const double & y = pose.pose.position.y;
+      const double yaw = tf2::getYaw(pose.pose.orientation);
       layered_costmap_->updateMap(x, y, yaw);
 
       geometry_msgs::msg::PolygonStamped footprint;

nav2_costmap_2d/src/layered_costmap.cpp

@@ -99,6 +99,12 @@ void LayeredCostmap::resizeMap(
   }
 }
 
+bool LayeredCostmap::isOutofBounds(double robot_x, double robot_y)
+{
+  unsigned int mx, my;
+  return costmap_.worldToMap(robot_x, robot_y, mx, my);
+}
+
 void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
 {
   // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
@@ -113,6 +119,11 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
     costmap_.updateOrigin(new_origin_x, new_origin_y);
   }
 
+  if (isOutofBounds(robot_x, robot_y)) {
+    RCLCPP_WARN(rclcpp::get_logger("nav2_costmap_2d"),
+      "Robot is out of bounds of the costmap!");
+  }
+
   if (plugins_.size() == 0) {
     return;
   }