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

ros-pkg: added publish_map service to rtabmap node. Updated demo_find_object.launch with higher icp2d constraints

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1645 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-13 19:25:44 +00:00
parent 4e3c30d8ed
commit 36c647e6fe
4 changed files with 187 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_find_object.launch
+2
View File
@@ -30,6 +30,8 @@
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.9"/>
<param name="LccIcp2/MaxFitness" type="string" value="0.1"/>
<param name="LccIcp2/Iterations" type="string" value="100"/>
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
src/CoreWrapper.cpp
+169
View File
@@ -197,6 +197,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -771,6 +772,7 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
if(req.graphOnly)
{
@@ -793,6 +795,7 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
localTransforms,
poses,
constraints,
mapIds,
req.optimized,
req.global);
}
@@ -800,6 +803,16 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
int i=0;
rep.data.mapIDs.resize(mapIds.size());
rep.data.maps.resize(mapIds.size());
i=0;
for(std::map<int, int>::iterator iter = mapIds.begin(); iter!=mapIds.end(); ++iter)
{
rep.data.mapIDs[i] = iter->first;
rep.data.maps[i] = iter->second;
++i;
}
rep.data.imageIDs.resize(images.size());
rep.data.images.resize(images.size());
i=0;
@@ -908,6 +921,162 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
return true;
}
bool CoreWrapper::publishMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("rtabmap: Publishing map...");
if(mapData_.getNumSubscribers())
{
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> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
rtabmap_.get3DMap(
images,
depths,
depths2d,
depthFxs,
depthFys,
depthCxs,
depthCys,
localTransforms,
poses,
constraints,
mapIds,
true,
true);
//RGB-D SLAM data
rtabmap::MapDataPtr msg(new rtabmap::MapData);
msg->header.stamp = ros::Time::now();
msg->header.frame_id = mapFrameId_;
int i=0;
msg->mapIDs.resize(mapIds.size());
msg->maps.resize(mapIds.size());
i=0;
for(std::map<int, int>::iterator iter = mapIds.begin(); iter!=mapIds.end(); ++iter)
{
msg->mapIDs[i] = iter->first;
msg->maps[i] = iter->second;
++i;
}
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;
}
// fx,fy,cx,cy parameters
msg->depthFxIDs.resize(depthFxs.size());
msg->depthFxs.resize(depthFxs.size());
i=0;
for(std::map<int, float>::iterator iter = depthFxs.begin(); iter!=depthFxs.end(); ++iter)
{
msg->depthFxIDs[i] = iter->first;
msg->depthFxs[i] = iter->second;
++i;
}
msg->depthFyIDs.resize(depthFys.size());
msg->depthFys.resize(depthFys.size());
i=0;
for(std::map<int, float>::iterator iter = depthFys.begin(); iter!=depthFys.end(); ++iter)
{
msg->depthFyIDs[i] = iter->first;
msg->depthFys[i] = iter->second;
++i;
}
msg->depthCxIDs.resize(depthCxs.size());
msg->depthCxs.resize(depthCxs.size());
i=0;
for(std::map<int, float>::iterator iter = depthCxs.begin(); iter!=depthCxs.end(); ++iter)
{
msg->depthCxIDs[i] = iter->first;
msg->depthCxs[i] = iter->second;
++i;
}
msg->depthCyIDs.resize(depthCys.size());
msg->depthCys.resize(depthCys.size());
i=0;
for(std::map<int, float>::iterator iter = depthCys.begin(); iter!=depthCys.end(); ++iter)
{
msg->depthCyIDs[i] = iter->first;
msg->depthCys[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;
}
mapData_.publish(msg);
}
return true;
}
void CoreWrapper::publishStats(const Statistics & stats)
{
ros::Time timeNow = ros::Time::now();
src/CoreWrapper.h
+2
View File
@@ -102,6 +102,7 @@ private:
bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap::Response& rep);
bool publishMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
rtabmap::ParametersMap loadParameters(const std::string & configFile);
void saveParameters(const std::string & configFile);
@@ -165,6 +166,7 @@ private:
ros::ServiceServer setModeLocalizationSrv_;
ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer getMapDataSrv_;
ros::ServiceServer publishMapDataSrv_;
boost::thread* transformThread_;
src/GuiWrapper.cpp
+14 -1
View File
@@ -295,6 +295,13 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
if(map.mapIDs.size() != map.maps.size())
{
ROS_WARN("rtabmapviz: receiving map... maps and IDs are not the same size (%d vs %d)!",
(int)map.maps.size(), (int)map.mapIDs.size());
}
if(map.imageIDs.size() != map.images.size())
{
@@ -349,6 +356,11 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
(int)map.constraints.size(), (int)map.constraintFromIDs.size(), (int)map.constraintToIDs.size(), (int)map.constraintTypes.size());
}
for(unsigned int i=0; i<map.mapIDs.size() && i < map.maps.size(); ++i)
{
mapIds.insert(std::make_pair(map.mapIDs[i], map.maps[i]));
}
for(unsigned int i=0; i<map.imageIDs.size() && i < map.images.size(); ++i)
{
images.insert(std::make_pair(map.imageIDs[i], map.images[i].bytes));
@@ -409,7 +421,8 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
depthCys,
localTransforms,
poses,
constraints));
constraints,
mapIds));
}
void GuiWrapper::handleEvent(UEvent * anEvent)