implement force update costmaps service

naturerobots · Dec 11, 2023 · 00dd8a1 · 00dd8a1
1 parent 3dedf54
commit 00dd8a1
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 1 deletion.
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
@@ -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 force_update_costmaps service
+   * @param request Empty request object.
+   * @param response Empty response object.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callForceUpdateCostmaps(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 force_update_costmap service
+  ros::ServiceServer force_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
@@ -180,6 +180,9 @@ CostmapNavigationServer::CostmapNavigationServer(const TFPtr& tf_listener_ptr)
   find_valid_pose_srv_ =
       private_nh_.advertiseService("find_valid_pose", &CostmapNavigationServer::callServiceFindValidPose, this);
 
+  force_update_costmaps_srv_ =
+      private_nh_.advertiseService("force_update_costmaps", &CostmapNavigationServer::callForceUpdateCostmaps, 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_);
   dsrv_costmap_->setCallback(boost::bind(&CostmapNavigationServer::reconfigure, this, _1, _2));
@@ -817,6 +820,20 @@ bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request&
   return true;
 }
 
+bool CostmapNavigationServer::callForceUpdateCostmaps(std_srvs::Empty::Request& request,
+                                                      std_srvs::Empty::Response& response)
+{
+  // force 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