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

Added add_link service and external_loop_detection_example node

Browse Source
This commit is contained in:
matlabbe
2020-02-11 23:36:56 -05:00
parent 354e362673
commit 231418e098
7 changed files with 196 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+12
View File
@@ -611,6 +611,7 @@ void CoreWrapper::onInit()
cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
addLinkSrv_ = nh.advertiseService("add_link", &CoreWrapper::addLinkCallback, this);
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
@@ -3228,6 +3229,17 @@ bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtab
return true;
}
bool CoreWrapper::addLinkCallback(rtabmap_ros::AddLink::Request& req, rtabmap_ros::AddLink::Response&)
{
if(rtabmap_.getMemory())
{
ROS_INFO("Adding external link %d -> %d", req.link.fromId, req.link.toId);
rtabmap_.addLink(linkFromROS(req.link));
return true;
}
return false;
}
void CoreWrapper::publishStats(const ros::Time & stamp)
{
UDEBUG("Publishing stats...");
src/ExternalLoopDetectionExample.cpp
+168
View File
@@ -0,0 +1,168 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <ros/publisher.h>
#include <ros/subscriber.h>
#include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/Info.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap_ros/AddLink.h>
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/utilite/UStl.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
/*
* Test:
* $ roslaunch rtabmap_ros demo_robot_mapping.launch
* Disable internal loop closure detection, in rtabmapviz->Preferences:
* ->Vocabulary, set Max words to -1 (loop closure detection disabled)
* ->Proximity Detection, uncheck proximity detection by space
* $ rosrun rtabmap_ros external_loop_detection_example
* $ rosbag play --clock demo_mapping.bag
*/
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::MapData, rtabmap_ros::Info> MyInfoMapSyncPolicy;
ros::ServiceClient addLinkSrv;
// This is used to keep in cache the old data of the map
std::map<int, rtabmap::SensorData> localData;
// In this example, we use rtabmap for our loop closure detector
rtabmap::Rtabmap loopClosureDetector;
rtabmap::RegistrationVis reg;
void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg, const rtabmap_ros::InfoConstPtr & infoMsg)
{
ROS_INFO("Received map data!");
rtabmap::Statistics stats;
rtabmap_ros::infoFromROS(*infoMsg, stats);
bool smallMovement = (bool)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
bool fastMovement = (bool)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
if(smallMovement || fastMovement)
{
// The signature has been ignored from rtabmap, don't process it
return;
}
rtabmap::Transform mapToOdom;
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
std::map<int, rtabmap::Signature> signatures;
rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
if(!signatures.empty() &&
signatures.rbegin()->second.sensorData().isValid() &&
localData.find(signatures.rbegin()->first) == localData.end())
{
int id = signatures.rbegin()->first;
const rtabmap::SensorData & s = signatures.rbegin()->second.sensorData();
cv::Mat rgb;
//rtabmap::LaserScan scan;
s.uncompressDataConst(&rgb, 0/*, &scan*/);
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(scan, scan.localTransform());
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())
{
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());
}
}
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "external_loop_detection_example");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
rtabmap::ParametersMap params;
params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemGenerateIds(), "false")); // use provided ids
params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false")); // BOW-only mode
loopClosureDetector.init(params, "");
// service to add link
addLinkSrv = nh.serviceClient<rtabmap_ros::AddLink>("/rtabmap/add_link");
// subscription
message_filters::Subscriber<rtabmap_ros::Info> infoTopic;
message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic;
infoTopic.subscribe(nh, "/rtabmap/info", 1);
mapDataTopic.subscribe(nh, "/rtabmap/mapData", 1);
message_filters::Synchronizer<MyInfoMapSyncPolicy> infoMapSync(
MyInfoMapSyncPolicy(10),
mapDataTopic,
infoTopic);
infoMapSync.registerCallback(&mapDataCallback);
ROS_INFO("Subscribed to %s and %s", mapDataTopic.getTopic().c_str(), infoTopic.getTopic().c_str());
ros::spin();
return 0;
}
src/MsgConversion.cpp
+3 -3
View File
@@ -889,9 +889,9 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
{
// multi-cameras model
if(msg.fx.size() &&
msg.fx.size() == msg.fy.size(),
msg.fx.size() == msg.cx.size(),
msg.fx.size() == msg.cy.size(),
msg.fx.size() == msg.fy.size() &&
msg.fx.size() == msg.cx.size() &&
msg.fx.size() == msg.cy.size() &&
msg.fx.size() == msg.localTransform.size())
{
for(unsigned int i=0; i<msg.fx.size(); ++i)