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

ROS package updated to RTAB-Map 0.6.2

Browse Source 
updated service name: publish_current_map_data and publish_full_map_data
fixed not published recalled images
Grid map assembler: remove obstacle if a new laser says empty
Modified MapData and InfoEx msg (MapCorrection field is not anymore in MapData)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1067 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-01-22 19:57:52 +00:00
parent 0eaf0a8871
commit c87beade6a
7 changed files with 57 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/CoreWrapper.cpp
+23 -10
View File
@@ -172,7 +172,8 @@ 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);
publishMapDataSrv_ = nh.advertiseService("publish_map_data", &CoreWrapper::publishMapDataCallback, this);
publishFullMapDataSrv_ = nh.advertiseService("publish_full_map_data", &CoreWrapper::publishFullMapDataCallback, this);
publishCurrentMapDataSrv_ = nh.advertiseService("publish_current_map_data", &CoreWrapper::publishCurrentMapDataCallback, this);
setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -603,16 +604,27 @@ bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Emp
return true;
}
bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
bool CoreWrapper::publishFullMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("rtabmap: Publishing map data...");
publishMapData(true);
return true;
}
bool CoreWrapper::publishCurrentMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
publishMapData(false);
return true;
}
void CoreWrapper::publishMapData(bool full)
{
ROS_INFO("rtabmap: Publishing map data (full=%s)...", full?"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;
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
Transform mapCorrection;
if(mapData_.getNumSubscribers())
{
@@ -625,7 +637,8 @@ bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Em
depthConstants,
localTransforms,
poses,
mapCorrection);
true,
full);
int i=0;
@@ -689,12 +702,8 @@ bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Em
++i;
}
transformToGeometryMsg(mapCorrection, msg->mapCorrection);
mapData_.publish(msg);
}
return true;
}
void CoreWrapper::publishStats(const Statistics & stats)
@@ -754,7 +763,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
++i;
}
transformToGeometryMsg(stats.mapCorrection(), msg->data.mapCorrection);
transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
transformToPoseMsg(stats.currentPose(), msg->currentPose);
@@ -823,6 +832,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
{
msg->data.imageIDs[index] = i->first;
msg->data.images[index].bytes = i->second;
++index;
}
msg->data.depthIDs.resize(stats.getDepths().size());
@@ -834,6 +844,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
{
msg->data.depthIDs[index] = i->first;
msg->data.depths[index].bytes = i->second;
++index;
}
msg->data.depth2DIDs.resize(stats.getDepth2ds().size());
@@ -845,6 +856,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
{
msg->data.depth2DIDs[index] = i->first;
msg->data.depth2Ds[index].bytes = i->second;
++index;
}
msg->data.localTransformIDs.resize(stats.getLocalTransforms().size());
@@ -856,6 +868,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
{
msg->data.localTransformIDs[index] = i->first;
transformToGeometryMsg(i->second, msg->data.localTransforms[index]);
++index;
}
msg->data.depthConstantIDs = uKeys(stats.getDepthConstants());
rtabmap/src/CoreWrapper.h
+6 -2
View File
@@ -74,7 +74,10 @@ 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 publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool publishFullMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool publishCurrentMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
void publishMapData(bool full);
rtabmap::ParametersMap loadParameters(const std::string & configFile);
void saveParameters(const std::string & configFile);
@@ -135,7 +138,8 @@ private:
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
ros::ServiceServer triggerNewMapSrv_;
ros::ServiceServer publishMapDataSrv_;
ros::ServiceServer publishFullMapDataSrv_;
ros::ServiceServer publishCurrentMapDataSrv_;
boost::thread* transformThread_;
rtabmap/src/GridMapAssemblerNode.cpp
+1 -1
View File
@@ -227,7 +227,7 @@ public:
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f", upperbound, grid.cols, x+1, slope, b).c_str());
for(int y = lowerbound; y<=(int)upperbound; ++y)
{
if(grid.at<char>(y, x) == -1)
//if(grid.at<char>(y, x) == -1)
{
grid.at<char>(y, x) = 0; // free space
}
rtabmap/src/GuiWrapper.cpp
+20 -8
View File
@@ -173,7 +173,7 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
}
//RGB-D SLAM data
stat.setMapCorrection(transformFromGeometryMsg(msg->data.mapCorrection));
stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection));
stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform));
stat.setCurrentPose(transformFromPoseMsg(msg->currentPose));
@@ -237,7 +237,6 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
Transform mapCorrection;
if(msg->imageIDs.size() != msg->images.size())
{
@@ -295,15 +294,12 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
poses.insert(std::make_pair(msg->poseIDs[i], t));
}
mapCorrection = transformFromGeometryMsg(msg->mapCorrection);
this->post(new RtabmapEvent3DMap(images,
depths,
depths2d,
depthConstants,
localTransforms,
poses,
mapCorrection));
poses));
}
void GuiWrapper::handleEvent(UEvent * anEvent)
@@ -393,9 +389,25 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
{
if(mapDataTopic_.getNumPublishers())
{
if(!ros::service::call("publish_map_data", srv))
if(!ros::service::call("publish_current_map_data", srv))
{
ROS_WARN("Can't call \"publish_map_data\" service");
ROS_WARN("Can't call \"publish_current_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::kCmdPublish3DMapFull)
{
if(mapDataTopic_.getNumPublishers())
{
if(!ros::service::call("publish_full_map_data", srv))
{
ROS_WARN("Can't call \"publish_full_map_data\" service");
this->post(new RtabmapEvent3DMap(1)); // service error
}
}