From d2312fba33f235bc32f1d4d79ee7bfe582e5d0e9 Mon Sep 17 00:00:00 2001 From: siferati <15384781+siferati@users.noreply.github.com> Date: Mon, 11 Dec 2023 14:32:28 +0900 Subject: [PATCH 1/2] implement force update costmaps service --- .../mbf_costmap_nav/costmap_navigation_server.h | 13 ++++++++++++- .../costmap_navigation_server.cpp | 17 +++++++++++++++++ 2 files changed, 29 insertions(+), 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 index 4621406f..540f004f 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 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 index 289040e7..7cc58998 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 @@ -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 >(private_nh_); dsrv_costmap_->setCallback(boost::bind(&CostmapNavigationServer::reconfigure, this, _1, _2)); @@ -818,6 +821,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 CostmapNavigationServer::requestedCostmap(std::uint8_t costmap_type) const { // selecting the requested costmap From 5acae8ccb00788ed0f53b03c84e18d6ec445c2ac Mon Sep 17 00:00:00 2001 From: siferati <15384781+siferati@users.noreply.github.com> Date: Tue, 12 Dec 2023 14:37:43 +0900 Subject: [PATCH 2/2] rename --- .../mbf_costmap_nav/costmap_navigation_server.h | 8 ++++---- .../mbf_costmap_nav/costmap_navigation_server.cpp | 15 +++++++-------- 2 files changed, 11 insertions(+), 12 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 540f004f..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 @@ -234,12 +234,12 @@ 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 + * @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 callForceUpdateCostmaps(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + bool callServiceUpdateCostmaps(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); /** * @brief Reconfiguration method called by dynamic reconfigure. @@ -294,8 +294,8 @@ 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_; + //! 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 7cc58998..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,13 +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); - - force_update_costmaps_srv_ = - private_nh_.advertiseService("force_update_costmaps", &CostmapNavigationServer::callForceUpdateCostmaps, 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 >(private_nh_); @@ -821,10 +820,10 @@ bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request& return true; } -bool CostmapNavigationServer::callForceUpdateCostmaps(std_srvs::Empty::Request& request, - std_srvs::Empty::Response& response) +bool CostmapNavigationServer::callServiceUpdateCostmaps(std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response) { - // force update both costmaps + // update both costmaps for (auto costmap : { local_costmap_ptr_, global_costmap_ptr_ }) { costmap->checkActivate();