Updated LidarViewer with ouster

CMakeLists.txt

@@ -205,7 +205,7 @@ option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_XVSDK         "Include XVisio SDK support"           OFF)
-option(WITH_OUSTER        "INclude Ouster SDK support"           ON)
+option(WITH_OUSTER        "Include Ouster SDK support"           ON)
 option(WITH_OCTOMAP       "Include OctoMap support"              ON)
 option(WITH_GRIDMAP       "Include GridMap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)

examples/LidarMapping/main.cpp

@@ -48,8 +48,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-lidar_mapping [OPTIONS] 0 IP PORT\n"
+			"rtabmap-lidar_mapping [OPTIONS] DRIVER ...\n"
-			"rtabmap-lidar_mapping [OPTIONS] 1 IP\n"
+			"rtabmap-lidar_mapping [OPTIONS] 0 IP PORT (with VLP16 LiDAR)\n"
+			"rtabmap-lidar_mapping [OPTIONS] 1 IP      (with Ouster LiDAR)\n"
 			"rtabmap-lidar_mapping [OPTIONS] DRIVER PCAP_FILEPATH [JSON_FILEPATH]\n"
 			"\n"
 			"DRIVER: 0=VLP16, 1=Ouster\n"
@@ -68,7 +69,7 @@ using namespace rtabmap;
 int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kWarning);
+	ULogger::setLevel(ULogger::kInfo);
 
 	std::string pcapFile;
 	std::string jsonFile;
@@ -306,5 +307,7 @@ int main(int argc, char * argv[])
 		printf("Saving rtabmap_trajectory.txt... failed!\n");
 	}
 
+	rtabmap->close(false);
+
 	return 0;
 }

tools/CMakeLists.txt

@@ -22,9 +22,7 @@ ENDIF(OPENCV_NONFREE_FOUND)
 
 IF(TARGET rtabmap_gui)
 	ADD_SUBDIRECTORY( CameraRGBD )
-	IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8")
 	ADD_SUBDIRECTORY( LidarViewer )
-	ENDIF()
 	ADD_SUBDIRECTORY( DatabaseViewer )
 	ADD_SUBDIRECTORY( EpipolarGeometry )
 	ADD_SUBDIRECTORY( OdometryViewer )

tools/LidarViewer/main.cpp

@@ -37,7 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UConversion.h"
-#include <pcl/visualization/cloud_viewer.h>
 #include <stdio.h>
 #include <signal.h>
 
@@ -49,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/console/parse.h>
 #include <rtabmap/core/lidar/LidarVLP16.h>
+#include <rtabmap/core/lidar/LidarOuster.h>
 
 using namespace pcl;
 using namespace pcl::console;
@@ -57,8 +57,14 @@ using namespace pcl::visualization;
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-lidar_viewer IP PORT driver\n"
+			"rtabmap-lidar_viewer DRIVER IP [PORT]\n"
-			"  driver       Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n");
+			"\n"
+			"DRIVER: 0=VLP16, 1=Ouster\n"
+			"PORT:   should be set if DRIVER=0\n"
+			"\n"
+			"Examples:\n"
+			"  rtabmap-lidar_viewer 0 192.168.1.201 2368\n"
+			"  rtabmap-lidar_viewer 1 192.168.1.2\n\n");
 	exit(1);
 }
 
@@ -80,28 +86,45 @@ int main(int argc, char * argv[])
 	int driver = 0;
 	std::string ip;
 	int port = 2368;
-	if(argc < 4)
+	if(argc < 3)
 	{
+		printf("Not enough arguments.\n");
 		showUsage();
 	}
 	else
 	{
-		ip = argv[1];
+		driver = uStr2Int(argv[1]);
-		port = uStr2Int(argv[2]);
+		if(driver < 0 || driver > 1)
-		driver = atoi(argv[3]);
-		if(driver < 0 || driver > 0)
 		{
-			UERROR("driver should be 0.");
+			printf("driver should be 0 or 1.\n");
+			showUsage();
+		}
+
+		ip = argv[2];
+		if(driver==0)
+		{
+			if(argc>=3)
+			{
+				port = uStr2Int(argv[3]);
+			}
+			else
+			{
+				printf("IP and PORT should be set with VLP16 driver\n");
 				showUsage();
 			}
 		}
-	printf("Using driver %d (ip=%s port=%d)\n", driver, ip.c_str(), port);
+	}
+	printf("Using driver %d (ip=%s%s)\n", driver, ip.c_str(), driver==0?uFormat(" port=%d", port).c_str():"");
 
-	rtabmap::LidarVLP16 * lidar = 0;
+	rtabmap::Lidar * lidar = 0;
 	if(driver == 0)
 	{
 		lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
 	}
+	else if(driver == 1)
+	{
+		lidar = new rtabmap::LidarOuster(ip);
+	}
 	else
 	{
 		UFATAL("");
@@ -109,7 +132,7 @@ int main(int argc, char * argv[])
 
 	if(!lidar->init())
 	{
-		printf("Lidar init failed! Please select another driver (see \"--help\").\n");
+		printf("Lidar init failed!\n");
 		delete lidar;
 		exit(1);
 	}
@@ -121,18 +144,18 @@ int main(int argc, char * argv[])
 	signal(SIGTERM, &sighandler);
 	signal(SIGINT, &sighandler);
 
-	rtabmap::SensorData data = lidar->takeScan();
+	rtabmap::SensorData data = lidar->takeData();
 	while(!data.laserScanRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform());
-		viewer->showCloud(cloud, "cloud");
 		printf("Scan size: %ld points\n", cloud->size());
+		viewer->showCloud(cloud, "cloud");
 
 		int c = cv::waitKey(10); // wait 10 ms or for key stroke
 		if(c == 27)
 			break; // if ESC, break and quit
 
-		data = lidar->takeScan();
+		data = lidar->takeData();
 	}
 	printf("Closing...\n");
 	if(viewer)