123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778 |
- #ifndef FREE_LIDAR_NODE_H_
- #define FREE_LIDAR_NODE_H_
- #include <free_lidar/lidar_driver.h>
- #include <free_lidar/c2_eth_driver.h>
- #include <free_lidar/c2_uart_driver.h>
- #include <free_lidar/h1_eth_driver.h>
- #include <free_lidar/trailing_filter.h>
- #include <ros/ros.h>
- #include <stdint.h>
- #include <stdio.h>
- #include <iostream>
- #include <std_msgs/String.h>
- #include <sensor_msgs/LaserScan.h>
- namespace free_optics {
- class LidarDriver;
- class C2EthDriver;
- class C2UartDriver;
- class H1EthDriver;
- class TrailingFilter;
- class FreeLidarNode
- {
- public:
- FreeLidarNode();
- ~FreeLidarNode();
- bool isConnected() {return is_connected_;}
- bool getScanData();
- sensor_msgs::LaserScan scanmsg;
- void cmdMsgCallback(const std_msgs::StringConstPtr &msg);
- int scan_frequency_, scan_resolution_;
- bool singleFilter(std::vector<float> tempdisx , int pointnum);
- // 对射变量
- std::deque<float> Dist_pre;
- std::deque<float> Dist_out;
- std::deque<float> Dist_flag;
- std::deque<float> Dist_flag2;
- int angle_m = 0;
- int angle_n = 0;
- int Anglenum;
- int Speednum;
- float idiff1 = 0;
- float idiff2 = 0;
- float idiff3 = 0;
- float idiff4 = 0;
- float idiff = 0;
- int Runtimes = 1 ;
- private:
- bool connect();
- ros::NodeHandle nh_;
- ros::Publisher scan_publisher_;
- ros::Publisher info_publisher_;
- ros::Subscriber cmd_subscriber_;
- std::string frame_id_;
- std::string scanner_ip_, port_name_;
- std::string scanner_type_;
- uint32_t scanner_fw_, scanner_sn_;
- float range_max_, range_min_;
- int32_t start_angle_, stop_angle_;
-
- bool filter_switch_;
- bool is_ethernet_;
- LidarDriver *driver_;
- TrailingFilter *filter_;
- bool is_connected_;
- };
- }
- #endif
|