Update nav2_navfn_planner/src/navfn_planner.cpp

check and resize planner costmap array
ros-navigation · Jun 3, 2019 · 50ab69a · 50ab69a
1 parent fcbf46f
commit 50ab69a
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -164,6 +164,15 @@ NavfnPlanner::computePathToPose(const std::shared_ptr<GoalHandle> goal_handle)
   try {
     // Get the current pose from the robot
     getCostmap(costmap_);
+    RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
+      costmap_.metadata.size_x, costmap_.metadata.size_y);
+
+    // Update planner based on the new costmap size
+    if (isPlannerOutOfDate()) {
+      current_costmap_size_[0] = costmap_.metadata.size_x;
+      current_costmap_size_[1] = costmap_.metadata.size_y;
+      planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
+    }
     auto start = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
 
     if (!robot_->getCurrentPose(start)) {