free_lidar_node.h 1.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778
  1. #ifndef FREE_LIDAR_NODE_H_
  2. #define FREE_LIDAR_NODE_H_
  3. #include <free_lidar/lidar_driver.h>
  4. #include <free_lidar/c2_eth_driver.h>
  5. #include <free_lidar/c2_uart_driver.h>
  6. #include <free_lidar/h1_eth_driver.h>
  7. #include <free_lidar/trailing_filter.h>
  8. #include <ros/ros.h>
  9. #include <stdint.h>
  10. #include <stdio.h>
  11. #include <iostream>
  12. #include <std_msgs/String.h>
  13. #include <sensor_msgs/LaserScan.h>
  14. namespace free_optics {
  15. class LidarDriver;
  16. class C2EthDriver;
  17. class C2UartDriver;
  18. class H1EthDriver;
  19. class TrailingFilter;
  20. class FreeLidarNode
  21. {
  22. public:
  23. FreeLidarNode();
  24. ~FreeLidarNode();
  25. bool isConnected() {return is_connected_;}
  26. bool getScanData();
  27. sensor_msgs::LaserScan scanmsg;
  28. void cmdMsgCallback(const std_msgs::StringConstPtr &msg);
  29. int scan_frequency_, scan_resolution_;
  30. bool singleFilter(std::vector<float> tempdisx , int pointnum);
  31. // 对射变量
  32. std::deque<float> Dist_pre;
  33. std::deque<float> Dist_out;
  34. std::deque<float> Dist_flag;
  35. std::deque<float> Dist_flag2;
  36. int angle_m = 0;
  37. int angle_n = 0;
  38. int Anglenum;
  39. int Speednum;
  40. float idiff1 = 0;
  41. float idiff2 = 0;
  42. float idiff3 = 0;
  43. float idiff4 = 0;
  44. float idiff = 0;
  45. int Runtimes = 1 ;
  46. private:
  47. bool connect();
  48. ros::NodeHandle nh_;
  49. ros::Publisher scan_publisher_;
  50. ros::Publisher info_publisher_;
  51. ros::Subscriber cmd_subscriber_;
  52. std::string frame_id_;
  53. std::string scanner_ip_, port_name_;
  54. std::string scanner_type_;
  55. uint32_t scanner_fw_, scanner_sn_;
  56. float range_max_, range_min_;
  57. int32_t start_angle_, stop_angle_;
  58. bool filter_switch_;
  59. bool is_ethernet_;
  60. LidarDriver *driver_;
  61. TrailingFilter *filter_;
  62. bool is_connected_;
  63. };
  64. }
  65. #endif