i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-12 22:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros: Added rtabmap/GetMap service, removed MapData from InfoEx msg

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1459 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-06 19:58:15 +00:00
parent 39cff6d0cb
commit 4d557e6a50
14 changed files with 455 additions and 675 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+5 -5
View File
@@ -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(
msg/Info.msg
-4
View File
@@ -10,10 +10,6 @@ int32 refId
int32 loopClosureId
int32 localLoopClosureId
# std::map<int, Poses> poses;
int32[] nodeIds
geometry_msgs/Pose[] nodePoses
geometry_msgs/Transform mapCorrection
geometry_msgs/Transform loopClosureTransform
msg/InfoEx.msg
-3
View File
@@ -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
package.xml
+2
View File
@@ -10,6 +10,8 @@
<author email="matlabbe@gmail.com">Jane Doe</author>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>genmsg</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
plugin_description.xml
+1 -1
View File
@@ -3,7 +3,7 @@
type="rtabmap::MapCloudDisplay"
base_class_type="rviz::Display">
<description>
Displays graph point clouds from rtabmap/InfoEx messages.
Displays graph point clouds from rtabmap/MapData messages.
</description>
</class>
</library>
src/CoreWrapper.cpp
+264 -332
View File
@@ -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<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
@@ -651,334 +627,290 @@ void CoreWrapper::publishMapData(bool global)
std::map<int, Transform> poses;
std::multimap<int, Link> 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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, float>::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<int, Transform>::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<int, Transform>::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<int, Link>::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<int, Transform> poses;
std::multimap<int, Link> constraints;
if(mapData_.getNumSubscribers())
int i=0;
rep.data.imageIDs.resize(images.size());
rep.data.images.resize(images.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = images.begin(); iter!=images.end(); ++iter)
{
rtabmap::MapDataPtr msg(new rtabmap::MapData);
std::map<int, Transform> poses;
std::multimap<int, Link> 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<int, Transform>::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<int, Link>::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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, float>::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<int, Transform>::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<int, Transform>::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<int, Link>::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<int, Transform>::const_iterator iter = stats.poses().begin();
iter!=stats.poses().end();
++iter)
//Features stuff...
msg->refWordsKeys = uKeys(stats.refWords());
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stats.refWords().size());
int index = 0;
for(std::multimap<int, cv::KeyPoint>::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<rtabmap::KeyPoint>(stats.loopWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::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<int, Transform>::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<int, Link>::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<int, Transform>::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<int, Link>::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<rtabmap::KeyPoint>(stats.refWords().size());
int index = 0;
for(std::multimap<int, cv::KeyPoint>::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<rtabmap::KeyPoint>(stats.loopWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, Transform>::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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, std::vector<unsigned char> >::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<int, Transform>::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);
}
}
src/CoreWrapper.h
+4 -11
View File
@@ -30,6 +30,8 @@
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Rtabmap.h>
#include "rtabmap/GetMap.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
@@ -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_;
src/GridMapAssemblerNode.cpp
+9 -32
View File
@@ -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<nav_msgs::OccupancyGrid>("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; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
for(unsigned int i=0; i<msg->depth2DIDs.size() && i<msg->depth2Ds.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<int, Transform> poses;
for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.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<int, std::vector<unsigned char> > 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; i<msg->depth2DIDs.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_;
src/GuiWrapper.cpp
+106 -146
View File
@@ -26,6 +26,7 @@
#include <rtabmap/core/util3d.h>
#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>(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<int, float> mapIntFloat;
for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
for(unsigned int i=0; i<infoMsg->posteriorKeys.size() && i<infoMsg->posteriorValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
mapIntFloat.insert(std::pair<int, float>(infoMsg->posteriorKeys.at(i), infoMsg->posteriorValues.at(i)));
}
stat.setPosterior(mapIntFloat);
mapIntFloat.clear();
for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
for(unsigned int i=0; i<infoMsg->likelihoodKeys.size() && i<infoMsg->likelihoodValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
mapIntFloat.insert(std::pair<int, float>(infoMsg->likelihoodKeys.at(i), infoMsg->likelihoodValues.at(i)));
}
stat.setLikelihood(mapIntFloat);
mapIntFloat.clear();
for(unsigned int i=0; i<msg->rawLikelihoodKeys.size() && i<msg->rawLikelihoodValues.size(); ++i)
for(unsigned int i=0; i<infoMsg->rawLikelihoodKeys.size() && i<infoMsg->rawLikelihoodValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(msg->rawLikelihoodKeys.at(i), msg->rawLikelihoodValues.at(i)));
mapIntFloat.insert(std::pair<int, float>(infoMsg->rawLikelihoodKeys.at(i), infoMsg->rawLikelihoodValues.at(i)));
}
stat.setRawLikelihood(mapIntFloat);
std::map<int, int> mapIntInt;
for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
for(unsigned int i=0; i<infoMsg->weightsKeys.size() && i<infoMsg->weightsValues.size(); ++i)
{
mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
mapIntInt.insert(std::pair<int, int>(infoMsg->weightsKeys.at(i), infoMsg->weightsValues.at(i)));
}
stat.setWeights(mapIntInt);
//SURF stuff...
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); ++i)
for(unsigned int i=0; i<infoMsg->refWordsKeys.size() && i<infoMsg->refWordsValues.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<int, cv::KeyPoint>(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<int, cv::KeyPoint>(infoMsg->refWordsKeys.at(i), pt));
}
stat.setRefWords(mapIntKeypoint);
mapIntKeypoint.clear();
for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); ++i)
for(unsigned int i=0; i<infoMsg->loopWordsKeys.size() && i<infoMsg->loopWordsValues.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<int, cv::KeyPoint>(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<int, cv::KeyPoint>(infoMsg->loopWordsKeys.at(i), pt));
}
stat.setLoopWords(mapIntKeypoint);
// Statistics data
for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
for(unsigned int i=0; i<infoMsg->statsKeys.size() && i<infoMsg->statsValues.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<int, std::vector<unsigned char> > images;
for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
for(unsigned int i=0; i<mapMsg->imageIDs.size() && i<mapMsg->images.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<int, std::vector<unsigned char> > depths;
for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
for(unsigned int i=0; i<mapMsg->depthIDs.size() && i<mapMsg->depths.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<int, std::vector<unsigned char> > depth2ds;
for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
for(unsigned int i=0; i<mapMsg->depth2DIDs.size() && i<mapMsg->depth2Ds.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<int, float> depthConstants;
for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
for(unsigned int i=0; i<mapMsg->depthConstantIDs.size() && i<mapMsg->depthConstants.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<int, Transform> localTransforms;
for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
for(unsigned int i=0; i<mapMsg->localTransformIDs.size() && i<mapMsg->localTransforms.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<int, Transform> poses;
for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
for(unsigned int i=0; i<mapMsg->poseIDs.size() && i<mapMsg->poses.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<int, Link> constraints;
for(unsigned int i=0; i<msg->data.constraintFromIDs.size() && i<msg->data.constraintToIDs.size() && i<msg->data.constraintTypes.size() && i < msg->data.constraints.size(); ++i)
for(unsigned int i=0; i<mapMsg->constraintFromIDs.size() && i<mapMsg->constraintToIDs.size() && i<mapMsg->constraintTypes.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<int, int> mapIds;
for(unsigned int i=0; i<msg->data.mapIDs.size() && i<msg->data.maps.size(); ++i)
for(unsigned int i=0; i<mapMsg->mapIDs.size() && i<mapMsg->maps.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<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
@@ -247,80 +252,80 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
std::map<int, Transform> poses;
std::multimap<int, Link> 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; i<msg->imageIDs.size() && i < msg->images.size(); ++i)
for(unsigned int i=0; i<map.imageIDs.size() && i < map.images.size(); ++i)
{
images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes));
images.insert(std::make_pair(map.imageIDs[i], map.images[i].bytes));
}
for(unsigned int i=0; i<msg->depthIDs.size() && i < msg->depths.size(); ++i)
for(unsigned int i=0; i<map.depthIDs.size() && i < map.depths.size(); ++i)
{
depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes));
depths.insert(std::make_pair(map.depthIDs[i], map.depths[i].bytes));
}
for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
for(unsigned int i=0; i<map.depth2DIDs.size() && i < map.depth2Ds.size(); ++i)
{
depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depth2Ds[i].bytes));
depths2d.insert(std::make_pair(map.depth2DIDs[i], map.depth2Ds[i].bytes));
}
for(unsigned int i=0; i<msg->depthConstantIDs.size() && i < msg->depthConstants.size(); ++i)
for(unsigned int i=0; i<map.depthConstantIDs.size() && i < map.depthConstants.size(); ++i)
{
depthConstants.insert(std::make_pair(msg->depthConstantIDs[i], msg->depthConstants[i]));
depthConstants.insert(std::make_pair(map.depthConstantIDs[i], map.depthConstants[i]));
}
for(unsigned int i=0; i<msg->localTransformIDs.size() && i < msg->localTransforms.size(); ++i)
for(unsigned int i=0; i<map.localTransformIDs.size() && i < map.localTransforms.size(); ++i)
{
Transform t = transformFromGeometryMsg(msg->localTransforms[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; i<msg->poseIDs.size() && i < msg->poses.size(); ++i)
for(unsigned int i=0; i<map.poseIDs.size() && i < map.poses.size(); ++i)
{
Transform t = transformFromPoseMsg(msg->poses[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; i<msg->constraintFromIDs.size() && i<msg->constraintToIDs.size() && i<msg->constraintTypes.size() && i < msg->constraints.size(); ++i)
for(unsigned int i=0; i<map.constraintFromIDs.size() && i<map.constraintToIDs.size() && i<map.constraintTypes.size() && i < map.constraints.size(); ++i)
{
Transform t = transformFromGeometryMsg(msg->constraints[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
src/GuiWrapper.h
+12 -4
View File
@@ -25,6 +25,7 @@
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
@@ -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<rtabmap::InfoEx> infoExTopic_;
message_filters::Subscriber<rtabmap::MapData> mapDataTopic_;
ros::Subscriber defaultSub_; // odometry only
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
@@ -84,6 +87,11 @@ private:
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
typedef message_filters::sync_policies::ExactTime<
rtabmap::InfoEx,
rtabmap::MapData> MyInfoMapSyncPolicy;
message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
src/MapAssemblerNode.cpp
+23 -115
View File
@@ -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<sensor_msgs::PointCloud2>("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; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
for(unsigned int i=0; i<msg->localTransformIDs.size() && i<msg->localTransforms.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; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
for(unsigned int i=0; i<msg->imageIDs.size() && i<msg->images.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; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
for(unsigned int i=0; i<msg->depthIDs.size() && i<msg->depths.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; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
for(unsigned int i=0; i<msg->depthConstantIDs.size() && i<msg->depthConstants.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; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
for(unsigned int i=0; i<msg->depth2DIDs.size() && i<msg->depth2Ds.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<pcl::PointXYZ>::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<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.size(); ++i)
{
Transform pose = transformFromPoseMsg(msg->data.poses[i]);
Transform pose = transformFromPoseMsg(msg->poses[i]);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter = rgbClouds_.find(msg->data.poseIDs[i]);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter = rgbClouds_.find(msg->poseIDs[i]);
if(iter != rgbClouds_.end())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
@@ -180,11 +179,11 @@ public:
// generate the assembled scan!
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.size(); ++i)
{
Transform pose = transformFromPoseMsg(msg->data.poses[i]);
Transform pose = transformFromPoseMsg(msg->poses[i]);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans_.find(msg->data.poseIDs[i]);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans_.find(msg->poseIDs[i]);
if(iter != scans_.end())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
@@ -208,103 +207,12 @@ public:
}
}
void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
{
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> 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; i<msg->imageIDs.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; i<msg->depthIDs.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; i<msg->depthConstantIDs.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; i<msg->localTransformIDs.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; i<msg->depth2DIDs.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<int, std::vector<unsigned char> >::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_;
src/rviz/MapCloudDisplay.cpp
+19 -19
View File
@@ -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; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
for(unsigned int i=0; i<msg->localTransformIDs.size() && i<msg->localTransforms.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; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
for(unsigned int i=0; i<msg->imageIDs.size() && i<msg->images.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; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
for(unsigned int i=0; i<msg->depthIDs.size() && i<msg->depths.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; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
for(unsigned int i=0; i<msg->depthConstantIDs.size() && i<msg->depthConstants.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; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.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<int, Transform> poses;
for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.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])));
}
{
src/rviz/MapCloudDisplay.h
+3 -3
View File
@@ -6,7 +6,7 @@
#include <queue>
#include <vector>
#include <rtabmap/InfoEx.h>
#include <rtabmap/MapData.h>
#include <rtabmap/core/Transform.h>
#include <pluginlib/class_loader.h>
@@ -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<rtabmap::InfoEx>
class MapCloudDisplay: public rviz::MessageFilterDisplay<rtabmap::MapData>
{
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:
/**
srv/GetMap.srv
+7
View File
@@ -0,0 +1,7 @@
#request
bool global
bool optimized
bool graphOnly
---
#response
rtabmap/MapData data