Updated for RTAB-Map 0.18 (saving full scan info)

CMakeLists.txt

@@ -30,7 +30,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.17.7 REQUIRED)
+find_package(RTABMap 0.18.0 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -231,13 +231,6 @@ SET(Libraries
 ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
 ENDIF(octomap_msgs_FOUND)
 
-IF(QT4_FOUND OR Qt5_FOUND)
-SET(Libraries 
-   ${Libraries}
-   ${QT_LIBRARIES}
-)
-ENDIF(QT4_FOUND OR Qt5_FOUND)
-
 # If rviz is found, add plugins
 IF(rviz_FOUND)
 MESSAGE(STATUS "WITH rviz")
@@ -351,7 +344,7 @@ ELSE()
 ENDIF()
 
 add_executable(data_player src/DbPlayerNode.cpp)
-target_link_libraries(data_player rtabmap_ros ${QT_LIBRARIES} ${Libraries})
+target_link_libraries(data_player rtabmap_ros ${Libraries})
 
 add_executable(odom_msg_to_tf src/OdomMsgToTFNode.cpp)
 target_link_libraries(odom_msg_to_tf rtabmap_ros ${Libraries})

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.17.7</version>
+  <version>0.18.0</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

@@ -1209,10 +1209,18 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(LaserScan::backwardCompatibility(scan,
+	SensorData data(scan2dMsg.get() !=0?
-					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
+					LaserScan::backwardCompatibility(scan,
-					scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
+							scan2dMsg->range_min,
-					scanLocalTransform),
+							scan2dMsg->range_max,
+							scan2dMsg->angle_min,
+							scan2dMsg->angle_max,
+							scan2dMsg->angle_increment,
+							scanLocalTransform):
+					LaserScan::backwardCompatibility(scan,
+							genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
+							genScan_?genScanMaxDepth_:0.0f,
+							scanLocalTransform),
 			rgb,
 			depth,
 			cameraModels,
@@ -1468,10 +1476,18 @@ void CoreWrapper::commonStereoCallback(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(LaserScan::backwardCompatibility(scan,
+	SensorData data(scan2dMsg.get() != 0?
-					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
+					LaserScan::backwardCompatibility(scan,
-					scan2dMsg.get() != 0?scan2dMsg->range_max:0,
+							scan2dMsg->range_min,
-					scanLocalTransform),
+							scan2dMsg->range_max,
+							scan2dMsg->angle_min,
+							scan2dMsg->angle_max,
+							scan2dMsg->angle_increment,
+							scanLocalTransform):
+					LaserScan::backwardCompatibility(scan,
+							scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
+							0,
+							scanLocalTransform),
 			left,
 			right,
 			stereoModel,

src/DbPlayerNode.cpp

@@ -119,11 +119,10 @@ int main(int argc, char** argv)
 	pnh.param("start_id", startId, startId);
 
 	// A general 360 lidar with 0.5 deg increment
-	double scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
+	double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
 	pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
 	pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
 	pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
-	pnh.param<double>("scan_time", scanTime, 0);
 	pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
 	pnh.param<double>("scan_range_max", scanRangeMax, 60);
 
@@ -170,6 +169,7 @@ int main(int argc, char** argv)
 	ros::Publisher rightCamInfoPub;
 	ros::Publisher odometryPub;
 	ros::Publisher scanPub;
+	ros::Publisher scanCloudPub;
 	ros::Publisher clockPub;
 	tf2_ros::TransformBroadcaster tfBroadcaster;
 
@@ -287,15 +287,27 @@ int main(int argc, char** argv)
 
 		if(!odom.data().laserScanRaw().isEmpty())
 		{
-			if(scanPub.getTopic().empty())
+			if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
 			{
 				scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
-				ROS_INFO("Scan will be published with those parameters:");
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
-				ROS_INFO("  scan_angle_min=%f", scanAngleMin);
+				{
-				ROS_INFO("  scan_angle_max=%f", scanAngleMax);
+					ROS_INFO("Scan will be published.");
-				ROS_INFO("  scan_angle_increment=%f", scanAngleIncrement);
+				}
-				ROS_INFO("  scan_range_min=%f", scanRangeMin);
+				else
-				ROS_INFO("  scan_range_max=%f", scanRangeMax);
+				{
+					ROS_INFO("Scan will be published with those parameters:");
+					ROS_INFO("  scan_angle_min=%f", scanAngleMin);
+					ROS_INFO("  scan_angle_max=%f", scanAngleMax);
+					ROS_INFO("  scan_angle_increment=%f", scanAngleIncrement);
+					ROS_INFO("  scan_range_min=%f", scanRangeMin);
+					ROS_INFO("  scan_range_max=%f", scanRangeMax);
+				}
+			}
+			else if(scanCloudPub.getTopic().empty())
+			{
+				scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
+				ROS_INFO("Scan cloud will be published.");
 			}
 		}
 
@@ -331,7 +343,7 @@ int main(int argc, char** argv)
 				tfBroadcaster.sendTransform(odomToBase);
 			}
 
-			if(!scanPub.getTopic().empty())
+			if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
 			{
 				geometry_msgs::TransformStamped baseToLaserScan;
 				baseToLaserScan.child_frame_id = scanFrameId;
@@ -444,44 +456,63 @@ int main(int argc, char** argv)
 			rightCamInfoPub.publish(camInfoB);
 		}
 
-		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().isEmpty())
+		if(!odom.data().laserScanRaw().isEmpty())
 		{
-			//inspired from pointcloud_to_laserscan package
+			if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
-			sensor_msgs::LaserScan msg;
-			msg.header.frame_id = scanFrameId;
-			msg.header.stamp = time;
-
-			msg.angle_min = scanAngleMin;
-			msg.angle_max = scanAngleMax;
-			msg.angle_increment = scanAngleIncrement;
-			msg.time_increment = 0.0;
-			msg.scan_time = scanTime;
-			msg.range_min = scanRangeMin;
-			msg.range_max = scanRangeMax;
-
-			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().data();
-			for (int i=0; i<scan.cols; ++i)
 			{
-				const float * ptr = scan.ptr<float>(0,i);
+				//inspired from pointcloud_to_laserscan package
-				double range = hypot(ptr[0], ptr[1]);
+				sensor_msgs::LaserScan msg;
-				if (range >= scanRangeMin && range <=scanRangeMax)
+				msg.header.frame_id = scanFrameId;
+				msg.header.stamp = time;
+
+				msg.angle_min = scanAngleMin;
+				msg.angle_max = scanAngleMax;
+				msg.angle_increment = scanAngleIncrement;
+				msg.time_increment = 0.0;
+				msg.scan_time = 0;
+				msg.range_min = scanRangeMin;
+				msg.range_max = scanRangeMax;
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
 				{
-					double angle = atan2(ptr[1], ptr[0]);
+					msg.angle_min = odom.data().laserScanRaw().angleMin();
-					if (angle >= msg.angle_min && angle <= msg.angle_max)
+					msg.angle_max = odom.data().laserScanRaw().angleMax();
+					msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
+					msg.range_min = odom.data().laserScanRaw().rangeMin();
+					msg.range_max = odom.data().laserScanRaw().rangeMax();
+				}
+
+				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().data();
+				for (int i=0; i<scan.cols; ++i)
+				{
+					const float * ptr = scan.ptr<float>(0,i);
+					double range = hypot(ptr[0], ptr[1]);
+					if (range >= msg.range_min && range <=msg.range_max)
 					{
-						int index = (angle - msg.angle_min) / msg.angle_increment;
+						double angle = atan2(ptr[1], ptr[0]);
-						if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
+						if (angle >= msg.angle_min && angle <= msg.angle_max)
 						{
-							msg.ranges[index] = range;
+							int index = (angle - msg.angle_min) / msg.angle_increment;
+							if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
+							{
+								msg.ranges[index] = range;
+							}
 						}
 					}
 				}
-			}
 
-			scanPub.publish(msg);
+				scanPub.publish(msg);
+			}
+			else if(scanCloudPub.getNumSubscribers())
+			{
+				sensor_msgs::PointCloud2 msg;
+				pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
+				msg.header.frame_id = scanFrameId;
+				msg.header.stamp = time;
+				scanCloudPub.publish(msg);
+			}
 		}
 
 		if(odom.data().userDataRaw().type() == CV_8SC1 &&

src/MsgConversion.cpp

@@ -936,7 +936,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point);
 	msg.grid_cell_size = signature.sensorData().gridCellSize();
 	msg.laserScanMaxPts = signature.sensorData().laserScanCompressed().maxPoints();
-	msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().maxRange();
+	msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().rangeMax();
 	msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
 	transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
 	msg.baseline = 0;