From bc19f73249cfb1071d151e9dd0b478105b7bdf10 Mon Sep 17 00:00:00 2001
From: Tiago Silva <15384781+siferati@users.noreply.github.com>
Date: Mon, 18 Dec 2023 15:29:10 +0900
Subject: [PATCH] Implement service to force update the costmaps (#340)

---
 .../costmap_navigation_server.h               | 13 +++++++++++-
 .../costmap_navigation_server.cpp             | 20 +++++++++++++++++--
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h b/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h
index 4621406f..a2650c6a 100644
--- a/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h
+++ b/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h
@@ -218,7 +218,7 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe
   bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request& request, mbf_msgs::CheckPath::Response& response);
 
   /**
-   * @brief Callback method for the make_plan service
+   * @brief Callback method for the clear_costmaps service
    * @param request Empty request object.
    * @param response Empty response object.
    * @return true, if the service completed successfully, false otherwise
@@ -233,6 +233,14 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe
    */
   bool callServiceFindValidPose(mbf_msgs::FindValidPose::Request& request, mbf_msgs::FindValidPose::Response& response);
 
+  /**
+   * @brief Callback method for the update_costmaps service
+   * @param request Empty request object.
+   * @param response Empty response object.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callServiceUpdateCostmaps(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+
   /**
    * @brief Reconfiguration method called by dynamic reconfigure.
    * @param config Configuration parameters. See the MoveBaseFlexConfig definition.
@@ -286,6 +294,9 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe
   //! Service Server for the find_valid_pose service
   ros::ServiceServer find_valid_pose_srv_;
 
+  //! Service Server for the update_costmap service
+  ros::ServiceServer update_costmaps_srv_;
+
   static constexpr double ANGLE_INCREMENT = 5.0 * M_PI / 180.0;  // 5 degrees
 };
 
diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
index 289040e7..24f1f0aa 100644
--- a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
+++ b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
@@ -175,10 +175,12 @@ CostmapNavigationServer::CostmapNavigationServer(const TFPtr& tf_listener_ptr)
       private_nh_.advertiseService("check_pose_cost", &CostmapNavigationServer::callServiceCheckPoseCost, this);
   check_path_cost_srv_ =
       private_nh_.advertiseService("check_path_cost", &CostmapNavigationServer::callServiceCheckPathCost, this);
-  clear_costmaps_srv_ =
-      private_nh_.advertiseService("clear_costmaps", &CostmapNavigationServer::callServiceClearCostmaps, this);
   find_valid_pose_srv_ =
       private_nh_.advertiseService("find_valid_pose", &CostmapNavigationServer::callServiceFindValidPose, this);
+  update_costmaps_srv_ =
+      private_nh_.advertiseService("update_costmaps", &CostmapNavigationServer::callServiceUpdateCostmaps, this);
+  clear_costmaps_srv_ =
+      private_nh_.advertiseService("clear_costmaps", &CostmapNavigationServer::callServiceClearCostmaps, this);
 
   // dynamic reconfigure server for mbf_costmap_nav configuration; also include abstract server parameters
   dsrv_costmap_ = boost::make_shared<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >(private_nh_);
@@ -818,6 +820,20 @@ bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request&
   return true;
 }
 
+bool CostmapNavigationServer::callServiceUpdateCostmaps(std_srvs::Empty::Request& request,
+                                                        std_srvs::Empty::Response& response)
+{
+  // update both costmaps
+  for (auto costmap : { local_costmap_ptr_, global_costmap_ptr_ })
+  {
+    costmap->checkActivate();
+    costmap->updateMap();
+    costmap->checkDeactivate();
+  }
+
+  return true;
+}
+
 std::pair<std::string, CostmapWrapper::Ptr> CostmapNavigationServer::requestedCostmap(std::uint8_t costmap_type) const
 {
   // selecting the requested costmap