i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added "cancel_goal" service

Browse Source
This commit is contained in:
matlabbe
2015-06-21 20:17:31 -04:00
parent fae3e2fd6d
commit 0a40895830
4 changed files with 39 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+18
View File
@@ -375,6 +375,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
#ifdef WITH_OCTOMAP
@@ -1849,6 +1850,23 @@ bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ro
return true;
}
bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
{
if(rtabmap_.getPath().size())
{
ROS_WARN("Goal cancelled!");
rtabmap_.clearPath();
currentMetricGoal_.setNull();
latestNodeWasReached_ = false;
if(mbClient_.isServerConnected())
{
mbClient_.cancelGoal();
}
}
return true;
}
bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res)
{
if(rtabmap_.labelLocation(req.node_id, req.node_label))