wj_718_lidar_protocol.h 1.5 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253
  1. #ifndef WJ_718_LIDAR_PROTOCOL_H
  2. #define WJ_718_LIDAR_PROTOCOL_H
  3. #include <iostream>
  4. #include "string.h"
  5. #include <boost/shared_ptr.hpp>
  6. #include <boost/asio.hpp>
  7. #include <boost/asio/placeholders.hpp>
  8. #include <boost/system/error_code.hpp>
  9. #include <boost/bind/bind.hpp>
  10. #include <ros/ros.h>
  11. #include <visualization_msgs/Marker.h>
  12. #include <sensor_msgs/LaserScan.h>
  13. #include <dynamic_reconfigure/server.h>
  14. #include <wj_718_lidar/wj_718_lidarConfig.h>
  15. using namespace std ;
  16. namespace wj_lidar
  17. {
  18. #define MAX_LENGTH_DATA_PROCESS 200000
  19. typedef struct TagDataCache
  20. {
  21. unsigned char m_acdata[MAX_LENGTH_DATA_PROCESS];
  22. unsigned int m_u32in;
  23. unsigned int m_u32out;
  24. }DataCache;
  25. class wj_718_lidar_protocol
  26. {
  27. public:
  28. wj_718_lidar_protocol();
  29. bool dataProcess(unsigned char *data,const int reclen);
  30. bool protocl(unsigned char *data,const int len);
  31. bool OnRecvProcess(unsigned char *data, int len);
  32. bool checkXor(unsigned char *recvbuf, int recvlen);
  33. void send_scan(const char *data,const int len);
  34. ros::NodeHandle nh;
  35. ros::Publisher marker_pub;
  36. sensor_msgs::LaserScan scan;
  37. bool setConfig(wj_718_lidar::wj_718_lidarConfig &new_config,uint32_t level);
  38. bool heartstate;
  39. private:
  40. unsigned char data_[MAX_LENGTH_DATA_PROCESS];
  41. DataCache m_sdata;
  42. wj_718_lidar::wj_718_lidarConfig config_;
  43. unsigned int g_u32PreFrameNo;
  44. float scandata[811];
  45. float scandata_te[811];
  46. float scaninden[811];
  47. int freq_scan;
  48. int resizeNum;
  49. };
  50. }
  51. #endif // WJ_718_LIDAR_PROTOCOL_H