Driver for lidar Ouster OS1-64 in folder src/Sensor

.gitignore

@@ -2,6 +2,7 @@
 /Debug/
 /Release/
 /build/
+.vscode/
 *.prefs
 .project
 .cproject

README.md

@@ -27,17 +27,17 @@ External camera (Click to jump to YouTube):
 
 
 ## Build & Run
-###[Build on NVIDIA Jetson TX1](https://github.com/yankailab/OpenKAI/blob/master/doc/JetsonTX1/build.md)
+### [Build on NVIDIA Jetson TX1](https://github.com/yankailab/OpenKAI/blob/master/doc/JetsonTX1/build.md)
 
-###[Build on Ubuntu desktop systems](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/build.md)
+### [Build on Ubuntu desktop systems](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/build.md)
 
 ## Application implementation
 There are two ways of implement an application based on OpenKAI: compilation vs. script.
 
 ### Compilation
 OpenKAI is implemented purely in C++, it is very easy to implement your application as C/C++ and compile with OpenKAI together. Refer to "OpenKAI/src/Application/Startup.cpp" as a starting point for application integration, and this guide:
 
-###[Develop OpenKAI apps with Eclipse CDT](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/eclipse.md)
+### [Develop OpenKAI apps with Eclipse CDT](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/eclipse.md)
 
 ### Kiss (OpenKai Simple Script)
 If the existing function Modules from OpenKAI already meet your needs, you can write a json-like Kiss script to define your application. Note that Kiss is treated as a config file and will be parsed all at once at the beginning of OpenKAI program execution. The Modules defined by Kiss file are statically created and get started to run. Therefore there is no difference in execution speed between the Compilation way and application defined by Kiss (To be updated).

kiss/test/_LeddarVu.kiss

@@ -8,31 +8,18 @@
 	"waitKey":30,
 	"presetDir":"/home/ubuntu/src/OpenKAI/data/",
 }
-
+/*
 {
 	"name":"OKview",
 	"class":"Window",
 	"bInst":1,
-	"bFullScreen":1,
+	"bFullScreen":0,
 	"bRec":0,
 	"recFile":"/media/ubuntu/data/OpenKAI_",
 	"recFPS":15,
 	"recCodec":"MJPG",	
 }
-
-{
-	"name":"camera",
-	"class":"_Camera",
-	"FPS":30,
-	"bInst":0,
-	"Window":"OKview",
-	"deviceID":1,
-	"width":1920,
-	"height":1080,
-	"fovV":60,
-	"fovH":90,
-}
-
+*/
 {
 	"name":"leddarVu0",
 	"class":"_LeddarVu",
@@ -68,75 +55,3 @@
 	"nAvr":3,
 	"nMed":3,
 }
-
-{
-	"name":"leddarVu1",
-	"class":"_LeddarVu",
-	"FPS":30,
-	"bInst":1,
-	"bLog":1,
-	"Window":"OKview",
-	"rMin":0.1,
-	"rMax":100.0,
-	"fovH":48,
-	"fovV":0.3,
-	"nDiv":8,
-	"showScale":30.0,
-	"showDegOffset":-24.0,
-	"showOriginOffsetX":0.5,
-	"showOriginOffsetY":1.0,
-	"_VisionBase":"camera",
-	"portName":"/dev/ttyACM1",
-	"baud":115200,
-	"bUse0x41":0,
-	"slaveAddr":1,
-	"nAccumulationsExpo":5,
-	"nOversamplingsExpo":1,
-	"nPoint":20,
-	"lightSrcPwr":100,
-	"bAutoLightSrcPwr":0,
-	"bDemergeObj":1,
-	"bStaticNoiseRemoval":1,
-	"bPrecision":1,
-	"bSaturationCompensation":1,
-	"bOvershootManagement":1,
-	"oprMode":1,
-	"nAvr":3,
-	"nMed":3,
-}
-
-{
-	"name":"leddarVu2",
-	"class":"_LeddarVu",
-	"FPS":30,
-	"bInst":1,
-	"bLog":1,
-	"Window":"OKview",
-	"rMin":0.1,
-	"rMax":100.0,
-	"fovH":48,
-	"fovV":0.3,
-	"nDiv":8,
-	"showScale":30.0,
-	"showDegOffset":-24.0,
-	"showOriginOffsetX":0.5,
-	"showOriginOffsetY":1.0,
-	"_VisionBase":"camera",
-	"portName":"/dev/ttyACM2",
-	"baud":115200,
-	"bUse0x41":0,
-	"slaveAddr":1,
-	"nAccumulationsExpo":5,
-	"nOversamplingsExpo":1,
-	"nPoint":20,
-	"lightSrcPwr":100,
-	"bAutoLightSrcPwr":0,
-	"bDemergeObj":1,
-	"bStaticNoiseRemoval":1,
-	"bPrecision":1,
-	"bSaturationCompensation":1,
-	"bOvershootManagement":1,
-	"oprMode":1,
-	"nAvr":3,
-	"nMed":3,
-}

kiss/test/_LidarOuster.kiss

@@ -0,0 +1,32 @@
+{
+	"name":"APP",
+	"class":"Startup",
+	"appName":"OpenKAI",
+	"bWindow":0,
+	"bDraw":0,
+	"bLog":0,
+	"waitKey":30,
+	"presetDir":"/home/ubuntu/src/OpenKAI/data/",
+}
+/*
+{
+	"name":"OKview",
+	"class":"Window",
+	"bInst":1,
+	"bFullScreen":0,
+	"bRec":0,
+	"recFile":"/media/ubuntu/data/OpenKAI_",
+	"recFPS":15,
+	"recCodec":"MJPG",	
+}
+*/
+{
+	"name":"LidarOuster",
+	"class":"_LidarOuster",
+	"FPS":10000,
+	"bInst":1,
+	"bLog":1,
+	"Window":"OKview",
+	"hostname":"192.168.100.70",
+	"udp_dest_host": "192.168.100.95",
+}

src/Config/Module.cpp

@@ -32,6 +32,7 @@ BASE* Module::createInstance(Kiss* pK)
 	ADD_MODULE(_Lane);
 	ADD_MODULE(_LidarSlam);
 	ADD_MODULE(_LeddarVu);
+	ADD_MODULE(_LidarOuster);
 	ADD_MODULE(_Mavlink);
 	ADD_MODULE(_Path);
 	ADD_MODULE(PIDctrl);

src/Config/Module.h

@@ -37,14 +37,20 @@
 #include "../Protocol/_Mavlink.h"
 #include "../Protocol/_RC.h"
 
+#include "../Sensor/LidarOuster/_LidarOuster.h"
 #include "../Sensor/_LeddarVu.h"
 #include "../Sensor/RPLIDAR/_RPLIDAR.h"
 #include "../SLAM/_ORB_SLAM2.h"
 #include "../SLAM/_LidarSlam.h"
 
+#ifdef USE_OPENCV_CONTRIB
 #include "../Tracker/_SingleTracker.h"
 #include "../Tracker/_MultiTracker.h"
+#endif
+
+#ifdef USE_OPENTRACKER
 #include "../Tracker/_EcoTracker.h"
+#endif
 
 #include "../UI/Window.h"
 

src/Sensor/LidarOuster/_LidarOuster.cpp

@@ -0,0 +1,136 @@
+/*
+ *  Created on: Nov 24, 2017
+ *      Author: yankai
+ */
+#include "_LidarOuster.h"
+
+namespace kai
+{
+
+_LidarOuster::_LidarOuster()
+{
+	m_hostname = "";
+	m_udp_dest_host = "";
+}
+
+_LidarOuster::~_LidarOuster()
+{
+	m_bThreadON = false;
+}
+
+void _LidarOuster::handle_lidar(uint8_t *buf)
+{
+	m_n_lidar_packets++;
+	m_lidar_col_0_ts = col_timestamp(nth_col(0, buf));
+	m_lidar_col_0_h_angle = col_h_angle(nth_col(0, buf));
+}
+
+void _LidarOuster::handle_imu(uint8_t *buf)
+{
+	m_n_imu_packets++;
+	m_imu_ts = imu_sys_ts(buf);
+	m_imu_av_z = imu_av_z(buf);
+	m_imu_la_y = imu_la_y(buf);
+}
+
+bool _LidarOuster::init(void *pKiss)
+{
+	IF_F(!this->_ThreadBase::init(pKiss));
+	Kiss *pK = (Kiss *)pKiss;
+
+	KISSm(pK, hostname);
+	KISSm(pK, udp_dest_host);
+	return true;
+}
+
+bool _LidarOuster::start(void)
+{
+	m_bThreadON = true;
+	int retCode = pthread_create(&m_threadID, 0, getUpdateThread, this);
+	if (retCode != 0)
+	{
+		LOG_E(retCode);
+		m_bThreadON = false;
+		return false;
+	}
+
+	return true;
+}
+
+void _LidarOuster::update(void)
+{
+	while (m_bThreadON)
+	{
+		if (!m_flag_open)
+		{
+			m_flag_open = open();
+			if (!m_flag_open)
+			{
+				this->sleepTime(USEC_1SEC);
+				continue;
+			}
+		}
+
+		this->autoFPSfrom();
+		updateLidar();
+		this->autoFPSto();
+	}
+}
+
+bool _LidarOuster::open(void)
+{
+	m_cli = init_client(m_hostname, m_udp_dest_host, 7502, 7503);
+	if (!m_cli)
+	{
+		std::cerr << "Failed to connect to : " << m_hostname << std::endl;
+		return false;
+	}
+	return true;
+}
+
+void _LidarOuster::updateLidar(void)
+{
+	client_state st = poll_client(*m_cli);
+	if (st & ERROR)
+	{
+		std::cerr << "Fail to update Lidar" << std::endl;
+	}
+	else if (st & LIDAR_DATA)
+	{
+		if (read_lidar_packet(*m_cli, m_lidar_buf))
+			handle_lidar(m_lidar_buf);
+	}
+	else if (st & IMU_DATA)
+	{
+		if (read_imu_packet(*m_cli, m_imu_buf))
+			handle_imu(m_imu_buf);
+	}
+}
+
+bool _LidarOuster::console(int &iY)
+{
+	IF_F(!this->_ThreadBase::console(iY));
+
+	string msg;
+	msg += "n_lidar_packets=" + li2str(m_n_lidar_packets);
+	msg += "; col_0_azimuth=" + f2str(m_lidar_col_0_h_angle, 6);
+	msg += "; col_0_ts=" + li2str(m_lidar_col_0_ts);
+
+	COL_MSG;
+	iY++;
+	mvaddstr(iY, CONSOLE_X_MSG, msg.c_str());
+
+	msg = "";
+	msg += "n_imu_packets=" + li2str(m_n_imu_packets);
+	msg += "; im_av_z=" + f2str(m_imu_av_z, 6);
+	msg += "; im_la_y=" + f2str(m_imu_la_y, 6);
+	msg += "; imu_ts=" + li2str(m_imu_ts);
+
+	COL_MSG;
+	iY++;
+	mvaddstr(iY, CONSOLE_X_MSG, msg.c_str());
+
+	return true;
+}
+
+} // namespace kai