wj_lidar_encapsulation.cpp 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257
  1. #include "wj_lidar_encapsulation.h"
  2. /**
  3. * 初始化连接
  4. * */
  5. void Wj_lidar_encapsulation::init_tcp_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_, Wj_lidar_encapsulation *p_handle)
  6. {
  7. if (p_handle == 0)
  8. {
  9. LOG(WARNING) << "ip: " << addr << " 万集异步读写线程参数错误";
  10. return;
  11. }
  12. boost::asio::ip::tcp::endpoint ep(boost::asio::ip::address_v4::from_string(addr), port);
  13. while (!p_handle->mb_exit)
  14. {
  15. if (*client_ == 0)
  16. {
  17. *client_ = new Async_Client(p_handle->m_io_service, ep, fundata_, p_handle);
  18. }
  19. if (*client_ != 0)
  20. {
  21. if(!(*client_)->initialize())
  22. {
  23. LOG(WARNING) << "ip: " << addr << " 万集异步读写客户端初始化失败";
  24. }
  25. if((*client_)->mb_exit){
  26. break;
  27. }
  28. }
  29. p_handle->m_io_service.run();
  30. LOG(WARNING) << "ip: " << addr << " 万集异步读写服务中止";
  31. usleep(1000 * 1000);
  32. }
  33. LOG(WARNING) << "ip: " << addr << " 万集异步读写线程退出";
  34. }
  35. /**
  36. * 创建初始化与读写线程以及连接超时判断
  37. * */
  38. Error_manager Wj_lidar_encapsulation::boost_tcp_init_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_)
  39. {
  40. int timecnt = 0;
  41. *client_ = NULL;
  42. m_client_thread = new std::thread(&init_tcp_connection, addr, port, client_, fundata_, this);
  43. m_client_thread->detach();
  44. // boost::thread tmp(&init_tcp_connection, addr, port, client_, fundata_, this);
  45. // tmp.detach();
  46. while (timecnt < 50)
  47. {
  48. timecnt++;
  49. usleep(20000); //20 ms
  50. if ((*client_)->client_initialize_status() && (*client_)->client_connection_status())
  51. {
  52. return Error_manager(Error_code::SUCCESS);
  53. }
  54. }
  55. // delete *client_;
  56. // *client_ = NULL;
  57. return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
  58. }
  59. // /**
  60. // * 异步写指令
  61. // * */
  62. // Error_manager Wj_lidar_encapsulation::boost_tcp_async_send(Async_Client *client_, const char *msg, const int len)
  63. // {
  64. // if (client_ == NULL || client_->client_connection_status() == 0)
  65. // {
  66. // LOG(WARNING) << "not connected , need to connect first \n";
  67. // return Error_manager(Error_code::WJ_LIDAR_WRITE_FAILED);
  68. // }
  69. // else
  70. // {
  71. // client_->client_async_write((char *)msg, len);
  72. // return Error_manager(Error_code::SUCCESS);
  73. // }
  74. // }
  75. // /**
  76. // * 异步读数据
  77. // * */
  78. // Error_manager Wj_lidar_encapsulation::boost_tcp_async_read(Async_Client *client_)
  79. // {
  80. // if (client_ == NULL || client_->client_connection_status() == 0)
  81. // {
  82. // LOG(WARNING) << "not connected , need to connect first \n";
  83. // return Error_manager(Error_code::WJ_LIDAR_READ_FAILED);
  84. // }
  85. // else
  86. // {
  87. // client_->client_async_read();
  88. // return Error_manager(Error_code::SUCCESS);;
  89. // }
  90. // }
  91. /**
  92. * 万集读数据回调
  93. *
  94. * */
  95. void Wj_lidar_encapsulation::read_callback(const char *data, const int len, const void *handle)
  96. {
  97. if (handle != 0)
  98. {
  99. Wj_lidar_encapsulation *p_wj_temp = (Wj_lidar_encapsulation *)handle;
  100. if (p_wj_temp->mp_protocol != 0 && p_wj_temp->mp_protocol->get_initialize_status())
  101. {
  102. // LOG(INFO) << "read callback, try to process data";
  103. if (p_wj_temp->mp_protocol->data_process(data, len) != SUCCESS)
  104. {
  105. LOG(WARNING) << p_wj_temp->m_ip << " data process failed, exceed max length";
  106. }
  107. else
  108. {
  109. // LOG(INFO) << "read callback, data processed";
  110. }
  111. }
  112. else
  113. {
  114. LOG(WARNING) << p_wj_temp->m_ip << " protocol null pointer or not initialized";
  115. }
  116. }
  117. }
  118. /**
  119. * 外部调用获取连接状态
  120. * */
  121. bool Wj_lidar_encapsulation::get_connection_status(){
  122. if(mp_client== 0 || !mp_client->client_initialize_status()){
  123. return false;
  124. }
  125. return mp_client->client_connection_status();
  126. }
  127. /**
  128. * 外部调用获取初始化状态
  129. * */
  130. bool Wj_lidar_encapsulation::get_initialize_status()
  131. {
  132. return mb_initialized;
  133. }
  134. /**
  135. * 外部调用获取点云
  136. * */
  137. Error_manager Wj_lidar_encapsulation::get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
  138. std::chrono::steady_clock::time_point command_time, int timeout_milli)
  139. {
  140. // 1. 检查参数合法性
  141. if (!mb_initialized || mp_protocol == 0 || !mp_protocol->get_initialize_status())
  142. {
  143. return Error_manager(Error_code::WJ_LIDAR_UNINITIALIZED);
  144. }
  145. // 2. 获取扫描时间,时间点位于指令时刻之后,则获得的点云合法
  146. // 若当前时间超过(指令时刻+超时时间段),则获取点云函数超时退出
  147. int current_duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - command_time).count();
  148. while (current_duration < timeout_milli)
  149. {
  150. current_duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - command_time).count();
  151. std::chrono::steady_clock::time_point scan_time_temp = mp_protocol->get_scan_time();
  152. int cloud_duration = std::chrono::duration_cast<std::chrono::milliseconds>(scan_time_temp - command_time).count();
  153. // 扫描时刻位于指令时刻之后
  154. if (cloud_duration > 0)
  155. {
  156. Error_manager code = mp_protocol->get_scan_points(cloud);
  157. // LOG(INFO) << "耗时(ms): " << current_duration;
  158. // 内部已打印错误信息
  159. return code;
  160. }
  161. usleep(1000 * 50);
  162. }
  163. return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT);
  164. }
  165. /**
  166. * 万集雷达封装类初始化
  167. * */
  168. Error_manager Wj_lidar_encapsulation::initialize(wj::wjLidarParams params)
  169. {
  170. // 1. 判断网络参数合法性并初始化异步读写实例
  171. if (params.has_net_config())
  172. {
  173. m_ip = params.net_config().ip_address();
  174. m_port = params.net_config().port();
  175. if (boost_tcp_init_connection(m_ip.c_str(), m_port, &mp_client, &read_callback) != SUCCESS)
  176. {
  177. LOG(WARNING) << m_ip << " 1 sec timeout, connection failed, will try to reconnect";
  178. return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
  179. }
  180. else
  181. {
  182. LOG(INFO) << m_ip << " is connected";
  183. }
  184. }
  185. else
  186. {
  187. LOG(ERROR) << "parameter error, without net config params";
  188. return Error_manager(Error_code::PARAMETER_ERROR);
  189. }
  190. // 2. 创建协议栈对象
  191. mp_protocol = new Wj_716_lidar_protocol();
  192. Error_manager code = mp_protocol->initialize(params);
  193. if (code != SUCCESS)
  194. {
  195. LOG(ERROR) << "wj_lidar " << m_ip << ":" << m_port << " initialize failed. " << code.to_string();
  196. }
  197. else
  198. {
  199. mb_initialized = true;
  200. LOG(INFO) << "***** wj_lidar " << m_ip << ":" << m_port << " ***** successfully initialized.";
  201. }
  202. return code;
  203. }
  204. // 万集雷达封装类构造函数
  205. Wj_lidar_encapsulation::Wj_lidar_encapsulation() : mb_initialized(false),
  206. mp_protocol(0),
  207. mp_client(0),
  208. mb_exit(0)
  209. {
  210. }
  211. // 万集雷达封装类析构函数
  212. Wj_lidar_encapsulation::~Wj_lidar_encapsulation()
  213. {
  214. mb_exit = true;
  215. if (mp_client != 0)
  216. {
  217. mp_client->close();
  218. delete mp_client;
  219. mp_client = 0;
  220. }
  221. LOG(INFO) << "client released";
  222. if (m_client_thread != 0)
  223. {
  224. if (m_client_thread->joinable())
  225. {
  226. m_client_thread->join();
  227. }
  228. delete m_client_thread;
  229. m_client_thread = 0;
  230. }
  231. LOG(INFO) << "client thread released";
  232. if (mp_protocol != 0)
  233. {
  234. delete mp_protocol;
  235. mp_protocol = 0;
  236. }
  237. LOG(INFO) << "protocol released";
  238. }