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

rtabmap: updated list_labels service to return also node ids, added get_plan_nodes service to get path AND node ids

Browse Source
This commit is contained in:
matlabbe
2019-03-19 16:06:04 -04:00
parent 7c104cf1b9
commit 6d149cd9bc
5 changed files with 104 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+88 -2
View File
@@ -562,6 +562,7 @@ void CoreWrapper::onInit()
getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
getPlanSrv_ = nh.advertiseService("get_plan", &CoreWrapper::getPlanCallback, this);
getPlanNodesSrv_ = nh.advertiseService("get_plan_nodes", &CoreWrapper::getPlanNodesCallback, 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);
@@ -2981,7 +2982,7 @@ bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::Get
{
NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
req.goal.header.frame_id.c_str(), mapFrameId_.c_str());
return true;
return false;
}
pose = t * pose;
}
@@ -2991,7 +2992,7 @@ bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::Get
NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
res.plan.header.frame_id = mapFrameId_;
res.plan.header.stamp = ros::Time::now();
res.plan.header.stamp = req.goal.header.stamp;
if(poses.size() == 0)
{
NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
@@ -3013,6 +3014,7 @@ bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::Get
if(!rtabmap_.getPathTransformToGoal().isIdentity())
{
res.plan.poses.resize(res.plan.poses.size()+1);
res.plan.poses[res.plan.poses.size()-1].header = res.plan.header;
Transform p = rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
rtabmap_ros::transformToPoseMsg(p, res.plan.poses[res.plan.poses.size()-1].pose);
}
@@ -3035,6 +3037,88 @@ bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::Get
return true;
}
bool CoreWrapper::getPlanNodesCallback(rtabmap_ros::GetPlan::Request &req, rtabmap_ros::GetPlan::Response &res)
{
Transform pose = rtabmap_ros::transformFromPoseMsg(req.goal.pose);
UTimer timer;
if(req.goal_node > 0 || !pose.isNull())
{
if(req.goal_node <= 0)
{
// transform goal in /map frame
if(mapFrameId_.compare(req.goal.header.frame_id) != 0)
{
Transform t = rtabmap_ros::getTransform(mapFrameId_, req.goal.header.frame_id, req.goal.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(t.isNull())
{
NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
req.goal.header.frame_id.c_str(), mapFrameId_.c_str());
return false;
}
pose = t * pose;
}
}
if((req.goal_node > 0 && rtabmap_.computePath(req.goal_node, req.tolerance)) ||
(req.goal_node <= 0 && rtabmap_.computePath(pose, req.tolerance)))
{
NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
res.plan.header.frame_id = mapFrameId_;
res.plan.header.stamp = req.goal_node > 0?ros::Time::now():req.goal.header.stamp;
if(poses.size() == 0)
{
NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
rtabmap_.getGoalReachedRadius());
if(!pose.isNull())
{
// just set the goal directly
res.plan.poses.resize(1);
res.plan.nodeIds.resize(1);
rtabmap_ros::transformToPoseMsg(pose, res.plan.poses[0]);
res.plan.nodeIds[0] = 0;
}
}
else
{
res.plan.poses.resize(poses.size());
res.plan.nodeIds.resize(poses.size());
int oi = 0;
for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
rtabmap_ros::transformToPoseMsg(iter->second, res.plan.poses[oi]);
res.plan.nodeIds[oi] = iter->first;
++oi;
}
if(!rtabmap_.getPathTransformToGoal().isIdentity())
{
res.plan.poses.resize(res.plan.poses.size()+1);
res.plan.nodeIds.resize(res.plan.nodeIds.size()+1);
Transform p = rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
rtabmap_ros::transformToPoseMsg(p, res.plan.poses[res.plan.poses.size()-1]);
res.plan.nodeIds[res.plan.nodeIds.size()-1] = 0;
}
// Just output the path on screen
std::stringstream stream;
for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter != poses.begin())
{
stream << " ";
}
stream << iter->first;
}
NODELET_INFO("Planned path: [%s]", stream.str().c_str());
}
}
rtabmap_.clearPath(0);
}
return true;
}
bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
{
double planningTime = 0.0;
@@ -3108,6 +3192,7 @@ bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtab
if(rtabmap_.getMemory())
{
std::map<int, std::string> labels = rtabmap_.getMemory()->getAllLabels();
res.ids = uKeys(labels);
res.labels = uValues(labels);
NODELET_INFO("List labels service: %d labels found.", (int)res.labels.size());
}
@@ -3500,6 +3585,7 @@ void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
if(!rtabmap_.getPathTransformToGoal().isIdentity() || !goalLocalTransform.isIdentity())
{
path.poses.resize(path.poses.size()+1);
path.poses[path.poses.size()-1].header = path.header;
pathNodes.nodeIds.resize(pathNodes.nodeIds.size()+1);
pathNodes.poses.resize(pathNodes.poses.size()+1);
Transform p = t * rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal() * goalLocalTransform;