i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27: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/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_)
{