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

Added remove_label service. Updated get_nodes_in_radius service (added parameter k, return also distances of nodes found)

Browse Source
This commit is contained in:
matlabbe
2022-01-20 19:00:05 -05:00
parent 8fff821a87
commit 0ebaaa7439
7 changed files with 65 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+30 -3
View File
@@ -709,6 +709,7 @@ void CoreWrapper::onInit()
cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
removeLabelSrv_ = nh.advertiseService("remove_label", &CoreWrapper::removeLabelCallback, this);
addLinkSrv_ = nh.advertiseService("add_link", &CoreWrapper::addLinkCallback, this);
getNodesInRadiusSrv_ = nh.advertiseService("get_nodes_in_radius", &CoreWrapper::getNodesInRadiusCallback, this);
#ifdef WITH_OCTOMAP_MSGS
@@ -2352,7 +2353,7 @@ std::map<int, Transform> CoreWrapper::filterNodesToAssemble(
std::map<int, Transform> output;
if(mappingMaxNodes_ > 0)
{
std::map<int, float> nodesDist = graph::findNearestNodes(nodes, currentPose, mappingMaxNodes_);
std::map<int, float> nodesDist = graph::findNearestNodes(currentPose, nodes, 0, 0, mappingMaxNodes_);
for(std::map<int, float>::iterator iter=nodesDist.begin(); iter!=nodesDist.end(); ++iter)
{
if(mappingAltitudeDelta_<=0.0 ||
@@ -4028,6 +4029,27 @@ bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtab
return true;
}
bool CoreWrapper::removeLabelCallback(rtabmap_ros::RemoveLabel::Request& req, rtabmap_ros::RemoveLabel::Response& res)
{
if(rtabmap_.getMemory())
{
int id = rtabmap_.getMemory()->getSignatureIdByLabel(req.label, true);
if(id == 0)
{
NODELET_WARN("Label \"%s\" not found in the map, cannot remove it!", req.label.c_str());
}
else if(!rtabmap_.labelLocation(id, ""))
{
NODELET_ERROR("Failed removing label \"%s\".", req.label.c_str());
}
else
{
NODELET_INFO("Removed label \"%s\".", req.label.c_str());
}
}
return true;
}
bool CoreWrapper::addLinkCallback(rtabmap_ros::AddLink::Request& req, rtabmap_ros::AddLink::Response&)
{
if(rtabmap_.getMemory())
@@ -4043,25 +4065,30 @@ bool CoreWrapper::getNodesInRadiusCallback(rtabmap_ros::GetNodesInRadius::Reques
{
ROS_INFO("Get nodes in radius (%f): node_id=%d pose=(%f,%f,%f)", req.radius, req.node_id, req.x, req.y, req.z);
std::map<int, Transform> poses;
std::map<int, float> dists;
if(req.node_id != 0 || (req.x == 0.0f && req.y == 0.0f && req.z == 0.0f))
{
poses = rtabmap_.getNodesInRadius(req.node_id, req.radius);
poses = rtabmap_.getNodesInRadius(req.node_id, req.radius, req.k, &dists);
}
else
{
poses = rtabmap_.getNodesInRadius(Transform(req.x, req.y, req.z, 0,0,0), req.radius);
poses = rtabmap_.getNodesInRadius(Transform(req.x, req.y, req.z, 0,0,0), req.radius, req.k, &dists);
}
//Optimized graph
res.ids.resize(poses.size());
res.poses.resize(poses.size());
res.distsSqr.resize(poses.size());
int index = 0;
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
iter != poses.end();
++iter)
{
UERROR("add %d", iter->first);
res.ids[index] = iter->first;
transformToPoseMsg(iter->second, res.poses[index]);
UASSERT(dists.find(iter->first) != dists.end());
res.distsSqr[index] = dists.at(iter->first);
++index;
}
src/GuiWrapper.cpp
+11
View File
@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/GetMap.h"
#include "rtabmap_ros/SetGoal.h"
#include "rtabmap_ros/SetLabel.h"
#include "rtabmap_ros/RemoveLabel.h"
#include "rtabmap_ros/PreferencesDialogROS.h"
float max3( const float& a, const float& b, const float& c)
@@ -400,6 +401,16 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
ROS_ERROR("Can't call \"set_label\" service");
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
{
UASSERT(cmdEvent->value1().isStr());
rtabmap_ros::RemoveLabel removeLabelSrv;
removeLabelSrv.request.label = cmdEvent->value1().toStr();
if(!ros::service::call("remove_label", removeLabelSrv))
{
ROS_ERROR("Can't call \"remove_label\" service");
}
}
else
{
ROS_WARN("Not handled command (%d)...", cmd);