Fixed build for upstream 0.16.1

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.16.0 REQUIRED)
+find_package(RTABMap 0.16.1 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

msg/NodeData.msg

@@ -38,6 +38,7 @@ geometry_msgs/Transform[] localTransform
 uint8[] laserScan
 int32 laserScanMaxPts
 float32 laserScanMaxRange
+int32 laserScanFormat
 geometry_msgs/Transform laserScanLocalTransform
 
 # compressed user data

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.16.0</version>
+  <version>0.16.1</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -1033,7 +1033,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 		if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
 		{
 			// backward compatibility, project 2D scan in /base_link frame
-			scan = util3d::transformLaserScan(scan, scanLocalTransform);
+			scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
 			scanLocalTransform = Transform::getIdentity();
 		}
 	}
@@ -1076,8 +1076,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(scan,
+	SensorData data(LaserScan::backwardCompatibility(scan,
-			LaserScanInfo(
 					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
 					scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
 					scanLocalTransform),
@@ -1280,7 +1279,7 @@ void CoreWrapper::commonStereoCallback(
 		if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
 		{
 			// backward compatibility, project 2D scan in /base_link frame
-			scan = util3d::transformLaserScan(scan, scanLocalTransform);
+			scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
 			scanLocalTransform = Transform::getIdentity();
 		}
 	}
@@ -1323,8 +1322,7 @@ void CoreWrapper::commonStereoCallback(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(scan,
+	SensorData data(LaserScan::backwardCompatibility(scan,
-			LaserScanInfo(
 					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
 					scan2dMsg.get() != 0?scan2dMsg->range_max:0,
 					scanLocalTransform),
@@ -1451,7 +1449,7 @@ void CoreWrapper::process(
 					rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first ||
 					rtabmap_.getMemory()->getLastWorkingSignature() == 0 ||
 					rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 ||
-					(!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().channels() == 2)) // 2d laser scan would fill empty space for latest data
+					(!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data
 				{
 					SensorData tmpData = data;
 					tmpData.setId(-1);

src/DbPlayerNode.cpp

@@ -262,7 +262,7 @@ int main(int argc, char** argv)
 		camInfoB.height = odom.data().depthOrRightRaw().rows;
 		camInfoB.width = odom.data().depthOrRightRaw().cols;
 
-		if(!odom.data().laserScanRaw().empty())
+		if(!odom.data().laserScanRaw().isEmpty())
 		{
 			if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
 		}
@@ -412,7 +412,7 @@ int main(int argc, char** argv)
 			rightCamInfoPub.publish(camInfoB);
 		}
 
-		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
+		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().isEmpty())
 		{
 			//inspired from pointcloud_to_laserscan package
 			sensor_msgs::LaserScan msg;
@@ -430,16 +430,14 @@ int main(int argc, char** argv)
 			uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
 			msg.ranges.assign(rangesSize, 0.0);
 
-			const cv::Mat & scan = odom.data().laserScanRaw();
+			const cv::Mat & scan = odom.data().laserScanRaw().data();
-			UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3);
-			UASSERT(scan.rows == 1);
 			for (int i=0; i<scan.cols; ++i)
 			{
-				cv::Vec2f pos = scan.at<cv::Vec2f>(i);
+				const float * ptr = scan.ptr<float>(0,i);
-				double range = hypot(pos[0], pos[1]);
+				double range = hypot(ptr[0], ptr[1]);
 				if (range >= scanRangeMin && range <=scanRangeMax)
 				{
-					double angle = atan2(pos[1], pos[0]);
+					double angle = atan2(ptr[1], ptr[0]);
 					if (angle >= msg.angle_min && angle <= msg.angle_max)
 					{
 						int index = (angle - msg.angle_min) / msg.angle_increment;

src/GuiWrapper.cpp

@@ -572,8 +572,7 @@ void GuiWrapper::commonDepthCallback(
 	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
-				scan,
+				LaserScan::backwardCompatibility(scan,
-				LaserScanInfo(
 						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
 						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
 						scanLocalTransform),
@@ -728,8 +727,7 @@ void GuiWrapper::commonStereoCallback(
 	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
-				scan,
+				LaserScan::backwardCompatibility(scan,
-				LaserScanInfo(
 						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
 						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
 						scanLocalTransform),

src/MapsManager.cpp

@@ -468,7 +468,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						cv::Mat ground, obstacles, emptyCells;
 						if(iter->first > 0)
 						{
-							cv::Mat rgb, depth, scan;
+							cv::Mat rgb, depth;
+							LaserScan scan;
 							bool generateGrid = data.gridCellSize() == 0.0f;
 							static bool warningShown = false;
 							if(occupancySavedInDB && generateGrid && !warningShown)
@@ -522,7 +523,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 								occupancyGrid_->parseParameters(parameters);
 							}
 
-							cv::Mat rgb, depth, scan;
+							cv::Mat rgb, depth;
+							LaserScan scan;
 							bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_);
 							data.uncompressData(
 								occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
@@ -892,7 +894,7 @@ void MapsManager::publishMaps(
 				}
 				if(jter!=gridMaps_.end() && jter->second.first.first.cols)
 				{
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first.first, iter->second, 0, 255, 0);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
 					if(cloudSubtractFiltering_)
 					{
@@ -939,7 +941,7 @@ void MapsManager::publishMaps(
 				}
 				if(jter!=gridMaps_.end() && jter->second.first.second.cols)
 				{
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first.second, iter->second, 255, 0, 0);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
 					if(cloudSubtractFiltering_)
 					{

src/MsgConversion.cpp

@@ -717,10 +717,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 			transformFromPoseMsg(msg.groundTruthPose),
 			stereoModel.isValidForProjection()?
 				rtabmap::SensorData(
-					compressedMatFromBytes(msg.laserScan),
+					rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
-					rtabmap::LaserScanInfo(
 							msg.laserScanMaxPts,
 							msg.laserScanMaxRange,
+							(rtabmap::LaserScan::Format)msg.laserScanFormat,
 							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
@@ -729,10 +729,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					msg.stamp,
 					compressedMatFromBytes(msg.userData)):
 				rtabmap::SensorData(
-					compressedMatFromBytes(msg.laserScan),
+					rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
-					rtabmap::LaserScanInfo(
 							msg.laserScanMaxPts,
 							msg.laserScanMaxRange,
+							(rtabmap::LaserScan::Format)msg.laserScanFormat,
 							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
@@ -770,16 +770,17 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	msg.gps.bearing = signature.sensorData().gps().bearing();
 	compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
 	compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
-	compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
+	compressedMatToBytes(signature.sensorData().laserScanCompressed().data(), msg.laserScan);
 	compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
 	compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground);
 	compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles);
 	compressedMatToBytes(signature.sensorData().gridEmptyCellsCompressed(), msg.grid_empty_cells);
 	point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point);
 	msg.grid_cell_size = signature.sensorData().gridCellSize();
-	msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints();
+	msg.laserScanMaxPts = signature.sensorData().laserScanCompressed().maxPoints();
-	msg.laserScanMaxRange = signature.sensorData().laserScanInfo().maxRange();
+	msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().maxRange();
-	transformToGeometryMsg(signature.sensorData().laserScanInfo().localTransform(), msg.laserScanLocalTransform);
+	msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
+	transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
 	msg.baseline = 0;
 	if(signature.sensorData().cameraModels().size())
 	{
@@ -958,7 +959,7 @@ 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);
+	info.localScanMap = rtabmap::LaserScan::backwardCompatibility(rtabmap::uncompressData(msg.localScanMap));
 
 	return info;
 }
@@ -1009,7 +1010,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);
+	msg.localScanMap = rtabmap::compressData(info.localScanMap.data());
 }
 
 cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)

src/OdometryROS.cpp

@@ -595,10 +595,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			}
 		}
 
-		if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty())
+		if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.isEmpty())
 		{
 			sensor_msgs::PointCloud2 cloudMsg;
-			if(info.localScanMap.channels() >= 5)
+			if(info.localScanMap.hasNormals())
 			{
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap);
 				pcl::toROSMsg(*cloud, cloudMsg);

src/WifiSignalSubNode.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ros/subscriber.h>
 
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/core/util3d_transforms.h>
 
 #include <rtabmap_ros/MapData.h>

src/nodelets/icp_odometry.cpp

@@ -232,8 +232,7 @@ private:
 		}
 
 		rtabmap::SensorData data(
-				scan,
+				LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform),
-				LaserScanInfo(maxLaserScans, scanMsg->range_max, localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
 				CameraModel(),
@@ -321,8 +320,7 @@ private:
 		}
 
 		rtabmap::SensorData data(
-				scan,
+				LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform),
-				LaserScanInfo(maxLaserScans, 0, localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
 				CameraModel(),

src/nodelets/rgbdicp_odometry.cpp

@@ -402,8 +402,7 @@ private:
 				}
 
 				rtabmap::SensorData data(
-						scan,
+						LaserScan::backwardCompatibility(scan,
-						LaserScanInfo(
 								scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
 								scanMsg.get() != 0?scanMsg->range_max:0,
 								localScanTransform),