i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 02:07:45 +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
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())