rslidar_driver.cpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322
  1. #include "rslidar_driver.h"
  2. // 万集雷达封装类构造函数
  3. RS_lidar_device::RS_lidar_device()
  4. {
  5. m_config_file = config_path + "velodyne_manager.prototxt";
  6. m_rs_lidar_device_status = E_UNKNOWN;
  7. mb_exit_process = false;
  8. }
  9. // 万集雷达封装类析构函数
  10. RS_lidar_device::~RS_lidar_device()
  11. {
  12. uninit();
  13. }
  14. Error_manager RS_lidar_device::init() {
  15. velodyne::velodyneManagerParams t_velo_params;
  16. if (!proto_tool::read_proto_param(m_config_file, t_velo_params))
  17. {
  18. return Error_manager(VELODYNE_MANAGER_READ_PROTOBUF_ERROR, MINOR_ERROR, "velodyne_manager read_proto_param failed");
  19. }
  20. return init();
  21. }
  22. // 初始化
  23. Error_manager RS_lidar_device::init(velodyne::velodyneLidarParams params)
  24. {
  25. m_params = params;
  26. Error_manager t_error;
  27. LOG(INFO) << " RS_lidar_device::init " << this;
  28. m_rs_lidar_device_status = E_UNKNOWN;
  29. m_rs_protocol_status = E_UNKNOWN;
  30. m_lidar_id = params.lidar_id();
  31. // 设置雷达内参矩阵
  32. Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(params.calib().r()*M_PI/180.0, Eigen::Vector3d::UnitX());
  33. Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(params.calib().p()*M_PI/180.0, Eigen::Vector3d::UnitY());
  34. Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(params.calib().y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
  35. Eigen::Vector3d trans(params.calib().cx(), params.calib().cy(), params.calib().cz());
  36. m_calib_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() * rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
  37. // LOG(INFO) <<"lidar "<<m_lidar_id<<",,,\n"<< rot_x.toRotationMatrix()<<"------------------\n"<<rot_y.toRotationMatrix()<<"-----------------\n"<<rot_z.toRotationMatrix()<<"---------------\n"<<m_calib_matrix;
  38. m_lidar_params.input_type = InputType::ONLINE_LIDAR;
  39. m_lidar_params.input_param.msop_port = params.port(); ///< Set the lidar msop port number, the default is 6699
  40. m_lidar_params.input_param.difop_port = params.difop(); ///< Set the lidar difop port number, the default is 7788
  41. // m_lidar_params.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788
  42. m_lidar_params.decoder_param.min_distance = params.min_range();
  43. m_lidar_params.decoder_param.max_distance = params.max_range();
  44. m_lidar_params.decoder_param.start_angle = params.min_angle();
  45. m_lidar_params.decoder_param.end_angle = params.max_angle();
  46. m_lidar_params.decoder_param.dense_points = true;
  47. // if(params.model() == "HELIOS16")
  48. m_lidar_params.lidar_type = LidarType::RSHELIOS_16P; ///< Set the lidar type. Make sure this type is correct
  49. // 驱动初始化
  50. m_lidar_driver.regPointCloudCallback(std::bind(&RS_lidar_device::driverGetPointCloudFromCallerCallback, this), std::bind(&RS_lidar_device::driverReturnPointCloudToCallerCallback, this, std::placeholders::_1)); ///< Register the point cloud callback functions
  51. m_lidar_driver.regExceptionCallback(std::bind(&RS_lidar_device::exceptionCallback, this, std::placeholders::_1)); ///< Register the exception callback function
  52. if (!m_lidar_driver.init(m_lidar_params)) ///< Call the init function
  53. {
  54. LOG(WARNING) << "Driver Initialize Error...";
  55. t_error = Error_manager(Error_code::VELODYNE_LIDAR_UNINITIALIZED, Error_level::MAJOR_ERROR,
  56. (std::string("RS_lidar_device::init driver error ")+std::to_string(params.port())).c_str());
  57. return t_error;
  58. }
  59. mp_cloud_handle_thread = new std::thread(std::bind(&RS_lidar_device::processCloud, this));
  60. m_lidar_driver.start();
  61. m_rs_protocol_status = E_READY;
  62. //初始化雷达通信模块
  63. std::string t_ip = params.ip();
  64. int t_port = params.port();
  65. m_rs_lidar_device_status = E_READY;
  66. LOG(INFO) << "rslidar device "<< m_lidar_params.input_param.msop_port<<" initialized.\n";
  67. m_lidar_params.print();
  68. return t_error;
  69. }
  70. //反初始化
  71. Error_manager RS_lidar_device::uninit()
  72. {
  73. mb_exit_process = true;
  74. if(mp_cloud_handle_thread != nullptr)
  75. {
  76. if(mp_cloud_handle_thread->joinable())
  77. mp_cloud_handle_thread->join();
  78. delete(mp_cloud_handle_thread);
  79. mp_cloud_handle_thread = nullptr;
  80. }
  81. m_lidar_driver.stop();
  82. if (m_rs_lidar_device_status != E_FAULT)
  83. {
  84. m_rs_lidar_device_status = E_UNKNOWN;
  85. }
  86. return Error_code::SUCCESS;
  87. }
  88. Error_manager RS_lidar_device::setConfigFile(std::string path) {
  89. m_config_file = path;
  90. return SUCCESS;
  91. }
  92. //检查雷达状态,是否正常运行
  93. Error_manager RS_lidar_device::check_status()
  94. {
  95. update_status();
  96. if (m_rs_lidar_device_status == E_READY)
  97. {
  98. return Error_code::SUCCESS;
  99. }
  100. else if (m_rs_lidar_device_status == E_BUSY)
  101. {
  102. return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
  103. " RS_lidar_device::check_status() error ");
  104. }
  105. else
  106. {
  107. return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
  108. " RS_lidar_device::check_status() error ");
  109. }
  110. return Error_code::SUCCESS;
  111. }
  112. //判断是否为待机,如果已经准备好,则可以执行任务。
  113. bool RS_lidar_device::is_ready()
  114. {
  115. update_status();
  116. return m_rs_lidar_device_status == E_READY;
  117. }
  118. Error_manager RS_lidar_device::get_new_ring_cloud_and_wait(std::mutex *p_mutex, std::vector<PointT> &cloud_vec,
  119. std::chrono::steady_clock::time_point command_time, int timeout_ms)
  120. {
  121. if(p_mutex == nullptr)
  122. {
  123. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  124. " POINTER IS NULL ");
  125. }
  126. //判断是否超时
  127. while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
  128. {
  129. //获取指令时间之后的点云, 如果没有就会循环
  130. if (m_capture_time > command_time)
  131. {
  132. std::unique_lock<std::mutex> lck(*p_mutex);
  133. {
  134. cloud_vec.clear();
  135. std::lock_guard<std::mutex> lck(m_cloud_mutex);
  136. if (m_ring_cloud.size() <= 0)
  137. return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
  138. for (size_t i = 0; i < m_ring_cloud.size(); i++)
  139. {
  140. cloud_vec.assign(m_ring_cloud.begin(), m_ring_cloud.end());
  141. }
  142. }
  143. return SUCCESS;
  144. }
  145. //等1ms
  146. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  147. }
  148. //超时退出就报错
  149. return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
  150. " RS_lidar_device::get_new_ring_cloud_and_wait error ");
  151. }
  152. //外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
  153. Error_manager RS_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  154. std::chrono::steady_clock::time_point command_time, int timeout_ms)
  155. {
  156. if (p_mutex == NULL || p_cloud.get() == NULL)
  157. {
  158. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  159. " POINTER IS NULL ");
  160. }
  161. //判断是否超时
  162. while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
  163. {
  164. //获取指令时间之后的点云, 如果没有就会循环
  165. if (m_capture_time > command_time)
  166. {
  167. std::unique_lock<std::mutex> lck(*p_mutex);
  168. {
  169. std::lock_guard<std::mutex> lck(m_cloud_mutex);
  170. if (m_ring_cloud.size() <= 0)
  171. return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
  172. for (size_t i = 0; i < m_ring_cloud.size(); i++)
  173. {
  174. p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
  175. }
  176. }
  177. return SUCCESS;
  178. }
  179. //else 等待下一次数据更新
  180. //等1ms
  181. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  182. }
  183. //超时退出就报错
  184. return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
  185. " RS_lidar_device::get_new_cloud_and_wait error ");
  186. }
  187. //外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
  188. Error_manager RS_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  189. std::chrono::steady_clock::time_point command_time)
  190. {
  191. if (p_mutex == NULL || p_cloud.get() == NULL)
  192. {
  193. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  194. " POINTER IS NULL ");
  195. }
  196. //获取指令时间之后的点云, 如果没有就会报错, 不会等待
  197. if (m_capture_time > command_time)
  198. {
  199. std::unique_lock<std::mutex> lck(*p_mutex);
  200. {
  201. std::lock_guard<std::mutex> lck(m_cloud_mutex);
  202. if (m_ring_cloud.size() <= 0)
  203. return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
  204. for (size_t i = 0; i < m_ring_cloud.size(); i++)
  205. {
  206. p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
  207. }
  208. }
  209. return SUCCESS;
  210. }
  211. else
  212. {
  213. return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
  214. " RS_lidar_device::get_current_cloud error ");
  215. }
  216. }
  217. //外部调用获取最新的点云, 如果没有就会报错, 不会等待
  218. Error_manager RS_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  219. std::chrono::steady_clock::time_point command_time)
  220. {
  221. if (p_mutex == NULL || p_cloud.get() == NULL)
  222. {
  223. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  224. " POINTER IS NULL ");
  225. }
  226. if (m_capture_time > command_time - std::chrono::milliseconds(int(60000.0/(m_params.rpm()*0.5))))
  227. {
  228. std::unique_lock<std::mutex> lck(*p_mutex);
  229. {
  230. std::lock_guard<std::mutex> lck(m_cloud_mutex);
  231. if (m_ring_cloud.size() <= 0)
  232. return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
  233. for (size_t i = 0; i < m_ring_cloud.size(); i++)
  234. {
  235. p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
  236. }
  237. }
  238. return SUCCESS;
  239. }
  240. else
  241. {
  242. return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
  243. " RS_lidar_device::get_current_cloud error ");
  244. }
  245. }
  246. //外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
  247. //注:函数内部不加锁, 由外部统一加锁.
  248. Error_manager RS_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  249. std::chrono::steady_clock::time_point command_time)
  250. {
  251. if (p_cloud.get() == NULL)
  252. {
  253. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  254. " POINTER IS NULL ");
  255. }
  256. if (m_capture_time > command_time - std::chrono::milliseconds(int(60000.0/(m_params.rpm()*0.5))))
  257. {
  258. std::lock_guard<std::mutex> lck(m_cloud_mutex);
  259. if (m_ring_cloud.size() <= 0)
  260. return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
  261. for (size_t i = 0; i < m_ring_cloud.size(); i++)
  262. {
  263. p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
  264. }
  265. return SUCCESS;
  266. }
  267. else
  268. {
  269. return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
  270. " RS_lidar_device::get_current_cloud error ");
  271. }
  272. }
  273. RS_lidar_device::RS_lidar_device_status RS_lidar_device::get_status()
  274. {
  275. update_status();
  276. return m_rs_lidar_device_status;
  277. }
  278. void RS_lidar_device::update_status()
  279. {
  280. if (m_rs_protocol_status == E_FAULT)
  281. {
  282. m_rs_lidar_device_status = E_FAULT;
  283. }
  284. else if(m_rs_protocol_status == E_BUSY)
  285. {
  286. m_rs_lidar_device_status = E_BUSY;
  287. }
  288. else if (m_rs_protocol_status == E_READY)
  289. {
  290. m_rs_lidar_device_status = E_READY;
  291. }
  292. else
  293. {
  294. // LOG(WARNING)<<"unknown????? "<<m_rs_socket_status<<", "<<m_rs_protocol_status;
  295. m_rs_lidar_device_status = E_UNKNOWN;
  296. }
  297. }