Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement service to force update the costmaps #340

Merged
merged 2 commits into from
Dec 18, 2023
Merged

Implement service to force update the costmaps #340

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d2312fb
implement force update costmaps service
siferati Dec 11, 2023
5acae8c
rename
siferati Dec 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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.
Expand Down Expand Up @@ -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
};

Expand Down
20 changes: 18 additions & 2 deletions mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down Expand Up @@ -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_ })
siferati marked this conversation as resolved.
Show resolved Hide resolved
{
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
Expand Down
Loading