lds_lidar.cpp 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622
  1. //
  2. // The MIT License (MIT)
  3. //
  4. // Copyright (c) 2019 Livox. All rights reserved.
  5. //
  6. // Permission is hereby granted, free of charge, to any person obtaining a copy
  7. // of this software and associated documentation files (the "Software"), to deal
  8. // in the Software without restriction, including without limitation the rights
  9. // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  10. // copies of the Software, and to permit persons to whom the Software is
  11. // furnished to do so, subject to the following conditions:
  12. //
  13. // The above copyright notice and this permission notice shall be included in
  14. // all copies or substantial portions of the Software.
  15. //
  16. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  19. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21. // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  22. // SOFTWARE.
  23. //
  24. #include "lds_lidar.h"
  25. #include <stdio.h>
  26. #include <string.h>
  27. #include <thread>
  28. #include <memory>
  29. namespace livox
  30. {
  31. /** Const varible ------------------------------------------------------------------------------- */
  32. /** User add broadcast code here */
  33. LidarDevice lidars_[kMaxLidarCount];
  34. bool is_initialized_=false;
  35. bool is_created=false;
  36. thread_safe_map<std::string,LdsLidar*> g_snPtr;
  37. static const char *local_broadcast_code_list[] = {
  38. "000000000000001",
  39. };
  40. /** Lds lidar function ---------------------------------------------------------------------------*/
  41. LdsLidar::LdsLidar()
  42. {
  43. if(is_created==false)
  44. {
  45. auto_connect_mode_ = true;
  46. whitelist_count_ = 0;
  47. is_initialized_ = false;
  48. lidar_count_ = 0;
  49. memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
  50. memset(lidars_, 0, sizeof(lidars_));
  51. for (uint32_t i = 0; i < kMaxLidarCount; i++)
  52. {
  53. lidars_[i].handle = kMaxLidarCount;
  54. /** Unallocated state */
  55. lidars_[i].connect_state = kConnectStateOff;
  56. }
  57. is_created=true;
  58. }
  59. }
  60. LdsLidar::~LdsLidar()
  61. {
  62. }
  63. int LdsLidar::AddBroadcastCode(std::string sn)
  64. {
  65. if(g_snPtr.find(sn)==true)
  66. {
  67. printf(" %s has added \n",sn.c_str());
  68. return -1;
  69. }
  70. g_snPtr[sn]=this;
  71. return 0;
  72. }
  73. void LdsLidar::LidarDataCallback(uint8_t handle, LivoxEthPacket *data,uint32_t data_num)
  74. {
  75. printf("data recieved ------------------------------------ \n");
  76. }
  77. int LdsLidar::InitLdsLidar()
  78. {
  79. if (is_initialized_==false)
  80. {
  81. if (!Init())
  82. {
  83. Uninit();
  84. printf("Livox-SDK init fail!\n");
  85. return -1;
  86. }
  87. LivoxSdkVersion _sdkversion;
  88. GetLivoxSdkVersion(&_sdkversion);
  89. printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
  90. }
  91. SetBroadcastCallback(LdsLidar::OnDeviceBroadcast);
  92. SetDeviceStateUpdateCallback(LdsLidar::OnDeviceChange);
  93. /** Add commandline input broadcast code */
  94. /*for (auto input_str : broadcast_code_strs)
  95. {
  96. LdsLidar::AddBroadcastCodeToWhitelist(input_str.c_str());
  97. }*/
  98. /** Add local broadcast code */
  99. /*LdsLidar::AddLocalBroadcastCode();
  100. if (whitelist_count_)
  101. {
  102. LdsLidar::DisableAutoConnectMode();
  103. printf("Disable auto connect mode!\n");
  104. printf("List all broadcast code in whiltelist:\n");
  105. for (uint32_t i = 0; i < whitelist_count_; i++)
  106. {
  107. printf("%s\n", broadcast_code_whitelist_[i]);
  108. }
  109. }
  110. else
  111. {
  112. LdsLidar::EnableAutoConnectMode();
  113. printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
  114. }*/
  115. /** Start livox sdk to receive lidar data */
  116. if (!Start())
  117. {
  118. Uninit();
  119. printf("Livox-SDK init fail!\n");
  120. return -1;
  121. }
  122. printf(" start -----------\n");
  123. /** Add here, only for callback use */
  124. is_initialized_ = true;
  125. printf("Livox-SDK init success!\n");
  126. return 0;
  127. }
  128. int LdsLidar::DeInitLdsLidar(void)
  129. {
  130. if (!is_initialized_)
  131. {
  132. printf("LiDAR data source is not exit");
  133. return -1;
  134. }
  135. Uninit();
  136. printf("Livox SDK Deinit completely!\n");
  137. return 0;
  138. }
  139. /** Static function in LdsLidar for callback or event process ------------------------------------*/
  140. /** Receiving point cloud data from Livox LiDAR. */
  141. void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data,
  142. uint32_t data_num, void *client_data)
  143. {
  144. using namespace std;
  145. LdsLidar *lidar_this = static_cast<LdsLidar *>(client_data);
  146. //printf("receive handle %d ptr %p\n", handle,client_data);
  147. if(lidar_this!= nullptr)
  148. lidar_this->LidarDataCallback(handle,data,data_num);
  149. /*LivoxEthPacket *eth_packet = data;
  150. if (!data || !data_num || (handle >= kMaxLidarCount))
  151. {
  152. return;
  153. }
  154. if (eth_packet)
  155. {
  156. lidar_this->data_recveive_count_[handle]++;
  157. if (lidar_this->data_recveive_count_[handle] % 100 == 0)
  158. {
  159. printf("receive packet count %d %d\n", handle, lidar_this->data_recveive_count_[handle]);
  160. uint64_t cur_timestamp = *((uint64_t *) (data->timestamp));
  161. if (data->data_type == kCartesian)
  162. {
  163. printf("1\n");
  164. LivoxRawPoint *p_point_data = (LivoxRawPoint *) data->data;
  165. }
  166. else if (data->data_type == kSpherical)
  167. {
  168. LivoxSpherPoint *p_point_data = (LivoxSpherPoint *) data->data;
  169. printf("2\n");
  170. }
  171. else if (data->data_type == kExtendCartesian)
  172. {
  173. LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *) data->data;
  174. }
  175. else if (data->data_type == kExtendSpherical)
  176. {
  177. printf("4\n");
  178. LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *) data->data;
  179. }
  180. else if (data->data_type == kDualExtendCartesian)
  181. {
  182. LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *) data->data;
  183. }
  184. else if (data->data_type == kDualExtendSpherical)
  185. {
  186. LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *) data->data;
  187. }
  188. else if (data->data_type == kImu)
  189. {
  190. LivoxImuPoint *p_point_data = (LivoxImuPoint *) data->data;
  191. }
  192. else if (data->data_type == kTripleExtendCartesian)
  193. {
  194. LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *) data->data;
  195. }
  196. else if (data->data_type == kTripleExtendSpherical)
  197. {
  198. LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *) data->data;
  199. }
  200. }
  201. }*/
  202. }
  203. void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
  204. {
  205. if (info == nullptr)
  206. {
  207. return;
  208. }
  209. if (info->dev_type == kDeviceTypeHub)
  210. {
  211. printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
  212. return;
  213. }
  214. /*if(g_snPtr.find(std::string(info->broadcast_code))==false)
  215. {
  216. printf("sn:%s has not added map size:%d\n",info->broadcast_code,g_snPtr.size());
  217. return;
  218. }*/
  219. if(!g_snPtr.find(info->broadcast_code))
  220. {
  221. return;
  222. }
  223. livox_status result = kStatusFailure;
  224. uint8_t handle = 0;
  225. result = AddLidarToConnect(info->broadcast_code, &handle);
  226. if (result == kStatusSuccess && handle < kMaxLidarCount)
  227. {
  228. LdsLidar *lidar = g_snPtr[info->broadcast_code];
  229. SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void *) lidar);
  230. LidarDevice *p_lidar = &(lidars_[handle]);
  231. p_lidar->handle = handle;
  232. p_lidar->connect_state = kConnectStateOff;
  233. p_lidar->config.enable_fan = true;
  234. p_lidar->config.return_mode = kStrongestReturn;
  235. p_lidar->config.coordinate = kCoordinateCartesian;
  236. p_lidar->config.imu_rate = kImuFreq200Hz;
  237. }
  238. else
  239. {
  240. printf("Add lidar to connect is failed : %d %d \n", result, handle);
  241. }
  242. }
  243. /** Callback function of changing of device state. */
  244. void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
  245. {
  246. if (info == nullptr)
  247. {
  248. return;
  249. }
  250. uint8_t handle = info->handle;
  251. if (handle >= kMaxLidarCount)
  252. {
  253. return;
  254. }
  255. if(g_snPtr.find(info->broadcast_code)==false)
  256. return;
  257. LdsLidar* ptr=g_snPtr[info->broadcast_code];
  258. LidarDevice *p_lidar = &(lidars_[handle]);
  259. if (type == kEventConnect)
  260. {
  261. QueryDeviceInformation(handle, DeviceInformationCb, ptr);
  262. if (p_lidar->connect_state == kConnectStateOff)
  263. {
  264. p_lidar->connect_state = kConnectStateOn;
  265. p_lidar->info = *info;
  266. }
  267. printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
  268. }
  269. else if (type == kEventDisconnect)
  270. {
  271. p_lidar->connect_state = kConnectStateOff;
  272. printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
  273. }
  274. else if (type == kEventStateChange)
  275. {
  276. p_lidar->info = *info;
  277. printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
  278. }
  279. if (p_lidar->connect_state == kConnectStateOn)
  280. {
  281. printf("Device Working State %d\n", p_lidar->info.state);
  282. if (p_lidar->info.state == kLidarStateInit)
  283. {
  284. printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
  285. }
  286. else
  287. {
  288. printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
  289. }
  290. printf("Device feature %d\n", p_lidar->info.feature);
  291. SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);
  292. if (p_lidar->info.state == kLidarStateNormal)
  293. {
  294. if (p_lidar->config.coordinate != 0)
  295. {
  296. SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, ptr);
  297. }
  298. else
  299. {
  300. SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, ptr);
  301. }
  302. p_lidar->config.set_bits |= kConfigCoordinate;
  303. if (kDeviceTypeLidarMid40 != info->type)
  304. {
  305. LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode) (p_lidar->config.return_mode), \
  306. LdsLidar::SetPointCloudReturnModeCb, ptr);
  307. p_lidar->config.set_bits |= kConfigReturnMode;
  308. }
  309. if (kDeviceTypeLidarMid40 != info->type && kDeviceTypeLidarMid70 != info->type)
  310. {
  311. LidarSetImuPushFrequency(handle, (ImuFreq) (p_lidar->config.imu_rate), \
  312. LdsLidar::SetImuRatePushFrequencyCb, ptr);
  313. p_lidar->config.set_bits |= kConfigImuRate;
  314. }
  315. p_lidar->connect_state = kConnectStateConfig;
  316. }
  317. }
  318. }
  319. /** Query the firmware version of Livox LiDAR. */
  320. void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \
  321. DeviceInformationResponse *ack, void *client_data)
  322. {
  323. if (status != kStatusSuccess)
  324. {
  325. printf("Device Query Informations Failed : %d\n", status);
  326. }
  327. if (ack)
  328. {
  329. printf("firm ver: %d.%d.%d.%d\n",
  330. ack->firmware_version[0],
  331. ack->firmware_version[1],
  332. ack->firmware_version[2],
  333. ack->firmware_version[3]);
  334. }
  335. }
  336. /** Callback function of Lidar error message. */
  337. void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message)
  338. {
  339. static uint32_t error_message_count = 0;
  340. if (message != NULL)
  341. {
  342. ++error_message_count;
  343. if (0 == (error_message_count % 100))
  344. {
  345. printf("handle: %u\n", handle);
  346. printf("temp_status : %u\n", message->lidar_error_code.temp_status);
  347. printf("volt_status : %u\n", message->lidar_error_code.volt_status);
  348. printf("motor_status : %u\n", message->lidar_error_code.motor_status);
  349. printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
  350. printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
  351. printf("pps_status : %u\n", message->lidar_error_code.device_status);
  352. printf("fan_status : %u\n", message->lidar_error_code.fan_status);
  353. printf("self_heating : %u\n", message->lidar_error_code.self_heating);
  354. printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
  355. printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
  356. printf("system_status : %u\n", message->lidar_error_code.system_status);
  357. }
  358. }
  359. }
  360. void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, \
  361. uint8_t response, void *client_data)
  362. {
  363. }
  364. void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
  365. uint8_t response, void *client_data)
  366. {
  367. LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
  368. if (handle >= kMaxLidarCount)
  369. {
  370. return;
  371. }
  372. LidarDevice *p_lidar = &(lidars_[handle]);
  373. if (status == kStatusSuccess)
  374. {
  375. p_lidar->config.set_bits &= ~((uint32_t) (kConfigReturnMode));
  376. printf("Set return mode success!\n");
  377. if (!p_lidar->config.set_bits)
  378. {
  379. LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
  380. p_lidar->connect_state = kConnectStateSampling;
  381. }
  382. }
  383. else
  384. {
  385. LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode) (p_lidar->config.return_mode), \
  386. LdsLidar::SetPointCloudReturnModeCb, lds_lidar);
  387. printf("Set return mode fail, try again!\n");
  388. }
  389. }
  390. void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
  391. uint8_t response, void *client_data)
  392. {
  393. LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
  394. if (handle >= kMaxLidarCount)
  395. {
  396. return;
  397. }
  398. LidarDevice *p_lidar = &(lidars_[handle]);
  399. if (status == kStatusSuccess)
  400. {
  401. p_lidar->config.set_bits &= ~((uint32_t) (kConfigCoordinate));
  402. printf("Set coordinate success!\n");
  403. if (!p_lidar->config.set_bits)
  404. {
  405. LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
  406. p_lidar->connect_state = kConnectStateSampling;
  407. }
  408. }
  409. else
  410. {
  411. if (p_lidar->config.coordinate != 0)
  412. {
  413. SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
  414. }
  415. else
  416. {
  417. SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
  418. }
  419. printf("Set coordinate fail, try again!\n");
  420. }
  421. }
  422. void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
  423. uint8_t response, void *client_data)
  424. {
  425. LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
  426. if (handle >= kMaxLidarCount)
  427. {
  428. return;
  429. }
  430. LidarDevice *p_lidar = &(lidars_[handle]);
  431. if (status == kStatusSuccess)
  432. {
  433. p_lidar->config.set_bits &= ~((uint32_t) (kConfigImuRate));
  434. printf("Set imu rate success!\n");
  435. if (!p_lidar->config.set_bits)
  436. {
  437. LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
  438. p_lidar->connect_state = kConnectStateSampling;
  439. }
  440. }
  441. else
  442. {
  443. LidarSetImuPushFrequency(handle, (ImuFreq) (p_lidar->config.imu_rate), \
  444. LdsLidar::SetImuRatePushFrequencyCb, lds_lidar);
  445. printf("Set imu rate fail, try again!\n");
  446. }
  447. }
  448. /** Callback function of starting sampling. */
  449. void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, \
  450. uint8_t response, void *client_data)
  451. {
  452. LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
  453. if (handle >= kMaxLidarCount)
  454. {
  455. return;
  456. }
  457. LidarDevice *p_lidar = &(lidars_[handle]);
  458. if (status == kStatusSuccess)
  459. {
  460. if (response != 0)
  461. {
  462. p_lidar->connect_state = kConnectStateOn;
  463. printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", \
  464. status, handle, response);
  465. }
  466. else
  467. {
  468. printf("Lidar start sample success\n");
  469. }
  470. }
  471. else if (status == kStatusTimeout)
  472. {
  473. p_lidar->connect_state = kConnectStateOn;
  474. printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n", \
  475. status, handle, response);
  476. }
  477. }
  478. /** Callback function of stopping sampling. */
  479. void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, \
  480. uint8_t response, void *client_data)
  481. {
  482. }
  483. /** Add broadcast code to whitelist */
  484. int LdsLidar::AddBroadcastCodeToWhitelist(const char *bd_code)
  485. {
  486. if (!bd_code || (strlen(bd_code) > kBroadcastCodeSize) || \
  487. (whitelist_count_ >= kMaxLidarCount))
  488. {
  489. return -1;
  490. }
  491. if (LdsLidar::FindInWhitelist(bd_code))
  492. {
  493. printf("%s is alrealy exist!\n", bd_code);
  494. return -1;
  495. }
  496. strcpy(broadcast_code_whitelist_[whitelist_count_], bd_code);
  497. ++whitelist_count_;
  498. return 0;
  499. }
  500. void LdsLidar::AddLocalBroadcastCode(void)
  501. {
  502. for (size_t i = 0; i < sizeof(local_broadcast_code_list) / sizeof(intptr_t); ++i)
  503. {
  504. std::string invalid_bd = "000000000";
  505. printf("Local broadcast code : %s\n", local_broadcast_code_list[i]);
  506. if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i]) + 1) && \
  507. (nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str())))
  508. {
  509. LdsLidar::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
  510. }
  511. else
  512. {
  513. printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
  514. }
  515. }
  516. }
  517. bool LdsLidar::FindInWhitelist(const char *bd_code)
  518. {
  519. if (!bd_code)
  520. {
  521. return false;
  522. }
  523. for (uint32_t i = 0; i < whitelist_count_; i++)
  524. {
  525. if (strncmp(bd_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0)
  526. {
  527. return true;
  528. }
  529. }
  530. return false;
  531. }
  532. }