rslidar_driver.h 7.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216
  1. #ifndef RS_LIDAR_ENCAPSULATION_HH
  2. #define RS_LIDAR_ENCAPSULATION_HH
  3. #include <iostream>
  4. #include <thread>
  5. #include <mutex>
  6. #include <chrono>
  7. #include <functional>
  8. #include "../device_base.hpp"
  9. #include <Eigen/Core>
  10. #include <Eigen/Geometry>
  11. #include <pcl/point_types.h>
  12. #include <pcl/point_cloud.h>
  13. #include <glog/logging.h>
  14. #include <rs_driver/api/lidar_driver.hpp>
  15. #ifdef ENABLE_PCL_POINTCLOUD
  16. #include <rs_driver/msg/pcl_point_cloud_msg.hpp>
  17. #else
  18. #include <rs_driver/msg/point_cloud_msg.hpp>
  19. #endif
  20. #include "velodyne_config.pb.h"
  21. using namespace robosense::lidar;
  22. typedef PointXYZIRT PointT;
  23. typedef PointCloudT<PointT> PointCloudMsg;
  24. class RS_lidar_device: public LidarDevice
  25. {
  26. public:
  27. //万集设备身状态
  28. enum RS_lidar_device_status
  29. {
  30. E_UNKNOWN = 0, //未知
  31. E_READY = 1, //正常待机
  32. E_DISCONNECT = 2, //断连
  33. E_BUSY = 3, //工作正忙
  34. E_FAULT =10, //故障
  35. };
  36. public:
  37. // 无参构造函数
  38. RS_lidar_device();
  39. // 析构函数
  40. ~RS_lidar_device();
  41. // ************************* rslidar driver callbacks *******************************
  42. // @brief point cloud callback function. The caller should register it to the lidar driver.
  43. // Via this fucntion, the driver gets an free/unused point cloud message from the caller.
  44. // @param msg The free/unused point cloud message.
  45. //
  46. std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void)
  47. {
  48. // std::cout<<"get cloud from caller callback"<<std::endl;
  49. // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver,
  50. // so please DO NOT do time-consuming task here.
  51. std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
  52. if (msg.get() != NULL)
  53. {
  54. return msg;
  55. }
  56. return std::make_shared<PointCloudMsg>();
  57. }
  58. //
  59. // @brief point cloud callback function. The caller should register it to the lidar driver.
  60. // Via this function, the driver gets/returns a stuffed point cloud message to the caller.
  61. // @param msg The stuffed point cloud message.
  62. //
  63. void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg)
  64. {
  65. // std::cout<<"return point callback"<<std::endl;
  66. // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver,
  67. // so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processCloud() below)
  68. // stuffed_cloud_queue.push(msg);
  69. stuffed_cloud_queue.push(msg);
  70. }
  71. //
  72. // @brief exception callback function. The caller should register it to the lidar driver.
  73. // Via this function, the driver inform the caller that something happens.
  74. // @param code The error code to represent the error/warning/information
  75. //
  76. void exceptionCallback(const Error &code)
  77. {
  78. // std::cout<<"exception callback"<<std::endl;
  79. // Note: This callback function runs in the packet-receving and packet-parsing/point-cloud_constructing thread of the driver,
  80. // so please DO NOT do time-consuming task here.
  81. if(ERRCODE_MSOPTIMEOUT == code.error_code)
  82. {
  83. m_rs_lidar_device_status = E_DISCONNECT;
  84. }else if(ERRCODE_SUCCESS == code.error_code)
  85. {
  86. m_rs_lidar_device_status = E_READY;
  87. m_rs_protocol_status = E_READY;
  88. // RS_WARNING << code.toString() << RS_REND;
  89. }else if(code.error_code_type == ErrCodeType::WARNING_CODE || code.error_code_type == ErrCodeType::ERROR_CODE)
  90. {
  91. m_rs_lidar_device_status = E_FAULT;
  92. m_rs_protocol_status = E_FAULT;
  93. // RS_WARNING << code.toString() << RS_REND;
  94. }
  95. RS_WARNING << code.toString() << RS_REND;
  96. }
  97. void processCloud(void)
  98. {
  99. while (!mb_exit_process)
  100. {
  101. usleep(1);
  102. std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();
  103. if (msg.get() == NULL)
  104. {
  105. continue;
  106. }
  107. m_capture_time = std::chrono::steady_clock::now();
  108. // RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;
  109. std::lock_guard<std::mutex> lck(m_cloud_mutex);
  110. m_ring_cloud.clear();
  111. for (size_t i = 0; i < msg->points.size(); i++)
  112. {
  113. Eigen::Vector4d point{msg->points[i].x, msg->points[i].y, msg->points[i].z, 1};
  114. point = m_calib_matrix * point;
  115. point.x() = point.x() / point.w();
  116. point.y() = point.y() / point.w();
  117. point.z() = point.z() / point.w();
  118. msg->points[i].x = point.x();
  119. msg->points[i].y = point.y();
  120. msg->points[i].z = point.z();
  121. m_ring_cloud.push_back(msg->points[i]);
  122. }
  123. free_cloud_queue.push(msg);
  124. }
  125. }
  126. // 初始化
  127. Error_manager init();
  128. Error_manager init(velodyne::velodyneLidarParams params);
  129. Error_manager uninit();
  130. Error_manager setConfigFile(std::string path);
  131. //检查雷达状态,是否正常运行
  132. Error_manager check_status();
  133. //判断是否为待机,如果已经准备好,则可以执行任务。
  134. bool is_ready();
  135. int get_lidar_id() { return m_lidar_id; }
  136. // 外部调用获取最新带环信息点云,通常用于标定
  137. Error_manager get_new_ring_cloud_and_wait(std::mutex* p_mutex, std::vector<PointT> &cloud_vec,
  138. std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);
  139. //外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
  140. Error_manager get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  141. std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);
  142. //外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
  143. // 注意!!!当前最新点云未成环,或许缺失数据
  144. Error_manager get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  145. std::chrono::steady_clock::time_point command_time);
  146. //外部调用获取最新的点云, 如果没有就会报错, 不会等待
  147. Error_manager get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  148. std::chrono::steady_clock::time_point command_time);
  149. //外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
  150. //注:函数内部不加锁, 由外部统一加锁.
  151. Error_manager get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
  152. std::chrono::steady_clock::time_point command_time);
  153. RS_lidar_device_status get_status();
  154. void update_status();
  155. protected:
  156. //状态
  157. RS_lidar_device_status m_rs_lidar_device_status; //设备状态
  158. RS_lidar_device_status m_rs_protocol_status;
  159. // 雷达编号
  160. int m_lidar_id;
  161. // 雷达配置
  162. velodyne::velodyneLidarParams m_params;
  163. RSDriverParam m_lidar_params;
  164. LidarDriver<PointCloudMsg> m_lidar_driver; ///< Declare the driver object
  165. // // 空点云缓存
  166. SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;
  167. SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;
  168. bool mb_exit_process;
  169. std::thread* mp_cloud_handle_thread;
  170. std::mutex m_cloud_mutex;
  171. // 完整可获取点云
  172. std::vector<PointT> m_ring_cloud;
  173. // 获取点云时间
  174. std::chrono::steady_clock::time_point m_capture_time;
  175. // 点云内参齐次矩阵
  176. Eigen::Matrix4d m_calib_matrix;
  177. private:
  178. private:
  179. std::string m_config_file;
  180. };
  181. #endif // !RS_LIDAR_ENCAPSULATION_HH