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

Added odom_info_lite topic (same as odom_info but without data). Optimized intermediate nodes processing time by avoiding converting all data in odom_info message.

Browse Source
This commit is contained in:
matlabbe
2021-03-24 17:40:54 -04:00
parent 6e34cde8e6
commit e75d035d99
5 changed files with 42 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MsgConversion.h
+1 -1
View File
@@ -168,7 +168,7 @@ rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg);
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ignoreData = false);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
include/rtabmap_ros/OdometryROS.h
+1
View File
@@ -113,6 +113,7 @@ private:
ros::Publisher odomPub_;
ros::Publisher odomInfoPub_;
ros::Publisher odomInfoLitePub_;
ros::Publisher odomLocalMap_;
ros::Publisher odomLocalScanMap_;
ros::Publisher odomLastFrame_;
src/CoreWrapper.cpp
+2 -1
View File
@@ -1909,7 +1909,7 @@ void CoreWrapper::process(
std::vector<float> odomVelocity;
if(iter->second.timeEstimation != 0.0f)
{
OdometryInfo info = odomInfoFromROS(iter->second);
OdometryInfo info = odomInfoFromROS(iter->second, true);
externalStats = rtabmap_ros::odomInfoToStatistics(info);
if(info.interval>0.0)
@@ -2098,6 +2098,7 @@ void CoreWrapper::process(
rtabmapROSStats_.clear();
}
timeMsgConversion += timer.ticks();
if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
{
timeRtabmap = timer.ticks();
src/MsgConversion.cpp
+24 -21
View File
@@ -1381,7 +1381,7 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
return stats;
}
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ignoreData)
{
rtabmap::OdometryInfo info;
info.lost = msg.lost;
@@ -1413,31 +1413,34 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.type = msg.type;
UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
{
info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
}
info.reg.matchesIDs = msg.wordMatches;
info.reg.inliersIDs = msg.wordInliers;
info.refCorners = points2fFromROS(msg.refCorners);
info.newCorners = points2fFromROS(msg.newCorners);
info.cornerInliers = msg.cornerInliers;
info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
info.transformGroundTruth = transformFromGeometryMsg(msg.transformGroundTruth);
info.guess = transformFromGeometryMsg(msg.guess);
UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
if(!ignoreData)
{
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
{
info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
}
info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
info.refCorners = points2fFromROS(msg.refCorners);
info.newCorners = points2fFromROS(msg.newCorners);
info.cornerInliers = msg.cornerInliers;
info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
info.transformGroundTruth = transformFromGeometryMsg(msg.transformGroundTruth);
info.guess = transformFromGeometryMsg(msg.guess);
UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
{
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
}
return info;
}
src/OdometryROS.cpp
+14 -1
View File
@@ -115,6 +115,7 @@ void OdometryROS::onInit()
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
odomInfoLitePub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info_lite", 1);
odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
@@ -856,13 +857,25 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
if(odomInfoPub_.getNumSubscribers())
if(odomInfoPub_.getNumSubscribers() || odomInfoLitePub_.getNumSubscribers())
{
rtabmap_ros::OdomInfo infoMsg;
odomInfoToROS(info, infoMsg);
infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
infoMsg.header.frame_id = odomFrameId_;
odomInfoPub_.publish(infoMsg);
infoMsg.wordInliers.clear();
infoMsg.wordMatches.clear();
infoMsg.wordsKeys.clear();
infoMsg.wordsValues.clear();
infoMsg.refCorners.clear();
infoMsg.newCorners.clear();
infoMsg.cornerInliers.clear();
infoMsg.localMapKeys.clear();
infoMsg.localMapValues.clear();
infoMsg.localScanMap.clear();
odomInfoLitePub_.publish(infoMsg);
}
if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())