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

ExternalLoopDetectionExample: added localization option, loading the map on initialization.

Browse Source
This commit is contained in:
matlabbe
2020-05-21 21:27:57 -04:00
parent cc79e19b1f
commit e69557c0ca
1 changed files with 97 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/ExternalLoopDetectionExample.cpp
+97 -18
View File
@@ -33,8 +33,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/Info.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap_ros/AddLink.h>
#include <rtabmap_ros/GetMap.h>
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/utilite/UStl.h>
@@ -56,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::MapData, rtabmap_ros::Info> MyInfoMapSyncPolicy;
ros::ServiceClient addLinkSrv;
ros::ServiceClient getMapSrv;
// This is used to keep in cache the old data of the map
std::map<int, rtabmap::SensorData> localData;
@@ -65,6 +69,8 @@ rtabmap::Rtabmap loopClosureDetector;
rtabmap::RegistrationVis reg;
bool g_localizationMode = false;
void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg, const rtabmap_ros::InfoConstPtr & infoMsg)
{
ROS_INFO("Received map data!");
@@ -100,37 +106,46 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg, const rtab
if(loopClosureDetector.process(rgb, id))
{
localData.insert(std::make_pair(id, s));
if(loopClosureDetector.getLoopClosureId()>0)
{
int fromId = id;
int toId = loopClosureDetector.getLoopClosureId();
ROS_INFO("Detected loop closure between %d and %d", fromId, toId);
//Compute transformation
rtabmap::RegistrationInfo regInfo;
rtabmap::SensorData tmpFrom = localData.at(fromId);
rtabmap::SensorData tmpTo = localData.at(toId);
tmpFrom.uncompressData();
tmpTo.uncompressData();
rtabmap::Transform t = reg.computeTransformation(tmpFrom, tmpTo, rtabmap::Transform(), &regInfo);
if(!t.isNull())
if(localData.find(toId) != localData.end())
{
rtabmap::Link link(fromId, toId, rtabmap::Link::kUserClosure, t, regInfo.covariance.inv());
rtabmap_ros::AddLinkRequest req;
rtabmap_ros::linkToROS(link, req.link);
rtabmap_ros::AddLinkResponse res;
if(!addLinkSrv.call(req, res))
//Compute transformation
rtabmap::RegistrationInfo regInfo;
rtabmap::SensorData tmpFrom = s;
rtabmap::SensorData tmpTo = localData.at(toId);
tmpFrom.uncompressData();
tmpTo.uncompressData();
rtabmap::Transform t = reg.computeTransformation(tmpFrom, tmpTo, rtabmap::Transform(), &regInfo);
if(!t.isNull())
{
ROS_ERROR("Failed to call %s service", addLinkSrv.getService().c_str());
rtabmap::Link link(fromId, toId, rtabmap::Link::kUserClosure, t, regInfo.covariance.inv());
rtabmap_ros::AddLinkRequest req;
rtabmap_ros::linkToROS(link, req.link);
rtabmap_ros::AddLinkResponse res;
if(!addLinkSrv.call(req, res))
{
ROS_ERROR("Failed to call %s service", addLinkSrv.getService().c_str());
}
}
else
{
ROS_WARN("Could not compute transformation between %d and %d: %s", fromId, toId, regInfo.rejectedMsg.c_str());
}
}
else
{
ROS_WARN("Could not compute transformation between %d and %d: %s", fromId, toId, regInfo.rejectedMsg.c_str());
ROS_WARN("Could not compute transformation between %d and %d because node data %d is not in cache.", fromId, toId, toId);
}
}
if(!g_localizationMode)
{
localData.insert(std::make_pair(id, s));
}
}
}
}
@@ -139,6 +154,9 @@ int main(int argc, char** argv)
{
ros::init(argc, argv, "external_loop_detection_example");
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
@@ -147,9 +165,70 @@ int main(int argc, char** argv)
params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false")); // BOW-only mode
loopClosureDetector.init(params, "");
pnh.param("localization", g_localizationMode, g_localizationMode);
// service to add link
addLinkSrv = nh.serviceClient<rtabmap_ros::AddLink>("/rtabmap/add_link");
// service to get map for initialization
getMapSrv = nh.serviceClient<rtabmap_ros::GetMap>("/rtabmap/get_map_data");
rtabmap_ros::GetMap::Request mapReq;
mapReq.global = true;
mapReq.graphOnly = false;
mapReq.optimized = false;
rtabmap_ros::GetMap::Response mapRes;
ros::Rate rate(1);
while(!getMapSrv.exists() && nh.ok())
{
ROS_INFO("Waiting for service \"%s\" to be available. If rtabmap is already started, "
"make sure the service name is the same or remap it.", getMapSrv.getService().c_str());
rate.sleep();
ros::spinOnce();
}
if(!nh.ok())
{
return 0;
}
ROS_INFO("Calling \"%s\" service to get data already in the map.", getMapSrv.getService().c_str());
if(getMapSrv.call(mapReq, mapRes) && !mapRes.data.nodes.empty())
{
ROS_INFO("Adding %d nodes to memory...", (int)mapRes.data.nodes.size());
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap::Transform mapToOdom;
rtabmap_ros::mapGraphFromROS(mapRes.data.graph, poses, links, mapToOdom);
int addedNodes = 0;
for(size_t i=0; i<mapRes.data.nodes.size(); ++i)
{
if(poses.find(mapRes.data.nodes.at(i).id) != poses.end())
{
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(mapRes.data.nodes.at(i));
rtabmap::SensorData compressedData = s.sensorData();
s.sensorData().uncompressData();
if(loopClosureDetector.process(s.sensorData(), rtabmap::Transform()))
{
localData.insert(std::make_pair(
loopClosureDetector.getStatistics().getLastSignatureData().id(),
compressedData));
++addedNodes;
}
}
}
ROS_INFO("Added %d/%d nodes to memory! Vocabulary size=%d",
addedNodes,
(int)mapRes.data.nodes.size(),
(int)loopClosureDetector.getMemory()->getVWDictionary()->getVisualWords().size());
}
if(g_localizationMode)
{
params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false")); // localization mode
loopClosureDetector.parseParameters(params);
ROS_INFO("Set detector in localization mode");
}
// subscription
message_filters::Subscriber<rtabmap_ros::Info> infoTopic;
message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic;