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,9 +1209,17 @@ 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,
+							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,
@@ -1468,9 +1476,17 @@ 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,
+							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,

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,9 +287,15 @@ 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);
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+				{
+					ROS_INFO("Scan will be published.");
+				}
+				else
+				{
 					ROS_INFO("Scan will be published with those parameters:");
 					ROS_INFO("  scan_angle_min=%f", scanAngleMin);
 					ROS_INFO("  scan_angle_max=%f", scanAngleMax);
@@ -298,6 +304,12 @@ int main(int argc, char** argv)
 					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.");
+			}
+		}
 
 		// publish transforms first
 		if(publishTf)
@@ -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,7 +456,9 @@ int main(int argc, char** argv)
 			rightCamInfoPub.publish(camInfoB);
 		}
 
-		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().isEmpty())
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
 			{
 				//inspired from pointcloud_to_laserscan package
 				sensor_msgs::LaserScan msg;
@@ -455,9 +469,17 @@ int main(int argc, char** argv)
 				msg.angle_max = scanAngleMax;
 				msg.angle_increment = scanAngleIncrement;
 				msg.time_increment = 0.0;
-			msg.scan_time = scanTime;
+				msg.scan_time = 0;
 				msg.range_min = scanRangeMin;
 				msg.range_max = scanRangeMax;
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+				{
+					msg.angle_min = odom.data().laserScanRaw().angleMin();
+					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);
@@ -467,7 +489,7 @@ int main(int argc, char** argv)
 				{
 					const float * ptr = scan.ptr<float>(0,i);
 					double range = hypot(ptr[0], ptr[1]);
-				if (range >= scanRangeMin && range <=scanRangeMax)
+					if (range >= msg.range_min && range <=msg.range_max)
 					{
 						double angle = atan2(ptr[1], ptr[0]);
 						if (angle >= msg.angle_min && angle <= msg.angle_max)
@@ -483,6 +505,15 @@ int main(int argc, char** argv)
 
 				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 &&
 		   odom.data().userDataRaw().cols >= 7 && // including null str ending

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;