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

ros-pkg fixed action "Publish graph" from GUI not linked to rtabmap related services

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1123 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-02-17 22:59:53 +00:00
parent 1f0052b682
commit 12922f5ceb
3 changed files with 102 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/CoreWrapper.cpp
+63 -1
View File
@@ -174,6 +174,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
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);
setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -616,6 +618,18 @@ bool CoreWrapper::publishLocalMapDataCallback(std_srvs::Empty::Request&, std_srv
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");
@@ -630,7 +644,6 @@ void CoreWrapper::publishMapData(bool global)
if(mapData_.getNumSubscribers())
{
rtabmap::MapDataPtr msg(new rtabmap::MapData);
msg->header.stamp = ros::Time::now();
rtabmap_.get3DMap(images,
depths,
@@ -718,6 +731,55 @@ void CoreWrapper::publishMapData(bool global)
++i;
}
msg->header.stamp = ros::Time::now();
mapData_.publish(msg);
}
}
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())
{
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);
}
}
rtabmap/src/CoreWrapper.h
+5
View File
@@ -76,8 +76,11 @@ private:
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);
rtabmap::ParametersMap loadParameters(const std::string & configFile);
void saveParameters(const std::string & configFile);
@@ -140,6 +143,8 @@ private:
ros::ServiceServer triggerNewMapSrv_;
ros::ServiceServer publishGlobalMapDataSrv_;
ros::ServiceServer publishLocalMapDataSrv_;
ros::ServiceServer publishGlobalGraphSrv_;
ros::ServiceServer publishLocalGraphSrv_;
boost::thread* transformThread_;
rtabmap/src/GuiWrapper.cpp
+34 -2
View File
@@ -415,7 +415,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
ROS_ERROR("Can't call \"trigger_new_map\" service");
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap)
else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapLocal)
{
if(mapDataTopic_.getNumPublishers())
{
@@ -431,7 +431,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
this->post(new RtabmapEvent3DMap(2)); // topic error
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapFull)
else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal)
{
if(mapDataTopic_.getNumPublishers())
{
@@ -447,6 +447,38 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
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
}
}
else
{
ROS_WARN("Not handled command (%d)...", cmd);