From 4d557e6a502bde2a0c310b7146a9671a34410e41 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 6 Jul 2014 19:58:15 +0000 Subject: [PATCH] ros: Added rtabmap/GetMap service, removed MapData from InfoEx msg git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1459 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- CMakeLists.txt | 10 +- msg/Info.msg | 4 - msg/InfoEx.msg | 3 - package.xml | 2 + plugin_description.xml | 2 +- src/CoreWrapper.cpp | 596 ++++++++++++++++------------------- src/CoreWrapper.h | 15 +- src/GridMapAssemblerNode.cpp | 41 +-- src/GuiWrapper.cpp | 252 +++++++-------- src/GuiWrapper.h | 16 +- src/MapAssemblerNode.cpp | 138 ++------ src/rviz/MapCloudDisplay.cpp | 38 +-- src/rviz/MapCloudDisplay.h | 6 +- srv/GetMap.srv | 7 + 14 files changed, 455 insertions(+), 675 deletions(-) create mode 100644 srv/GetMap.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 29490f3b..bab99311 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs image_transport tf tf_conversions laser_geometry pcl_conversions pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader + genmsg ) ## System dependencies are found with CMake's conventions @@ -45,11 +46,10 @@ add_message_files( ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) + add_service_files( + FILES + GetMap.srv + ) ## Generate added messages and services with any dependencies listed here generate_messages( diff --git a/msg/Info.msg b/msg/Info.msg index e113f2b3..3302974b 100644 --- a/msg/Info.msg +++ b/msg/Info.msg @@ -10,10 +10,6 @@ int32 refId int32 loopClosureId int32 localLoopClosureId -# std::map poses; -int32[] nodeIds -geometry_msgs/Pose[] nodePoses - geometry_msgs/Transform mapCorrection geometry_msgs/Transform loopClosureTransform diff --git a/msg/InfoEx.msg b/msg/InfoEx.msg index 4f3b769b..3d5536b1 100644 --- a/msg/InfoEx.msg +++ b/msg/InfoEx.msg @@ -9,9 +9,6 @@ int32 refId int32 loopClosureId int32 localLoopClosureId -# The map data (rgb, depth, depthConstant, depth2D, localTransform, poses, constraints, map ids) -rtabmap/MapData data - geometry_msgs/Transform mapCorrection geometry_msgs/Transform loopClosureTransform diff --git a/package.xml b/package.xml index ed3c5e34..68862f37 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,8 @@ Jane Doe catkin + genmsg + cv_bridge roscpp rospy diff --git a/plugin_description.xml b/plugin_description.xml index 4e85e8b8..61f19678 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -3,7 +3,7 @@ type="rtabmap::MapCloudDisplay" base_class_type="rviz::Display"> - Displays graph point clouds from rtabmap/InfoEx messages. + Displays graph point clouds from rtabmap/MapData messages. diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 65d81745..5dd8f338 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -30,6 +30,7 @@ #include "rtabmap/Info.h" #include "rtabmap/InfoEx.h" #include "rtabmap/MapData.h" +#include "rtabmap/GetMap.h" #include "rtabmap/MsgConversion.h" @@ -173,11 +174,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this); resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this); triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this); - publishGlobalMapDataSrv_ = nh.advertiseService("publish_global_map_data", &CoreWrapper::publishGlobalMapDataCallback, this); - publishLocalMapDataSrv_ = nh.advertiseService("publish_local_map_data", &CoreWrapper::publishLocalMapDataCallback, this); - publishGlobalGraphSrv_ = nh.advertiseService("publish_global_graph", &CoreWrapper::publishGlobalGraphCallback, this); - publishLocalGraphSrv_ = nh.advertiseService("publish_local_graph", &CoreWrapper::publishLocalGraphCallback, this); - + getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize); @@ -616,33 +613,12 @@ bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Emp return true; } -bool CoreWrapper::publishGlobalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap::Response& rep) { - publishMapData(true); - return true; -} - -bool CoreWrapper::publishLocalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - publishMapData(false); - return true; -} - -bool CoreWrapper::publishGlobalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - publishGraph(true); - return true; -} - -bool CoreWrapper::publishLocalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - publishGraph(false); - return true; -} - -void CoreWrapper::publishMapData(bool global) -{ - ROS_INFO("rtabmap: Publishing map data (global=%s)...", global?"true":"false"); + ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", + req.global?"true":"false", + req.optimized?"true":"false", + req.graphOnly?"true":"false"); std::map > images; std::map > depths; std::map > depths2d; @@ -651,334 +627,290 @@ void CoreWrapper::publishMapData(bool global) std::map poses; std::multimap constraints; - if(mapData_.getNumSubscribers()) + if(req.graphOnly) { - rtabmap::MapDataPtr msg(new rtabmap::MapData); - - rtabmap_.get3DMap(images, + rtabmap_.getGraph( + poses, + constraints, + req.optimized, + req.global); + } + else + { + rtabmap_.get3DMap( + images, depths, depths2d, depthConstants, localTransforms, poses, constraints, - true, - global); - - int i=0; - - msg->imageIDs.resize(images.size()); - msg->images.resize(images.size()); - i=0; - for(std::map >::iterator iter = images.begin(); iter!=images.end(); ++iter) - { - msg->imageIDs[i] = iter->first; - msg->images[i].bytes = iter->second; - ++i; - } - - msg->depthIDs.resize(depths.size()); - msg->depths.resize(depths.size()); - i=0; - for(std::map >::iterator iter = depths.begin(); iter!=depths.end(); ++iter) - { - msg->depthIDs[i] = iter->first; - msg->depths[i].bytes = iter->second; - ++i; - } - - msg->depth2DIDs.resize(depths2d.size()); - msg->depth2Ds.resize(depths2d.size()); - i=0; - for(std::map >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter) - { - msg->depth2DIDs[i] = iter->first; - msg->depth2Ds[i].bytes = iter->second; - ++i; - } - - msg->depthConstantIDs.resize(depthConstants.size()); - msg->depthConstants.resize(depthConstants.size()); - i=0; - for(std::map::iterator iter = depthConstants.begin(); iter!=depthConstants.end(); ++iter) - { - msg->depthConstantIDs[i] = iter->first; - msg->depthConstants[i] = iter->second; - ++i; - } - - msg->localTransformIDs.resize(localTransforms.size()); - msg->localTransforms.resize(localTransforms.size()); - i=0; - for(std::map::iterator iter = localTransforms.begin(); iter!=localTransforms.end(); ++iter) - { - msg->localTransformIDs[i] = iter->first; - transformToGeometryMsg(iter->second, msg->localTransforms[i]); - ++i; - } - - msg->poseIDs.resize(poses.size()); - msg->poses.resize(poses.size()); - i=0; - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - msg->poseIDs[i] = iter->first; - transformToPoseMsg(iter->second, msg->poses[i]); - ++i; - } - - msg->constraintFromIDs.resize(constraints.size()); - msg->constraintToIDs.resize(constraints.size()); - msg->constraintTypes.resize(constraints.size()); - msg->constraints.resize(constraints.size()); - i=0; - for(std::multimap::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter) - { - msg->constraintFromIDs[i] = iter->first; - msg->constraintToIDs[i] = iter->second.to(); - msg->constraintTypes[i] = iter->second.type(); - transformToGeometryMsg(iter->second.transform(), msg->constraints[i]); - ++i; - } - - msg->header.stamp = ros::Time::now(); - mapData_.publish(msg); + req.optimized, + req.global); } -} -void CoreWrapper::publishGraph(bool global) -{ - ROS_INFO("rtabmap: Publishing graph (global=%s)...", global?"true":"false"); - std::map poses; - std::multimap constraints; - if(mapData_.getNumSubscribers()) + int i=0; + + rep.data.imageIDs.resize(images.size()); + rep.data.images.resize(images.size()); + i=0; + for(std::map >::iterator iter = images.begin(); iter!=images.end(); ++iter) { - rtabmap::MapDataPtr msg(new rtabmap::MapData); - - std::map poses; - std::multimap constraints; - - rtabmap_.getGraph(poses, - constraints, - true, - global); - - int i=0; - msg->poseIDs.resize(poses.size()); - msg->poses.resize(poses.size()); - i=0; - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - msg->poseIDs[i] = iter->first; - transformToPoseMsg(iter->second, msg->poses[i]); - ++i; - } - - msg->constraintFromIDs.resize(constraints.size()); - msg->constraintToIDs.resize(constraints.size()); - msg->constraintTypes.resize(constraints.size()); - msg->constraints.resize(constraints.size()); - i=0; - for(std::multimap::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter) - { - msg->constraintFromIDs[i] = iter->first; - msg->constraintToIDs[i] = iter->second.to(); - msg->constraintTypes[i] = iter->second.type(); - transformToGeometryMsg(iter->second.transform(), msg->constraints[i]); - ++i; - } - - msg->header.stamp = ros::Time::now(); - mapData_.publish(msg); + rep.data.imageIDs[i] = iter->first; + rep.data.images[i].bytes = iter->second; + ++i; } + + rep.data.depthIDs.resize(depths.size()); + rep.data.depths.resize(depths.size()); + i=0; + for(std::map >::iterator iter = depths.begin(); iter!=depths.end(); ++iter) + { + rep.data.depthIDs[i] = iter->first; + rep.data.depths[i].bytes = iter->second; + ++i; + } + + rep.data.depth2DIDs.resize(depths2d.size()); + rep.data.depth2Ds.resize(depths2d.size()); + i=0; + for(std::map >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter) + { + rep.data.depth2DIDs[i] = iter->first; + rep.data.depth2Ds[i].bytes = iter->second; + ++i; + } + + rep.data.depthConstantIDs.resize(depthConstants.size()); + rep.data.depthConstants.resize(depthConstants.size()); + i=0; + for(std::map::iterator iter = depthConstants.begin(); iter!=depthConstants.end(); ++iter) + { + rep.data.depthConstantIDs[i] = iter->first; + rep.data.depthConstants[i] = iter->second; + ++i; + } + + rep.data.localTransformIDs.resize(localTransforms.size()); + rep.data.localTransforms.resize(localTransforms.size()); + i=0; + for(std::map::iterator iter = localTransforms.begin(); iter!=localTransforms.end(); ++iter) + { + rep.data.localTransformIDs[i] = iter->first; + transformToGeometryMsg(iter->second, rep.data.localTransforms[i]); + ++i; + } + + rep.data.poseIDs.resize(poses.size()); + rep.data.poses.resize(poses.size()); + i=0; + for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + rep.data.poseIDs[i] = iter->first; + transformToPoseMsg(iter->second, rep.data.poses[i]); + ++i; + } + + rep.data.constraintFromIDs.resize(constraints.size()); + rep.data.constraintToIDs.resize(constraints.size()); + rep.data.constraintTypes.resize(constraints.size()); + rep.data.constraints.resize(constraints.size()); + i=0; + for(std::multimap::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter) + { + rep.data.constraintFromIDs[i] = iter->first; + rep.data.constraintToIDs[i] = iter->second.to(); + rep.data.constraintTypes[i] = iter->second.type(); + transformToGeometryMsg(iter->second.transform(), rep.data.constraints[i]); + ++i; + } + + rep.data.header.stamp = ros::Time::now(); + rep.data.header.frame_id = mapFrameId_; + + return true; } void CoreWrapper::publishStats(const Statistics & stats) { - if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers()) + ros::Time timeNow = ros::Time::now(); + if(infoPub_.getNumSubscribers()) { - if(infoPub_.getNumSubscribers()) + //ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); + rtabmap::InfoPtr msg(new rtabmap::Info); + msg->header.stamp = timeNow; + msg->header.frame_id = mapFrameId_; + + msg->refId = stats.refImageId(); + msg->loopClosureId = stats.loopClosureId(); + msg->localLoopClosureId = stats.localLoopClosureId(); + + transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection); + transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); + transformToPoseMsg(stats.currentPose(), msg->currentPose); + + infoPub_.publish(msg); + } + + if(infoPubEx_.getNumSubscribers()) + { + //ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId()); + rtabmap::InfoExPtr msg(new rtabmap::InfoEx); + msg->header.stamp = timeNow; + msg->header.frame_id = mapFrameId_; + + msg->refId = stats.refImageId(); + msg->loopClosureId = stats.loopClosureId(); + msg->localLoopClosureId = stats.localLoopClosureId(); + + transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection); + transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); + transformToPoseMsg(stats.currentPose(), msg->currentPose); + + // Detailed info + if(stats.extended()) { - //ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); - rtabmap::InfoPtr msg(new rtabmap::Info); - msg->header.stamp = ros::Time::now(); - msg->header.frame_id = mapFrameId_; + //Posterior, likelihood, childCount + msg->posteriorKeys = uKeys(stats.posterior()); + msg->posteriorValues = uValues(stats.posterior()); + msg->likelihoodKeys = uKeys(stats.likelihood()); + msg->likelihoodValues = uValues(stats.likelihood()); + msg->rawLikelihoodKeys = uKeys(stats.rawLikelihood()); + msg->rawLikelihoodValues = uValues(stats.rawLikelihood()); + msg->weightsKeys = uKeys(stats.weights()); + msg->weightsValues = uValues(stats.weights()); - msg->refId = stats.refImageId(); - msg->loopClosureId = stats.loopClosureId(); - msg->localLoopClosureId = stats.localLoopClosureId(); - - msg->nodeIds.resize(stats.poses().size()); - msg->nodePoses.resize(stats.poses().size()); - int i=0; - for(std::map::const_iterator iter = stats.poses().begin(); - iter!=stats.poses().end(); - ++iter) + //Features stuff... + msg->refWordsKeys = uKeys(stats.refWords()); + msg->refWordsValues = std::vector(stats.refWords().size()); + int index = 0; + for(std::multimap::const_iterator i=stats.refWords().begin(); + i!=stats.refWords().end(); + ++i) { - msg->nodeIds[i] = iter->first; - transformToPoseMsg(iter->second, msg->nodePoses[i]); - ++i; + msg->refWordsValues.at(index).angle = i->second.angle; + msg->refWordsValues.at(index).response = i->second.response; + msg->refWordsValues.at(index).ptx = i->second.pt.x; + msg->refWordsValues.at(index).pty = i->second.pt.y; + msg->refWordsValues.at(index).size = i->second.size; + msg->refWordsValues.at(index).octave = i->second.octave; + msg->refWordsValues.at(index).class_id = i->second.class_id; + ++index; } - transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection); - transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); - transformToPoseMsg(stats.currentPose(), msg->currentPose); + msg->loopWordsKeys = uKeys(stats.loopWords()); + msg->loopWordsValues = std::vector(stats.loopWords().size()); + index = 0; + for(std::multimap::const_iterator i=stats.loopWords().begin(); + i!=stats.loopWords().end(); + ++i) + { + msg->loopWordsValues.at(index).angle = i->second.angle; + msg->loopWordsValues.at(index).response = i->second.response; + msg->loopWordsValues.at(index).ptx = i->second.pt.x; + msg->loopWordsValues.at(index).pty = i->second.pt.y; + msg->loopWordsValues.at(index).size = i->second.size; + msg->loopWordsValues.at(index).octave = i->second.octave; + msg->loopWordsValues.at(index).class_id = i->second.class_id; + ++index; + } - infoPub_.publish(msg); + // Statistics data + msg->statsKeys = uKeys(stats.data()); + msg->statsValues = uValues(stats.data()); + } + infoPubEx_.publish(msg); + } + + if(mapData_.getNumSubscribers()) + { + //RGB-D SLAM data + rtabmap::MapDataPtr msg(new rtabmap::MapData); + msg->header.stamp = timeNow; + msg->header.frame_id = mapFrameId_; + + msg->mapIDs = uKeys(stats.getMapIds()); + msg->maps = uValues(stats.getMapIds()); + + msg->poseIDs.resize(stats.poses().size()); + msg->poses.resize(stats.poses().size()); + int index = 0; + for(std::map::const_iterator iter = stats.poses().begin(); + iter!=stats.poses().end(); + ++iter) + { + msg->poseIDs[index] = iter->first; + transformToPoseMsg(iter->second, msg->poses[index]); + ++index; } - if(infoPubEx_.getNumSubscribers()) + msg->constraintFromIDs.resize(stats.constraints().size()); + msg->constraintToIDs.resize(stats.constraints().size()); + msg->constraintTypes.resize(stats.constraints().size()); + msg->constraints.resize(stats.constraints().size()); + index=0; + for(std::multimap::const_iterator iter = stats.constraints().begin(); iter!=stats.constraints().end(); ++iter) { - //ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId()); - rtabmap::InfoExPtr msg(new rtabmap::InfoEx); - msg->header.stamp = ros::Time::now(); - msg->header.frame_id = mapFrameId_; - - msg->refId = stats.refImageId(); - msg->loopClosureId = stats.loopClosureId(); - msg->localLoopClosureId = stats.localLoopClosureId(); - - msg->data.header = msg->header; - msg->data.poseIDs.resize(stats.poses().size()); - msg->data.poses.resize(stats.poses().size()); - int i=0; - for(std::map::const_iterator iter = stats.poses().begin(); - iter!=stats.poses().end(); - ++iter) - { - msg->data.poseIDs[i] = iter->first; - transformToPoseMsg(iter->second, msg->data.poses[i]); - ++i; - } - - msg->data.constraintFromIDs.resize(stats.constraints().size()); - msg->data.constraintToIDs.resize(stats.constraints().size()); - msg->data.constraintTypes.resize(stats.constraints().size()); - msg->data.constraints.resize(stats.constraints().size()); - i=0; - for(std::multimap::const_iterator iter = stats.constraints().begin(); iter!=stats.constraints().end(); ++iter) - { - msg->data.constraintFromIDs[i] = iter->first; - msg->data.constraintToIDs[i] = iter->second.to(); - msg->data.constraintTypes[i] = iter->second.type(); - transformToGeometryMsg(iter->second.transform(), msg->data.constraints[i]); - ++i; - } - - transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection); - transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); - transformToPoseMsg(stats.currentPose(), msg->currentPose); - - // Detailed info - if(stats.extended()) - { - //Posterior, likelihood, childCount - msg->posteriorKeys = uKeys(stats.posterior()); - msg->posteriorValues = uValues(stats.posterior()); - msg->likelihoodKeys = uKeys(stats.likelihood()); - msg->likelihoodValues = uValues(stats.likelihood()); - msg->rawLikelihoodKeys = uKeys(stats.rawLikelihood()); - msg->rawLikelihoodValues = uValues(stats.rawLikelihood()); - msg->weightsKeys = uKeys(stats.weights()); - msg->weightsValues = uValues(stats.weights()); - - //Features stuff... - msg->refWordsKeys = uKeys(stats.refWords()); - msg->refWordsValues = std::vector(stats.refWords().size()); - int index = 0; - for(std::multimap::const_iterator i=stats.refWords().begin(); - i!=stats.refWords().end(); - ++i) - { - msg->refWordsValues.at(index).angle = i->second.angle; - msg->refWordsValues.at(index).response = i->second.response; - msg->refWordsValues.at(index).ptx = i->second.pt.x; - msg->refWordsValues.at(index).pty = i->second.pt.y; - msg->refWordsValues.at(index).size = i->second.size; - msg->refWordsValues.at(index).octave = i->second.octave; - msg->refWordsValues.at(index).class_id = i->second.class_id; - ++index; - } - - msg->loopWordsKeys = uKeys(stats.loopWords()); - msg->loopWordsValues = std::vector(stats.loopWords().size()); - index = 0; - for(std::multimap::const_iterator i=stats.loopWords().begin(); - i!=stats.loopWords().end(); - ++i) - { - msg->loopWordsValues.at(index).angle = i->second.angle; - msg->loopWordsValues.at(index).response = i->second.response; - msg->loopWordsValues.at(index).ptx = i->second.pt.x; - msg->loopWordsValues.at(index).pty = i->second.pt.y; - msg->loopWordsValues.at(index).size = i->second.size; - msg->loopWordsValues.at(index).octave = i->second.octave; - msg->loopWordsValues.at(index).class_id = i->second.class_id; - ++index; - } - - // Statistics data - msg->statsKeys = uKeys(stats.data()); - msg->statsValues = uValues(stats.data()); - - msg->data.mapIDs = uKeys(stats.getMapIds()); - msg->data.maps = uValues(stats.getMapIds()); - - //RGB-D SLAM data - msg->data.imageIDs.resize(stats.getImages().size()); - msg->data.images.resize(stats.getImages().size()); - index = 0; - for(std::map >::const_iterator i=stats.getImages().begin(); - i!=stats.getImages().end(); - ++i) - { - msg->data.imageIDs[index] = i->first; - msg->data.images[index].bytes = i->second; - ++index; - } - - msg->data.depthIDs.resize(stats.getDepths().size()); - msg->data.depths.resize(stats.getDepths().size()); - index = 0; - for(std::map >::const_iterator i=stats.getDepths().begin(); - i!=stats.getDepths().end(); - ++i) - { - msg->data.depthIDs[index] = i->first; - msg->data.depths[index].bytes = i->second; - ++index; - } - - msg->data.depth2DIDs.resize(stats.getDepth2ds().size()); - msg->data.depth2Ds.resize(stats.getDepth2ds().size()); - index = 0; - for(std::map >::const_iterator i=stats.getDepth2ds().begin(); - i!=stats.getDepth2ds().end(); - ++i) - { - msg->data.depth2DIDs[index] = i->first; - msg->data.depth2Ds[index].bytes = i->second; - ++index; - } - - msg->data.localTransformIDs.resize(stats.getLocalTransforms().size()); - msg->data.localTransforms.resize(stats.getLocalTransforms().size()); - index = 0; - for(std::map::const_iterator i=stats.getLocalTransforms().begin(); - i!=stats.getLocalTransforms().end(); - ++i) - { - msg->data.localTransformIDs[index] = i->first; - transformToGeometryMsg(i->second, msg->data.localTransforms[index]); - ++index; - } - - msg->data.depthConstantIDs = uKeys(stats.getDepthConstants()); - msg->data.depthConstants = uValues(stats.getDepthConstants()); - } - infoPubEx_.publish(msg); + msg->constraintFromIDs[index] = iter->first; + msg->constraintToIDs[index] = iter->second.to(); + msg->constraintTypes[index] = iter->second.type(); + transformToGeometryMsg(iter->second.transform(), msg->constraints[index]); + ++index; } + + msg->imageIDs.resize(stats.getImages().size()); + msg->images.resize(stats.getImages().size()); + index = 0; + for(std::map >::const_iterator i=stats.getImages().begin(); + i!=stats.getImages().end(); + ++i) + { + msg->imageIDs[index] = i->first; + msg->images[index].bytes = i->second; + ++index; + } + + msg->depthIDs.resize(stats.getDepths().size()); + msg->depths.resize(stats.getDepths().size()); + index = 0; + for(std::map >::const_iterator i=stats.getDepths().begin(); + i!=stats.getDepths().end(); + ++i) + { + msg->depthIDs[index] = i->first; + msg->depths[index].bytes = i->second; + ++index; + } + + msg->depth2DIDs.resize(stats.getDepth2ds().size()); + msg->depth2Ds.resize(stats.getDepth2ds().size()); + index = 0; + for(std::map >::const_iterator i=stats.getDepth2ds().begin(); + i!=stats.getDepth2ds().end(); + ++i) + { + msg->depth2DIDs[index] = i->first; + msg->depth2Ds[index].bytes = i->second; + ++index; + } + + msg->localTransformIDs.resize(stats.getLocalTransforms().size()); + msg->localTransforms.resize(stats.getLocalTransforms().size()); + index = 0; + for(std::map::const_iterator i=stats.getLocalTransforms().begin(); + i!=stats.getLocalTransforms().end(); + ++i) + { + msg->localTransformIDs[index] = i->first; + transformToGeometryMsg(i->second, msg->localTransforms[index]); + ++index; + } + + msg->depthConstantIDs = uKeys(stats.getDepthConstants()); + msg->depthConstants = uValues(stats.getDepthConstants()); + + mapData_.publish(msg); } } diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 24a8da7d..37267426 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -30,6 +30,8 @@ #include #include +#include "rtabmap/GetMap.h" + #include #include #include @@ -74,13 +76,7 @@ private: bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool publishGlobalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool publishLocalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool publishGlobalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool publishLocalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - - void publishMapData(bool global); - void publishGraph(bool global); + bool getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap::Response& rep); rtabmap::ParametersMap loadParameters(const std::string & configFile); void saveParameters(const std::string & configFile); @@ -141,10 +137,7 @@ private: ros::ServiceServer pauseSrv_; ros::ServiceServer resumeSrv_; ros::ServiceServer triggerNewMapSrv_; - ros::ServiceServer publishGlobalMapDataSrv_; - ros::ServiceServer publishLocalMapDataSrv_; - ros::ServiceServer publishGlobalGraphSrv_; - ros::ServiceServer publishLocalGraphSrv_; + ros::ServiceServer getMapDataSrv_; boost::thread* transformThread_; diff --git a/src/GridMapAssemblerNode.cpp b/src/GridMapAssemblerNode.cpp index 2225fc2f..195b787f 100644 --- a/src/GridMapAssemblerNode.cpp +++ b/src/GridMapAssemblerNode.cpp @@ -57,32 +57,31 @@ public: UASSERT(gridCellSize_ > 0.0); ros::NodeHandle nh; - infoExTopic_ = nh.subscribe("infoEx", 1, &GridMapAssembler::infoExReceivedCallback, this); mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this); gridMap_ = nh.advertise("grid_map", 1); - getMapService_ = nh.advertiseService("get_map", &GridMapAssembler::getMapCallback, this); + getMapService_ = nh.advertiseService("get_grid_map", &GridMapAssembler::getGridMapCallback, this); } ~GridMapAssembler() { } - void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) + void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) { - for(unsigned int i=0; idata.depth2DIDs.size() && idata.depth2Ds.size(); ++i) + for(unsigned int i=0; idepth2DIDs.size() && idepth2Ds.size(); ++i) { - if(!uContains(scans_, msg->data.depth2DIDs[i])) + if(!uContains(scans_, msg->depth2DIDs[i])) { - cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes); - scans_.insert(std::make_pair(msg->data.depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); + cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); + scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); } } std::map poses; - for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) { - poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i]))); + poses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i]))); } if(filterRadius_ > 0.0 && filterAngle_ > 0.0) @@ -124,28 +123,7 @@ public: } } - void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) - { - std::map > depths2d; - - if(msg->depth2DIDs.size() != msg->depth2Ds.size()) - { - ROS_WARN("grid_map_assembler: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!", - (int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size()); - } - - // fill maps - for(unsigned int i=0; idepth2DIDs.size() && i < msg->depth2Ds.size(); ++i) - { - if(!uContains(scans_, msg->depth2DIDs[i])) - { - cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); - scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); - } - } - } - - bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) + bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { if(map_.data.size()) { @@ -161,7 +139,6 @@ private: double filterRadius_; double filterAngle_; - ros::Subscriber infoExTopic_; ros::Subscriber mapDataTopic_; ros::Publisher gridMap_; diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 880103d7..14a7e86c 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -26,6 +26,7 @@ #include #include "rtabmap/MsgConversion.h" +#include "rtabmap/GetMap.h" #include "PreferencesDialogROS.h" @@ -86,8 +87,10 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : UEventsManager::addHandler(this); UEventsManager::addHandler(mainWindow_); - infoExTopic_ = nh.subscribe("infoEx", 1, &GuiWrapper::infoExReceivedCallback, this); - mapDataTopic_ = nh.subscribe("mapData", 1, &GuiWrapper::mapDataReceivedCallback, this); + infoExTopic_.subscribe(nh, "infoEx", 1); + mapDataTopic_.subscribe(nh, "mapData", 1); + infoMapSync_ = new message_filters::Synchronizer(MyInfoMapSyncPolicy(queueSize), infoExTopic_, mapDataTopic_); + infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2)); } GuiWrapper::~GuiWrapper() @@ -101,7 +104,9 @@ int GuiWrapper::exec() return app_->exec(); } -void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) +void GuiWrapper::infoMapCallback( + const rtabmap::InfoExConstPtr & infoMsg, + const rtabmap::MapDataConstPtr & mapMsg) { //ROS_INFO("rtabmapviz: RTAB-Map info ex received!"); @@ -110,134 +115,134 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) stat.setExtended(true); // Extended - stat.setRefImageId(msg->refId); - stat.setLoopClosureId(msg->loopClosureId); - stat.setLocalLoopClosureId(msg->localLoopClosureId); + stat.setRefImageId(infoMsg->refId); + stat.setLoopClosureId(infoMsg->loopClosureId); + stat.setLocalLoopClosureId(infoMsg->localLoopClosureId); //Posterior, likelihood, childCount std::map mapIntFloat; - for(unsigned int i=0; iposteriorKeys.size() && iposteriorValues.size(); ++i) + for(unsigned int i=0; iposteriorKeys.size() && iposteriorValues.size(); ++i) { - mapIntFloat.insert(std::pair(msg->posteriorKeys.at(i), msg->posteriorValues.at(i))); + mapIntFloat.insert(std::pair(infoMsg->posteriorKeys.at(i), infoMsg->posteriorValues.at(i))); } stat.setPosterior(mapIntFloat); mapIntFloat.clear(); - for(unsigned int i=0; ilikelihoodKeys.size() && ilikelihoodValues.size(); ++i) + for(unsigned int i=0; ilikelihoodKeys.size() && ilikelihoodValues.size(); ++i) { - mapIntFloat.insert(std::pair(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i))); + mapIntFloat.insert(std::pair(infoMsg->likelihoodKeys.at(i), infoMsg->likelihoodValues.at(i))); } stat.setLikelihood(mapIntFloat); mapIntFloat.clear(); - for(unsigned int i=0; irawLikelihoodKeys.size() && irawLikelihoodValues.size(); ++i) + for(unsigned int i=0; irawLikelihoodKeys.size() && irawLikelihoodValues.size(); ++i) { - mapIntFloat.insert(std::pair(msg->rawLikelihoodKeys.at(i), msg->rawLikelihoodValues.at(i))); + mapIntFloat.insert(std::pair(infoMsg->rawLikelihoodKeys.at(i), infoMsg->rawLikelihoodValues.at(i))); } stat.setRawLikelihood(mapIntFloat); std::map mapIntInt; - for(unsigned int i=0; iweightsKeys.size() && iweightsValues.size(); ++i) + for(unsigned int i=0; iweightsKeys.size() && iweightsValues.size(); ++i) { - mapIntInt.insert(std::pair(msg->weightsKeys.at(i), msg->weightsValues.at(i))); + mapIntInt.insert(std::pair(infoMsg->weightsKeys.at(i), infoMsg->weightsValues.at(i))); } stat.setWeights(mapIntInt); //SURF stuff... std::multimap mapIntKeypoint; - for(unsigned int i=0; irefWordsKeys.size() && irefWordsValues.size(); ++i) + for(unsigned int i=0; irefWordsKeys.size() && irefWordsValues.size(); ++i) { cv::KeyPoint pt; - pt.angle = msg->refWordsValues.at(i).angle; - pt.response = msg->refWordsValues.at(i).response; - pt.pt.x = msg->refWordsValues.at(i).ptx; - pt.pt.y = msg->refWordsValues.at(i).pty; - pt.size = msg->refWordsValues.at(i).size; - mapIntKeypoint.insert(std::pair(msg->refWordsKeys.at(i), pt)); + pt.angle = infoMsg->refWordsValues.at(i).angle; + pt.response = infoMsg->refWordsValues.at(i).response; + pt.pt.x = infoMsg->refWordsValues.at(i).ptx; + pt.pt.y = infoMsg->refWordsValues.at(i).pty; + pt.size = infoMsg->refWordsValues.at(i).size; + mapIntKeypoint.insert(std::pair(infoMsg->refWordsKeys.at(i), pt)); } stat.setRefWords(mapIntKeypoint); mapIntKeypoint.clear(); - for(unsigned int i=0; iloopWordsKeys.size() && iloopWordsValues.size(); ++i) + for(unsigned int i=0; iloopWordsKeys.size() && iloopWordsValues.size(); ++i) { cv::KeyPoint pt; - pt.angle = msg->loopWordsValues.at(i).angle; - pt.response = msg->loopWordsValues.at(i).response; - pt.pt.x = msg->loopWordsValues.at(i).ptx; - pt.pt.y = msg->loopWordsValues.at(i).pty; - pt.size = msg->loopWordsValues.at(i).size; - mapIntKeypoint.insert(std::pair(msg->loopWordsKeys.at(i), pt)); + pt.angle = infoMsg->loopWordsValues.at(i).angle; + pt.response = infoMsg->loopWordsValues.at(i).response; + pt.pt.x = infoMsg->loopWordsValues.at(i).ptx; + pt.pt.y = infoMsg->loopWordsValues.at(i).pty; + pt.size = infoMsg->loopWordsValues.at(i).size; + mapIntKeypoint.insert(std::pair(infoMsg->loopWordsKeys.at(i), pt)); } stat.setLoopWords(mapIntKeypoint); // Statistics data - for(unsigned int i=0; istatsKeys.size() && istatsValues.size(); i++) + for(unsigned int i=0; istatsKeys.size() && istatsValues.size(); i++) { - stat.addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i)); + stat.addStatistic(infoMsg->statsKeys.at(i), infoMsg->statsValues.at(i)); } //RGB-D SLAM data - stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection)); - stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform)); - stat.setCurrentPose(transformFromPoseMsg(msg->currentPose)); + stat.setMapCorrection(transformFromGeometryMsg(infoMsg->mapCorrection)); + stat.setLoopClosureTransform(transformFromGeometryMsg(infoMsg->loopClosureTransform)); + stat.setCurrentPose(transformFromPoseMsg(infoMsg->currentPose)); std::map > images; - for(unsigned int i=0; idata.imageIDs.size() && idata.images.size(); ++i) + for(unsigned int i=0; iimageIDs.size() && iimages.size(); ++i) { - images.insert(std::make_pair(msg->data.imageIDs[i], msg->data.images[i].bytes)); + images.insert(std::make_pair(mapMsg->imageIDs[i], mapMsg->images[i].bytes)); } stat.setImages(images); std::map > depths; - for(unsigned int i=0; idata.depthIDs.size() && idata.depths.size(); ++i) + for(unsigned int i=0; idepthIDs.size() && idepths.size(); ++i) { - depths.insert(std::make_pair(msg->data.depthIDs[i], msg->data.depths[i].bytes)); + depths.insert(std::make_pair(mapMsg->depthIDs[i], mapMsg->depths[i].bytes)); } stat.setDepths(depths); std::map > depth2ds; - for(unsigned int i=0; idata.depth2DIDs.size() && idata.depth2Ds.size(); ++i) + for(unsigned int i=0; idepth2DIDs.size() && idepth2Ds.size(); ++i) { - depth2ds.insert(std::make_pair(msg->data.depth2DIDs[i], msg->data.depth2Ds[i].bytes)); + depth2ds.insert(std::make_pair(mapMsg->depth2DIDs[i], mapMsg->depth2Ds[i].bytes)); } stat.setDepth2ds(depth2ds); std::map depthConstants; - for(unsigned int i=0; idata.depthConstantIDs.size() && idata.depthConstants.size(); ++i) + for(unsigned int i=0; idepthConstantIDs.size() && idepthConstants.size(); ++i) { - depthConstants.insert(std::make_pair(msg->data.depthConstantIDs[i], msg->data.depthConstants[i])); + depthConstants.insert(std::make_pair(mapMsg->depthConstantIDs[i], mapMsg->depthConstants[i])); } stat.setDepthConstants(depthConstants); std::map localTransforms; - for(unsigned int i=0; idata.localTransformIDs.size() && idata.localTransforms.size(); ++i) + for(unsigned int i=0; ilocalTransformIDs.size() && ilocalTransforms.size(); ++i) { - localTransforms.insert(std::make_pair(msg->data.localTransformIDs[i], transformFromGeometryMsg(msg->data.localTransforms[i]))); + localTransforms.insert(std::make_pair(mapMsg->localTransformIDs[i], transformFromGeometryMsg(mapMsg->localTransforms[i]))); } stat.setLocalTransforms(localTransforms); std::map poses; - for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) { - poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i]))); + poses.insert(std::make_pair(mapMsg->poseIDs[i], transformFromPoseMsg(mapMsg->poses[i]))); } stat.setPoses(poses); std::multimap constraints; - for(unsigned int i=0; idata.constraintFromIDs.size() && idata.constraintToIDs.size() && idata.constraintTypes.size() && i < msg->data.constraints.size(); ++i) + for(unsigned int i=0; iconstraintFromIDs.size() && iconstraintToIDs.size() && iconstraintTypes.size() && i < mapMsg->constraints.size(); ++i) { - Transform t = transformFromGeometryMsg(msg->data.constraints[i]); - constraints.insert(std::make_pair(msg->data.constraintFromIDs[i], Link(msg->data.constraintFromIDs[i], msg->data.constraintToIDs[i], t, (Link::Type)msg->data.constraintTypes[i]))); + Transform t = transformFromGeometryMsg(mapMsg->constraints[i]); + constraints.insert(std::make_pair(mapMsg->constraintFromIDs[i], Link(mapMsg->constraintFromIDs[i], mapMsg->constraintToIDs[i], t, (Link::Type)mapMsg->constraintTypes[i]))); } stat.setConstraints(constraints); std::map mapIds; - for(unsigned int i=0; idata.mapIDs.size() && idata.maps.size(); ++i) + for(unsigned int i=0; imapIDs.size() && imaps.size(); ++i) { - mapIds.insert(std::make_pair(msg->data.mapIDs[i], msg->data.maps[i])); + mapIds.insert(std::make_pair(mapMsg->mapIDs[i], mapMsg->maps[i])); } stat.setMapIds(mapIds); this->post(new RtabmapEvent(stat)); } -void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) +void GuiWrapper::processRequestedMap(const rtabmap::MapData & map) { std::map > images; std::map > depths; @@ -247,80 +252,80 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) std::map poses; std::multimap constraints; - if(msg->imageIDs.size() != msg->images.size()) + if(map.imageIDs.size() != map.images.size()) { ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!", - (int)msg->images.size(), (int)msg->imageIDs.size()); + (int)map.images.size(), (int)map.imageIDs.size()); } - if(msg->depthIDs.size() != msg->depths.size()) + if(map.depthIDs.size() != map.depths.size()) { ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!", - (int)msg->depths.size(), (int)msg->depthIDs.size()); + (int)map.depths.size(), (int)map.depthIDs.size()); } - if(msg->depth2DIDs.size() != msg->depth2Ds.size()) + if(map.depth2DIDs.size() != map.depth2Ds.size()) { ROS_WARN("rtabmapviz: receiving map... depths2D and IDs are not the same size (%d vs %d)!", - (int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size()); + (int)map.depth2Ds.size(), (int)map.depth2DIDs.size()); } - if(msg->depthConstantIDs.size() != msg->depthConstants.size()) + if(map.depthConstantIDs.size() != map.depthConstants.size()) { ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!", - (int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size()); + (int)map.depthConstants.size(), (int)map.depthConstantIDs.size()); } - if(msg->poseIDs.size() != msg->poses.size()) + if(map.poseIDs.size() != map.poses.size()) { ROS_WARN("rtabmapviz: receiving map... poses and IDs are not the same size (%d vs %d)!", - (int)msg->poses.size(), (int)msg->poseIDs.size()); + (int)map.poses.size(), (int)map.poseIDs.size()); } - if(msg->constraintFromIDs.size() != msg->constraints.size() || - msg->constraintToIDs.size() != msg->constraints.size() || - msg->constraintTypes.size() != msg->constraints.size()) + if(map.constraintFromIDs.size() != map.constraints.size() || + map.constraintToIDs.size() != map.constraints.size() || + map.constraintTypes.size() != map.constraints.size()) { ROS_WARN("rtabmapviz: receiving map... constraints and IDs are not the same size (%d vs %d vs %d vs %d)!", - (int)msg->constraints.size(), (int)msg->constraintFromIDs.size(), (int)msg->constraintToIDs.size(), (int)msg->constraintTypes.size()); + (int)map.constraints.size(), (int)map.constraintFromIDs.size(), (int)map.constraintToIDs.size(), (int)map.constraintTypes.size()); } - for(unsigned int i=0; iimageIDs.size() && i < msg->images.size(); ++i) + for(unsigned int i=0; iimageIDs[i], msg->images[i].bytes)); + images.insert(std::make_pair(map.imageIDs[i], map.images[i].bytes)); } - for(unsigned int i=0; idepthIDs.size() && i < msg->depths.size(); ++i) + for(unsigned int i=0; idepthIDs[i], msg->depths[i].bytes)); + depths.insert(std::make_pair(map.depthIDs[i], map.depths[i].bytes)); } - for(unsigned int i=0; idepth2DIDs.size() && i < msg->depth2Ds.size(); ++i) + for(unsigned int i=0; idepth2DIDs[i], msg->depth2Ds[i].bytes)); + depths2d.insert(std::make_pair(map.depth2DIDs[i], map.depth2Ds[i].bytes)); } - for(unsigned int i=0; idepthConstantIDs.size() && i < msg->depthConstants.size(); ++i) + for(unsigned int i=0; idepthConstantIDs[i], msg->depthConstants[i])); + depthConstants.insert(std::make_pair(map.depthConstantIDs[i], map.depthConstants[i])); } - for(unsigned int i=0; ilocalTransformIDs.size() && i < msg->localTransforms.size(); ++i) + for(unsigned int i=0; ilocalTransforms[i]); - localTransforms.insert(std::make_pair(msg->localTransformIDs[i], t)); + Transform t = transformFromGeometryMsg(map.localTransforms[i]); + localTransforms.insert(std::make_pair(map.localTransformIDs[i], t)); } - for(unsigned int i=0; iposeIDs.size() && i < msg->poses.size(); ++i) + for(unsigned int i=0; iposes[i]); - poses.insert(std::make_pair(msg->poseIDs[i], t)); + Transform t = transformFromPoseMsg(map.poses[i]); + poses.insert(std::make_pair(map.poseIDs[i], t)); } - for(unsigned int i=0; iconstraintFromIDs.size() && iconstraintToIDs.size() && iconstraintTypes.size() && i < msg->constraints.size(); ++i) + for(unsigned int i=0; iconstraints[i]); - constraints.insert(std::make_pair(msg->constraintFromIDs[i], Link(msg->constraintFromIDs[i], msg->constraintToIDs[i], t, (Link::Type)msg->constraintTypes[i]))); + Transform t = transformFromGeometryMsg(map.constraints[i]); + constraints.insert(std::make_pair(map.constraintFromIDs[i], Link(map.constraintFromIDs[i], map.constraintToIDs[i], t, (Link::Type)map.constraintTypes[i]))); } this->post(new RtabmapEvent3DMap(images, @@ -365,12 +370,12 @@ void GuiWrapper::handleEvent(UEvent * anEvent) } else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0) { - std_srvs::Empty srv; + std_srvs::Empty emptySrv; rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent; rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd(); if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory) { - if(!ros::service::call("reset", srv)) + if(!ros::service::call("reset", emptySrv)) { ROS_ERROR("Can't call \"reset\" service"); } @@ -387,10 +392,10 @@ void GuiWrapper::handleEvent(UEvent * anEvent) } // Pause visual_odometry - ros::service::call("pause_odom", srv); + ros::service::call("pause_odom", emptySrv); // Pause rtabmap - if(!ros::service::call("pause", srv)) + if(!ros::service::call("pause", emptySrv)) { ROS_ERROR("Can't call \"pause\" service"); } @@ -398,13 +403,13 @@ void GuiWrapper::handleEvent(UEvent * anEvent) else { // Resume rtabmap - if(!ros::service::call("resume", srv)) + if(!ros::service::call("resume", emptySrv)) { ROS_ERROR("Can't call \"resume\" service"); } // Pause visual_odometry - ros::service::call("resume_odom", srv); + ros::service::call("resume_odom", emptySrv); // Resume the camera if the rtabmap/camera node is used if(!cameraNodeName_.empty()) @@ -416,73 +421,28 @@ void GuiWrapper::handleEvent(UEvent * anEvent) } else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap) { - if(!ros::service::call("trigger_new_map", srv)) + if(!ros::service::call("trigger_new_map", emptySrv)) { ROS_ERROR("Can't call \"trigger_new_map\" service"); } } - else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapLocal) + else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapLocal || + cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal || + cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal || + cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal) { - if(mapDataTopic_.getNumPublishers()) + rtabmap::GetMap getMapSrv; + getMapSrv.request.global = cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal || cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal; + getMapSrv.request.optimized = cmdEvent->getInt(); + getMapSrv.request.graphOnly = cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal || cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal; + if(!ros::service::call("get_map", getMapSrv)) { - if(!ros::service::call("publish_local_map_data", srv)) - { - ROS_WARN("Can't call \"publish_local_map_data\" service"); - this->post(new RtabmapEvent3DMap(1)); // service error - } + ROS_WARN("Can't call \"get_map\" service"); + this->post(new RtabmapEvent3DMap(1)); // service error } else { - ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str()); - this->post(new RtabmapEvent3DMap(2)); // topic error - } - } - else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal) - { - if(mapDataTopic_.getNumPublishers()) - { - if(!ros::service::call("publish_global_map_data", srv)) - { - ROS_WARN("Can't call \"publish_global_map_data\" service"); - this->post(new RtabmapEvent3DMap(1)); // service error - } - } - else - { - ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str()); - this->post(new RtabmapEvent3DMap(2)); // topic error - } - } - else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal) - { - if(mapDataTopic_.getNumPublishers()) - { - if(!ros::service::call("publish_local_graph", srv)) - { - ROS_WARN("Can't call \"publish_local_graph\" service"); - this->post(new RtabmapEvent3DMap(1)); // service error - } - } - else - { - ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str()); - this->post(new RtabmapEvent3DMap(2)); // topic error - } - } - else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal) - { - if(mapDataTopic_.getNumPublishers()) - { - if(!ros::service::call("publish_global_graph", srv)) - { - ROS_WARN("Can't call \"publish_global_graph\" service"); - this->post(new RtabmapEvent3DMap(1)); // service error - } - } - else - { - ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str()); - this->post(new RtabmapEvent3DMap(2)); // topic error + processRequestedMap(getMapSrv.response.data); } } else diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h index a5cde398..648fd7bb 100644 --- a/src/GuiWrapper.h +++ b/src/GuiWrapper.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -48,8 +49,7 @@ protected: virtual void handleEvent(UEvent * anEvent); private: - void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg); - void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg); + void infoMapCallback(const rtabmap::InfoExConstPtr & infoMsg, const rtabmap::MapDataConstPtr & mapMsg); void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize); void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom @@ -66,9 +66,9 @@ private: const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void processRequestedMap(const rtabmap::MapData & map); + private: - ros::Subscriber infoExTopic_; - ros::Subscriber mapDataTopic_; QApplication * app_; rtabmap::MainWindow * mainWindow_; std::string cameraNodeName_; @@ -77,6 +77,9 @@ private: std::string frameId_; tf::TransformListener tfListener_; + message_filters::Subscriber infoExTopic_; + message_filters::Subscriber mapDataTopic_; + ros::Subscriber defaultSub_; // odometry only image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageDepthSub_; @@ -84,6 +87,11 @@ private: message_filters::Subscriber odomSub_; message_filters::Subscriber scanSub_; + typedef message_filters::sync_policies::ExactTime< + rtabmap::InfoEx, + rtabmap::MapData> MyInfoMapSyncPolicy; + message_filters::Synchronizer * infoMapSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, nav_msgs::Odometry, diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 77570676..d5cf1b75 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -53,7 +53,6 @@ public: pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); ros::NodeHandle nh; - infoExTopic_ = nh.subscribe("infoEx", 1, &MapAssembler::infoExReceivedCallback, this); mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); assembledMapClouds_ = nh.advertise("assembled_clouds", 1); @@ -64,40 +63,40 @@ public: { } - void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) + void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) { - for(unsigned int i=0; idata.localTransformIDs.size() && idata.localTransforms.size(); ++i) + for(unsigned int i=0; ilocalTransformIDs.size() && ilocalTransforms.size(); ++i) { - int id = msg->data.localTransformIDs[i]; + int id = msg->localTransformIDs[i]; if(!uContains(rgbClouds_, id)) { - rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]); + rtabmap::Transform localTransform = transformFromGeometryMsg(msg->localTransforms[i]); if(!localTransform.isNull()) { cv::Mat image, depth; float depthConstant = 0.0f; - for(unsigned int i=0; idata.imageIDs.size() && idata.images.size(); ++i) + for(unsigned int i=0; iimageIDs.size() && iimages.size(); ++i) { - if(msg->data.imageIDs[i] == id) + if(msg->imageIDs[i] == id) { - image = util3d::uncompressImage(msg->data.images[i].bytes); + image = util3d::uncompressImage(msg->images[i].bytes); break; } } - for(unsigned int i=0; idata.depthIDs.size() && idata.depths.size(); ++i) + for(unsigned int i=0; idepthIDs.size() && idepths.size(); ++i) { - if(msg->data.depthIDs[i] == id) + if(msg->depthIDs[i] == id) { - depth = util3d::uncompressImage(msg->data.depths[i].bytes); + depth = util3d::uncompressImage(msg->depths[i].bytes); break; } } - for(unsigned int i=0; idata.depthConstantIDs.size() && idata.depthConstants.size(); ++i) + for(unsigned int i=0; idepthConstantIDs.size() && idepthConstants.size(); ++i) { - if(msg->data.depthConstantIDs[i] == id) + if(msg->depthConstantIDs[i] == id) { - depthConstant = msg->data.depthConstants[i]; + depthConstant = msg->depthConstants[i]; break; } } @@ -124,11 +123,11 @@ public: } - for(unsigned int i=0; idata.depth2DIDs.size() && idata.depth2Ds.size(); ++i) + for(unsigned int i=0; idepth2DIDs.size() && idepth2Ds.size(); ++i) { - if(!uContains(scans_, msg->data.depth2DIDs[i])) + if(!uContains(scans_, msg->depth2DIDs[i])) { - cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes); + cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); if(!depth2d.empty()) { pcl::PointCloud::Ptr cloud = util3d::depth2DToPointCloud(depth2d); @@ -137,7 +136,7 @@ public: cloud = util3d::voxelize(cloud, scanVoxelSize_); } - scans_.insert(std::make_pair(msg->data.depth2DIDs[i], cloud)); + scans_.insert(std::make_pair(msg->depth2DIDs[i], cloud)); } } } @@ -148,11 +147,11 @@ public: // generate the assembled cloud! pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) { - Transform pose = transformFromPoseMsg(msg->data.poses[i]); + Transform pose = transformFromPoseMsg(msg->poses[i]); - std::map::Ptr >::iterator iter = rgbClouds_.find(msg->data.poseIDs[i]); + std::map::Ptr >::iterator iter = rgbClouds_.find(msg->poseIDs[i]); if(iter != rgbClouds_.end()) { pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, pose); @@ -180,11 +179,11 @@ public: // generate the assembled scan! pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) { - Transform pose = transformFromPoseMsg(msg->data.poses[i]); + Transform pose = transformFromPoseMsg(msg->poses[i]); - std::map::Ptr >::iterator iter = scans_.find(msg->data.poseIDs[i]); + std::map::Ptr >::iterator iter = scans_.find(msg->poseIDs[i]); if(iter != scans_.end()) { pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, pose); @@ -208,103 +207,12 @@ public: } } - void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) - { - std::map > images; - std::map > depths; - std::map > depths2d; - std::map depthConstants; - std::map localTransforms; - std::map poses; - - if(msg->imageIDs.size() != msg->images.size()) - { - ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!", - (int)msg->images.size(), (int)msg->imageIDs.size()); - } - - if(msg->depthIDs.size() != msg->depths.size()) - { - ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!", - (int)msg->depths.size(), (int)msg->depthIDs.size()); - } - - if(msg->depthConstantIDs.size() != msg->depthConstants.size()) - { - ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!", - (int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size()); - } - - if(msg->depth2DIDs.size() != msg->depth2Ds.size()) - { - ROS_WARN("rtabmapviz: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!", - (int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size()); - } - - // fill maps - for(unsigned int i=0; iimageIDs.size() && i < msg->images.size(); ++i) - { - if(!uContains(rgbClouds_, msg->imageIDs[i])) - { - images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes)); - } - } - - for(unsigned int i=0; idepthIDs.size() && i < msg->depths.size(); ++i) - { - if(!uContains(rgbClouds_, msg->depthIDs[i])) - { - depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes)); - } - } - - for(unsigned int i=0; idepthConstantIDs.size() && i < msg->depthConstants.size(); ++i) - { - if(!uContains(rgbClouds_, msg->depthConstantIDs[i])) - { - depthConstants.insert(std::make_pair(msg->depthConstantIDs[i], msg->depthConstants[i])); - } - } - - for(unsigned int i=0; ilocalTransformIDs.size() && i < msg->localTransforms.size(); ++i) - { - if(!uContains(rgbClouds_, msg->localTransformIDs[i])) - { - Transform t = transformFromGeometryMsg(msg->localTransforms[i]); - localTransforms.insert(std::make_pair(msg->localTransformIDs[i], t)); - } - } - - for(unsigned int i=0; idepth2DIDs.size() && i < msg->depth2Ds.size(); ++i) - { - if(!uContains(scans_, msg->depth2DIDs[i])) - { - cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); - scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); - } - } - - // create clouds - for(std::map >::iterator iter = images.begin(); iter!=images.end(); ++iter) - { - if(uContains(depths, iter->first) && uContains(depthConstants, iter->first) && uContains(localTransforms, iter->first)) - { - cv::Mat image = util3d::uncompressImage(iter->second); - cv::Mat depth = util3d::uncompressImage(depths.at(iter->first)); - float depthConstant = depthConstants.at(iter->first); - rtabmap::Transform localTransform = localTransforms.at(iter->first); - rgbClouds_.insert(std::make_pair(iter->first, util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_))); - } - } - } - private: int cloudDecimation_; double cloudMaxDepth_; double cloudVoxelSize_; double scanVoxelSize_; - ros::Subscriber infoExTopic_; ros::Subscriber mapDataTopic_; ros::Publisher assembledMapClouds_; diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index fbfa5d4f..ecc94ee9 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -154,51 +154,51 @@ void MapCloudDisplay::onInitialize() spinner_.start(); } -void MapCloudDisplay::processMessage( const rtabmap::InfoExConstPtr& msg ) +void MapCloudDisplay::processMessage( const rtabmap::MapDataConstPtr& msg ) { // Add new clouds... - for(unsigned int i=0; idata.localTransformIDs.size() && idata.localTransforms.size(); ++i) + for(unsigned int i=0; ilocalTransformIDs.size() && ilocalTransforms.size(); ++i) { - int id = msg->data.localTransformIDs[i]; + int id = msg->localTransformIDs[i]; if(cloud_infos_.find(id) == cloud_infos_.end()) { // Cloud not added to RVIZ, add it! - rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]); + rtabmap::Transform localTransform = transformFromGeometryMsg(msg->localTransforms[i]); if(!localTransform.isNull()) { cv::Mat image, depth; float depthConstant = 0.0f; rtabmap::Transform pose; - for(unsigned int i=0; idata.imageIDs.size() && idata.images.size(); ++i) + for(unsigned int i=0; iimageIDs.size() && iimages.size(); ++i) { - if(msg->data.imageIDs[i] == id) + if(msg->imageIDs[i] == id) { - image = util3d::uncompressImage(msg->data.images[i].bytes); + image = util3d::uncompressImage(msg->images[i].bytes); break; } } - for(unsigned int i=0; idata.depthIDs.size() && idata.depths.size(); ++i) + for(unsigned int i=0; idepthIDs.size() && idepths.size(); ++i) { - if(msg->data.depthIDs[i] == id) + if(msg->depthIDs[i] == id) { - depth = util3d::uncompressImage(msg->data.depths[i].bytes); + depth = util3d::uncompressImage(msg->depths[i].bytes); break; } } - for(unsigned int i=0; idata.depthConstantIDs.size() && idata.depthConstants.size(); ++i) + for(unsigned int i=0; idepthConstantIDs.size() && idepthConstants.size(); ++i) { - if(msg->data.depthConstantIDs[i] == id) + if(msg->depthConstantIDs[i] == id) { - depthConstant = msg->data.depthConstants[i]; + depthConstant = msg->depthConstants[i]; break; } } - for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) { - if(msg->data.poseIDs[i] == id) + if(msg->poseIDs[i] == id) { - pose = transformFromPoseMsg(msg->data.poses[i]); + pose = transformFromPoseMsg(msg->poses[i]); break; } } @@ -223,7 +223,7 @@ void MapCloudDisplay::processMessage( const rtabmap::InfoExConstPtr& msg ) sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); pcl::toROSMsg(*cloud, *cloudMsg); - cloudMsg->header = msg->data.header; + cloudMsg->header = msg->header; CloudInfoPtr info(new CloudInfo); info->message_ = cloudMsg; @@ -242,9 +242,9 @@ void MapCloudDisplay::processMessage( const rtabmap::InfoExConstPtr& msg ) // Update graph std::map poses; - for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) { - poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i]))); + poses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i]))); } { diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index 9e631f23..1305769c 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/src/rviz/MapCloudDisplay.h @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include @@ -41,7 +41,7 @@ class PointCloudCommon; * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b * all being 8 bits. */ -class MapCloudDisplay: public rviz::MessageFilterDisplay +class MapCloudDisplay: public rviz::MessageFilterDisplay { Q_OBJECT public: @@ -99,7 +99,7 @@ protected: virtual void onInitialize(); /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const rtabmap::InfoExConstPtr& cloud ); + virtual void processMessage( const rtabmap::MapDataConstPtr& cloud ); private: /** diff --git a/srv/GetMap.srv b/srv/GetMap.srv new file mode 100644 index 00000000..fb998c35 --- /dev/null +++ b/srv/GetMap.srv @@ -0,0 +1,7 @@ +#request +bool global +bool optimized +bool graphOnly +--- +#response +rtabmap/MapData data