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

OdomInfo msg: added local scan map field. OdometryROS: publishing odom_local_scan_map

Browse Source
This commit is contained in:
matlabbe
2016-08-08 14:37:32 -04:00
parent bc76e4ec4e
commit d6377eb513
4 changed files with 33 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+6
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <zlib.h>
#include <ros/ros.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h>
#include <pcl_conversions/pcl_conversions.h>
@@ -711,6 +712,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.features = msg.features;
info.inliers = msg.inliers;
info.localMapSize = msg.localMapSize;
info.localScanMapSize = msg.localScanMapSize;
info.timeEstimation = msg.timeEstimation;
info.variance = msg.variance;
info.timeParticleFiltering = msg.timeParticleFiltering;
@@ -742,6 +744,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
info.localScanMap = rtabmap::uncompressData(msg.localScanMap);
return info;
}
@@ -752,6 +756,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.features = info.features;
msg.inliers = info.inliers;
msg.localMapSize = info.localMapSize;
msg.localScanMapSize = info.localScanMapSize;
msg.timeEstimation = info.timeEstimation;
msg.variance = info.variance;
msg.timeParticleFiltering = info.timeParticleFiltering;
@@ -777,6 +782,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localMapKeys = uKeys(info.localMap);
points3fToROS(uValues(info.localMap), msg.localMapValues);
msg.localScanMap = rtabmap::compressData(info.localScanMap);
}
}
src/OdometryROS.cpp
+21
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/OdometryF2M.h>
#include <rtabmap/core/OdometryF2F.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
@@ -98,6 +99,7 @@ void OdometryROS::onInit()
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 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);
Transform initialPose = Transform::getIdentity();
@@ -502,6 +504,25 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
}
}
if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty())
{
sensor_msgs::PointCloud2 cloudMsg;
if(info.localScanMap.channels() == 6)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap);
pcl::toROSMsg(*cloud, cloudMsg);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap);
pcl::toROSMsg(*cloud, cloudMsg);
}
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLocalScanMap_.publish(cloudMsg);
}
}
else if(publishNullWhenLost_)
{